summaryrefslogtreecommitdiff
path: root/2005/i/robert/src/ovision/see/space.cc
blob: c77a6b2a7df23e7d5044400e3d0e3c6e11af5c00 (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
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
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;
}