summaryrefslogtreecommitdiff
path: root/2004
diff options
context:
space:
mode:
Diffstat (limited to '2004')
-rw-r--r--2004/i/nono/src/ovision/adjust.cc4
-rw-r--r--2004/i/nono/src/ovision/comm.cc26
-rw-r--r--2004/i/nono/src/ovision/comm.h4
-rw-r--r--2004/i/nono/src/ovision/group.cc8
-rw-r--r--2004/i/nono/src/ovision/map.cc78
-rw-r--r--2004/i/nono/src/ovision/map.h2
-rw-r--r--2004/i/nono/src/ovision/oconfig.cc65
-rw-r--r--2004/i/nono/src/ovision/oconfig.h16
-rw-r--r--2004/i/nono/src/ovision/segmNN.cc8
-rw-r--r--2004/i/nono/src/ovision/segmNN.h3
-rw-r--r--2004/i/nono/src/ovision/space.cc279
-rw-r--r--2004/i/nono/src/ovision/space.h40
-rw-r--r--2004/i/nono/src/ovision/testdist.cc3
13 files changed, 225 insertions, 311 deletions
diff --git a/2004/i/nono/src/ovision/adjust.cc b/2004/i/nono/src/ovision/adjust.cc
index a58081e..03b6d75 100644
--- a/2004/i/nono/src/ovision/adjust.cc
+++ b/2004/i/nono/src/ovision/adjust.cc
@@ -18,8 +18,6 @@ using namespace std;
int window;
Comm *comm;
-#define NB_POINTS_UI 6
-
int point[NB_POINTS_UI][2] = {{-1,-1},{-1,-1},{-1,-1},{-1,-1},{-1,-1}};
int width=538; int height=395;
@@ -247,6 +245,8 @@ KeyPressed(unsigned char key, int x, int y)
glutDestroyWindow(window);
Leave(0);
}
+ if (key == 'r')
+ comm->ReloadConfig ("rc/vision.conf");
}
/// Gestion souris
diff --git a/2004/i/nono/src/ovision/comm.cc b/2004/i/nono/src/ovision/comm.cc
index d3541bf..116b7d8 100644
--- a/2004/i/nono/src/ovision/comm.cc
+++ b/2004/i/nono/src/ovision/comm.cc
@@ -41,8 +41,8 @@ Comm::Comm(char *filename)
int *pPoint = GetPpoint();
for (int i=0; i<3; i++)
{
- pPoint[i*2] = oconfig->tabPoint[i][0];
- pPoint[i*2+1] = oconfig->tabPoint[i][1];
+ pPoint[i*2] = oconfig->tabPoint[i*4];
+ pPoint[i*2+1] = oconfig->tabPoint[i*4+1];
}
// Ouverture de l'image pilote et stockage
@@ -96,6 +96,14 @@ Comm::~Comm()
}
+/// Recharge le fichier de config
+void
+Comm::ReloadConfig (char *filename)
+{
+ oconfig->Load (filename);
+}
+
+
/// Segmente et group les couleurs
void
Comm::SegmAndGroup()
@@ -371,22 +379,22 @@ Comm::ExecuteUiCmds(char *buffer)
int *pPoint;
pPoint = GetPpoint();
- for (int i=0; i<NB_POINTS; i++)
+ for (int i=0; i<NB_POINTS_UI; i++)
if (pPoint[i*2] == -1)
{
cerr << "Comm::ExecuteUiCmds : Pas assez de points selectionnes\n";
return;
}
- for (int i = 0; i<NB_POINTS; i++)
+ for (int i = 0; i<NB_POINTS_UI; i++)
{
- oconfig->tabPoint[i][0] = pPoint[i*2];
- oconfig->tabPoint[i][1] = pPoint[i*2+1];
- oconfig->tabPoint[i][2] = 0;
- oconfig->tabPoint[i][3] = 0;
+ oconfig->tabPoint[i*4+0] = pPoint[i*2];
+ oconfig->tabPoint[i*4+1] = pPoint[i*2+1];
+ oconfig->tabPoint[i*4+2] = 0;
+ oconfig->tabPoint[i*4+3] = 0;
}
- oconfig->CreateDistFile("rc/dist");
+ oconfig->CreateDistFile("rc/dist", NB_POINTS_UI);
break;
case 'y':
diff --git a/2004/i/nono/src/ovision/comm.h b/2004/i/nono/src/ovision/comm.h
index c2e80c6..d939bf5 100644
--- a/2004/i/nono/src/ovision/comm.h
+++ b/2004/i/nono/src/ovision/comm.h
@@ -6,6 +6,7 @@
+
#include "adjust.h"
#include "imgFile.h"
@@ -13,6 +14,7 @@
#include "oconfig.h"
#include "group.h"
+#define NB_POINTS_UI 6
/// Interprete les commandes envoyes par l'interface UI et les executent
class Comm {
@@ -58,6 +60,8 @@ class Comm {
/// Execute une commande venant de l'interface
void ExecuteUiCmds(char *buffer);
+ /// Recharge le fichier de config
+ void ReloadConfig (char *filename);
protected:
/// Segmentation et groupement des couleurs
void SegmAndGroup();
diff --git a/2004/i/nono/src/ovision/group.cc b/2004/i/nono/src/ovision/group.cc
index a3170db..d09445c 100644
--- a/2004/i/nono/src/ovision/group.cc
+++ b/2004/i/nono/src/ovision/group.cc
@@ -118,10 +118,10 @@ void Group::Plague(int type, unsigned char numColor, int x, int y) {
// TODO ajouter une inertie ?
// Parcours de l'objet trouve de haut en bas
- while ((xmax < img->width-1)&& (segm->FindColorNN(img->tabData + ((++xmax)+y* img->width)*3) == numColor)) {}
- while ((xmin > 0) && (segm->FindColorNN(img->tabData + ((--xmin)+y* img->width)*3) == numColor)) {}
- while ((ymax < img->height-1) && (segm->FindColorNN(img->tabData + (x+(++ymax)* img->width)*3) == numColor)) {}
- while ((ymin > 0) && (segm->FindColorNN(img->tabData + (x+(--ymin)* img->width)*3) == numColor)) {}
+ while ((xmax < img->width-1)&& (segm->FindColorNN(img->tabData + ((++xmax)+y* img->width)*3, 1) == numColor)) {}
+ while ((xmin > 0) && (segm->FindColorNN(img->tabData + ((--xmin)+y* img->width)*3, 1) == numColor)) {}
+ while ((ymax < img->height-1) && (segm->FindColorNN(img->tabData + (x+(++ymax)* img->width)*3, 1) == numColor)) {}
+ while ((ymin > 0) && (segm->FindColorNN(img->tabData + (x+(--ymin)* img->width)*3, 1) == numColor)) {}
// test si la zone trouve est une zone valide
if (!IsValidZone (type, xmin, xmax, ymin, ymax))
diff --git a/2004/i/nono/src/ovision/map.cc b/2004/i/nono/src/ovision/map.cc
index 9787594..d166755 100644
--- a/2004/i/nono/src/ovision/map.cc
+++ b/2004/i/nono/src/ovision/map.cc
@@ -10,6 +10,9 @@
using namespace std;
+#define TABLE_WIDTH 2100
+#define TABLE_HEIGHT 2400
+
/// Constructeurs.
Map::Map (Space *space)
@@ -36,14 +39,13 @@ Map::~Map (void)
void
-Map::GetPosFromLoc (int locImgX, int locImgY, double &locX, double &locY)
+Map::GetPosFromLoc (int locImgX, int locImgY, double &posX, double &posY)
{
double locX, locY;
- double posX, posY;
double posRobotX, posRobotY, angleRobot;
- space->GetLoc(locImgX, locImgY, &locX, &locY);
- tracker.getPos(posRobotX, posRobotY, angleRobot);
+ space->GetLoc(locImgX, locImgY, locX, locY);
+ // tracker.getPos(posRobotX, posRobotY, angleRobot);
space->GetPos(locX, locY, posRobotX, posRobotY, angleRobot, posX, posY);
}
@@ -176,58 +178,62 @@ Map::AddBallsToMap(Group *group)
ZONE *pCur = group->zoneListBall;
double pos[2];
- bool ballLost = 1;
+// bool ballLost = 1;
+ // On supprime l'ancienne liste de balle
+ ball.clear ();
while (pCur)
{
- // TODO passer par la classe space
- //GetPosFromLoc
- pos[0] = pCur->centerx;
- pos[1] = pCur->centery;
+ // On choppe la position par rapport au robot
+ space->GetLoc (pCur->centerx, pCur->centery, pos[0], pos[1]);
// Cherche si la balle est deja dans la liste
list<tBALL>::iterator iCur;
- // Si la balle n'est pas dans la liste on l'ajoute
- if (!TestSimilarBall(pCur, iCur))
- {
- AddBall(pos, pCur);
- }
- // Si elle l'est
- else
+ // On regarde si le group trouvé n'est pas en dehors de la table
+ if (1) //TODO (pos[0] < TABLE_WIDTH) && (pos[1] < TABLE_HEIGHT))
{
- // on regarde si c'est la balle qui a ete locké
- if (IsLock ())
- if (iCur == curBall)
- ballLost = 0;
-
- // on incremente son marqueur de viabilite
- if (iCur->skepticism < oconfig->skepticism_max)
- iCur->skepticism++;
-
- if (pCur->centery <= iCur->precision)
+ // Si la balle n'est pas dans la liste on l'ajoute
+// if (!TestSimilarBall(pCur, iCur))
+// {
+ AddBall(pos, pCur);
+/* }
+ // Si elle l'est
+ else
{
- ///TODO mis a jour de la position
- iCur->position[0] = pCur->centerx;
- iCur->position[1] = pCur->centery;
-
- iCur->bottom = pCur->bottom * oconfig->ball_bottom_time_out;
+ // on regarde si c'est la balle qui a ete locké
+ if (IsLock ())
+ if (iCur == curBall)
+ ballLost = 0;
+
+ // on incremente son marqueur de viabilite
+ if (iCur->skepticism < oconfig->skepticism_max)
+ iCur->skepticism++;
+
+ if (pCur->centery <= iCur->precision)
+ {
+ ///TODO mis a jour de la position
+ iCur->position[0] = pCur->centerx;
+ iCur->position[1] = pCur->centery;
+
+ iCur->bottom = pCur->bottom * oconfig->ball_bottom_time_out;
+ }
}
- }
+*/ }
pCur = pCur->next;
}
// TODO decremente d'autre marqueur
- if (IsLock () && ballLost)
+/* if (IsLock () && ballLost)
{
// si la balle sort par le bas pour etre prise
if (curBall->bottom)
curBall->bottom--;
else curBall->skepticism -= oconfig->ball_lost_weight;
}
-
+*/
// presence robot ennemi
@@ -246,7 +252,7 @@ Map::AddBallsToMap(Group *group)
void
Map::UpdateMap()
{
- double robotPosX, robotPosY, robotAngle;
+ double robotPosX, robotPosY;//, robotAngle;
double distRobotBall;
// On met a jour tous les scores
@@ -286,7 +292,7 @@ Map::ShowBalls()
for(iter = ball.begin(); iter != ball.end(); iter++)
{
/// Affichage des infos de la balle
- cout << i << ": " << iter->position[0] << "\t" << iter->position[1] << "\t" << iter->skepticism << "\t" << iter->score << "\t" << iter->bottom << endl;
+ cout << i << ": " << iter->position[0] << "\t" << iter->position[1] << endl;
i++;
}
cout << endl;
diff --git a/2004/i/nono/src/ovision/map.h b/2004/i/nono/src/ovision/map.h
index 2fa8fee..ab958a5 100644
--- a/2004/i/nono/src/ovision/map.h
+++ b/2004/i/nono/src/ovision/map.h
@@ -135,7 +135,7 @@ class Map
/// Retourne NULL si la balle locke est perdu
bool getCurBallPos (double &x, double &y);
- void GetPosFromLoc (int locImgX, int locImgY, double &locX, double &locY);
+ void GetPosFromLoc (int locImgX, int locImgY, double &posX, double &posY);
protected:
};
diff --git a/2004/i/nono/src/ovision/oconfig.cc b/2004/i/nono/src/ovision/oconfig.cc
index 5fbd46e..f424159 100644
--- a/2004/i/nono/src/ovision/oconfig.cc
+++ b/2004/i/nono/src/ovision/oconfig.cc
@@ -36,7 +36,7 @@ void OConfig::Parse(char *var, char *arg) {
fclose(file);
}
-
+
// Affecte la valeur de argu a la variable var
if (!strcmp(var, "Hauteur_Image")) height = atoi(argu);
else if(!strcmp(var,"Largeur_Image")) width = atoi(argu);
@@ -44,6 +44,7 @@ void OConfig::Parse(char *var, char *arg) {
else if(!strcmp(var,"NN_Neighborhood_Learning")) nn_nl = atof(argu);
else if(!strcmp(var,"NN_Number_Iteration_Learning")) nn_nil = atol(argu);
else if(!strcmp(var,"NN_Nombre_Couleurs")) nn_NbCouleurs = atoi(argu);
+ else if(!strcmp(var,"NN_Threshold_Output")) nn_threshold_output = atoi(argu);
else if(!strcmp(var,"NN_Influence_Luminosite")) nn_influ_lum = atof(argu);
else if(!strcmp(var,"imgPath")) strcpy(imgPath, argu);
else if(!strcmp(var,"NN_Lazy_Threshold")) nn_lazy_threshold = atoi(argu);
@@ -60,13 +61,8 @@ void OConfig::Parse(char *var, char *arg) {
else if(!strcmp(var,"Minimum_Length_Zone")) minLengthZone = atoi(argu);
else if(!strcmp(var,"Ball_Lost")) ball_lost_weight = atoi(argu);
else if(!strcmp(var,"Ball_Bottom_Time_Out")) ball_bottom_time_out = atoi(argu);
- else if(!strcmp(var,"Source"))
- {
- if (!strcmp(argu, "file")) source = SOURCE_FILE;
- else if (!strcmp(argu, "usbcam")) source = SOURCE_USB_CAM;
- else if (!strcmp(argu, "cam")) source = SOURCE_CAM;
- }
-
+ else if(!strcmp(var,"One_NN_Learning_Rate")) one_nn_learning_rate = atof(argu);
+ else if(!strcmp(var,"One_NN_Learning_Iteration")) one_nn_learning_iteration = atoi(argu);
}
@@ -74,14 +70,27 @@ void OConfig::Parse(char *var, char *arg) {
/// @param *filename nom du fichier de config
OConfig::OConfig(char *filename) {
+ instance = this;
+
+ Load (filename);
+
+
+ colorMode = 0;
+ color = NULL;
+ node = NULL;
+ index = NULL;
+ LoadNNFile("rc/poids");
+ LoadDistFile("rc/dist");
+}
+
+void OConfig::Load (char *filename)
+{
const int NBARG = 3;
char *cut[NBARG] = {NULL};
FILE *file;
char ligne[50];
int i;
- instance = this;
-
// Ouverture du fichier de conf
file = fopen(filename, "r");
if (!file) cerr << "OConfig::OConfig : Error during config file opening" << endl;
@@ -105,12 +114,6 @@ OConfig::OConfig(char *filename) {
Parse(cut[0], cut[2]);
}
- colorMode = 0;
- color = NULL;
- node = NULL;
- index = NULL;
- LoadNNFile("rc/poids");
- LoadDistFile("rc/dist");
}
@@ -314,26 +317,32 @@ void OConfig::LoadDistFile(char *filename) {
cerr << "OConfig::LoadDistFile : Error during poids file opening" << endl;
return;
}
-
- int i=0;
- while (i<NB_POINTS)
+
+ // Parcours des lignes et analyse
+ int point[4];
+ while((fgets(buf, 50, file)) && (!feof(file)))
{
- fgets(buf, 50, file);
- if (buf[0] != '#')
+ if ((buf[0] >= '0') && (buf[0] <= '9'))
{
- sscanf(buf, "%i\t%i\t%i\t%i\n", &tabPoint[i][0], &tabPoint[i][1], &tabPoint[i][2], &tabPoint[i][3]);
- i++;
+ sscanf(buf, "%i\t%i\t%i\t%i\n", &point[0], &point[1], &point[2], &point[3]);
+ tabPoint.push_back(point[0]);
+ tabPoint.push_back(point[1]);
+ tabPoint.push_back(point[2]);
+ tabPoint.push_back(point[3]);
+ //cout << point[0] << " " << point[1] << endl;
}
}
-
+
+ cout << endl;
fclose(file);
+ nbDistPoint = tabPoint.size () / 4;
}
/// Creation d'un fichier pour la tabPointle des distances
-void OConfig::CreateDistFile(char *filename) {
+void OConfig::CreateDistFile(char *filename, int numPoint) {
- if (!tabPoint) {
+ if (tabPoint.size () /4) {
cerr << "OConfig::CreateDistFile : tabPoint vide\n";
return;
}
@@ -345,8 +354,8 @@ void OConfig::CreateDistFile(char *filename) {
fprintf(file, "#imgX\timgY\tdistX\tdistY\n");
- for (int i=0; i<NB_POINTS; i++)
- fprintf(file, "%i\t%i\t%i\t%i\n", tabPoint[i][0], tabPoint[i][1], tabPoint[i][2], tabPoint[i][3]);
+ // for (int i=0; i<numPoint; i++)
+// fprintf(file, "%i\t%i\t%i\t%i\n", tabPoint[i*4+0], tabPoint[i*4+1], tabPoint[i*4+2], tabPoint[i*4+3]);
fclose(file);
}
diff --git a/2004/i/nono/src/ovision/oconfig.h b/2004/i/nono/src/ovision/oconfig.h
index 96151f8..abdf805 100644
--- a/2004/i/nono/src/ovision/oconfig.h
+++ b/2004/i/nono/src/ovision/oconfig.h
@@ -4,6 +4,7 @@
// nono - Programme du robot Efrei Robotique I1-I2 2004
// Copyright (C) 2004 Olivier Gaillard
+#include <vector>
/// Constantes pour les modes d'espace de couleur
#define RGB 0
@@ -16,8 +17,6 @@
#define SOURCE_USB_CAM 1
#define SOURCE_CAM 2
-#define NB_POINTS 3
-
/// Charge le fichier config et distribue les variables
class OConfig {
@@ -65,6 +64,9 @@ class OConfig {
/// influence de la luminosite dans l'integration de l'image
float nn_influ_lum;
+ /// seuil de couleur indéfini si la sortie du réseau n'est pas assez grande
+ int nn_threshold_output;
+
/// tableau des poids du reseau de neurones
unsigned char *node;
@@ -121,8 +123,12 @@ class OConfig {
/////////////////////////////// SPACE ////////////////////////////////////////////////////////////////
/// Points utilises pour le calcul de distance
- int tabPoint[NB_POINTS][4];
+ std::vector<int> tabPoint;
+ int nbDistPoint;
+
+ float one_nn_learning_rate;
+ int one_nn_learning_iteration;
/////////////////////////////// UI ////////////////////////////////////////////////////////////////
/// tableau d'index des couleurs a melanger (merge)
@@ -150,7 +156,7 @@ class OConfig {
void CreateNNFile(const char *file, int mode, int nbOutput);
/// Creatio du fichier de la table de distance
- void CreateDistFile(char *filename);
+ void CreateDistFile(char *filename, int numPoint);
/// Chargement du fichier de la table de distance
void LoadDistFile(char *filename);
@@ -158,10 +164,12 @@ class OConfig {
static OConfig* GetInstance ()
{return instance;}
+ void Load (char *filename);
protected:
/// Parse une ligne du fichier de config
void Parse(char *var, char *arg);
+
};
diff --git a/2004/i/nono/src/ovision/segmNN.cc b/2004/i/nono/src/ovision/segmNN.cc
index c17ff33..9e4db20 100644
--- a/2004/i/nono/src/ovision/segmNN.cc
+++ b/2004/i/nono/src/ovision/segmNN.cc
@@ -189,13 +189,14 @@ SegmNN::TrainNN()
/// Renvoie le code de la couleur segmentee
/// @param *x pointeur vers un tableau contenant une valeur RGB
unsigned char
-SegmNN::FindColorNN(unsigned char *x)
+SegmNN::FindColorNN(unsigned char *x, bool testUndefined)
{
int numOutputMax=0;
int output[nbOutput];
int j,tmp;
// Calcul des valeurs de sorties pour ce pixel
+ // si on est en yuv
if (img->yuv)
{
for(j=0; j<nbOutput; j++)
@@ -213,6 +214,7 @@ SegmNN::FindColorNN(unsigned char *x)
numOutputMax = j;
}
}
+ // si on est en rgb
else {
for(j=0; j<nbOutput; j++)
{
@@ -227,6 +229,10 @@ SegmNN::FindColorNN(unsigned char *x)
}
}
+ if ((testUndefined) && (output[numOutputMax] > oconfig->nn_threshold_output))
+ numOutputMax = UNDEFINED;
+
+// if (testUndefined) cout << output[numOutputMax] << endl;
return numOutputMax;
}
diff --git a/2004/i/nono/src/ovision/segmNN.h b/2004/i/nono/src/ovision/segmNN.h
index ecd733a..3a2bfa2 100644
--- a/2004/i/nono/src/ovision/segmNN.h
+++ b/2004/i/nono/src/ovision/segmNN.h
@@ -11,6 +11,7 @@
#define MIN 0
#define MAX 1
+#define UNDEFINED 254
/// Constantes pour la creation du NN
#define LOAD_FROM_FILE 1
@@ -66,7 +67,7 @@ class SegmNN
void TestNN();
/// Renvoie le code la couleur segmentee
- unsigned char FindColorNN(unsigned char *x);
+ unsigned char FindColorNN(unsigned char *x, bool testUndefined = 0);
protected:
diff --git a/2004/i/nono/src/ovision/space.cc b/2004/i/nono/src/ovision/space.cc
index 8b41521..4b0ad30 100644
--- a/2004/i/nono/src/ovision/space.cc
+++ b/2004/i/nono/src/ovision/space.cc
@@ -7,22 +7,22 @@
#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 imgHeight, int imgWidth)
+Space::Space (int imgWidth, int imgHeight)
{
Space::imgHeight = imgHeight;
Space::imgWidth = imgWidth;
- Space::oconfig = oconfig;
+ Space::oconfig = OConfig::GetInstance ();
- for (int i=0; i<NB_POINTS; i++)
- coefX[i] = coefY[i] = i;
+ tabY = NULL;
+ tabX = NULL;
}
@@ -32,241 +32,118 @@ Space::~Space ()
}
-/// Ajoute un point pour l'etalonnage
-void Space::AddSetupPoint(int x, int y, int distx, int disty)
+void Space::CreateGnuPlotFile (int y)
{
- if (setupTab.size() > NB_POINTS)
+ FILE *file;
+ double locY, locX;
+
+ file = fopen("dataY", "w+");
+ for (int i=0; i<296; i++)
{
- std::vector<SETUP_POINT>::iterator iter;
- iter = setupTab.begin();
- setupTab.erase(iter);
+ GetLoc(0, i, locX, locY);
+ fprintf(file, "%i\t%lf\n", i, locY);
}
-
- 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<NB_POINTS; i++)
- AddSetupPoint(oconfig->tabPoint[i][0], oconfig->tabPoint[i][1], oconfig->tabPoint[i][2], oconfig->tabPoint[i][3]);
-}
-
+ fclose(file);
-
-double Space::CoeffDeterminant(double *matrix, int i, int n)
-{
- return (std::pow ((double) -1,(double) i))*matrix[i*n];
-}
-
-/// Calcul du determinant d'une matrice
-double Space::Determinant(double *matrix, int n)
-{
- if (n == 3)
- return Determinant33(matrix);
-
- double subDet = 0;
- double coeff;
- double *mat = new double[(n-1)*(n-1)];
- for (int m=0; m<n; m++)
+ file = fopen("dataX", "w+");
+ for (int i=0; i<352; i++)
{
- coeff = CoeffDeterminant(matrix, m, n);
-
- int k = 0;
- for (int l=n; l<n*n; l++)
- {
- if (l%n != m)
- {
- mat[k] = matrix[l];
- k++;
- }
- }
- cout << "o\n";
-
- subDet += coeff * Determinant(mat, n-1);
+ GetLoc(0, y, locX, locY);
+ fprintf(file, "%i\t%lf\n", i, locX);
}
+ fclose(file);
- delete [] mat;
- return subDet;
}
-/// Calcul du determinant d'une matrice
-double Space::Determinant33(double *matrix)
-{
- double det = 0;
-
- det += matrix[0*3+2] * (matrix[1*3+0] * matrix[2*3+1] - matrix[2*3+0] * matrix[1*3+1]);
- det -= matrix[1*3+2] * (matrix[0*3+0] * matrix[2*3+1] - matrix[2*3+0] * matrix[0*3+1]);
- det += matrix[2*3+2] * (matrix[0*3+0] * matrix[1*3+1] - matrix[1*3+0] * matrix[0*3+1]);
-
- return det;
-}
-/// Calcul du déterminant caractéristique d'une matrice
-double Space::CaracteristicDeterminant(double *matrix, double *endMat, int numCol)
+/// Ajoute un point pour l'etalonnage
+void
+Space::AddSetupPoint(int x, int y, int distx, int disty)
{
- double save[3];
-
- // Deplacement de la ième colonne et sauvegarde
- // pour le calcul du determinant caracteristique
- for (int i=0; i<3; i++)
- {
- save[i] = matrix[i*3+numCol];
- matrix[i*3+numCol] = endMat[i];
- }
-
- // Calcul du determinant caracteristique
- double carDet = Space::Determinant(matrix, 3);
+ SETUP_POINT setupPoint;
- // Restauration de la matrice d'origine
- for (int i=0; i<3; i++)
- matrix[i*3+numCol] = save[i];
+ setupPoint.x = x;
+ setupPoint.y = y;
+ setupPoint.distx = distx;
+ setupPoint.disty = disty;
- return carDet;
+ setupTab.push_back(setupPoint);
}
-
-int Space::BestFitDoTab(double *mat, double *endMat)
+/// Chargement des points a partir d'un fichier
+void
+Space::LoadFromFile()
{
- for (int i=0; i<3; i++)
- endMat[i] = 0;
-
- for (int i=0; i<9; i++)
- mat[i] = 0;
-
- // distance
- for(int i=0; i<NB_POINTS; i++)
- {
- endMat[0] += setupTab[i].disty;
- endMat[1] += setupTab[i].y * setupTab[i].disty;
- endMat[2] += setupTab[i].y * setupTab[i].y * setupTab[i].disty;
-
- mat[0] += 1;
- mat[1] += setupTab[i].y;
- mat[2] += setupTab[i].y * setupTab[i].y;
- mat[3] += setupTab[i].y;
- mat[4] += setupTab[i].y * setupTab[i].y;
- mat[5] += setupTab[i].y * setupTab[i].y * setupTab[i].y;
- mat[6] += setupTab[i].y * setupTab[i].y;
- mat[7] += setupTab[i].y * setupTab[i].y * setupTab[i].y;
- mat[8] += setupTab[i].y * setupTab[i].y * setupTab[i].y * setupTab[i].y;
- }
+ 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::Setup(double aX, double bX, double cX, double aY, double bY, double cY)
+void
+Space::FindCoeffLine (double x1, double y1, double x2, double y2, double &a, double &b)
{
- coefX[0] = cX;
- coefX[1] = bX;
- coefX[2] = aX;
- coefY[0] = cY;
- coefY[1] = bY;
- coefY[2] = aY;
+ a = (x2 - x1) / (y2 - y1);
+ b = x1 - a * y1;
}
/// Etalonnage des distances
-int Space::Setup()
+int
+Space::Setup(double a, double b, double c)
{
- if (setupTab.size() < NB_POINTS)
- {
- cerr << "Space::Setup : Pas assez de points donnés, utilisé AddSetupPoint\n";
- cerr << "Il y a " << setupTab.size () << " et il faut " << NB_POINTS << endl;
- return 0;
- }
-
- std::cout << "nbPoints: " << NB_POINTS << endl;
-
- // Calcul des coeff pour l'approximation de la profondeur
- double mat[NB_POINTS*NB_POINTS];
- double endMat[NB_POINTS];
-
- double det = Space::Determinant(mat, 3);
-
- if (det != 0)
- {
- for (int i=0; i<3; i++)
- {
- coefY[i] = (double)Space::CaracteristicDeterminant(mat, endMat, i) / det;
- cout << coefY[i] << endl;
- }
- }
- else
- {
- cerr << "Space::Setup : Determinant egal a zero, donner d'autres points\n";
- return 0;
- }
-
- // Calcul des coeff pour l'approximation de l'horizontal
-/* for (int i=0; i<NB_POINTS; i++)
- {
- endMat[i] = setupTab[i].distx;
- for (int j=0; j<NB_POINTS-1; j++)
- mat[i*NB_POINTS+j] = pow((double)setupTab[i].x, (double)(NB_POINTS - j -1));
-
- mat[i*(NB_POINTS+1) -1] = 1;
- }
-
-
- det = Space::Determinant(mat, NB_POINTS);
+ 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);
- if (det != 0)
- {
- for (int i=0; i<NB_POINTS; i++)
- {
- coefX[NB_POINTS-i-1] = (double)Space::CaracteristicDeterminant(mat, endMat, i) / det;
- }
+ cout << a1 << " " << b1 << " " << a2 << " " << b2 << endl;
+
+ delete []tabY;
+ tabY = new double[imgHeight];
+
+ delete []tabX;
+ tabX = new double[imgHeight*imgWidth];
- }
- else
+ double diffPix;
+ double unitPix;
+ double center;
+ for (int y=0; y<imgHeight; y++)
{
- cerr << "Space::Setup : Coordonnees X egales a zero, donner d'autres points\n";
- return 0;
+ 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);
}
-
-
- /* double matrix2[NB_POINTS][4] = {{setupTab[0].y * setupTab[0].x, setupTab[0].x, 1, setupTab[0].distx},
- {setupTab[1].y * setupTab[1].x, setupTab[1].x, 1, setupTab[1].distx},
- {setupTab[2].y * setupTab[2].x, setupTab[2].x, 1, setupTab[2].distx}};
-
- det = Space::Determinant(matrix2);
- if (det != 0)
- {
- aX = (double)Space::CaracteristicDeterminant(matrix2, 0) / det;
- bX = (double)Space::CaracteristicDeterminant(matrix2, 1) / det;
- cX = (double)Space::CaracteristicDeterminant(matrix2, 2) / det;
- cout << "aX: " << aX << " bX: " << bX << " cX: " << cX << endl;
- }
- else
- {
- cerr << "Space::Setup : Coordonnees X egales a zero, donner d'autres points\n";
- return 0;
- }
- */
+
return 1;
}
+
/// Position d'un objet dans le referentiel du robot
-void Space::GetLoc(int locImgX, int locImgY, double &locX, double &locY)
+void
+Space::GetLoc(int locImgX, int locImgY, double &locX, double &locY)
{
// Calcul de locX
- locX = (locImgY * locImgX * coefX[2] + coefX[1]*locImgX + coefX[0]);
+ locX = tabX[locImgY*imgWidth + locImgX];
// Calcul de locY
- locY = (locImgY * locImgY * coefY[2] + locImgY * coefY[1] + coefY[0]);
+ locY = aY*locImgY*locImgY + bY*locImgY + cY;
}
-void Space::GetLoc(int locImgX, int locImgY, int &locX, int &locY)
+
+void
+Space::GetLoc(int locImgX, int locImgY, int &locX, int &locY)
{
double x,y;
GetLoc(locImgX, locImgY, x, y);
@@ -274,11 +151,13 @@ void Space::GetLoc(int locImgX, int locImgY, int &locX, int &locY)
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(int locX, int locY, int posRobotX, int posRobotY, double angleRobot, double &posX, double &posY)
+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);
diff --git a/2004/i/nono/src/ovision/space.h b/2004/i/nono/src/ovision/space.h
index 52f6e6d..2668e76 100644
--- a/2004/i/nono/src/ovision/space.h
+++ b/2004/i/nono/src/ovision/space.h
@@ -7,11 +7,16 @@
#include <vector>
#include <iostream>
-#define POS_ROBOT_ETALONNOGE_X 300
-
#include "group.h"
#include "oconfig.h"
+#define NB_NODES_X 5
+#define NB_NODES_Y 3
+
+#define START_WEIGHT_MIN 0.1
+#define START_WEIGHT_MAX 0.9
+
+
struct SETUP_POINT
{
int x, y;
@@ -26,8 +31,8 @@ class Space
OConfig *oconfig;
/// tableau d'index des distances
- uint *tabX;
- uint *tabY;
+ double *tabX;
+ double *tabY;
/// liste des distances etalonnees
std::vector<SETUP_POINT> setupTab;
@@ -38,11 +43,7 @@ class Space
/// largeur de l'image
int imgWidth;
- /// coeff pour l'approximation de la profondeur
- double coefY[NB_POINTS];
-
- /// coeff pour l'approximation de l'horizontal
- double coefX[NB_POINTS];
+ double aY, bY, cY;
public:
@@ -56,6 +57,9 @@ class Space
void GetLoc(int locImgX, int locImgY, int &locX, int &locY);
void GetLoc(int locImgX, int locImgY, double &locX, double &locY);
+ /// Donne la position reelle sur la table de la balle
+ void GetPos(double locX, double locY, double posRobotX, double posRobotY, double angleRobot, double &posX, double &posY);
+
/// Ajoute un point pour l'etalonnage
void AddSetupPoint(int x, int y, int distx, int disty);
@@ -63,24 +67,14 @@ class Space
void LoadFromFile();
/// Etalonnage des distances
- int Setup();
+ int Setup(double aY, double bY, double cY);
- void Setup(double aX, double bX, double cX, double aY, double bY, double cY);
protected:
- /// Calcul du determinant
- double Determinant33(double *matrix);
- double Determinant(double *matrix, int n);
-
- double CoeffDeterminant (double *matrix, int i, int n);
-
- /// Calcul du determinant caracteristique
- double CaracteristicDeterminant(double *matrix, double *endMat, int numCol);
-
- /// Donne la position reelle sur la table de la balle
- void GetPos(int locX, int locY, int posRobotX, int posRobotY, double angleRobot, int &posX, int &posY);
+ void FindCoeffLine (double x1, double y1, double x2, double y2, double &a, double &b);
- int BestFitDoTab(double *mat, double *endMat);
+ void CreateGnuPlotFile (int y = 100);
+ void LogErrorPoint (FILE *f, int iter);
};
#endif // space_h
diff --git a/2004/i/nono/src/ovision/testdist.cc b/2004/i/nono/src/ovision/testdist.cc
index e720628..38ee0a4 100644
--- a/2004/i/nono/src/ovision/testdist.cc
+++ b/2004/i/nono/src/ovision/testdist.cc
@@ -24,8 +24,7 @@ int main(int argc, char **argv)
// space.AddSetupPoint (253, 234, 1000, 900);
// Calcul des équations pour la conversion des coordonnées
- space.Setup ();
-
+ space.Setup (0.00603759, 0.593767, 291.474);
int locImgX = atoi (argv[1]);
int locImgY = atoi (argv[2]);