// 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 #include "imgFile.h" #include "segmNN.h" #include "group.h" using namespace std; /*namespace std { struct less { 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::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); /* // 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); }