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.cc279
1 files changed, 79 insertions, 200 deletions
diff --git a/2004/i/nono/src/ovision/space.cc b/2004/i/nono/src/ovision/space.cc
index 8b41521..4b0ad30 100644
--- a/2004/i/nono/src/ovision/space.cc
+++ b/2004/i/nono/src/ovision/space.cc
@@ -7,22 +7,22 @@
#include "space.h"
#include "group.h"
-
+#include "stdio.h"
using namespace std;
/// Constructeur
/// @param imgHeight hauteur de l'image
/// @param imgWidth largeur de l'image
-Space::Space (int imgHeight, int imgWidth)
+Space::Space (int imgWidth, int imgHeight)
{
Space::imgHeight = imgHeight;
Space::imgWidth = imgWidth;
- Space::oconfig = oconfig;
+ Space::oconfig = OConfig::GetInstance ();
- for (int i=0; i<NB_POINTS; i++)
- coefX[i] = coefY[i] = i;
+ tabY = NULL;
+ tabX = NULL;
}
@@ -32,241 +32,118 @@ Space::~Space ()
}
-/// Ajoute un point pour l'etalonnage
-void Space::AddSetupPoint(int x, int y, int distx, int disty)
+void Space::CreateGnuPlotFile (int y)
{
- if (setupTab.size() > NB_POINTS)
+ FILE *file;
+ double locY, locX;
+
+ file = fopen("dataY", "w+");
+ for (int i=0; i<296; i++)
{
- std::vector<SETUP_POINT>::iterator iter;
- iter = setupTab.begin();
- setupTab.erase(iter);
+ GetLoc(0, i, locX, locY);
+ fprintf(file, "%i\t%lf\n", i, locY);
}
-
- 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<NB_POINTS; i++)
- AddSetupPoint(oconfig->tabPoint[i][0], oconfig->tabPoint[i][1], oconfig->tabPoint[i][2], oconfig->tabPoint[i][3]);
-}
-
+ fclose(file);
-
-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
-double Space::Determinant(double *matrix, int n)
-{
- 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++)
+ file = fopen("dataX", "w+");
+ for (int i=0; i<352; i++)
{
- coeff = CoeffDeterminant(matrix, m, n);
-
- 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);
+ GetLoc(0, y, locX, locY);
+ fprintf(file, "%i\t%lf\n", i, locX);
}
+ fclose(file);
- 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
-double Space::CaracteristicDeterminant(double *matrix, double *endMat, int numCol)
+/// Ajoute un point pour l'etalonnage
+void
+Space::AddSetupPoint(int x, int y, int distx, int disty)
{
- 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*3+numCol];
- matrix[i*3+numCol] = endMat[i];
- }
-
- // Calcul du determinant caracteristique
- double carDet = Space::Determinant(matrix, 3);
+ SETUP_POINT setupPoint;
- // Restauration de la matrice d'origine
- for (int i=0; i<3; i++)
- matrix[i*3+numCol] = save[i];
+ setupPoint.x = x;
+ setupPoint.y = y;
+ setupPoint.distx = distx;
+ setupPoint.disty = disty;
- return carDet;
+ setupTab.push_back(setupPoint);
}
-
-int Space::BestFitDoTab(double *mat, double *endMat)
+/// Chargement des points a partir d'un fichier
+void
+Space::LoadFromFile()
{
- 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;
- }
+ for (int i=0; i<oconfig->nbDistPoint; i++)
+ AddSetupPoint(oconfig->tabPoint[i*4+0], oconfig->tabPoint[i*4+1], oconfig->tabPoint[i*4+2], oconfig->tabPoint[i*4+3]);
+ cout << "Nombre de points loadé pour le calibrage de la distance: " << setupTab.size () << endl;
}
-void Space::Setup(double aX, double bX, double cX, double aY, double bY, double cY)
+void
+Space::FindCoeffLine (double x1, double y1, double x2, double y2, double &a, double &b)
{
- coefX[0] = cX;
- coefX[1] = bX;
- coefX[2] = aX;
- coefY[0] = cY;
- coefY[1] = bY;
- coefY[2] = aY;
+ a = (x2 - x1) / (y2 - y1);
+ b = x1 - a * y1;
}
/// Etalonnage des distances
-int Space::Setup()
+int
+Space::Setup(double a, double b, double c)
{
- 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
- double mat[NB_POINTS*NB_POINTS];
- double endMat[NB_POINTS];
-
- double det = Space::Determinant(mat, 3);
-
- if (det != 0)
- {
- for (int i=0; i<3; i++)
- {
- coefY[i] = (double)Space::CaracteristicDeterminant(mat, endMat, i) / det;
- cout << coefY[i] << 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
-/* 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);
+ aY = a;
+ bY = b;
+ cY = c;
+
+ double a1, b1, a2, b2;
+
+ FindCoeffLine (65, 9, 118, 180, a1, b1);
+ FindCoeffLine (198, 10, 192, 180, a2, b2);
- if (det != 0)
- {
- for (int i=0; i<NB_POINTS; i++)
- {
- coefX[NB_POINTS-i-1] = (double)Space::CaracteristicDeterminant(mat, endMat, i) / det;
- }
+ cout << a1 << " " << b1 << " " << a2 << " " << b2 << endl;
+
+ delete []tabY;
+ tabY = new double[imgHeight];
+
+ delete []tabX;
+ tabX = new double[imgHeight*imgWidth];
- }
- else
+ double diffPix;
+ double unitPix;
+ double center;
+ for (int y=0; y<imgHeight; y++)
{
- cerr << "Space::Setup : Coordonnees X egales a zero, donner d'autres points\n";
- return 0;
+ diffPix = (a2*y+b2) - (a1*y+b1);
+ unitPix = 100.0f / diffPix;
+ tabY[y] = unitPix;
+
+ center = a2*y + b2;
+// cout << y << "\t" << tabY[y] << endl;
+ for (int x=0; x<imgWidth; x++)
+ tabX[y*imgWidth + x] = unitPix *(x - center);
}
-
-
- /* 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)
+void
+Space::GetLoc(int locImgX, int locImgY, double &locX, double &locY)
{
// Calcul de locX
- locX = (locImgY * locImgX * coefX[2] + coefX[1]*locImgX + coefX[0]);
+ locX = tabX[locImgY*imgWidth + locImgX];
// Calcul de locY
- locY = (locImgY * locImgY * coefY[2] + locImgY * coefY[1] + coefY[0]);
+ locY = aY*locImgY*locImgY + bY*locImgY + cY;
}
-void Space::GetLoc(int locImgX, int locImgY, int &locX, int &locY)
+
+void
+Space::GetLoc(int locImgX, int locImgY, int &locX, int &locY)
{
double x,y;
GetLoc(locImgX, locImgY, x, y);
@@ -274,11 +151,13 @@ void Space::GetLoc(int locImgX, int locImgY, int &locX, int &locY)
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, double &posX, double &posY)
+void
+Space::GetPos(double locX, double locY, double posRobotX, double posRobotY, double angleRobot, double &posX, double &posY)
{
double sinus = sin(angleRobot);
double cosinus = cos(angleRobot);