summaryrefslogtreecommitdiff
path: root/2004/i/nono/src/ovision/space.cc
blob: 4b0ad30edec584e0a5bac446d139b841ca51772d (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
// space.cc - Classe Space
// nono - Programme du robot Efrei Robotique I1-I2 2004
// Copyright (C) 2004 Olivier Gaillard

/// @file space.cc Etalonnage des distances et localisation de la balle

#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 imgWidth, int imgHeight)
{
    Space::imgHeight = imgHeight;
    Space::imgWidth = imgWidth;

    Space::oconfig = OConfig::GetInstance ();

    tabY = NULL;
    tabX = NULL;
}


/// Destructeur
Space::~Space ()
{
}


void Space::CreateGnuPlotFile (int y)
{
    FILE *file;
    double locY, locX;

    file = fopen("dataY", "w+");
    for (int i=0; i<296; i++)
      {
	GetLoc(0, i, locX, locY);
	fprintf(file, "%i\t%lf\n", i, locY);
      }
    fclose(file);

    file = fopen("dataX", "w+");
    for (int i=0; i<352; i++)
      {
	GetLoc(0, y, locX, locY);
	fprintf(file, "%i\t%lf\n", i, locX);
      }
    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()
{
    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::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)
{
    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);

    cout << a1 << " " << b1 << " " << a2 << " " << b2 << endl;
    
    delete []tabY;
    tabY = new double[imgHeight];
    
    delete []tabX;
    tabX = new double[imgHeight*imgWidth];

    double diffPix;
    double unitPix;
    double center;
    for (int y=0; y<imgHeight; y++)
      {
	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);
      }
    
    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; 
}


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;
}