From 4fda347a9166d249073913cc1da8b9e25a6ce0cc Mon Sep 17 00:00:00 2001 From: gaillaro Date: Sun, 16 May 2004 20:53:16 +0000 Subject: qq modifs sur map --- 2004/i/nono/src/ovision/Makefile.defs | 4 +- 2004/i/nono/src/ovision/adjust.cc | 34 +++--- 2004/i/nono/src/ovision/comm.cc | 8 +- 2004/i/nono/src/ovision/group.cc | 92 ++++++++------ 2004/i/nono/src/ovision/group.h | 14 ++- 2004/i/nono/src/ovision/map.cc | 210 +++++++++++++++++++++++--------- 2004/i/nono/src/ovision/map.h | 82 ++++++++----- 2004/i/nono/src/ovision/oconfig.cc | 10 +- 2004/i/nono/src/ovision/oconfig.h | 47 +++++-- 2004/i/nono/src/ovision/segmNN.cc | 36 +++--- 2004/i/nono/src/ovision/segmNN.h | 4 +- 2004/i/nono/src/ovision/space.cc | 207 +++++++++++++++++++++++++------ 2004/i/nono/src/ovision/space.h | 18 ++- 2004/i/nono/src/ovision/test_ovision.cc | 27 ++-- 2004/i/nono/src/ovision/testdist.cc | 14 +-- 2004/i/nono/src/ovision/testmap.cc | 8 +- 2004/i/nono/src/ovision/ui.cc | 5 +- 17 files changed, 577 insertions(+), 243 deletions(-) (limited to '2004/i/nono/src/ovision') 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= 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(iBuildNN(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; itabPoint[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 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 #include -#include 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::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::iterator &iter) +Map::DelBall(list::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::iterator &iter) +Map::TestSimilarBall(ZONE *pBall, list::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::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::iterator iCur; - + list::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::iterator iter; + list::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::iterator iter; + list::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; + std::list 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::iterator &iter); + void DelBall(std::list::iterator &iter); /// Test si une balle trouvà correspond a une balle de la map - int TestSimilarBall(ZONE *pBall , std::list::iterator &iter); + int TestSimilarBall(ZONE *pBall , std::list::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::iterator curBall; + std::list::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::iterator &GetCurBall(); + /// Renvoie l'iterator sur la balle courante + const std::list::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 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 (inn_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; inode[i]; + node[i] = oconfig->node[i]; delete [] index; index = new int[nbOutput]; for (int i = 0; iindex[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; inn_nil; i++) + for(long i=0; inn_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 #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 3) + if (setupTab.size() > NB_POINTS) { std::vector::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; itabPoint[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 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 \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) { -- cgit v1.2.3