From 79c6701a4dce6289b6af64fdbad2e6aa17ea4e1c Mon Sep 17 00:00:00 2001 From: gaillaro Date: Sun, 2 May 2004 05:47:48 +0000 Subject: ajout de la classe Space --- 2004/i/nono/src/ovision/map.cc | 20 ++- 2004/i/nono/src/ovision/map.h | 18 ++- 2004/i/nono/src/ovision/space.cc | 317 +++++++++------------------------------ 2004/i/nono/src/ovision/space.h | 121 +++++---------- 4 files changed, 139 insertions(+), 337 deletions(-) diff --git a/2004/i/nono/src/ovision/map.cc b/2004/i/nono/src/ovision/map.cc index 6bf02e5..288ce52 100644 --- a/2004/i/nono/src/ovision/map.cc +++ b/2004/i/nono/src/ovision/map.cc @@ -28,6 +28,16 @@ Map::~Map (void) { } + +/// Accessors +const std::list::iterator & +Map::GetCurBall() const +{ + checkCurBall = false; + return CurBall; +} + + /// Ajoute une balle a la map void Map::AddBall(int *pos, ZONE *pZone) @@ -105,7 +115,13 @@ Map::AddBallsToMap(Group *group) while (pCur) { - // TODO passer par la classe space + if (!Islock()) + { + pos[0] = pCur->centerx-180; + pos[1] = pCur->centery; + SetLock(true); + } +/* // TODO passer par la classe space pos[0] = pCur->centerx; pos[1] = pCur->centery; @@ -126,7 +142,7 @@ Map::AddBallsToMap(Group *group) } pCur = pCur->next; - } + */ } // TODO decremente d'autre marqueur diff --git a/2004/i/nono/src/ovision/map.h b/2004/i/nono/src/ovision/map.h index db4416a..e0b64a0 100644 --- a/2004/i/nono/src/ovision/map.h +++ b/2004/i/nono/src/ovision/map.h @@ -50,9 +50,6 @@ class Map /// Liste de balles trouvees std::list ball; - /// Balle ayant le plus haut score - std::list::iterator curBall; - /// Stocke le numero de la balle locke int lock; @@ -68,6 +65,10 @@ class Map /// Distance robot-balle int Dist(int *pos1, int *pos2); + /// Balle ayant le plus haut score + std::list::iterator curBall; + + public: @@ -77,7 +78,10 @@ class Map /// Zone ou est situe le robot int zoneRobot; - + + /// Permet de savoir si une balle est dans curBall + bool CheckCurBall; + /// Constructeurs. Map (OConfig *config); @@ -88,7 +92,7 @@ class Map int IsLock(); /// Lock une balle pour savoir quel balle le robot suit - void SetLock(int value); + void SetLock(bool value); /// Ajoute des balles a la liste void AddBallsToMap(Group *group); @@ -99,6 +103,10 @@ class Map //Affiche les balles de la map void ShowBalls(); + + /// Accessors + std::list::iterator GetCurBall(); + protected: }; diff --git a/2004/i/nono/src/ovision/space.cc b/2004/i/nono/src/ovision/space.cc index 3a6bf63..34fb2a6 100644 --- a/2004/i/nono/src/ovision/space.cc +++ b/2004/i/nono/src/ovision/space.cc @@ -25,13 +25,14 @@ struct less { /// Constructeur /// @param imgHeight hauteur de l'image /// @param imgWidth largeur de l'image -Space::Space (uint imgHeight, uint imgWidth) +Space::Space (int imgHeight, int imgWidth) { Space::imgHeight = imgHeight; Space::imgWidth = imgWidth; - indexTabY = NULL; - indexTabX = NULL; + aY = bY = cY = 0; + aX = aY = 0; + } @@ -44,290 +45,117 @@ 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; +// cout << "\nTabLocY (" << tabLocY.size() << "):" << 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) +/// Ajoute un point pour l'etalonnage +void Space::AddSetupPoint(int x, int y, int distx, int disty) { - 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::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(); - } - } - } + SETUP_POINT setupPoint; + + setupPoint.x = x; + setupPoint.y = y; + setupPoint.distx = distx; + setupPoint.disty = disty; + setupTab.push_back(setupPoint); } -/// Cree la liste des distances etalonnees a partir d'image -void Space::DoDistTab() + +int Space::Determinant(int matrix[][4]) { - ImgFile img; - OConfig config("vision.conf"); - SegmNN segm(&img, &config); - TABLOC loc; + int det = 0; - Date &d = Date::GetInstance(); + det += matrix[0][2] * (matrix[1][0] * matrix[2][1] - matrix[2][0] * matrix[1][1]); + det -= matrix[1][2] * (matrix[0][0] * matrix[2][1] - matrix[2][0] * matrix[0][1]); + det += matrix[2][2] * (matrix[0][0] * matrix[1][1] - matrix[1][0] * matrix[0][1]); - Motor m; - Movement *mov = new MovementGoto(-0.1, 400); - m.setMovement(mov); + return det; +} - double x, y, angle; - double lastY = 0; - int lastGroupY = 0; +int Space::CaracteristicDeterminant(int matrix[][4], int numCol) +{ + int save[3]; - m.go(); - - - // On cherche la composante de profondeur - for (int i=0; i < 10; i++) + // Deplacement de la iĂme colonne et sauvegarde + // pour le calcul du determinant caracteristique + for (int i=0; i<3; 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); + save[i] = matrix[i][numCol]; + matrix[i][numCol] = matrix[i][3]; } - m.ungo(); + // Calcul du determinant caracteristique + int carDet = Space::Determinant(matrix); + // Restauration de la matrice d'origine + for (int i=0; i<3; i++) + matrix[i][numCol] = save[i]; - return 0; + return carDet; } - - - - - - - - - -/* 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); - - /* - // 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() +/// Etalonnage des distances +int Space::Setup() { - 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++) + if (setupTab.size() < 3) { - // 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; + cerr << "Space::Setup : Pas assez de points donnĂs, utilisĂ AddSetupPoint\n"; + return 0; } - for(uint y=0; y < imgHeight; y++) + // Calcul des coeff pour l'approximation de la profondeur + int matrix[3][4] = {{setupTab[0].y * setupTab[0].y, setupTab[0].y, 1, setupTab[0].disty}, + {setupTab[1].y * setupTab[1].y, setupTab[1].y, 1, setupTab[1].disty}, + {setupTab[2].y * setupTab[2].y, setupTab[2].y, 1, setupTab[2].disty}}; + int det = Space::Determinant(matrix); + if (det != 0) { - for(uint x=0; x < imgWidth; x++) - { - indexTabX[y*imgWidth+x] = 0;//distance * cos(angle) * locImageX / POS_ROBOT_ETALONNOGE_X ; - } + aY = (double)Space::CaracteristicDeterminant(matrix, 0) / det; + bY = (double)Space::CaracteristicDeterminant(matrix, 1) / det; + cY = (double)Space::CaracteristicDeterminant(matrix, 2) / det; + } + else + { + cerr << "Space::Setup : Determinant egal a zero, donner d'autres points\n"; + return 0; } + // Calcul des coeff pour l'approximation de l'horizontal + if ((setupTab[0].x != 0) && (setupTab[2].x != 0)) + { + aX = (setupTab[2].distx - setupTab[0].distx * (double)setupTab[2].x / setupTab[0].x) / (setupTab[2].y * setupTab[2].x - setupTab[0].y * setupTab[2].x); + bX = setupTab[0].distx / setupTab[0].x - aX * setupTab[0].y; + cout << "aX: " << aX << " bX: " << bX << endl; + } + else + { + cerr << "Space::Setup : Coordonnees X egales a zero, donner d'autres points\n"; + return 0; + } - + return 1; } -/// 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) +void Space::GetLoc(int locImgX, int locImgY, int &locX, int &locY) { - locX = 0; -// locY = tabLocY[indexTab[locImgY]].distance; + locX = (int)((locImgY * aX + bX) * locImgX); + locY = (int)(locImgY*locImgY*aY + locImgY*bY + cY); } -*/ /// 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) +/*void Space::GetPos(Vec *vOrig, Vec *vDir, float angle) { Vec vPos; float sinus = sin(angle); @@ -339,3 +167,4 @@ void Space::GetPos(Vec *vOrig, Vec *vDir, float angle) printf("Position: %f %f\n", vPos.x, vPos.y); } +*/ diff --git a/2004/i/nono/src/ovision/space.h b/2004/i/nono/src/ovision/space.h index 30f69b6..a10a913 100644 --- a/2004/i/nono/src/ovision/space.h +++ b/2004/i/nono/src/ovision/space.h @@ -6,116 +6,65 @@ #include #include -#include "date/date.h" -#include "motor/motor.h" - -#define TAILLE_MAX 1000 #define POS_ROBOT_ETALONNOGE_X 300 #include "group.h" -struct TABLOC { - unsigned int height; - unsigned int surface; - unsigned int locImageX; - unsigned int locImageY; - unsigned int distance; - unsigned int angle; -}; - -/// Classe Vec simplifiant l'utilisation de vecteur 2D -class Vec +struct SETUP_POINT { - public: - /// Coordonnees du vecteur - float x; - float y; - - /// Constructeurs - Vec(void); - Vec(float x1, float y1, float x2, float y2); - Vec(float x, float y); - - /// Operateur + - inline Vec operator+(Vec v) - {return Vec(x + v.x, y + v.y);} - + int x, y; + int distx, disty; }; -/// Constructeur -inline Vec::Vec(void) -{ -} - -/// Constructeur avec initialisation des coordonnees -/// @param vx coordonnees du vecteur -/// @param vy coordonnees du vecteur -inline Vec::Vec(float vx, float vy) -{ - x = vx; - y = vy; -} - - -/// Constructeur avec calcul des coordonnees suivant des points -/// @param x1 Premier point du vecteur -/// @param y1 Premier point du vecteur -/// @param x2 Deuxieme point du vecteur -/// @param y2 Deuxieme point du vecteur -inline Vec::Vec(float x1, float y1, float x2, float y2) -{ - x = x2 - x1; - y = y2 - y1; -} - /// Etalonnage des distances et localisation de la balle class Space { /// tableau d'index des distances - uint *indexTabX; - uint *indexTabY; - + uint *tabX; + uint *tabY; + /// liste des distances etalonnees - std::vector tabLocX; - std::vector tabLocY; - - /// hauteur de l'image - uint imgHeight; + std::vector setupTab; - /// largeur de l'image - uint imgWidth; - - - public: + /// hautdddeur de l'image + int imgHeight; - /// Permet d'ajouter une distance pour l'etalonnage - void SetDist(ZONE zone, unsigned int distance); + /// largeur de l'image + int imgWidth; - /// Renvoie la distane en fonction du pixel donne - unsigned int GetDistX(unsigned int locImageX, unsigned int locImageY); - unsigned int GetDistY(unsigned int locImageY); + /// coeff pour l'approximation de la profondeur + double aY, bY, cY; - /// Affiche la liste des distance etalonnees - void ShowTab(); + /// coeff pour l'approximation de l'horizontal + double aX, bX; - /// Cree la liste des distances etalonnees a partir d'image - void DoDistTab(); + public: - /// Cree le tableau d'index a partir de la liste - void DoIndexTab(); - - /// Donne la position reelle sur la table de la balle - void GetPos(Vec *vOrig, Vec *vDir, float angle); - // Constructeur - Space (uint imgWidth, uint imgHeight); - + Space (int width, int height); + // Destructeur ~Space (); - + + /// Affiche les tables de correspondances de distances + void ShowTab (); + + /// Position d'un objet dans le referentiel du robot + void GetLoc(int locImgX, int locImgY, int &locX, int &locY); + + /// Ajoute un point pour l'etalonnage + void AddSetupPoint(int x, int y, int distx, int disty); + + /// Etalonnage des distances + int Setup(); protected: + /// Calcul du determinant + int Determinant(int matrix[][4]); + + /// Calcul du determinant caracteristique + int CaracteristicDeterminant(int matrix[][4], int numRow); }; #endif // space_h -- cgit v1.2.3