summaryrefslogtreecommitdiff
path: root/2004/i/nono/src/ovision/space.cc
diff options
context:
space:
mode:
Diffstat (limited to '2004/i/nono/src/ovision/space.cc')
-rw-r--r--2004/i/nono/src/ovision/space.cc207
1 files changed, 168 insertions, 39 deletions
diff --git a/2004/i/nono/src/ovision/space.cc b/2004/i/nono/src/ovision/space.cc
index a9e232b..8b41521 100644
--- a/2004/i/nono/src/ovision/space.cc
+++ b/2004/i/nono/src/ovision/space.cc
@@ -5,24 +5,24 @@
/// @file space.cc Etalonnage des distances et localisation de la balle
#include "space.h"
-#include <math.h>
#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::Space (int imgHeight, int imgWidth)
{
Space::imgHeight = imgHeight;
Space::imgWidth = imgWidth;
Space::oconfig = oconfig;
- aY = bY = cY = 0;
- aX = aY = 0;
+ for (int i=0; i<NB_POINTS; i++)
+ coefX[i] = coefY[i] = i;
}
@@ -35,7 +35,7 @@ Space::~Space ()
/// Ajoute un point pour l'etalonnage
void Space::AddSetupPoint(int x, int y, int distx, int disty)
{
- if (setupTab.size() > 3)
+ if (setupTab.size() > NB_POINTS)
{
std::vector<SETUP_POINT>::iterator iter;
iter = setupTab.begin();
@@ -56,66 +56,150 @@ void Space::AddSetupPoint(int x, int y, int distx, int disty)
/// Chargement des points a partir d'un fichier
void Space::LoadFromFile()
{
- for (int i=0; i<3; i++)
+ for (int i=0; i<NB_POINTS; i++)
AddSetupPoint(oconfig->tabPoint[i][0], oconfig->tabPoint[i][1], oconfig->tabPoint[i][2], oconfig->tabPoint[i][3]);
}
+
+double Space::CoeffDeterminant(double *matrix, int i, int n)
+{
+ return (std::pow ((double) -1,(double) i))*matrix[i*n];
+}
+
/// Calcul du determinant d'une matrice
-int Space::Determinant(int matrix[][4])
+double Space::Determinant(double *matrix, int n)
{
- int det = 0;
+ if (n == 3)
+ return Determinant33(matrix);
+
+ double subDet = 0;
+ double coeff;
+ double *mat = new double[(n-1)*(n-1)];
+ for (int m=0; m<n; m++)
+ {
+ coeff = CoeffDeterminant(matrix, m, n);
- 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]);
+ int k = 0;
+ for (int l=n; l<n*n; l++)
+ {
+ if (l%n != m)
+ {
+ mat[k] = matrix[l];
+ k++;
+ }
+ }
+ cout << "o\n";
+
+ subDet += coeff * Determinant(mat, n-1);
+ }
+
+ delete [] mat;
+ return subDet;
+}
+
+/// Calcul du determinant d'une matrice
+double Space::Determinant33(double *matrix)
+{
+ double det = 0;
+
+ det += matrix[0*3+2] * (matrix[1*3+0] * matrix[2*3+1] - matrix[2*3+0] * matrix[1*3+1]);
+ det -= matrix[1*3+2] * (matrix[0*3+0] * matrix[2*3+1] - matrix[2*3+0] * matrix[0*3+1]);
+ det += matrix[2*3+2] * (matrix[0*3+0] * matrix[1*3+1] - matrix[1*3+0] * matrix[0*3+1]);
return det;
}
/// Calcul du déterminant caractéristique d'une matrice
-int Space::CaracteristicDeterminant(int matrix[][4], int numCol)
+double Space::CaracteristicDeterminant(double *matrix, double *endMat, int numCol)
{
- int save[3];
-
- // Deplacement de la iÃme colonne et sauvegarde
+ double 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];
+ save[i] = matrix[i*3+numCol];
+ matrix[i*3+numCol] = endMat[i];
}
// Calcul du determinant caracteristique
- int carDet = Space::Determinant(matrix);
+ double carDet = Space::Determinant(matrix, 3);
// Restauration de la matrice d'origine
for (int i=0; i<3; i++)
- matrix[i][numCol] = save[i];
+ matrix[i*3+numCol] = save[i];
return carDet;
}
+
+
+int Space::BestFitDoTab(double *mat, double *endMat)
+{
+ for (int i=0; i<3; i++)
+ endMat[i] = 0;
+
+ for (int i=0; i<9; i++)
+ mat[i] = 0;
+
+ // distance
+ for(int i=0; i<NB_POINTS; i++)
+ {
+ endMat[0] += setupTab[i].disty;
+ endMat[1] += setupTab[i].y * setupTab[i].disty;
+ endMat[2] += setupTab[i].y * setupTab[i].y * setupTab[i].disty;
+
+ mat[0] += 1;
+ mat[1] += setupTab[i].y;
+ mat[2] += setupTab[i].y * setupTab[i].y;
+ mat[3] += setupTab[i].y;
+ mat[4] += setupTab[i].y * setupTab[i].y;
+ mat[5] += setupTab[i].y * setupTab[i].y * setupTab[i].y;
+ mat[6] += setupTab[i].y * setupTab[i].y;
+ mat[7] += setupTab[i].y * setupTab[i].y * setupTab[i].y;
+ mat[8] += setupTab[i].y * setupTab[i].y * setupTab[i].y * setupTab[i].y;
+ }
+
+}
+
+
+void Space::Setup(double aX, double bX, double cX, double aY, double bY, double cY)
+{
+ coefX[0] = cX;
+ coefX[1] = bX;
+ coefX[2] = aX;
+ coefY[0] = cY;
+ coefY[1] = bY;
+ coefY[2] = aY;
+}
+
+
/// Etalonnage des distances
int Space::Setup()
{
- if (setupTab.size() < 3)
+ if (setupTab.size() < NB_POINTS)
{
cerr << "Space::Setup : Pas assez de points donnés, utilisé AddSetupPoint\n";
+ cerr << "Il y a " << setupTab.size () << " et il faut " << NB_POINTS << endl;
return 0;
}
+ std::cout << "nbPoints: " << NB_POINTS << endl;
+
// 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);
+ double mat[NB_POINTS*NB_POINTS];
+ double endMat[NB_POINTS];
+
+ double det = Space::Determinant(mat, 3);
+
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;
+ for (int i=0; i<3; i++)
+ {
+ coefY[i] = (double)Space::CaracteristicDeterminant(mat, endMat, i) / det;
+ cout << coefY[i] << endl;
+ }
}
else
{
@@ -124,11 +208,25 @@ int Space::Setup()
}
// Calcul des coeff pour l'approximation de l'horizontal
- if ((setupTab[0].x != 0) && (setupTab[2].x != 0))
+/* for (int i=0; i<NB_POINTS; i++)
+ {
+ endMat[i] = setupTab[i].distx;
+ for (int j=0; j<NB_POINTS-1; j++)
+ mat[i*NB_POINTS+j] = pow((double)setupTab[i].x, (double)(NB_POINTS - j -1));
+
+ mat[i*(NB_POINTS+1) -1] = 1;
+ }
+
+
+ det = Space::Determinant(mat, NB_POINTS);
+
+ if (det != 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;
+ for (int i=0; i<NB_POINTS; i++)
+ {
+ coefX[NB_POINTS-i-1] = (double)Space::CaracteristicDeterminant(mat, endMat, i) / det;
+ }
+
}
else
{
@@ -136,26 +234,57 @@ int Space::Setup()
return 0;
}
+
+ /* double matrix2[NB_POINTS][4] = {{setupTab[0].y * setupTab[0].x, setupTab[0].x, 1, setupTab[0].distx},
+ {setupTab[1].y * setupTab[1].x, setupTab[1].x, 1, setupTab[1].distx},
+ {setupTab[2].y * setupTab[2].x, setupTab[2].x, 1, setupTab[2].distx}};
+
+ det = Space::Determinant(matrix2);
+ if (det != 0)
+ {
+ aX = (double)Space::CaracteristicDeterminant(matrix2, 0) / det;
+ bX = (double)Space::CaracteristicDeterminant(matrix2, 1) / det;
+ cX = (double)Space::CaracteristicDeterminant(matrix2, 2) / det;
+ cout << "aX: " << aX << " bX: " << bX << " cX: " << cX << 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, double &locX, double &locY)
+{
+ // Calcul de locX
+ locX = (locImgY * locImgX * coefX[2] + coefX[1]*locImgX + coefX[0]);
+
+ // Calcul de locY
+ locY = (locImgY * locImgY * coefY[2] + locImgY * coefY[1] + coefY[0]);
+}
+
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);
+ double x,y;
+ GetLoc(locImgX, locImgY, x, y);
+ locX = (int)x;
+ locY = (int)y;
}
/// 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)
+void Space::GetPos(int locX, int locY, int posRobotX, int posRobotY, double angleRobot, double &posX, double &posY)
{
- float sinus = sin(angleRobot);
- float cosinus = cos(angleRobot);
+ double sinus = sin(angleRobot);
+ double 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);
+ posX = posRobotX + locX*cosinus - locY*sinus;
+ posY = posRobotY + locX*sinus + locY*cosinus;
}
+