// skittle.cc - Classe Skittle // robert - Programme du robot APBteam // Copyright (C) 2005 Olivier Gaillard /// @file skittle.cc Reconnaissance des objets quilles #include #include "skittle.hh" #include "hotelling.hh" /// Constructeur Skittle::Skittle (Img *img, Segm *segm) : segm_ (segm), img_ (img), pcX_ (0), pcY_ (0) { oconfig_ = OConfig::getInstance (); } /// Destructeur Skittle::~Skittle (void) { } /// Recherche de la composante principale void Skittle::pca (Zone &zone) { // Agrandissement de la zone de recherche const int grow = oconfig_->skittleGrow; int ymin = img_->minWithBorder (zone.ymin - grow, 0); int ymax = img_->maxWithBorder (zone.ymax + grow, img_->height_); int xmin = img_->minWithBorder (zone.xmin - grow, 0); int xmax = img_->maxWithBorder (zone.xmax + grow, img_->width_); /// Initialisation du saut de point int jump = oconfig_->jumpPointDist/oconfig_->skittleDivJump; if (!jump) jump = 1; // Parcours d'une partie des pixels de l'image // Sauvegarde des points et de la densité int tmpY; int nbPoints = 0; std::vector l; for (int y = ymin; y < ymax; y += jump) { tmpY = y*img_->width_; for (int x = xmin; x < xmax; x += jump) if (segm_->giveColor (img_->tabData_ + ((tmpY + x) * 3), true, true) == zone.color) { // *f2 << x << " " << y << std::endl; Hpoint h (x, y); l.push_back (h); if ((x >= zone.xmin) && (x <= zone.xmax) && (y >= zone.ymin) && (y <= zone.ymax)) ++nbPoints; } } // Calcul de la densité zone.area = (zone.xmax - zone.xmin) * (zone.ymax - zone.ymin); zone.density = nbPoints * jump * jump / (double)zone.area; // Calcul de la composante principale Hotelling hote (l); hote.eigenVectors (); hote.get (pcX_, pcY_, pc2X_, pc2Y_); } /// Utilisé pour la recherche de la courbure, cherche le point le plus haut int Skittle::climb (const int startX, int startY, const int color) { while ((segm_->giveColor (img_->tabData_ + ((startY*img_->width_ + startX) * 3), true, true) == color) && (startY > 0)) { --startY; // *f << startX << " " << startY << "\n"; } return startY; } /// Vérifie qu'un catadiopre est à proximité Skittle::Pos Skittle::whereIsReflectBand (const Zone &zone, const std::vector &listReflectBand) { const double minAreaRatio = 0.1; const double maxAreaRatio = 0.4; // Test les variables de composantes principales + liste transmise if (!pcX_ || !pcY_ || listReflectBand.size ()) return error; // Analyse de la taille if (zone.area < minAreaRatio || zone.area > maxAreaRatio) // valeur réelle 0.2 // Parcours la liste des catadiopres for (std::vector::const_iterator iter = listReflectBand.begin (); iter != listReflectBand.end (); ++iter) { // Test la distance cartésienne if (dist (iter->centerx, iter->centery, zone.centerx, zone.centery)) // Test la distance orthogonale if (orthoDist (iter->centerx - zone.centerx, iter->centery - zone.centery, pcX_, pcY_)) { if (iter->centerx > zone.centerx) return up; else return down; } } return farAway; } /// Recherche de du côté de la courbure Skittle::Pos Skittle::bend (const Zone &zone) { /// Border const int border = oconfig_->skittleBorder; /// Initialisation du saut de point int jump = 1; //oconfig_->jumpPointDist/oconfig_->skittleDivJump; if (!jump) jump = 1; /// Initialisation du point de départ const int startY = img_->minWithBorder ((int)(zone.ymin + (zone.ymax - zone.ymin) * 0.25), 0); /// Point supérieur de la quille int upperPoint; /// Recherche du premier et du dernier point int firstPoint = img_->maxWithBorder (zone.xmin + border, img_->width_); int lastPoint = img_->minWithBorder (zone.xmax - border, 0); // int limitPoint = firstPoint > lastPoint ? firstPoint : lastPoint; int limitPoint = climb (zone.centerx, startY, zone.color); // std::cout << " " << firstPoint << " " << lastPoint << " " << limitPoint << std::endl; int above = 0; int below = 0; // std::cout << " "; int last = 0; int trash = 0; /// Parcours des autres points for (int i = firstPoint + jump; i < lastPoint - jump; i+=jump) { upperPoint = climb (i, startY, zone.color); if (last == upperPoint) ++trash; else { // std::cout << upperPoint << " "; if (upperPoint > limitPoint) ++below; else ++above; } last = upperPoint; } // std::cout << std::endl; const int score = (int)((lastPoint - firstPoint - trash) / jump * oconfig_->skittleScoreBendRatio) - 2; // std::cout << "-- " << above << " " << below << " " << score << std::endl; Pos bending = error; if (above > score) bending = up; else if (below > score) bending = down; return bending; } /// Analyse la zone bool Skittle::analyse (Zone &zone, const std::vector &listReflectBand) { // delete f; delete f2; // f = new std::ofstream ("data", std::ios_base::trunc); // f2 = new std::ofstream ("data2", std::ios_base::trunc); // group est partiel ? // if (zone.partial) // quille verticale sur l'image ? zone.ratio = (zone.ymax - zone.ymin)/(double)(zone.xmax - zone.xmin); if (zone.ratio > oconfig_->skittleVerticalRatio) { zone.vertical = true; // debout ou couché ? //bend_ = bend (zone); whereIsReflectBand (zone, listReflectBand); } else { zone.vertical = false; bend_ = down; // calcul de l'orientation pca (zone); } bend (zone); pca (zone); feet_ = whereIsBottom (zone); if (checkArea (zone) == 1) zone.alone = true; else zone.alone = false; // f->close (); // f2->close (); return true; } /// Test si la zone contient les vis double Skittle::isBottom (const Zone &zone, const Pos pos) { int newCenterx, newCentery; // Recherche de la zone à analyser int shiftx = (int)(pcX_*(zone.xmax-zone.centerx)); int shifty = (int)(1.2*pcY_*(zone.ymax-zone.centery)); if (((pos == down) && (pcY_ > 0)) || ((pos == up) && (pcY_ <0))) { shiftx *= -1; shifty *= -1; } newCenterx = (int)(zone.centerx - shiftx); newCentery = (int)(zone.centery - shifty); //std::cout << "--- " << (zone.xmax - zone.xmin) * (zone.ymax - zone.ymin); //std::cout << "%%%% " << newCenterx << " " << newCentery << std::endl; //std::cout << "%%% " << zone.centerx << " " << zone.centery << std::endl; //std::cout << "%% " << zone.xmin << " " << zone.xmax << " " << zone.ymin << " " << zone.ymax << std::endl; // Recherche des extrémités int startx = (int)(newCenterx - (zone.xmax - zone.xmin)*pc2X_); int endx = (int)(newCenterx + (zone.xmax - zone.xmin)*pc2X_); //std::cout << "% " << startx << " " << endx << std::endl; // Test si la zone est valide if ((startx < 0) || (endx > img_->width_)) return -1; // Parcours de la zone int last = -1; int diff = 0; int y, color; for (int x=startx; x < endx; ++x) { y = (int)(newCentery + (x - newCenterx)/2*pc2Y_); if ((y < 0) || (y > img_->height_)) return -1; color = segm_->giveColor (img_->tabData_ + ((y*img_->width_ + x) * 3), true, true); if (color != last) { ++diff; last = color; } } //std::cout << diff << std::endl; return diff; } /// Cherche les vis de la quille Skittle::Pos Skittle::whereIsBottom (const Zone &zone) { double isUp, isDown; isUp = isBottom (zone, up); isDown = isBottom (zone, down); if ((isUp == -1) || (isDown == -1)) return error; if (isUp > isDown) return up; else if (isDown> isUp) return down; return error; } /// Verifie la cohérence de la taille de la quille int Skittle::checkArea (const Zone &zone) { if (zone.ymax < 150) if ((zone.xmax - zone.xmin) * (zone.ymax - zone.ymin) > 14000) return 2; else if (zone.ymax >= 150) if ((zone.xmax - zone.xmin) * (zone.ymax - zone.ymin) > 19000) return 2; return 1; } /// Affiche les infos sur la quille void Skittle::show () const { std::cout << " position : " << (bend_ == up ? "debout" : (bend_ == down ? "couché" : "error")) << "\n"; std::cout << " orientation : " << pcX_ << ", " << pcY_ << "\n"; std::cout << " catadiopre: " << (reflectBand_ == up ? "up" : (reflectBand_ == down ? "down" : (reflectBand_ == farAway ? "farAway" : "error"))) << std::endl; std::cout << " vis : " << (feet_ == up ? "up" : (feet_ == down ? "down" : "error")) << std::endl; }