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/space.cc | 317 +++++++++------------------------------ 1 file changed, 73 insertions(+), 244 deletions(-) (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 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); } +*/ -- cgit v1.2.3