// 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 "group.h" using namespace std; /// Constructeur /// @param imgHeight hauteur de l'image /// @param imgWidth largeur de l'image Space::Space (int imgHeight, int imgWidth, OConfig *oconfig) { Space::imgHeight = imgHeight; Space::imgWidth = imgWidth; Space::oconfig = oconfig; aY = bY = cY = 0; aX = aY = 0; } /// Destructeur Space::~Space () { } /// Ajoute un point pour l'etalonnage void Space::AddSetupPoint(int x, int y, int distx, int disty) { if (setupTab.size() > 3) { std::vector::iterator iter; iter = setupTab.begin(); setupTab.erase(iter); } SETUP_POINT setupPoint; setupPoint.x = x; setupPoint.y = y; setupPoint.distx = distx; setupPoint.disty = disty; setupTab.push_back(setupPoint); } /// Chargement des points a partir d'un fichier void Space::LoadFromFile() { for (int i=0; i<3; i++) AddSetupPoint(oconfig->tabPoint[i][0], oconfig->tabPoint[i][1], oconfig->tabPoint[i][2], oconfig->tabPoint[i][3]); } /// Calcul du determinant d'une matrice 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; } /// Calcul du déterminant caractéristique d'une matrice 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; // cout << "aY: " << aY << " bY: " << bY << "cY: " << cY << endl; } 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 locX, locY : position de la balle dans le rÃfÃrentiel du robot /// @param posRobotX, posRobotY, angleRobot : position du robot sur la table /// @param posX, posY : variable de retour pour la position de la balle sur la table void Space::GetPos(int locX, int locY, int posRobotX, int posRobotY, double angleRobot, int &posX, int &posY) { float sinus = sin(angleRobot); float cosinus = cos(angleRobot); // Calcul des coordonnes avec le changement de repere posX = (int)(posRobotX + locX*cosinus - locY*sinus); posY = (int)(posRobotY + locX*sinus + locY*cosinus); }