summaryrefslogtreecommitdiff
path: root/2005/i/robert/src/ovision/see/space.cc
diff options
context:
space:
mode:
Diffstat (limited to '2005/i/robert/src/ovision/see/space.cc')
-rw-r--r--2005/i/robert/src/ovision/see/space.cc189
1 files changed, 189 insertions, 0 deletions
diff --git a/2005/i/robert/src/ovision/see/space.cc b/2005/i/robert/src/ovision/see/space.cc
new file mode 100644
index 0000000..c77a6b2
--- /dev/null
+++ b/2005/i/robert/src/ovision/see/space.cc
@@ -0,0 +1,189 @@
+// space.cc - Classe Space
+// robert - Programme du robot APBteam
+// Copyright (C) 2005 Olivier Gaillard
+
+/// @file space.cc Etalonnage des distances et localisation de la balle
+
+#include "space.hh"
+
+#include "group.hh"
+#include "stdio.hh"
+
+using namespace std;
+
+/// Constructeur
+/// @param imgHeight hauteur de l'image
+/// @param imgWidth largeur de l'image
+Space::Space (int imgWidth, int imgHeight)
+{
+ Space::imgHeight = imgHeight;
+ Space::imgWidth = imgWidth;
+
+ Space::oconfig = OConfig::GetInstance ();
+
+ tabY = NULL;
+ tabX = NULL;
+}
+
+
+/// Destructeur
+Space::~Space ()
+{
+}
+
+
+/// Cree un fichier gnuplot avec la courbe des points donnés par la courbe x et y
+void
+Space::CreateGnuPlotFile (int y)
+{
+ FILE *file;
+ double locY, locX;
+
+ // Ouverture du fichier
+ file = fopen("dataY", "w+");
+
+ // Parcours pour tous les pixels verticaux de l'image
+ for (int i=0; i<296; i++)
+ {
+ GetLoc(0, i, locX, locY);
+ fprintf(file, "%i\t%f\n", i, locY);
+ }
+ // Fermeture du fichier
+ fclose(file);
+
+ // Ouverture du fichier
+ file = fopen("dataX", "w+");
+
+ // Parcours de tous les pixels horizontaux
+ for (int i=0; i<352; i++)
+ {
+ GetLoc(0, y, locX, locY);
+ fprintf(file, "%i\t%f\n", i, locX);
+ }
+ // Fermeture du fichier
+ fclose(file);
+
+}
+
+
+/// 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);
+}
+
+
+/// Chargement des points a partir d'un fichier
+void
+Space::LoadFromFile()
+{
+ // Parcours de tous les points du fichier dist
+ 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;
+}
+
+
+/// Donne les coefficients d'une ligne
+void
+Space::FindCoeffLine (double x1, double y1, double x2, double y2, double &a, double &b)
+{
+ a = (x2 - x1) / (y2 - y1);
+ b = x1 - a * y1;
+}
+
+
+/// Etalonnage des distances
+int
+Space::Setup(double a, double b, double c)
+{
+ // Assignation des coefficients pour le calcul des y
+ aY = a;
+ bY = b;
+ cY = c;
+
+ double a1, b1, a2, b2;
+
+ // Cherche les coordonnées de 2 droites
+ FindCoeffLine (65, 9, 118, 180, a1, b1);
+ FindCoeffLine (198, 10, 192, 180, a2, b2);
+
+ // Allocation de la mémoire
+ delete []tabY;
+ tabY = new double[imgHeight];
+
+ delete []tabX;
+ tabX = new double[imgHeight*imgWidth];
+
+ // Création du tableau de correspondance pour les x
+ double diffPix;
+ double unitPix;
+ double center;
+ for (int y=0; y<imgHeight; y++)
+ {
+ // Calcul de la "longeur réelle" d'un pixel
+ diffPix = (a2*y+b2) - (a1*y+b1);
+ unitPix = 100.0f / diffPix;
+ tabY[y] = unitPix;
+
+ // Calcul du centre du robot sur l'image en fonction du y
+ center = a2*y + b2;
+// cout << y << "\t" << tabY[y] << endl;
+
+ // Parcours de tous les x de la ligne et calcul de la distance correspondante
+ for (int x=0; x<imgWidth; x++)
+ tabX[y*imgWidth + x] = unitPix *(x - center);
+ }
+
+ 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 = tabX[locImgY*imgWidth + locImgX];
+
+ // Calcul de locY
+ locY = aY*locImgY*locImgY + bY*locImgY + cY;
+}
+
+
+
+/// Position d'un objet dans le referentiel du robot
+void
+Space::GetLoc(int locImgX, int locImgY, int &locX, int &locY)
+{
+ 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(double locX, double locY, double posRobotX, double posRobotY, double angleRobot, double &posX, double &posY)
+{
+ double sinus = sin(angleRobot);
+ double cosinus = cos(angleRobot);
+
+ // Calcul des coordonnes avec le changement de repere
+ posX = posRobotX + locX*cosinus - locY*sinus;
+ posY = posRobotY + locX*sinus + locY*cosinus;
+}
+