From 109db2b0cce1b89c75e32f3705a1c5975391b3c5 Mon Sep 17 00:00:00 2001 From: gaillaro Date: Sat, 1 May 2004 11:52:08 +0000 Subject: Ajout de ovision. --- 2004/i/nono/src/ovision/space.cc | 341 +++++++++++++++++++++++++++++++++++++++ 1 file changed, 341 insertions(+) create mode 100644 2004/i/nono/src/ovision/space.cc (limited to '2004/i/nono/src/ovision/space.cc') 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 + +#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); +} -- cgit v1.2.3