// 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 (int imgHeight, int imgWidth) { Space::imgHeight = imgHeight; Space::imgWidth = imgWidth; aY = bY = cY = 0; aX = aY = 0; } /// Destructeur Space::~Space () { } /// Affiche la liste des distance etalonneess void Space::ShowTab() { // cout << "\nTabLocY (" << tabLocY.size() << "):" << endl; // Parcours de la liste et affichage ces elements pour y } /// Ajoute un point pour l'etalonnage void Space::AddSetupPoint(int x, int y, int distx, int disty) { SETUP_POINT setupPoint; setupPoint.x = x; setupPoint.y = y; setupPoint.distx = distx; setupPoint.disty = disty; setupTab.push_back(setupPoint); } int Space::Determinant(int matrix[][4]) { int det = 0; 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]); return det; } int Space::CaracteristicDeterminant(int matrix[][4], int numCol) { int save[3]; // Deplacement de la iĂme colonne et sauvegarde // pour le calcul du determinant caracteristique for (int i=0; i<3; i++) { save[i] = matrix[i][numCol]; matrix[i][numCol] = matrix[i][3]; } // 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 carDet; } /// Etalonnage des distances int Space::Setup() { if (setupTab.size() < 3) { cerr << "Space::Setup : Pas assez de points donnĂs, utilisĂ AddSetupPoint\n"; return 0; } // 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) { 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; } /// Position d'un objet dans le referentiel du robot void Space::GetLoc(int locImgX, int locImgY, int &locX, int &locY) { 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) { 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); } */