summaryrefslogtreecommitdiff
path: root/2004/i/nono/src/ovision/space.cc
diff options
context:
space:
mode:
Diffstat (limited to '2004/i/nono/src/ovision/space.cc')
-rw-r--r--2004/i/nono/src/ovision/space.cc317
1 files changed, 73 insertions, 244 deletions
diff --git a/2004/i/nono/src/ovision/space.cc b/2004/i/nono/src/ovision/space.cc
index 3a6bf63..34fb2a6 100644
--- a/2004/i/nono/src/ovision/space.cc
+++ b/2004/i/nono/src/ovision/space.cc
@@ -25,13 +25,14 @@ struct less<TABLOC*> {
/// Constructeur
/// @param imgHeight hauteur de l'image
/// @param imgWidth largeur de l'image
-Space::Space (uint imgHeight, uint imgWidth)
+Space::Space (int imgHeight, int imgWidth)
{
Space::imgHeight = imgHeight;
Space::imgWidth = imgWidth;
- indexTabY = NULL;
- indexTabX = NULL;
+ aY = bY = cY = 0;
+ aX = aY = 0;
+
}
@@ -44,290 +45,117 @@ Space::~Space ()
/// Affiche la liste des distance etalonneess
void Space::ShowTab()
{
- cout << "\nTabLocY (" << tabLocY.size() << "):" << endl;
- cout << "num\tlocImageY\tdistance\tsurface" << endl;
-
- // Parcours de la liste et affichage ces elements pour y
- for (uint i=0; i < tabLocY.size(); i++)
- {
- cout << i << "\t" << tabLocY[i].locImageY << "\t\t" << tabLocY[i].distance << "\t\t" << tabLocY[i].surface << endl;
- }
-
- cout << "\nTabLocX (" << tabLocX.size() << "):" << endl;
- cout << "num\tlocImageX\tlocImageY\tangle\tdistance" << endl;
+// cout << "\nTabLocY (" << tabLocY.size() << "):" << endl;
// Parcours de la liste et affichage ces elements pour y
- for (uint i=0; i < tabLocX.size(); i++)
- {
- cout << i << "\t" << tabLocX[i].locImageX << "\t\t" << tabLocX[i].locImageY << "\t\t" << tabLocX[i].angle << "\t" << tabLocX[i].distance << endl;
- }
}
-
-
-/// Permet d'ajouter une distance pour l'etalonnage
-/// @param zone zone de l'image ou la balle se situe
-/// @param distance distance de la balle au robot
-void Space::SetDist(ZONE zone, unsigned int distance)
+/// Ajoute un point pour l'etalonnage
+void Space::AddSetupPoint(int x, int y, int distx, int disty)
{
- TABLOC loc;
-
- // Initialisation des valeurs a ajouter a la liste
- loc.locImageY = zone.centery;
- loc.distance = distance;
- loc.surface = (zone.xmax - zone.xmin) * (zone.ymax - zone.ymin);
-
- // On range cette loc dans le tableau
- // Si c'est le premier element
- if (!tabLocY.size())
- {
- tabLocY.push_back(loc);
- }
- // Si ce n'est pas le premier element
- else
- {
- // Trie par ordre croissant des distances
- vector<TABLOC>::iterator it;
- it = tabLocY.begin();
- // Parcours de la liste pour ajouter au bon endroit
- while (it != tabLocY.end())
- {
- if (it->distance < distance)
- it++;
- else
- {
- tabLocY.insert(it, loc);
- it = tabLocY.end();
- }
- }
- }
+ SETUP_POINT setupPoint;
+
+ setupPoint.x = x;
+ setupPoint.y = y;
+ setupPoint.distx = distx;
+ setupPoint.disty = disty;
+ setupTab.push_back(setupPoint);
}
-/// Cree la liste des distances etalonnees a partir d'image
-void Space::DoDistTab()
+
+int Space::Determinant(int matrix[][4])
{
- ImgFile img;
- OConfig config("vision.conf");
- SegmNN segm(&img, &config);
- TABLOC loc;
+ int det = 0;
- Date &d = Date::GetInstance();
+ 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]);
- Motor m;
- Movement *mov = new MovementGoto(-0.1, 400);
- m.setMovement(mov);
+ return det;
+}
- double x, y, angle;
- double lastY = 0;
- int lastGroupY = 0;
+int Space::CaracteristicDeterminant(int matrix[][4], int numCol)
+{
+ int save[3];
- m.go();
-
-
- // On cherche la composante de profondeur
- for (int i=0; i < 10; i++)
+ // Deplacement de la iĂme colonne et sauvegarde
+ // pour le calcul du determinant caracteristique
+ for (int i=0; i<3; i++)
{
- m.waitOK();
- m.tracker_.getPos(&x, &y, &angle);
-
- if (y > lastY+10)
- {
- img.ReadRaw();
- if (config.colorMode == YUV) img.RGBtoYUV();
-
- segm.Segm();
-
- Group group(&img, &segm);
- group.JumpPoints(config.groupColor);
-
- if (group.zoneListBall)
- {
- ZONE *pBall = group.zoneListBall;
-
- // on teste s'il y a eu un changement dans l'image
- if (pBall->centery < lastGroupY)
- {
- //group.ShowZones();
- loc.distance = y;
- loc.surface = (pBall->xmax - pBall->xmin) * (pBall->ymax - pBall->ymin);
- loc.locImageY = pBall->centery;
-
- tabLocY.push_back(loc);
-
- lastGroupY = pBall->centery;
- }
- }
- lastY = y;
- }
-
- d.wait(10);
+ save[i] = matrix[i][numCol];
+ matrix[i][numCol] = matrix[i][3];
}
- m.ungo();
+ // Calcul du determinant caracteristique
+ int carDet = Space::Determinant(matrix);
+ // Restauration de la matrice d'origine
+ for (int i=0; i<3; i++)
+ matrix[i][numCol] = save[i];
- return 0;
+ return carDet;
}
-
-
-
-
-
-
-
-
-
-/* const int nbFile = 6;
- char file[nbFile][40] = {{"../thumbrobot/9.jpg"}, {"../thumbrobot/7.jpg"}, {"../thumbrobot/8.jpg"},
- {"../thumbrobot/6.jpg"}, {"../thumbrobot/10.jpg"}, {"../thumbrobot/11.jpg"}};
-
- segm.BuildNN(config.nn_NbCouleurs, LOAD_FROM_FILE);
-
- // On cherche la composante de profondeur
- for (int i=0; i < nbFile; i++)
- {
- img.ReadRaw(file[i]);
- if (config.colorMode == YUV) img.RGBtoYUV();
-
- segm.Segm();
-
- Group group(&img, &segm);
- group.JumpPoints(config.groupColor);
-
- if (group.zoneListBall)
- {
- ZONE *pBall = group.zoneListBall;
- //group.ShowZones();
- loc.distance = i;
- loc.surface = (pBall->xmax - pBall->xmin) * (pBall->ymax - pBall->ymin);
- loc.locImageY = pBall->centery;
-
- tabLocY.push_back(loc);
- }
- }
-
- // On cherche la composante honrizontale
-/* for (int i=0; i < nbFile; i++)
- {
- img.ReadRaw(file[i]);
- if (config.colorMode == YUV) img.RGBtoYUV();
-
- segm.Segm();
-
- Group group(&img, &segm);
- group.JumpPoints(config.groupColor);
-
- if (group.zoneListBall)
- {
- ZONE *pBall = group.zoneListBall;
- loc.locImageY = pBall->centery;
- loc.locImageX = pBall->centerx;
- loc.distance = (int)sqrt((double)((pBall->centery*pBall->centery)+(pBall->centerx*pBall->centerx)));
- loc.angle = i;
-
- // get angle
-
- tabLocX.push_back(loc);
- }
- }
-*/
-// tabLocY.sort(tabLoc.begin(), tabLoc.end(), less<BALL>);
-
- /*
- // Exemple de remplissage de la table au lieu d'utiliser les images
- for(int i=0; i<10; i++) {
- loc.locImageY = i*11;
- loc.distance = i;
- loc.surface = 100;
-
- tabLocY.push_back(loc);
- }
- */
-}
-
-
-/// Cree le tableau d'index a partir de la liste
-void Space::DoIndexTab()
+/// Etalonnage des distances
+int Space::Setup()
{
- uint start, stop;
-
- // Allocation de la memoire
- if (indexTabY) delete [] indexTabY;
- indexTabY = new uint[imgHeight];
- if (indexTabX) delete [] indexTabX;
- indexTabX = new uint[imgHeight*imgWidth];
-
- // Parcours de la liste des distances ajoutées avec SetDist
- // et creation d'un tableau
- for(uint y=0; y < tabLocY.size(); y++)
+ if (setupTab.size() < 3)
{
- // Recherche des bornes entre 2 distances de la liste
- if (y == 0)
- {
- start = 0;
- stop = tabLocY[1].locImageY / 2;
- }
- else if (y == tabLocY.size()-1)
- {
- start = stop;
- stop = imgHeight;
- }
- else
- {
- start = stop;
- stop = (tabLocY[y+1].locImageY - tabLocY[y].locImageY) / 2 + tabLocY[y].locImageY;
- }
-
- // Remplissage du tableau d'index
- for(uint j=start; j < stop; j++)
- indexTabY[j] = y;
+ cerr << "Space::Setup : Pas assez de points donnĂs, utilisĂ AddSetupPoint\n";
+ return 0;
}
- for(uint y=0; y < imgHeight; y++)
+ // 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);
+ if (det != 0)
{
- for(uint x=0; x < imgWidth; x++)
- {
- indexTabX[y*imgWidth+x] = 0;//distance * cos(angle) * locImageX / POS_ROBOT_ETALONNOGE_X ;
- }
+ aY = (double)Space::CaracteristicDeterminant(matrix, 0) / det;
+ bY = (double)Space::CaracteristicDeterminant(matrix, 1) / det;
+ cY = (double)Space::CaracteristicDeterminant(matrix, 2) / det;
+ }
+ else
+ {
+ cerr << "Space::Setup : Determinant egal a zero, donner d'autres points\n";
+ return 0;
}
+ // Calcul des coeff pour l'approximation de l'horizontal
+ if ((setupTab[0].x != 0) && (setupTab[2].x != 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;
+ }
+ else
+ {
+ cerr << "Space::Setup : Coordonnees X egales a zero, donner d'autres points\n";
+ return 0;
+ }
-
+ return 1;
}
-/// Renvoie la distance en fonction du pixel donne
-/// @param locImageY pixel de l'image ou la balle a ete trouvee
-uint Space::GetDistY(unsigned int locImageY)
-{
- // Retourne la distance inscrit dans la table tabLocY
- return tabLocY[indexTab[locImageY]].distance;
-}
-
-uint Space::GetDistX(unsigned int locImageX, unsigned int locImageY)
-{
- // Retourne la distance inscrit dans la table tabLocY
- return 1;
-}
/// Position d'un objet dans le referentiel du robot
-/*void Space::GetLoc(uint locImgX, uint locImgY, uint *locX, uint *locY)
+void Space::GetLoc(int locImgX, int locImgY, int &locX, int &locY)
{
- locX = 0;
-// locY = tabLocY[indexTab[locImgY]].distance;
+ locX = (int)((locImgY * aX + bX) * locImgX);
+ locY = (int)(locImgY*locImgY*aY + locImgY*bY + cY);
}
-*/
/// Donne la position reelle sur la table de la balle
/// @param *vOrig position du robot sur la table
/// @param *vDir position de la balle par rapport au robot
/// @param angle rotation du robot par rapport a sa position d'origine
-void Space::GetPos(Vec *vOrig, Vec *vDir, float angle)
+/*void Space::GetPos(Vec *vOrig, Vec *vDir, float angle)
{
Vec vPos;
float sinus = sin(angle);
@@ -339,3 +167,4 @@ void Space::GetPos(Vec *vOrig, Vec *vDir, float angle)
printf("Position: %f %f\n", vPos.x, vPos.y);
}
+*/