summaryrefslogtreecommitdiff
path: root/2004/i/nono/src/ovision/space.cc
diff options
context:
space:
mode:
authorgaillaro2004-05-01 11:52:08 +0000
committergaillaro2004-05-01 11:52:08 +0000
commit109db2b0cce1b89c75e32f3705a1c5975391b3c5 (patch)
tree28da709aee1cbab296b99378e42a1e737403addf /2004/i/nono/src/ovision/space.cc
parentf95f1fb2bc439178a9e7ca6102b57c02c791f3f0 (diff)
Ajout de ovision.
Diffstat (limited to '2004/i/nono/src/ovision/space.cc')
-rw-r--r--2004/i/nono/src/ovision/space.cc341
1 files changed, 341 insertions, 0 deletions
diff --git a/2004/i/nono/src/ovision/space.cc b/2004/i/nono/src/ovision/space.cc
new file mode 100644
index 0000000..770a2d9
--- /dev/null
+++ b/2004/i/nono/src/ovision/space.cc
@@ -0,0 +1,341 @@
+// space.cc - Classe Space
+// nono - Programme du robot Efrei Robotique I1-I2 2004
+// Copyright (C) 2004 Olivier Gaillard
+
+/// @file space.cc Etalonnage des distances et localisation de la balle
+
+#include "space.h"
+#include <math.h>
+
+#include "imgFile.h"
+#include "segmNN.h"
+#include "group.h"
+
+using namespace std;
+/*namespace std
+{
+struct less<TABLOC*> {
+ bool operator()(TABLOC *loc1, TABLOC *loc2)
+ {
+ return loc1->locImageY < loc2->locImageY;
+ }
+}
+*/
+
+/// Constructeur
+/// @param imgHeight hauteur de l'image
+/// @param imgWidth largeur de l'image
+Space::Space (uint imgHeight, uint imgWidth)
+{
+ Space::imgHeight = imgHeight;
+ Space::imgWidth = imgWidth;
+
+ indexTabY = NULL;
+ indexTabX = NULL;
+}
+
+
+/// Destructeur
+Space::~Space ()
+{
+}
+
+
+/// Affiche la liste des distance etalonneess
+void Space::ShowTab()
+{
+ cout << "\nTabLocY (" << tabLocY.size() << "):" << endl;
+ cout << "num\tlocImageY\tdistance\tsurface" << endl;
+
+ // Parcours de la liste et affichage ces elements pour y
+ for (uint i=0; i < tabLocY.size(); i++)
+ {
+ cout << i << "\t" << tabLocY[i].locImageY << "\t\t" << tabLocY[i].distance << "\t\t" << tabLocY[i].surface << endl;
+ }
+
+ cout << "\nTabLocX (" << tabLocX.size() << "):" << endl;
+ cout << "num\tlocImageX\tlocImageY\tangle\tdistance" << endl;
+
+ // Parcours de la liste et affichage ces elements pour y
+ for (uint i=0; i < tabLocX.size(); i++)
+ {
+ cout << i << "\t" << tabLocX[i].locImageX << "\t\t" << tabLocX[i].locImageY << "\t\t" << tabLocX[i].angle << "\t" << tabLocX[i].distance << endl;
+ }
+}
+
+
+
+/// Permet d'ajouter une distance pour l'etalonnage
+/// @param zone zone de l'image ou la balle se situe
+/// @param distance distance de la balle au robot
+void Space::SetDist(ZONE zone, unsigned int distance)
+{
+ TABLOC loc;
+
+ // Initialisation des valeurs a ajouter a la liste
+ loc.locImageY = zone.centery;
+ loc.distance = distance;
+ loc.surface = (zone.xmax - zone.xmin) * (zone.ymax - zone.ymin);
+
+ // On range cette loc dans le tableau
+ // Si c'est le premier element
+ if (!tabLocY.size())
+ {
+ tabLocY.push_back(loc);
+ }
+ // Si ce n'est pas le premier element
+ else
+ {
+ // Trie par ordre croissant des distances
+ vector<TABLOC>::iterator it;
+ it = tabLocY.begin();
+ // Parcours de la liste pour ajouter au bon endroit
+ while (it != tabLocY.end())
+ {
+ if (it->distance < distance)
+ it++;
+ else
+ {
+ tabLocY.insert(it, loc);
+ it = tabLocY.end();
+ }
+ }
+ }
+
+}
+
+
+/// Cree la liste des distances etalonnees a partir d'image
+void Space::DoDistTab()
+{
+ ImgFile img;
+ Config config("vision.conf");
+ SegmNN segm(&img, &config);
+ TABLOC loc;
+
+ Date &d = Date::GetInstance();
+
+ Motor m;
+ Movement *mov = new MovementGoto(-0.1, 400);
+ m.setMovement(mov);
+
+ double x, y, angle;
+ double lastY = 0;
+ int lastGroupY = 0;
+
+ m.go();
+
+
+ // On cherche la composante de profondeur
+ for (int i=0; i < 10; i++)
+ {
+ m.waitOK();
+ m.tracker_.getPos(&x, &y, &angle);
+
+ if (y > lastY+10)
+ {
+ img.ReadRaw();
+ if (config.colorMode == YUV) img.RGBtoYUV();
+
+ segm.Segm();
+
+ Group group(&img, &segm);
+ group.JumpPoints(config.groupColor);
+
+ if (group.zoneListBall)
+ {
+ ZONE *pBall = group.zoneListBall;
+
+ // on teste s'il y a eu un changement dans l'image
+ if (pBall->centery < lastGroupY)
+ {
+ //group.ShowZones();
+ loc.distance = y;
+ loc.surface = (pBall->xmax - pBall->xmin) * (pBall->ymax - pBall->ymin);
+ loc.locImageY = pBall->centery;
+
+ tabLocY.push_back(loc);
+
+ lastGroupY = pBall->centery;
+ }
+ }
+ lastY = y;
+ }
+
+ d.wait(10);
+ }
+
+ m.ungo();
+
+
+ return 0;
+}
+
+
+
+
+
+
+
+
+
+
+/* const int nbFile = 6;
+ char file[nbFile][40] = {{"../thumbrobot/9.jpg"}, {"../thumbrobot/7.jpg"}, {"../thumbrobot/8.jpg"},
+ {"../thumbrobot/6.jpg"}, {"../thumbrobot/10.jpg"}, {"../thumbrobot/11.jpg"}};
+
+ segm.BuildNN(config.nn_NbCouleurs, LOAD_FROM_FILE);
+
+ // On cherche la composante de profondeur
+ for (int i=0; i < nbFile; i++)
+ {
+ img.ReadRaw(file[i]);
+ if (config.colorMode == YUV) img.RGBtoYUV();
+
+ segm.Segm();
+
+ Group group(&img, &segm);
+ group.JumpPoints(config.groupColor);
+
+ if (group.zoneListBall)
+ {
+ ZONE *pBall = group.zoneListBall;
+ //group.ShowZones();
+ loc.distance = i;
+ loc.surface = (pBall->xmax - pBall->xmin) * (pBall->ymax - pBall->ymin);
+ loc.locImageY = pBall->centery;
+
+ tabLocY.push_back(loc);
+ }
+ }
+
+ // On cherche la composante honrizontale
+/* for (int i=0; i < nbFile; i++)
+ {
+ img.ReadRaw(file[i]);
+ if (config.colorMode == YUV) img.RGBtoYUV();
+
+ segm.Segm();
+
+ Group group(&img, &segm);
+ group.JumpPoints(config.groupColor);
+
+ if (group.zoneListBall)
+ {
+ ZONE *pBall = group.zoneListBall;
+ loc.locImageY = pBall->centery;
+ loc.locImageX = pBall->centerx;
+ loc.distance = (int)sqrt((double)((pBall->centery*pBall->centery)+(pBall->centerx*pBall->centerx)));
+ loc.angle = i;
+
+ // get angle
+
+ tabLocX.push_back(loc);
+ }
+ }
+*/
+// tabLocY.sort(tabLoc.begin(), tabLoc.end(), less<BALL>);
+
+ /*
+ // Exemple de remplissage de la table au lieu d'utiliser les images
+ for(int i=0; i<10; i++) {
+ loc.locImageY = i*11;
+ loc.distance = i;
+ loc.surface = 100;
+
+ tabLocY.push_back(loc);
+ }
+ */
+}
+
+
+/// Cree le tableau d'index a partir de la liste
+void Space::DoIndexTab()
+{
+ uint start, stop;
+
+ // Allocation de la memoire
+ if (indexTabY) delete [] indexTabY;
+ indexTabY = new uint[imgHeight];
+ if (indexTabX) delete [] indexTabX;
+ indexTabX = new uint[imgHeight*imgWidth];
+
+ // Parcours de la liste des distances ajoutées avec SetDist
+ // et creation d'un tableau
+ for(uint y=0; y < tabLocY.size(); y++)
+ {
+ // Recherche des bornes entre 2 distances de la liste
+ if (y == 0)
+ {
+ start = 0;
+ stop = tabLocY[1].locImageY / 2;
+ }
+ else if (y == tabLocY.size()-1)
+ {
+ start = stop;
+ stop = imgHeight;
+ }
+ else
+ {
+ start = stop;
+ stop = (tabLocY[y+1].locImageY - tabLocY[y].locImageY) / 2 + tabLocY[y].locImageY;
+ }
+
+ // Remplissage du tableau d'index
+ for(uint j=start; j < stop; j++)
+ indexTabY[j] = y;
+ }
+
+ for(uint y=0; y < imgHeight; y++)
+ {
+ for(uint x=0; x < imgWidth; x++)
+ {
+ indexTabX[y*imgWidth+x] = 0;//distance * cos(angle) * locImageX / POS_ROBOT_ETALONNOGE_X ;
+ }
+ }
+
+
+
+}
+
+
+/// Renvoie la distance en fonction du pixel donne
+/// @param locImageY pixel de l'image ou la balle a ete trouvee
+uint Space::GetDistY(unsigned int locImageY)
+{
+ // Retourne la distance inscrit dans la table tabLocY
+ return tabLocY[indexTab[locImageY]].distance;
+}
+
+uint Space::GetDistX(unsigned int locImageX, unsigned int locImageY)
+{
+ // Retourne la distance inscrit dans la table tabLocY
+ return 1;
+}
+
+
+/// Position d'un objet dans le referentiel du robot
+/*void Space::GetLoc(uint locImgX, uint locImgY, uint *locX, uint *locY)
+{
+ locX = 0;
+// locY = tabLocY[indexTab[locImgY]].distance;
+}
+
+*/
+
+/// Donne la position reelle sur la table de la balle
+/// @param *vOrig position du robot sur la table
+/// @param *vDir position de la balle par rapport au robot
+/// @param angle rotation du robot par rapport a sa position d'origine
+void Space::GetPos(Vec *vOrig, Vec *vDir, float angle)
+{
+ Vec vPos;
+ float sinus = sin(angle);
+ float cosinus = cos(angle);
+
+ // Calcul des coordonnes avec le changement de repere
+ vPos.x = vOrig->x + vDir->x*cosinus - vDir->y*sinus;
+ vPos.y = vOrig->y + vDir->x*sinus + vDir->y*cosinus;
+
+ printf("Position: %f %f\n", vPos.x, vPos.y);
+}