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.cc135
1 files changed, 53 insertions, 82 deletions
diff --git a/2005/i/robert/src/ovision/see/space.cc b/2005/i/robert/src/ovision/see/space.cc
index c77a6b2..dc618fc 100644
--- a/2005/i/robert/src/ovision/see/space.cc
+++ b/2005/i/robert/src/ovision/see/space.cc
@@ -4,184 +4,155 @@
/// @file space.cc Etalonnage des distances et localisation de la balle
-#include "space.hh"
+#include <iostream>
+#include <fstream>
+#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::Space (const int imgWidth, const int imgHeight)
+ : imgHeight_ (imgHeight), imgWidth_ (imgWidth), tabX_ (0), tabY_ (0)
{
- Space::imgHeight = imgHeight;
- Space::imgWidth = imgWidth;
-
- Space::oconfig = OConfig::GetInstance ();
-
- tabY = NULL;
- tabX = NULL;
+ oconfig_ = OConfig::getInstance ();
}
-
/// Destructeur
Space::~Space ()
{
+ delete [] tabX_;
+ delete [] tabY_;
}
-
/// Cree un fichier gnuplot avec la courbe des points donnés par la courbe x et y
void
-Space::CreateGnuPlotFile (int y)
+Space::createGnuPlotFile (const int y)
{
- FILE *file;
double locY, locX;
-
// Ouverture du fichier
- file = fopen("dataY", "w+");
-
+ std::ofstream file ("dataY");
// Parcours pour tous les pixels verticaux de l'image
- for (int i=0; i<296; i++)
+ for (int i=0; i<imgHeight_; i++)
{
- GetLoc(0, i, locX, locY);
- fprintf(file, "%i\t%f\n", i, locY);
+ getLoc (0, i, locX, locY);
+ file << i << "\t" << locY << "\n";
}
// Fermeture du fichier
- fclose(file);
-
+ file.close ();
// Ouverture du fichier
- file = fopen("dataX", "w+");
-
+ std::ofstream file2 ("dataX");
// Parcours de tous les pixels horizontaux
- for (int i=0; i<352; i++)
+ for (int i=0; i<imgWidth_; i++)
{
- GetLoc(0, y, locX, locY);
- fprintf(file, "%i\t%f\n", i, locX);
+ getLoc (0, y, locX, locY);
+ file2 << i << "\t" << locY << "\n";
}
// Fermeture du fichier
- fclose(file);
-
+ file2.close ();
}
-
/// Ajoute un point pour l'etalonnage
void
-Space::AddSetupPoint(int x, int y, int distx, int disty)
+Space::addSetupPoint (const int x, const int y, const int distx, const int disty)
{
- SETUP_POINT setupPoint;
-
+ /// Ajout d'un point à la liste
+ SetupPoint setupPoint;
setupPoint.x = x;
setupPoint.y = y;
setupPoint.distx = distx;
setupPoint.disty = disty;
-
- setupTab.push_back(setupPoint);
+ setupTab.push_back (setupPoint);
}
-
/// Chargement des points a partir d'un fichier
void
-Space::LoadFromFile()
+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;
+ for (int i=0; i<oconfig_->spaceNbDistPoint; i++)
+ addSetupPoint(oconfig_->spaceTabPoint[i*4+0], oconfig_->spaceTabPoint[i*4+1],
+ oconfig_->spaceTabPoint[i*4+2], oconfig_->spaceTabPoint[i*4+3]);
+ std::cout << "Nombre de points loadés pour le calibrage de la distance: " << setupTab.size () << std::endl;
}
-
/// Donne les coefficients d'une ligne
void
-Space::FindCoeffLine (double x1, double y1, double x2, double y2, double &a, double &b)
+Space::findCoeffLine (const double x1, const double y1, const 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)
+Space::setup (const double a, const double b, const double c)
{
// Assignation des coefficients pour le calcul des y
- aY = a;
- bY = b;
- cY = c;
-
+ aY_ = a; bY_ = b; cY_ = c;
+ // Cherche les coefficients de 2 droites
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);
-
+ 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];
-
+ 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++)
+ 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;
+ 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;
+ // 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);
+ 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)
+Space::getLoc (const int locImgX, const int locImgY, double &locX, double &locY)
{
// Calcul de locX
- locX = tabX[locImgY*imgWidth + locImgX];
-
+ locX = tabX_[locImgY*imgWidth_ + locImgX];
// Calcul de locY
- locY = aY*locImgY*locImgY + bY*locImgY + cY;
+ 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)
+Space::getLoc (const int locImgX, const int locImgY, int &locX, int &locY)
{
double x,y;
- GetLoc(locImgX, locImgY, 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)
+Space::getPos (const double locX, const double locY, const double posRobotX, const double posRobotY,
+ const double angleRobot, double &posX, double &posY)
{
- double sinus = sin(angleRobot);
- double cosinus = cos(angleRobot);
-
+ 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;