summaryrefslogtreecommitdiff
path: root/2005/i/robert/src/ovision/see/space.cc
blob: c58268d936e6589da245390317d681f1964b0cc4 (plain)
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
// 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 <iostream>
#include <fstream>

#include "space.hh"
#include "group.hh"

/// Constructeur
/// @param imgHeight hauteur de l'image
/// @param imgWidth largeur de l'image
Space::Space (const int imgWidth, const int imgHeight)
    : imgHeight_ (imgHeight), imgWidth_ (imgWidth), tabX_ (0), tabY_ (0)
{
    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 (const int y) 
{
    double locY, locX;
    // Ouverture du fichier
    std::ofstream file ("dataY"); 
    // Parcours pour tous les pixels verticaux de l'image
    for (int i=0; i<imgHeight_; i++)
      {
	getLoc (0, i, locX, locY);
	file << i << "\t" << locY << "\n";
      }
    // Fermeture du fichier
    file.close ();
    // Ouverture du fichier
    std::ofstream file2 ("dataX");
    // Parcours de tous les pixels horizontaux
    for (int i=0; i<imgWidth_; i++)
      {
	getLoc (0, y, locX, locY);
	file2 << i << "\t" << locY << "\n";
      }
    // Fermeture du fichier
    file2.close ();
}

/// Ajoute un point pour l'etalonnage
void 
Space::addSetupPoint (const int x, const int y, const int distx, const int disty)
{
    /// Ajout d'un point � la liste
    SetupPoint 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_->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 (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 (const double a, const double b, const double c)
{
    // Assignation des coefficients pour le calcul des y
    aY_ = a; bY_ = b; cY_ = c;
    // Cherche les coefficients de 2 droites
    double a1, b1, 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_];
    // 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 (const int locImgX, const int locImgY, double &locX, double &locY)
{
    // Calcul de locX
//    locX = tabX_[locImgY*imgWidth_ + locImgX];

    ///XXX patch
    locX = 100-(double)locImgX/60 * 50;
    // Calcul de locY
    locY = aY_*locImgY*locImgY + bY_*locImgY + cY_; 
}

/// Position d'un objet dans le referentiel du robot
void 
Space::getLoc (const int locImgX, const 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 (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); 
    // Calcul des coordonnes avec le changement de repere
    posX = posRobotX + locX*cosinus - locY*sinus;
    posY = posRobotY + locX*sinus + locY*cosinus;
}