summaryrefslogtreecommitdiff
path: root/2004/i
diff options
context:
space:
mode:
authorgaillaro2004-05-16 20:53:16 +0000
committergaillaro2004-05-16 20:53:16 +0000
commit4fda347a9166d249073913cc1da8b9e25a6ce0cc (patch)
treee722b674f7dd2ce118e74bc35d8f1a4b8c308f01 /2004/i
parent3bd75fa0e5b695ffb46ded75f3023f694f1f0b7b (diff)
qq modifs sur map
Diffstat (limited to '2004/i')
-rw-r--r--2004/i/nono/src/ovision/Makefile.defs4
-rw-r--r--2004/i/nono/src/ovision/adjust.cc34
-rw-r--r--2004/i/nono/src/ovision/comm.cc8
-rw-r--r--2004/i/nono/src/ovision/group.cc92
-rw-r--r--2004/i/nono/src/ovision/group.h14
-rw-r--r--2004/i/nono/src/ovision/map.cc210
-rw-r--r--2004/i/nono/src/ovision/map.h82
-rw-r--r--2004/i/nono/src/ovision/oconfig.cc10
-rw-r--r--2004/i/nono/src/ovision/oconfig.h47
-rw-r--r--2004/i/nono/src/ovision/segmNN.cc36
-rw-r--r--2004/i/nono/src/ovision/segmNN.h4
-rw-r--r--2004/i/nono/src/ovision/space.cc207
-rw-r--r--2004/i/nono/src/ovision/space.h18
-rw-r--r--2004/i/nono/src/ovision/test_ovision.cc27
-rw-r--r--2004/i/nono/src/ovision/testdist.cc14
-rw-r--r--2004/i/nono/src/ovision/testmap.cc8
-rw-r--r--2004/i/nono/src/ovision/ui.cc5
17 files changed, 577 insertions, 243 deletions
diff --git a/2004/i/nono/src/ovision/Makefile.defs b/2004/i/nono/src/ovision/Makefile.defs
index e9548dd..7e5d67f 100644
--- a/2004/i/nono/src/ovision/Makefile.defs
+++ b/2004/i/nono/src/ovision/Makefile.defs
@@ -2,7 +2,8 @@ TARGETS += test_ovision testimg testmap optim test_ovision_tracker
LIBS += ovision.a
testimg_SOURCES = testimg.cc ovision.a video4linux.a
-test_ovision_SOURCES = test_ovision.cc ovision.a image.a video4linux.a
+test_ovision_SOURCES = test_ovision.cc ovision.a image.a video4linux.a motor.a config.a serial.a
+test_ovision_ogl_SOURCES = test_ovision_ogl.cc ovision.a image.a video4linux.a motor.a config.a serial.a
test_ovision_tracker_SOURCES = test_ovision_tracker.cc ovision.a image.a \
video4linux.a motor.a date.a \
serial.a utils.a logger.a config.a
@@ -14,6 +15,7 @@ ovision_a_SOURCES = img.cc group.cc oconfig.cc map.cc segmNN.cc space.cc
testimg: $(testimg_SOURCES:%.cc=%.o)
test_ovision: $(test_ovision_SOURCES:%.cc=%.o)
+test_ovision_ogl: $(test_ovision_ogl_SOURCES:%.cc=%.o)
test_ovision_tracker: $(test_ovision_tracker_SOURCES:%.cc=%.o)
diff --git a/2004/i/nono/src/ovision/adjust.cc b/2004/i/nono/src/ovision/adjust.cc
index 3c84def..a58081e 100644
--- a/2004/i/nono/src/ovision/adjust.cc
+++ b/2004/i/nono/src/ovision/adjust.cc
@@ -17,7 +17,10 @@ using namespace std;
int window;
Comm *comm;
-int point[3][2] = {{-1,-1},{-1,-1},{-1,-1}};
+
+#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;
int* GetPpoint()
@@ -208,20 +211,21 @@ DrawGLScene()
glColor3ub(0,0,255);
glDisable(GL_TEXTURE_2D);
// Dessine points de calibration de la distance
- for (int i=0; i<3; i++)
+ for (int i=0; i<NB_POINTS_UI; i++)
{
if (point[i][0] >= 0)
/* x = (int)((double)WIDTH/width * x);
y = height - y;
y = (int)((double)HEIGHT/height * y);*/
glBegin(GL_POINTS);
- glVertex3f(point[i][0] * 352 / IMG_WIDTH + BORDER,
- point[i][1] * 288 / IMG_HEIGHT + 3 * BORDER ,
+ glVertex3f( (float)((point[i][0] + BORDER) * (double)352 / IMG_WIDTH),
+ (float)((point[i][1] + BORDER) * (double)288 / IMG_HEIGHT),
+ -0.9f);
+ glVertex3f( (float)((point[i][0] + BORDER) * (double)352 / IMG_WIDTH),
+ (float)((point[i][1] + BORDER) * (double)288 / IMG_HEIGHT + 288 + BORDER),
-0.9f);
glEnd();
- std::cout << point[i][0] * 352 / IMG_WIDTH + BORDER << " " <<
- point[i][1] * 288 / IMG_HEIGHT + 3 * BORDER << endl;
}
glEnable(GL_TEXTURE_2D);
@@ -253,7 +257,6 @@ MouseFunc(int button, int state, int x, int y)
{
// On calcul les coordonnees de l'image
x = (int)((double)WIDTH/width * x);
- y = height - y;
y = (int)((double)HEIGHT/height * y);
int imgX = -1;
int imgY = -1;
@@ -270,43 +273,44 @@ MouseFunc(int button, int state, int x, int y)
imgY = y * IMG_HEIGHT / 288 - 3 * BORDER - 288;
}
+ cout << "x " << imgX << " y " << imgY << endl;
int i=0;
- // cout << "Point ajoute: " << imgX << ", " << imgY << endl;
-
if (imgX >= 0)
{
+ imgY = IMG_HEIGHT - imgY;
+
// Ajout de point
if (button == GLUT_LEFT_BUTTON)
{
- while(i < 3)
+ while(i < NB_POINTS_UI)
{
if (point[i][0] < 0)
{
point[i][0] = imgX;
point[i][1] = imgY;
- i=5;
+ i=NB_POINTS_UI+3;
cout << "Point ajoute: " << imgX << ", " << imgY << endl;
}
i++;
}
- if (i == 5) cout << "MouseFunc : 3 points sont deja selectionnes" << endl;
+ if (i == NB_POINTS_UI+3) cout << "MouseFunc : 3 points sont deja selectionnes" << endl;
}
// Suppression de point
else if (button == GLUT_RIGHT_BUTTON)
{
- while(i<3)
+ while(i<NB_POINTS_UI)
{
if ((abs(point[i][0] - imgX) < 5) && (abs(point[i][1] - imgY) < 5))
{
point[i][0] = -1;
point[i][1] = -1;
- i=5;
+ i=NB_POINTS_UI+3;
}
i++;
}
- if (i == 5) cout << "MouseFunc : 0 point selectionne" << endl;
+ if (i == NB_POINTS_UI+3) cout << "MouseFunc : 0 point selectionne" << endl;
}
}
DrawGLScene();
diff --git a/2004/i/nono/src/ovision/comm.cc b/2004/i/nono/src/ovision/comm.cc
index d81cf9c..d3541bf 100644
--- a/2004/i/nono/src/ovision/comm.cc
+++ b/2004/i/nono/src/ovision/comm.cc
@@ -65,7 +65,7 @@ Comm::Comm(char *filename)
data[i] = new unsigned char[img.nbPixels*3];
// Initialisation de la segmentation
- segm = new SegmNN(&img, oconfig);
+ segm = new SegmNN(&img);
segm->BuildNN(oconfig->nn_NbCouleurs, LOAD_FROM_FILE);
group = new Group(&img, segm);
@@ -108,7 +108,7 @@ Comm::SegmAndGroup()
// Creation des groupes
if (group) delete group;
group = new Group(&img, segm);
- group->JumpPoints(oconfig->groupColor);
+ group->JumpPoints(oconfig->groupColor, oconfig->goalColor);
group->TabOut();
img.DoImg(group->tabOut, data[1]);
@@ -371,14 +371,14 @@ Comm::ExecuteUiCmds(char *buffer)
int *pPoint;
pPoint = GetPpoint();
- for (int i=0; i<3; i++)
+ for (int i=0; i<NB_POINTS; i++)
if (pPoint[i*2] == -1)
{
cerr << "Comm::ExecuteUiCmds : Pas assez de points selectionnes\n";
return;
}
- for (int i = 0; i<3; i++)
+ for (int i = 0; i<NB_POINTS; i++)
{
oconfig->tabPoint[i][0] = pPoint[i*2];
oconfig->tabPoint[i][1] = pPoint[i*2+1];
diff --git a/2004/i/nono/src/ovision/group.cc b/2004/i/nono/src/ovision/group.cc
index 5ffc246..a3170db 100644
--- a/2004/i/nono/src/ovision/group.cc
+++ b/2004/i/nono/src/ovision/group.cc
@@ -11,9 +11,7 @@
#include <math.h>
using namespace std;
-#define BALL 1
-#define GOAL 2
-
+#define BORD 5
/// Constructeur
@@ -23,6 +21,7 @@ Group::Group(Img *img, SegmNN *segm) {
// Sauvegarde des pointeurs
Group::img = img;
Group::segm = segm;
+ oconfig = OConfig::GetInstance ();
width = img->width;
height = img->height;
@@ -74,13 +73,38 @@ void Group::DoDeltaTable ()
/// Retourne le delta utilisé pour la dissociation de 2 balles proches
-int Group::GetDelta (int y)
+int Group::GetDelta (int type, int y)
{
-// return 50;
- return (int)(20 + y*0.1);
+ // return 50;
+
+ if (type == BALL)
+ return (int)(25 + y*0.1);
+ else return 3*(int)(25 + y*0.1);
+}
+
+
+/// Test si la zone trouvé est valide
+bool Group::IsValidZone (int type, int xmin, int xmax, int ymin, int ymax) {
+
+
+ // test si la zone trouve un trop petite -> parasite
+ if ( (abs(xmin - xmax) < oconfig->minLengthZone) || (abs(ymin - ymax) < oconfig->minLengthZone) )
+ return 0;
+
+ if (type == BALL)
+ {
+ }
+ else if (type == GOAL)
+ {
+ // test si la zone trouve touche le haut de l'image
+ if (ymin > 0) return 0;
+ }
+
+ return 1;
}
+
/// Cherche l'objet complet a partir d'un pixel donne
/// @param numColor numero de la couleur a chercher
@@ -99,46 +123,29 @@ void Group::Plague(int type, unsigned char numColor, int x, int y) {
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)&& (tabSegm[(++xmax)+y* img->width] == numColor)) {}
-while ((xmin > 0) && (tabSegm[(--xmin)+y* img->width] == numColor)) {}
-while ((ymax < img->height-1) && (tabSegm[x+(++ymax)* img->width] == numColor)) {}
-while ((ymin > 0) && (tabSegm[x+(--ymin)* img->width] == numColor)) {}
-*/
-
- // Verification la validite des entrees
-/* if ((xmin < 0) || (xmin > img->width))
- xmin = 0;
- if ((xmax > img->img->width) || (xmax < 0))
- xmax = img->width;
- if ((ymin < 0) || (ymin > height))
- ymin = 0;
- if ((ymax > img->height) || (ymax < 0))
- ymax = img->height;
-*/
- // Calcul du centre de l'image
- int centerx, centery;
- centerx = (xmax+xmin)/2;
- centery = (ymax+ymin)/2;
+ // test si la zone trouve est une zone valide
+ if (!IsValidZone (type, xmin, xmax, ymin, ymax))
+ return;
ZONE *pCur;
-
+
if (type == BALL)
- {
pCur = zoneListBall;
- }
else if (type == GOAL)
- {
-// if (!((pCur->ymin <= 0) && (pCur->ymax >= img->width)))
-// return;
pCur = zoneListGoal;
- }
+ // Calcul du centre de l'image
+ int centerx, centery;
+ centerx = (xmax+xmin)/2;
+ centery = (ymax+ymin)/2;
+
+
ZONE *pLast=NULL;
int imgY;
while (pCur) {
imgY = (pCur->centery + centery) / 2;
// si on a deja ce groupe on actualise les donnees du groupe
- if ((numColor == pCur->idColor) && (abs(pCur->centerx - centerx) <= GetDelta(imgY)) && (abs(pCur->centery - centery) <= GetDelta(imgY))) {
+ if ((numColor == pCur->idColor) && (abs(pCur->centerx - centerx) <= GetDelta(type, imgY)) && (abs(pCur->centery - centery) <= GetDelta(type, imgY))) {
if (xmin < pCur->xmin) pCur->xmin = xmin;
if (xmax > pCur->xmax) pCur->xmax = xmax;
if (ymin < pCur->ymin) pCur->ymin = ymin;
@@ -186,10 +193,19 @@ while ((ymin > 0) && (tabSegm[x+(--ymin)* img->width] == numColor)) {}
pLast->idColor = numColor;
// test si la balle est vu partiellement ou completement
- if ((pLast->xmin <= 0) || (pLast->ymin <= 0) || (pLast->xmax >= img->width) || (pLast->ymax >= img->height))
- pLast->partial = 1;
- else pLast->partial = 0;
-
+ if (pLast->ymax >= img->height - BORD)
+ {
+ pLast->bottom = 1;
+ if ((pLast->xmin <= 0) || (pLast->xmax >= img->width) || (pLast->ymin <= 0))
+ pLast->partial = 1;
+ else pLast->partial = 0;
+ }
+ else
+ {
+ pLast->bottom = 0;
+ pLast->partial = 0;
+ }
+
pLast->next = NULL;
}
}
diff --git a/2004/i/nono/src/ovision/group.h b/2004/i/nono/src/ovision/group.h
index c0bf594..f7f41cf 100644
--- a/2004/i/nono/src/ovision/group.h
+++ b/2004/i/nono/src/ovision/group.h
@@ -9,7 +9,8 @@
// TODO delta devrait diminuer avec la profondeur
#define PRECISION 80 // distance a partir 2 groups sont 2 balles distinctes
-
+#define BALL 1
+#define GOAL 2
/// Liste chainee des zones trouvees par la classe group
struct ZONE {
@@ -28,6 +29,10 @@ struct ZONE {
/// l'objet est vue partiellement ou completement
bool partial;
+ /// l'objet est situé en bas de l'image
+ bool bottom;
+
+
/// maillon suivant
ZONE *next;
};
@@ -42,6 +47,8 @@ class Group {
/// classe image
Img *img;
+ OConfig *oconfig;
+
/// largeur de l'image a analyser
int width;
@@ -79,7 +86,10 @@ class Group {
void DoDeltaTable ();
/// Retourne le delta utilisé pour la dissociation de 2 balles proches
- int GetDelta (int y);
+ int GetDelta (int type, int y);
+
+ /// Test si la zone trouvé est valide
+ bool IsValidZone (int type, int xmin, int xmax, int ymin, int ymax);
protected:
/// Cherche l'objet complet a partir d'un pixel
void Plague(int type, unsigned char numColor, int x, int y);
diff --git a/2004/i/nono/src/ovision/map.cc b/2004/i/nono/src/ovision/map.cc
index 832200d..9787594 100644
--- a/2004/i/nono/src/ovision/map.cc
+++ b/2004/i/nono/src/ovision/map.cc
@@ -7,17 +7,22 @@
#include "map.h"
#include <iostream>
#include <math.h>
-#include <list>
using namespace std;
/// Constructeurs.
-Map::Map (OConfig *config, Space *space)
+Map::Map (Space *space)
+// : motor (Motor::getInstance ()), tracker (motor.getTracker ())
{
- lock = 0;
- Map::config = config;
+ oconfig = OConfig::GetInstance ();
+
+ lock = 0;
+ treeFound = 0;
+ checkCurBall = 0;
+
+ Map::oconfig = oconfig;
Map::space = space;
posGoal[0] = 105;
@@ -30,53 +35,60 @@ Map::~Map (void)
}
-/// Accessors
-const std::list<BALL>::iterator &
-Map::GetCurBall()
+void
+Map::GetPosFromLoc (int locImgX, int locImgY, double &locX, double &locY)
{
- checkCurBall = false;
- return curBall;
+ double locX, locY;
+ double posX, posY;
+ double posRobotX, posRobotY, angleRobot;
+
+ space->GetLoc(locImgX, locImgY, &locX, &locY);
+ tracker.getPos(posRobotX, posRobotY, angleRobot);
+ space->GetPos(locX, locY, posRobotX, posRobotY, angleRobot, posX, posY);
}
-
/// Ajoute une balle a la map
void
-Map::AddBall(int *pos, ZONE *pZone)
+Map::AddBall(double *pos, ZONE *pZone)
{
- BALL ballTmp;
+ tBALL ballTmp;
ballTmp.position[0] = pos[0];
ballTmp.position[1] = pos[1];
// definit la position de la balle
- ballTmp.zone = pos[0]%300 + (pos[1]%300)*7;
+ // ballTmp.zone = (double)((int)(pos[0])%300 + ((int)(pos[1])%300)*7);
ballTmp.partial = pZone->partial;
+ if (pZone->bottom)
+ ballTmp.bottom = oconfig->ball_bottom_time_out ;
+ else ballTmp.bottom = 0;
ballTmp.skepticism = 0;
+ ballTmp.precision = pZone->centery;
// calcul du score partie
- ballTmp.preScore =
- config->distance_ball_goal_weight * Dist(pos, posGoal);
+ ballTmp.preScore = 0;
+// oconfig->distance_ball_goal_weight * Dist (pos, posGoal);
ball.push_back(ballTmp);
}
/// Supprime une balle de la map
void
-Map::DelBall(list<BALL>::iterator &iter)
+Map::DelBall(list<tBALL>::iterator &iter)
{
ball.erase(iter);
+ checkCurBall = 0;
}
/// Test si une balle trouvee correspond a une balle de la map
int
-Map::TestSimilarBall(ZONE *pBall, list<BALL>::iterator &iter)
+Map::TestSimilarBall(ZONE *pBall, list<tBALL>::iterator &iter)
{
- //TODO tester d'abord les zones et ensuite les pos
for(iter = ball.begin(); iter != ball.end(); iter++)
{
- int pos[2] = {pBall->centerx, pBall->centery};
- if (Dist(iter->position, pos) < (iter->partial || pBall->partial)?config->map_error_part:config->map_error)
+ double pos[2] = {pBall->centerx, pBall->centery};
+ if (Dist(iter->position, pos) < ((iter->partial || pBall->partial)?oconfig->map_error_part:oconfig->map_error))
return 1;
}
@@ -84,16 +96,31 @@ Map::TestSimilarBall(ZONE *pBall, list<BALL>::iterator &iter)
}
/// Calcul de distance
-int
-Map::Dist(int *pos1, int *pos2)
+double
+Map::Dist(double pos1X, double pos1Y, double pos2X, double pos2Y)
+{
+ double x = pos2X - pos1X;
+ double y = pos2Y - pos1Y;
+ return sqrt((double)(x * x + y * y));
+}
+
+double
+Map::Dist(double *pos1, double *pos2)
+{
+ double x = pos2[0]-pos1[0];
+ double y = pos2[1]-pos1[1];
+ return sqrt((double)(x * x + y * y));
+}
+
+
+double
+Map::Angle (double ballPosY, double robotPosY, double distRobotBall)
{
- int x = pos2[0]-pos1[0];
- int y = pos2[1]-pos1[1];
- return (int)sqrt((double)(x * x + y * y));
+ return acos ((ballPosY - robotPosY)/distRobotBall);
}
/// Retourne si une balle est locke
-int
+bool
Map::IsLock()
{
return lock;
@@ -106,75 +133,144 @@ Map::SetLock(bool value)
lock = value;
}
+
+/// Donne la position de la balle la plus proche ou locke
+bool
+Map::getCurBallPos (double &x, double &y)
+{
+ if (checkCurBall == true)
+ {
+ x = curBall->position[0];
+ y = curBall->position[1];
+ return 1;
+ }
+ return false;
+}
+
+
+/// Retourne si un palmier est sur l'image
+bool
+Map::IsTree ()
+{
+ return treeFound;
+}
+
+
+/// Donne la position du palmier à l'image
+bool
+Map::GetTreePos (double &x, double &y)
+{
+ if (treeFound == true)
+ {
+ x = posTree[0];
+ y = posTree[1];
+ return 1;
+ }
+ return 0;
+}
+
/// Met a jour la map avec une nouvelle liste de balle
void
Map::AddBallsToMap(Group *group)
{
ZONE *pCur = group->zoneListBall;
- int pos[2];
+ double pos[2];
+ bool ballLost = 1;
+
+
while (pCur)
{
- if (!IsLock())
- {
- int posX, posY;
- space->GetLoc(pCur->centerx, pCur->centery, posX, posY);
- cout << "posX: " << posX << " posY: " << posY << endl;
- SetLock(true);
- }
-/* // TODO passer par la classe space
+
+ // TODO passer par la classe space
+ //GetPosFromLoc
pos[0] = pCur->centerx;
pos[1] = pCur->centery;
// Cherche si la balle est deja dans la liste
- list<BALL>::iterator iCur;
-
+ 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 on incremente son marqueur de viabilite
+ // Si elle l'est
else
{
- if (iCur->skepticism < config->skepticism_max)
+ // 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)
+ {
+ // 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
+
// Detection de poteau ou de goal
pCur = group->zoneListGoal;
if (pCur)
- {}
+ {
+ treeFound = 1;
+ space->GetLoc (pCur->centerx, pCur->ymin, posTree[0], posTree[1]);
+ }
+ else
+ treeFound = 0;
}
/// Met a jour les scores
void
Map::UpdateMap()
{
+ double robotPosX, robotPosY, robotAngle;
+ double distRobotBall;
+
// On met a jour tous les scores
- list<BALL>::iterator iter;
+ list<tBALL>::iterator iter;
for(iter = ball.begin(); iter != ball.end(); iter++)
{
- // iter->score = preScore
- // + config->distance_ball_robot_weight * distancerobot
- // + config->angle_ball_weight *
- // + config->ball_density_weight *
- // + config->ennemy_presence_weight *
- // + config->skepticism_weight * iter->skepticism
- // + config->ball_precision_weight *
- // + ... iter->partial;
+ distRobotBall = Dist(robotPosX, robotPosY, iter->position[0], iter->position[1]);
- if ((iter->score > curBall->score) && (!IsLock())) curBall = iter;
- }
-
+ iter->score = iter->preScore
+ + oconfig->distance_ball_robot_weight * distRobotBall
+// + oconfig->angle_ball_weight * abs (robotAngle - Angle (iter->position[2], robotPosY, distRobotBall));
+ // + oconfig->ball_density_weight *
+ // + oconfig->ennemy_presence_weight *
+ + oconfig->skepticism_weight * iter->skepticism;
+ // + oconfig->ball_precision_weight *
+ // + ... iter->partial;
+ if (checkCurBall)
+ if ((!IsLock()) && (iter->score > curBall->score))
+ {
+ curBall = iter;
+ checkCurBall = 1;
+ }
+ }
}
@@ -182,15 +278,15 @@ Map::UpdateMap()
void
Map::ShowBalls()
{
- list<BALL>::iterator iter;
+ list<tBALL>::iterator iter;
int i=0;
- cout << "balles:\n";
+ cout << "balles: (" << ball.size () << ")" << endl;;
/// Parcours de la liste
for(iter = ball.begin(); iter != ball.end(); iter++)
{
/// Affichage des infos de la balle
- cout << i << ": " << iter->position[0] << "\t" << iter->position[1] << endl;
+ cout << i << ": " << iter->position[0] << "\t" << iter->position[1] << "\t" << iter->skepticism << "\t" << iter->score << "\t" << iter->bottom << endl;
i++;
}
cout << endl;
diff --git a/2004/i/nono/src/ovision/map.h b/2004/i/nono/src/ovision/map.h
index c940bca..2fa8fee 100644
--- a/2004/i/nono/src/ovision/map.h
+++ b/2004/i/nono/src/ovision/map.h
@@ -8,6 +8,7 @@
#include "group.h"
#include "oconfig.h"
#include "space.h"
+#include "motor/motor.h"
#define LOCKED 1
@@ -19,15 +20,15 @@
/// Structure stockant les balles
-struct BALL {
+struct tBALL {
/// position de la balle
- int position[2];
+ double position[2];
/// score balle pour le choix de la prochaine balle a aller chercher
- int score;
+ double score;
/// score precalcule sans distance pour eviter les calculs redondants
- int preScore;
+ double preScore;
// zone balle, facilite le calcul de la distance robot-balle
int zone;
@@ -36,7 +37,13 @@ struct BALL {
int skepticism;
/// balle vu seulement partiellement par la camera
- int partial;
+ bool partial;
+
+ /// précision de la position de la balle pour mettre à jour sa position
+ int precision;
+
+ /// marqueur pour savoir si la balle a quitte le champ de vision par le bas
+ int bottom;
};
@@ -44,36 +51,48 @@ struct BALL {
class Map
{
/// Variables configurables
- OConfig *config;
+ OConfig *oconfig;
Space *space;
+// Motor &motor;
+// const Tracker &tracker;
/// position des goals
- int posGoal[2];
+ double posGoal[2];
/// Liste de balles trouvees
- std::list<BALL> ball;
+ std::list<tBALL> ball;
- /// Stocke le numero de la balle locke
- int lock;
+ /// balle locke ?
+ bool lock;
+
+ /// Palmier sur l'image ?
+ bool treeFound;
+
+ /// Position du palmier à l'écran
+ double posTree[2];
/// Ajoute une balle a la map
- void AddBall(int *pos, ZONE *pZone);
+ void AddBall(double *pos, ZONE *pZone);
/// Supprime une balle de la map
- void DelBall(std::list<BALL>::iterator &iter);
+ void DelBall(std::list<tBALL>::iterator &iter);
/// Test si une balle trouvà correspond a une balle de la map
- int TestSimilarBall(ZONE *pBall , std::list<BALL>::iterator &iter);
+ int TestSimilarBall(ZONE *pBall , std::list<tBALL>::iterator &iter);
/// Distance robot-balle
- int Dist(int *pos1, int *pos2);
-
+ double Dist(double *pos1, double *pos2);
+ double Dist(double pos1X, double pos1Y, double pos2X, double pos2Y);
+
+ /// Calcul de l'angle robot-balle
+ double Angle (double ballPosY, double robotPosY, double distRobotBall);
+
/// Balle ayant le plus haut score
- std::list<BALL>::iterator curBall;
+ std::list<tBALL>::iterator curBall;
+ /// Permet de savoir si une balle est dans curBall
+ bool checkCurBall;
-
-
public:
/// Balle ayant la plus grand score
@@ -82,34 +101,41 @@ class Map
/// Zone ou est situe le robot
int zoneRobot;
- /// Permet de savoir si une balle est dans curBall
- bool checkCurBall;
-
/// Constructeurs.
- Map (OConfig *config, Space *space);
+ Map (Space *space);
/// Destructeur.
~Map (void);
/// Retourne si une balle est locke
- int IsLock();
+ bool IsLock ();
/// Lock une balle pour savoir quel balle le robot suit
- void SetLock(bool value);
+ void SetLock (bool value);
+
+ /// Presence de palmier sur l'image
+ bool IsTree ();
/// Ajoute des balles a la liste
- void AddBallsToMap(Group *group);
+ void AddBallsToMap (Group *group);
/// Met a jour les scores des balles
- void UpdateMap();
+ void UpdateMap ();
//Affiche les balles de la map
- void ShowBalls();
+ void ShowBalls ();
+ bool GetTreePos (double &x, double &y);
/// Accessors
- const std::list<BALL>::iterator &GetCurBall();
+ /// Renvoie l'iterator sur la balle courante
+ const std::list<tBALL>::iterator &GetCurBall ();
+
+ /// Donne la position de la balle la plus proche ou locke
+ /// Retourne NULL si la balle locke est perdu
+ bool getCurBallPos (double &x, double &y);
+ void GetPosFromLoc (int locImgX, int locImgY, double &locX, double &locY);
protected:
};
diff --git a/2004/i/nono/src/ovision/oconfig.cc b/2004/i/nono/src/ovision/oconfig.cc
index d4799a5..5fbd46e 100644
--- a/2004/i/nono/src/ovision/oconfig.cc
+++ b/2004/i/nono/src/ovision/oconfig.cc
@@ -11,6 +11,7 @@
#include <string>
using namespace std;
+OConfig *OConfig::instance = 0;
/// Parse une ligne du fichier de config
/// @param *var nom de la variable a fixer
@@ -56,6 +57,9 @@ void OConfig::Parse(char *var, char *arg) {
else if(!strcmp(var,"Ball_Precision")) ball_precision_weight = atoi(argu);
else if(!strcmp(var,"Skepticism")) skepticism_weight = atoi(argu);
else if(!strcmp(var,"Skepticism_Max")) skepticism_max = atoi(argu);
+ 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;
@@ -76,6 +80,8 @@ OConfig::OConfig(char *filename) {
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;
@@ -310,7 +316,7 @@ void OConfig::LoadDistFile(char *filename) {
}
int i=0;
- while (i<3)
+ while (i<NB_POINTS)
{
fgets(buf, 50, file);
if (buf[0] != '#')
@@ -339,7 +345,7 @@ void OConfig::CreateDistFile(char *filename) {
fprintf(file, "#imgX\timgY\tdistX\tdistY\n");
- for (int i=0; i<3; i++)
+ 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]);
fclose(file);
diff --git a/2004/i/nono/src/ovision/oconfig.h b/2004/i/nono/src/ovision/oconfig.h
index 9b3daf1..96151f8 100644
--- a/2004/i/nono/src/ovision/oconfig.h
+++ b/2004/i/nono/src/ovision/oconfig.h
@@ -16,12 +16,17 @@
#define SOURCE_USB_CAM 1
#define SOURCE_CAM 2
+#define NB_POINTS 3
/// Charge le fichier config et distribue les variables
class OConfig {
-
+
+ static OConfig* instance;
+
public:
+
+ /////////////////////////////// IMG ///////////////////////////////////////////////////
/// hauteur de l'image
unsigned int height;
@@ -31,6 +36,11 @@ class OConfig {
/// source d'aquisition
int source;
+ /// mode de couleur (RGB, YUV, HSI)
+ int colorMode;
+
+
+ /////////////////////////////// RESEAU DE NEURONES ///////////////////////////////////////////////////
/// nombre d'iteration pour l'apprentissage(number iteration learning)
long nn_nil;
@@ -55,6 +65,11 @@ class OConfig {
/// influence de la luminosite dans l'integration de l'image
float nn_influ_lum;
+ /// tableau des poids du reseau de neurones
+ unsigned char *node;
+
+
+ ////////////////////////////////// MAP ///////////////////////////////////////////////////////////////
/// erreur accepte pour la construction de la map
int map_error;
int map_error_part;
@@ -82,31 +97,40 @@ class OConfig {
/// Max de la valeur
int skepticism_max;
+
+ /// Décrémentation lorsqu'une balle est perdue
+ int ball_lost_weight;
- /// mode de couleur (RGB, YUV, HSI)
- int colorMode;
-
+ /// temps avant que le marqueur de la balle commence a descendre si elle est partie vers le bas
+ int ball_bottom_time_out;
+
+
+ /////////////////////////////// GROUP ////////////////////////////////////////////////////////////////
/// numero de la couleur des balles a chercher
int groupColor;
/// numero de la couleur des poteaux a chercher
int goalColor;
+ /// taille minimum des zones trouvées
+ int minLengthZone;
+
/// tableau de correspondances des couleurs RGB
unsigned char *color;
+
+
+ /////////////////////////////// SPACE ////////////////////////////////////////////////////////////////
+ /// Points utilises pour le calcul de distance
+ int tabPoint[NB_POINTS][4];
- /// tableau des poids du reseau de neurones
- unsigned char *node;
-
+
+ /////////////////////////////// UI ////////////////////////////////////////////////////////////////
/// tableau d'index des couleurs a melanger (merge)
int *index;
/// Chemin d'acces des images
char imgPath[30];
- /// Points utilises pour le calcul de distance
- int tabPoint[3][4];
-
/// Constructeur
OConfig (char *filename);
@@ -131,6 +155,9 @@ class OConfig {
/// Chargement du fichier de la table de distance
void LoadDistFile(char *filename);
+ static OConfig* GetInstance ()
+ {return instance;}
+
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 afc4bd1..c17ff33 100644
--- a/2004/i/nono/src/ovision/segmNN.cc
+++ b/2004/i/nono/src/ovision/segmNN.cc
@@ -15,12 +15,12 @@ using namespace std;
/// Constructor SegmNN
/// @param img classe img permettant d'acceder au donnees de l'image a traiter
-/// @param config classe config permettant d'acceder aux variables de configuration
-SegmNN::SegmNN(Img *img, OConfig *config) {
+/// @param oconfig classe oconfig permettant d'acceder aux variables de oconfiguration
+SegmNN::SegmNN(Img *img) {
// Sauvegarde les pointeurs
SegmNN::img = img;
- SegmNN::config = config;
- nbOutput = config->nn_NbCouleurs;
+ oconfig = OConfig::GetInstance ();
+ nbOutput = oconfig->nn_NbCouleurs;
node = NULL;
index = NULL;
@@ -63,7 +63,7 @@ SegmNN::BuildNN(int nbOutput, int loadFromFile)
if (loadFromFile)
{
// Verifie si le nombre de poids donne dans le fichier est suffisant
- if (config->nbNodeMax < nbOutput)
+ if (oconfig->nbNodeMax < nbOutput)
{
cerr << "SegmNN::BuildNN : Nombre de nodes insuffisants dans le fichier poids\n";
}
@@ -73,11 +73,11 @@ SegmNN::BuildNN(int nbOutput, int loadFromFile)
delete [] node;
node = new unsigned char[nbOutput*3];
for (int i = 0; i<nbOutput*3; i++)
- node[i] = config->node[i];
+ node[i] = oconfig->node[i];
delete [] index;
index = new int[nbOutput];
for (int i = 0; i<nbOutput; i++)
- index[i] = config->index[i];
+ index[i] = oconfig->index[i];
freq = new unsigned int[nbOutput];
}
return;
@@ -122,10 +122,10 @@ SegmNN::TrainNN()
int numOutputMax=0;
tabData = img->tabData;
- config->colorMode = img->hsi*2 + img->yuv;
- config->groupColor = 0;
+ oconfig->colorMode = img->hsi*2 + img->yuv;
+ oconfig->groupColor = 0;
- for(long i=0; i<config->nn_nil; i++)
+ for(long i=0; i<oconfig->nn_nil; i++)
{
// On choisit un pixel au hasard
pixelNum = (unsigned long)(img->nbPixels*(rand()/(RAND_MAX+1.0)));
@@ -138,7 +138,7 @@ SegmNN::TrainNN()
for(int k=0; k<3; k++)
{
if ((img->yuv && k==0) || (img->hsi && k==2))
- output[j] += abs((int)(config->nn_influ_lum*node[j*3+k]-tabData[pixelNum*3+k])*abs(node[j*3+k]-tabData[pixelNum*3+k]));
+ output[j] += abs((int)(oconfig->nn_influ_lum*node[j*3+k]-tabData[pixelNum*3+k])*abs(node[j*3+k]-tabData[pixelNum*3+k]));
else
output[j] += abs(node[j*3+k]-tabData[pixelNum*3+k])*abs(node[j*3+k]-tabData[pixelNum*3+k]);
}
@@ -152,7 +152,7 @@ SegmNN::TrainNN()
for(int k=0; k<3; k++)
{
node[numOutputMax*3+k] =
- (unsigned char)((node[numOutputMax*3+k] + config->nn_sl*tabData[pixelNum*3+k])/(1+config->nn_sl));
+ (unsigned char)((node[numOutputMax*3+k] + oconfig->nn_sl*tabData[pixelNum*3+k])/(1+oconfig->nn_sl));
// Recompense pour la sortie qui travaille
freq[numOutputMax]++;
@@ -162,7 +162,7 @@ SegmNN::TrainNN()
{
for (int k=0; k < nbOutput; k++)
{
- if (freq[numOutputMax] < config->nn_lazy_threshold)
+ if (freq[numOutputMax] < oconfig->nn_lazy_threshold)
{
// Regeneration de nouveaux poids
for(int m=0; m<3; m++)
@@ -202,7 +202,7 @@ SegmNN::FindColorNN(unsigned char *x)
{
// XXX Ne pas oublier de mettre abs si on ne calcule pas les carrés
tmp = node[j*3]-x[0];
- output[j] = abs((int)(config->nn_influ_lum*tmp*tmp));
+ output[j] = abs((int)(oconfig->nn_influ_lum*tmp*tmp));
tmp = node[j*3+1]-x[1];
output[j] += tmp*tmp;
tmp = node[j*3+2]-x[2];
@@ -295,11 +295,11 @@ SegmNN::TestNN()
// Parcours de toutes les valeurs de nombres d'itérations
for (int i_nil = 0; i_nil<5; i_nil++) {
- config->nn_nil = nil[i_nil];
+ oconfig->nn_nil = nil[i_nil];
// Parcours de toutes les valeurs de Step Learning
for (int i_sl = 0; i_sl<4; i_sl++) {
- config->nn_sl = sl[i_sl];
+ oconfig->nn_sl = sl[i_sl];
// Apprentissage du NN
TrainNN();
@@ -311,8 +311,8 @@ SegmNN::TestNN()
sprintf(filename, "NN/%i-%f-%luNN.jpg", nc[i_nc], sl[i_sl], nil[i_nil]);
img->WriteSegm(filename, tabSegm);
sprintf(filename, "NN/%i-%f-%luNN", nc[i_nc], sl[i_sl], nil[i_nil]);
- config->node = node;
- config->CreateNNFile(filename, config->colorMode, nbOutput);
+ oconfig->node = node;
+ oconfig->CreateNNFile(filename, oconfig->colorMode, nbOutput);
}
}
}
diff --git a/2004/i/nono/src/ovision/segmNN.h b/2004/i/nono/src/ovision/segmNN.h
index d98c92f..ecd733a 100644
--- a/2004/i/nono/src/ovision/segmNN.h
+++ b/2004/i/nono/src/ovision/segmNN.h
@@ -23,7 +23,7 @@ class SegmNN
Img *img;
// Classe config
- OConfig *config;
+ OConfig *oconfig;
public:
// tableau avec couleurs segmentees
@@ -42,7 +42,7 @@ class SegmNN
int nbOutput;
/// Constructeur
- SegmNN (Img *img, OConfig *config);
+ SegmNN (Img *img);
/// Destructeur
~SegmNN (void);
diff --git a/2004/i/nono/src/ovision/space.cc b/2004/i/nono/src/ovision/space.cc
index a9e232b..8b41521 100644
--- a/2004/i/nono/src/ovision/space.cc
+++ b/2004/i/nono/src/ovision/space.cc
@@ -5,24 +5,24 @@
/// @file space.cc Etalonnage des distances et localisation de la balle
#include "space.h"
-#include <math.h>
#include "group.h"
+
using namespace std;
/// Constructeur
/// @param imgHeight hauteur de l'image
/// @param imgWidth largeur de l'image
-Space::Space (int imgHeight, int imgWidth, OConfig *oconfig)
+Space::Space (int imgHeight, int imgWidth)
{
Space::imgHeight = imgHeight;
Space::imgWidth = imgWidth;
Space::oconfig = oconfig;
- aY = bY = cY = 0;
- aX = aY = 0;
+ for (int i=0; i<NB_POINTS; i++)
+ coefX[i] = coefY[i] = i;
}
@@ -35,7 +35,7 @@ Space::~Space ()
/// Ajoute un point pour l'etalonnage
void Space::AddSetupPoint(int x, int y, int distx, int disty)
{
- if (setupTab.size() > 3)
+ if (setupTab.size() > NB_POINTS)
{
std::vector<SETUP_POINT>::iterator iter;
iter = setupTab.begin();
@@ -56,66 +56,150 @@ void Space::AddSetupPoint(int x, int y, int distx, int disty)
/// Chargement des points a partir d'un fichier
void Space::LoadFromFile()
{
- for (int i=0; i<3; i++)
+ 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]);
}
+
+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
-int Space::Determinant(int matrix[][4])
+double Space::Determinant(double *matrix, int n)
{
- int det = 0;
+ 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++)
+ {
+ coeff = CoeffDeterminant(matrix, m, n);
- det += matrix[0][2] * (matrix[1][0] * matrix[2][1] - matrix[2][0] * matrix[1][1]);
- det -= matrix[1][2] * (matrix[0][0] * matrix[2][1] - matrix[2][0] * matrix[0][1]);
- det += matrix[2][2] * (matrix[0][0] * matrix[1][1] - matrix[1][0] * matrix[0][1]);
+ 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);
+ }
+
+ 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
-int Space::CaracteristicDeterminant(int matrix[][4], int numCol)
+double Space::CaracteristicDeterminant(double *matrix, double *endMat, int numCol)
{
- int save[3];
-
- // Deplacement de la iÃme colonne et sauvegarde
+ 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][numCol];
- matrix[i][numCol] = matrix[i][3];
+ save[i] = matrix[i*3+numCol];
+ matrix[i*3+numCol] = endMat[i];
}
// Calcul du determinant caracteristique
- int carDet = Space::Determinant(matrix);
+ double carDet = Space::Determinant(matrix, 3);
// Restauration de la matrice d'origine
for (int i=0; i<3; i++)
- matrix[i][numCol] = save[i];
+ matrix[i*3+numCol] = save[i];
return carDet;
}
+
+
+int Space::BestFitDoTab(double *mat, double *endMat)
+{
+ 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;
+ }
+
+}
+
+
+void Space::Setup(double aX, double bX, double cX, double aY, double bY, double cY)
+{
+ coefX[0] = cX;
+ coefX[1] = bX;
+ coefX[2] = aX;
+ coefY[0] = cY;
+ coefY[1] = bY;
+ coefY[2] = aY;
+}
+
+
/// Etalonnage des distances
int Space::Setup()
{
- if (setupTab.size() < 3)
+ 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
- int matrix[3][4] = {{setupTab[0].y * setupTab[0].y, setupTab[0].y, 1, setupTab[0].disty},
- {setupTab[1].y * setupTab[1].y, setupTab[1].y, 1, setupTab[1].disty},
- {setupTab[2].y * setupTab[2].y, setupTab[2].y, 1, setupTab[2].disty}};
- int det = Space::Determinant(matrix);
+ double mat[NB_POINTS*NB_POINTS];
+ double endMat[NB_POINTS];
+
+ double det = Space::Determinant(mat, 3);
+
if (det != 0)
{
- aY = (double)Space::CaracteristicDeterminant(matrix, 0) / det;
- bY = (double)Space::CaracteristicDeterminant(matrix, 1) / det;
- cY = (double)Space::CaracteristicDeterminant(matrix, 2) / det;
- // cout << "aY: " << aY << " bY: " << bY << "cY: " << cY << endl;
+ for (int i=0; i<3; i++)
+ {
+ coefY[i] = (double)Space::CaracteristicDeterminant(mat, endMat, i) / det;
+ cout << coefY[i] << endl;
+ }
}
else
{
@@ -124,11 +208,25 @@ int Space::Setup()
}
// Calcul des coeff pour l'approximation de l'horizontal
- if ((setupTab[0].x != 0) && (setupTab[2].x != 0))
+/* 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);
+
+ if (det != 0)
{
- aX = (setupTab[2].distx - setupTab[0].distx * (double)setupTab[2].x / setupTab[0].x) / (setupTab[2].y * setupTab[2].x - setupTab[0].y * setupTab[2].x);
- bX = setupTab[0].distx / setupTab[0].x - aX * setupTab[0].y;
-// cout << "aX: " << aX << " bX: " << bX << endl;
+ for (int i=0; i<NB_POINTS; i++)
+ {
+ coefX[NB_POINTS-i-1] = (double)Space::CaracteristicDeterminant(mat, endMat, i) / det;
+ }
+
}
else
{
@@ -136,26 +234,57 @@ int Space::Setup()
return 0;
}
+
+ /* 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)
+{
+ // Calcul de locX
+ locX = (locImgY * locImgX * coefX[2] + coefX[1]*locImgX + coefX[0]);
+
+ // Calcul de locY
+ locY = (locImgY * locImgY * coefY[2] + locImgY * coefY[1] + coefY[0]);
+}
+
void Space::GetLoc(int locImgX, int locImgY, int &locX, int &locY)
{
- locX = (int)((locImgY * aX + bX) * locImgX);
- locY = (int)(locImgY*locImgY*aY + locImgY*bY + cY);
+ 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(int locX, int locY, int posRobotX, int posRobotY, double angleRobot, int &posX, int &posY)
+void Space::GetPos(int locX, int locY, int posRobotX, int posRobotY, double angleRobot, double &posX, double &posY)
{
- float sinus = sin(angleRobot);
- float cosinus = cos(angleRobot);
+ double sinus = sin(angleRobot);
+ double cosinus = cos(angleRobot);
// Calcul des coordonnes avec le changement de repere
- posX = (int)(posRobotX + locX*cosinus - locY*sinus);
- posY = (int)(posRobotY + locX*sinus + locY*cosinus);
+ posX = posRobotX + locX*cosinus - locY*sinus;
+ posY = posRobotY + locX*sinus + locY*cosinus;
}
+
diff --git a/2004/i/nono/src/ovision/space.h b/2004/i/nono/src/ovision/space.h
index c9843e7..52f6e6d 100644
--- a/2004/i/nono/src/ovision/space.h
+++ b/2004/i/nono/src/ovision/space.h
@@ -39,21 +39,22 @@ class Space
int imgWidth;
/// coeff pour l'approximation de la profondeur
- double aY, bY, cY;
+ double coefY[NB_POINTS];
/// coeff pour l'approximation de l'horizontal
- double aX, bX;
+ double coefX[NB_POINTS];
public:
// Constructeur
- Space (int width, int height, OConfig *oconfig);
+ Space (int width, int height);
// Destructeur
~Space ();
/// Position d'un objet dans le referentiel du robot
void GetLoc(int locImgX, int locImgY, int &locX, int &locY);
+ void GetLoc(int locImgX, int locImgY, double &locX, double &locY);
/// Ajoute un point pour l'etalonnage
void AddSetupPoint(int x, int y, int distx, int disty);
@@ -64,15 +65,22 @@ class Space
/// Etalonnage des distances
int Setup();
+ void Setup(double aX, double bX, double cX, double aY, double bY, double cY);
protected:
/// Calcul du determinant
- int Determinant(int matrix[][4]);
+ double Determinant33(double *matrix);
+ double Determinant(double *matrix, int n);
+
+ double CoeffDeterminant (double *matrix, int i, int n);
+
/// Calcul du determinant caracteristique
- int CaracteristicDeterminant(int matrix[][4], int numRow);
+ 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);
+
+ int BestFitDoTab(double *mat, double *endMat);
};
#endif // space_h
diff --git a/2004/i/nono/src/ovision/test_ovision.cc b/2004/i/nono/src/ovision/test_ovision.cc
index 1a10385..8e4956b 100644
--- a/2004/i/nono/src/ovision/test_ovision.cc
+++ b/2004/i/nono/src/ovision/test_ovision.cc
@@ -5,6 +5,7 @@
#include "oconfig.h"
#include "group.h"
#include "space.h"
+#include <stdlib.h>
int
main()
@@ -19,18 +20,18 @@ main()
Video4Linux v4l("/dev/video", cs, 55000);
v4l.calibrate ();
- Space space(img.width, img.height, &oconfig);
+ Space space(img.width, img.height);
space.AddSetupPoint (356, 23, 300, 300);
space.AddSetupPoint (283, 171, 600, 600);
space.AddSetupPoint (253, 234, 1000, 900);
space.Setup ();
- SegmNN segmNN(&img, &oconfig);
+ SegmNN segmNN(&img);
segmNN.BuildNN(oconfig.nn_NbCouleurs, LOAD_FROM_FILE);
Group group(&img, &segmNN);
- Map map(&oconfig, &space);
+ Map map(&space);
/////////////////////////////////////////////////////////////////////////////////////////
@@ -45,26 +46,32 @@ main()
img.load(v4l);
img.load(v4l);
sprintf(filename, "test%i.rgb", i);
- img.WriteRGB(filename);
+// img.WriteRGB(filename);
// segmNN.Segm();
group.JumpPoints(oconfig.groupColor, oconfig.goalColor);
group.ShowZones();
std::cout << "-------------\n" << std::endl;
- if (group.zoneListBall)
+
+ ZONE *pCur = group.zoneListBall;
+
+ int j=0;
+ while (pCur)
{
int x,y;
- double angle;
x = group.zoneListBall->centerx;
y = img.height - group.zoneListBall->centery;
space.GetLoc(x, y, x, y);
- std::cout << x << " " << y << std::endl;
- std::cout << y << std::endl;
-
+ std::cout << j << ":" << x << " " << y << std::endl;
+ pCur = pCur->next;
}
std::cout << "-------------\n" << std::endl;
+ std::cout << "Map:\n" << std::endl;
+ map.AddBallsToMap (&group);
+ map.ShowBalls ();
- getchar();
+ sleep(1);
+
i++;
}
/////////////////////////////////////////////////////////////////////////////////////////
diff --git a/2004/i/nono/src/ovision/testdist.cc b/2004/i/nono/src/ovision/testdist.cc
index f2ed486..e720628 100644
--- a/2004/i/nono/src/ovision/testdist.cc
+++ b/2004/i/nono/src/ovision/testdist.cc
@@ -10,18 +10,18 @@ int main(int argc, char **argv)
{
std::cout << "testmap renvoie les coordonnees dans le référentiel du robot par rapport aux coordonées de l'image donnée.\n";
std::cout << "Usage: testmap <coordonnees x de l'image> <coordonnees y de l'image>\n";
- return 0;
+ return 1;
}
OConfig oconfig ("rc/vision.conf");
- Space space (imgWidth, imgHeight, &oconfig);
+ Space space (imgWidth, imgHeight);
// Entree des 3 points nécessaires à la création des équations
- //space.LoadFromFile();
- space.AddSetupPoint (109, 90, 150, 9);
- space.AddSetupPoint (84, 20, 150, 2);
- space.AddSetupPoint (50, 10, 150, 1);
+ space.LoadFromFile();
+// space.AddSetupPoint (356, 23, 300, 300);
+// space.AddSetupPoint (283, 171, 600, 600);
+// space.AddSetupPoint (253, 234, 1000, 900);
// Calcul des équations pour la conversion des coordonnées
space.Setup ();
@@ -36,5 +36,5 @@ int main(int argc, char **argv)
std::cout << "Coordonnées de la balle " << locX << " " << locY << std::endl;
- return 1;
+ return 0;
}
diff --git a/2004/i/nono/src/ovision/testmap.cc b/2004/i/nono/src/ovision/testmap.cc
index 191156c..7469095 100644
--- a/2004/i/nono/src/ovision/testmap.cc
+++ b/2004/i/nono/src/ovision/testmap.cc
@@ -22,7 +22,7 @@ main()
-/* Video4Linux::ColorSpace cs;
+ Video4Linux::ColorSpace cs;
cs = Video4Linux::rgb;
Video4Linux v4l("/dev/video", cs, 30000);
v4l.calibrate ();
@@ -31,18 +31,18 @@ main()
if ((oconfig.colorMode == YUV) && (!img.yuv)) img.RGBtoYUV();
img.WriteRGB("test.rgb");
- Space space(img.width, img.height, &oconfig);
+ Space space(img.width, img.height);
space.AddSetupPoint (109, 36, 150, 900);
space.AddSetupPoint (84, 102, 150, 600);
space.AddSetupPoint (50, 259, 150, 300);
space.Setup ();
- SegmNN segmNN(&img, &oconfig);
+ SegmNN segmNN(&img);
segmNN.BuildNN(oconfig.nn_NbCouleurs, LOAD_FROM_FILE);
Group group(&img, &segmNN);
- Map map(&oconfig, &space);
+ Map map(&space);
segmNN.Segm();
group.JumpPoints(oconfig.groupColor);
diff --git a/2004/i/nono/src/ovision/ui.cc b/2004/i/nono/src/ovision/ui.cc
index 7317803..4475e08 100644
--- a/2004/i/nono/src/ovision/ui.cc
+++ b/2004/i/nono/src/ovision/ui.cc
@@ -566,7 +566,10 @@ void
UI::GoSelectGroup(int type)
{
// Menu de selection de couleur
- int tmp = ChooseColor(NUM_COULEUR, oconfig.groupColor);
+ int tmp;
+ if (type == BALL) tmp = ChooseColor(NUM_COULEUR, oconfig.groupColor);
+ else if (type == GOAL) tmp = ChooseColor(NUM_COULEUR, oconfig.goalColor);
+
if (tmp != -1)
{