Tekkotsu Homepage
Demos
Overview
Downloads
Dev. Resources
Reference
Credits

BrickData.cc

Go to the documentation of this file.
00001 //-*-c++-*-
00002 
00003 #include <iostream>
00004 #include <vector>
00005 
00006 #include "BaseData.h"    // superclass
00007 #include "Point.h"       // Point data member
00008 #include "ShapeTypes.h"  // brickDataType
00009 
00010 #include "SketchSpace.h"
00011 #include "Sketch.h"
00012 #include "visops.h"
00013 
00014 #include "ShapeSpace.h"  // required by DATASTUFF_CC
00015 #include "ShapeRoot.h"   // required by DATASTUFF_CC
00016 
00017 #include "BrickData.h"
00018 #include "ShapeBrick.h"
00019 #include "ShapePoint.h"
00020 #include "Region.h"
00021 
00022 using namespace std;
00023 
00024 namespace DualCoding {
00025 
00026 BrickData::BrickData(ShapeSpace& _space,
00027          const EndPoint &_GFL, const EndPoint &_GFR, 
00028          const EndPoint &_GBL, const EndPoint &_GBR, 
00029          const EndPoint &_TFL, const EndPoint &_TFR, 
00030          const EndPoint &_TBL, const EndPoint &_TBR) 
00031   : BaseData(_space,brickDataType), 
00032     GFL(_GFL), GFR(_GFR), GBL(_GBL), GBR(_GBR),
00033     TFL(_TFL), TFR(_TFR), TBL(_TBL), TBR(_TBR),
00034     centroid((GFL + GFR + GBL + GBR + TFL + TFR + TBL + TBR) / 8)
00035 {
00036 }
00037 
00038   
00039 DATASTUFF_CC(BrickData);
00040 
00041 bool BrickData::isMatchFor(const ShapeRoot& other) const {
00042   if (!(isSameTypeAs(other) && isSameColorAs(other)))
00043     return false;
00044   // const Shape<BrickData>& other_point = ShapeRootTypeConst(other,BrickData);
00045   // float dist = the_point.distanceFrom(other_point->getCentroid());
00046   float dist = 0;
00047   return dist < 20; // *** DST hack
00048 }
00049 
00050 void BrickData::mergeWith(const ShapeRoot& other) {
00051   const Shape<BrickData>& other_brick = ShapeRootTypeConst(other,BrickData);
00052   if (other_brick->confidence <= 0)
00053     return;
00054   /*
00055   const int other_conf = other_point->confidence;
00056   confidence += other_conf;
00057   the_point = (the_point*confidence + other_point->getCentroid()*other_conf) / (confidence+other_conf);*/
00058 }
00059 
00060 bool BrickData::updateParams(const ShapeRoot& other, bool) {
00061   const Shape<BrickData>& other_brick = *static_cast<const Shape<BrickData>*>(&other);
00062   //  ++confidence;
00063   GFL = (GFL*(confidence-1) + other_brick->getGFL())/confidence;
00064   GFR = (GFR*(confidence-1) + other_brick->getGFR())/confidence;
00065   GBL = (GBL*(confidence-1) + other_brick->getGBL())/confidence;
00066   GBR = (GBR*(confidence-1) + other_brick->getGBR())/confidence;
00067   TFL = (TFL*(confidence-1) + other_brick->getTFL())/confidence;
00068   TFR = (TFR*(confidence-1) + other_brick->getTFR())/confidence;
00069   TBL = (TBL*(confidence-1) + other_brick->getTBL())/confidence;
00070   TBR = (TBR*(confidence-1) + other_brick->getTBR())/confidence;
00071   deleteRendering();
00072   return true;
00073 }
00074 
00075 //! Print information about this shape. (Virtual in BaseData.)
00076 void
00077 BrickData::printParams() const {
00078   cout << "Type = " << getTypeName();
00079   cout << "Shape ID = " << getId() << endl;
00080   cout << "Parent ID = " << getParentId() << endl;
00081   
00082   // Print critical points.
00083   cout << endl;
00084   //cout << "center{" << getCentroid().coordX() << ", " << getCentroid().coordY() << "}" << endl;
00085   /* Didn't work initially... not sure why, didn't try too hard.
00086     cout<<"GFL: "; GFL.printData(); cout<<endl;
00087   cout<<"GFR: "; GFR.printData(); cout<<endl;
00088   cout<<"GBL: "; GBL.printData(); cout<<endl;
00089   cout<<"GBR: "; GBR.printData(); cout<<endl;
00090   cout<<"TFL: "; TFL.printData(); cout<<endl;
00091   cout<<"TFR: "; TFR.printData(); cout<<endl;
00092   cout<<"TBL: "; TBL.printData(); cout<<endl;
00093   cout<<"TBR: "; TBR.printData(); cout<<endl;*/
00094   printf("color = %d %d %d\n",getColor().red,getColor().green,getColor().blue);
00095   cout << "viewable = " << isViewable() << endl;
00096 }
00097 
00098 
00099 //! Transformations. (Virtual in BaseData.)
00100 void BrickData::applyTransform(const NEWMAT::Matrix& Tmat, const ReferenceFrameType_t newref) {
00101   GFL.applyTransform(Tmat,newref);
00102   GFR.applyTransform(Tmat,newref);
00103   GBL.applyTransform(Tmat,newref);
00104   GBR.applyTransform(Tmat,newref);
00105   TFL.applyTransform(Tmat,newref);
00106   TFR.applyTransform(Tmat,newref);
00107   TBL.applyTransform(Tmat,newref);
00108   TBR.applyTransform(Tmat,newref);
00109 }
00110 
00111 void BrickData::projectToGround(const NEWMAT::Matrix& camToBase,
00112         const NEWMAT::ColumnVector& groundplane) {
00113   GFL.projectToGround(camToBase,groundplane);
00114   GFR.projectToGround(camToBase,groundplane);
00115   GBL.projectToGround(camToBase,groundplane);
00116   GBR.projectToGround(camToBase,groundplane);
00117 
00118   // Compute height at every corner except BL (because we didn't actually observe that corner)
00119   float FLHeight, FRHeight, BRHeight;
00120   TFL.projectToGround(camToBase,groundplane);
00121   TFR.projectToGround(camToBase,groundplane);
00122   TBR.projectToGround(camToBase,groundplane);
00123   FLHeight = TFL.getHeightAbovePoint(GFL, groundplane);
00124   FRHeight = TFR.getHeightAbovePoint(GFR, groundplane);
00125   BRHeight = TBR.getHeightAbovePoint(GBR, groundplane);
00126   float brickHeight = (FLHeight + FRHeight+ BRHeight) / 3.0;
00127   TFL.setCoords(GFL.coordX(), GFL.coordY(), GFL.coordZ() + FLHeight);
00128   TFR.setCoords(GFR.coordX(), GFR.coordY(), GFR.coordZ() + FRHeight);
00129   TBL.setCoords(GBL.coordX(), GBL.coordY(), GBL.coordZ() + brickHeight);
00130   TBR.setCoords(GBR.coordX(), GBR.coordY(), GBR.coordZ() + BRHeight);
00131   centroid = (GFL + GFR + GBL + GBR)/4;
00132   centroid.setCoords(centroid.coordX(), centroid.coordY(), brickHeight/2);
00133   centroid.setRefFrameType(egocentric);
00134   std::cout << "New centroid: " << centroid << endl;
00135 
00136 }
00137 
00138 // ==================================================
00139 // BEGIN SKETCH MANIPULATION AND BRICK EXTRACTION CODE
00140 // ==================================================
00141 
00142 
00143 //! Brick extraction.
00144 
00145 
00146 //! Render into a sketch space and return reference. (Private.)
00147 Sketch<bool>* BrickData::render() const {
00148   SketchSpace &renderspace = space->getDualSpace();
00149   //int const width = renderspace.getWidth();
00150   //int const height = renderspace.getHeight();
00151   //float x1,y1,x2,y2;
00152   Sketch<bool>* draw_result = 
00153     new Sketch<bool>(renderspace, "render("+getName()+")");
00154   (*draw_result)->setParentId(getViewableId());
00155   (*draw_result)->setColor(getColor());
00156   *draw_result = 0;
00157   LineData GF(*space, GFL, GFR);
00158   *draw_result = *draw_result & GF.getRendering();
00159   LineData GL(*space, GFL, GBL);
00160   *draw_result = *draw_result & GL.getRendering();
00161   LineData GB(*space, GBL, GBR);
00162   *draw_result = *draw_result & GB.getRendering();
00163   LineData GR(*space, GBR, GFR);
00164   *draw_result = *draw_result & GR.getRendering();
00165   
00166   return draw_result;
00167 }
00168 
00169 
00170 
00171 
00172   // Old version of brick extraction
00173   // Final version resides in extractBrick
00174 std::vector<Shape<BrickData> > BrickData::findBricks(ShapeSpace& ShS, std::vector<Shape<LineData> > lines)
00175 {
00176   const float lengthConst = .3;
00177   std::vector<Shape<LineData> > resultLines;
00178   std::vector<Shape<BrickData> > resultBricks;
00179   float longLength, shortLength;
00180 
00181   if (lines.size() < 3)
00182     {
00183       return resultBricks;
00184     }
00185 
00186   lines = stable_sort(lines, not2(LineData::LengthLessThan()));
00187 
00188   DO_SHAPEVEC(lines, LineData, l1, {
00189     DO_SHAPENEXT(lines, LineData, l1, l2, {
00190     if (l1->getLength() > l2->getLength()){
00191       longLength = l1->getLength();
00192       shortLength = l2->getLength();
00193     }
00194     else {
00195       longLength = l2->getLength();
00196       shortLength = l1->getLength();
00197     }
00198     if (LineData::ParallelTest()(l1,l2) && !LineData::ColinearTest()(l1,l2) && 
00199   (shortLength / longLength > lengthConst)) {
00200       DO_SHAPENEXT(lines, LineData, l2, l3, {
00201   if (LineData::ParallelTest()(l1,l3) && 
00202       !LineData::ColinearTest()(l1,l3) &&
00203       LineData::ParallelTest()(l2,l3) && 
00204       !LineData::ColinearTest()(l2,l3) &&
00205       (l3->getLength() / longLength > lengthConst) && 
00206       (shortLength / l3->getLength() > lengthConst)) {
00207     NEW_SHAPE_N(l1_2, LineData,  new LineData(ShS, l1->leftPt(), l1->rightPt()));
00208     NEW_SHAPE_N(l2_2, LineData, new LineData(ShS, l2->leftPt(), l2->rightPt()));
00209     NEW_SHAPE_N(l3_2, LineData, new LineData(ShS, l3->leftPt(), l3->rightPt()));
00210     l1_2->setParentId(l1->getViewableId());
00211     l2_2->setParentId(l1->getViewableId());
00212     l3_2->setParentId(l1->getViewableId());
00213     resultLines.push_back(l1_2);
00214     resultLines.push_back(l2_2);
00215     resultLines.push_back(l3_2);
00216   }
00217       });
00218     }
00219     });
00220   });
00221         
00222   if (resultLines.size() < 3) {
00223       return resultBricks;
00224   }
00225   
00226   for (unsigned int i=0; i<resultLines.size(); i+=3)
00227     {
00228       const Shape<LineData>& final1 = ShapeRootTypeConst(resultLines[i+0],LineData);
00229       const Shape<LineData>& final2 = ShapeRootTypeConst(resultLines[i+1],LineData);
00230       const Shape<LineData>& final3 = ShapeRootTypeConst(resultLines[i+2],LineData);
00231       /*cout<<"3 lines:"<<endl;
00232   final1->printEnds();
00233   final2->printEnds();
00234   final3->printEnds();*/
00235       
00236       std::vector<Shape<LineData> > threeLines;
00237       
00238       if (final1->bottomPt().isBelow(final2->bottomPt(),camcentric))
00239   {
00240     if (final1->bottomPt().isBelow(final3->bottomPt(),camcentric))
00241       {
00242         threeLines.push_back(final1);
00243         if (final2->bottomPt().isBelow(final3->bottomPt(),camcentric))
00244     {
00245       threeLines.push_back(final2);
00246       threeLines.push_back(final3);
00247     }
00248         else
00249     {
00250       threeLines.push_back(final3);
00251       threeLines.push_back(final3);
00252     }
00253       }
00254     else
00255       {
00256         threeLines.push_back(final3);
00257         threeLines.push_back(final1);
00258         threeLines.push_back(final2);
00259       }
00260   }
00261       else
00262   {
00263     if (final2->bottomPt().isBelow(final3->bottomPt(),camcentric))
00264       {
00265         threeLines.push_back(final2);
00266         if (final1->bottomPt().isBelow(final3->bottomPt(),camcentric))
00267     {
00268       threeLines.push_back(final1);
00269       threeLines.push_back(final3);
00270     }
00271         else
00272     {
00273       threeLines.push_back(final3);
00274       threeLines.push_back(final1);
00275     }
00276       }
00277     else
00278       {
00279         threeLines.push_back(final3);
00280         threeLines.push_back(final2);
00281         threeLines.push_back(final1);
00282       }
00283   }
00284       const Shape<LineData> &bottom = ShapeRootTypeConst(threeLines[0], LineData);
00285       const Shape<LineData> &mid = ShapeRootTypeConst(threeLines[1], LineData);
00286       const Shape<LineData> &top = ShapeRootTypeConst(threeLines[2], LineData);
00287       
00288       /*cout<<"Sorted Lines: "<<endl;
00289   bottom->printEnds();
00290   mid->printEnds();
00291   top->printEnds();*/
00292       
00293       Point gbl(top->leftPt()+(bottom->leftPt() - mid->leftPt()));
00294       Point gbr(top->rightPt()+(bottom->rightPt() - mid->rightPt()));
00295       Shape<BrickData> newBrick (ShS, (Point&)(bottom->leftPt()), (Point&)bottom->rightPt(), 
00296          gbl, gbr, 
00297          (Point&)mid->leftPt(), (Point&)mid->rightPt(), 
00298          (Point&)top->leftPt(), (Point&)top->rightPt());
00299       
00300       newBrick->setParentId(final1->getViewableId());
00301 
00302       NEW_SHAPE(brickl1, LineData, Shape<LineData>(bottom.getData()));
00303       NEW_SHAPE(brickl2, LineData, Shape<LineData>(mid.getData()));
00304       NEW_SHAPE(brickl3, LineData, Shape<LineData>(top.getData()));
00305 
00306       brickl1->setParentId(newBrick->getViewableId());
00307       brickl2->setParentId(newBrick->getViewableId());
00308       brickl3->setParentId(newBrick->getViewableId());
00309       
00310       resultBricks.push_back(newBrick);
00311     }
00312   
00313   return resultBricks;
00314 }
00315 
00316 
00317 // Find bricks from 3 vectors of candidate blobs
00318 // Each vector is blobs of a different face color
00319 //
00320 // Also an old version. Final version is in extractBrick
00321 std::vector<Shape<BrickData> > BrickData::findBricksFromBlobs(ShapeSpace& ShS, 
00322                     std::vector<Shape<BlobData> > blobs1,
00323                     std::vector<Shape<BlobData> > blobs2,
00324                     std::vector<Shape<BlobData> > blobs3) 
00325 {
00326 
00327   const float distanceThresh = 10;
00328 
00329   unsigned int i, i1, i2;
00330   Shape<BlobData> blob1, blob2, blob3;
00331 
00332   std::vector<Shape<BrickData> > resultBricks;
00333 
00334   Point GFL, GFR, GBL, GBR, TFL, TFR, TBL, TBR;
00335   Point c12,c13,c23;
00336 
00337   std::vector<std::vector<Point> > corners1;
00338   std::vector<std::vector<Point> > corners2;
00339   std::vector<std::vector<Point> > corners3;
00340   std::vector<bool> used1;
00341   std::vector<bool> used2;
00342   std::vector<bool> used3;
00343   for (i=0; i<blobs1.size(); i++) { 
00344     used1.push_back(false); 
00345     corners1.push_back(blobs1[i]->findCornersDiagonal()); 
00346   }
00347   for (i=0; i<blobs2.size(); i++) { 
00348     used2.push_back(false); 
00349     corners2.push_back(blobs2[i]->findCornersDiagonal()); 
00350   }
00351   for (i=0; i<blobs3.size(); i++) { 
00352     used3.push_back(false); 
00353     corners3.push_back(blobs3[i]->findCornersDiagonal()); 
00354   }
00355 
00356   // Look for bricks with a good common edge between each pair of viable brick face colors
00357   int used;
00358   for (i1=0; i1<blobs1.size(); i1++){
00359     for (i2=0; i2<blobs2.size();i2++){
00360       if (!used1[i1] && !used2[i2]) {
00361   used = addBrickWithTwoSides(ShS, corners1[i1], corners2[i2], corners3, 
00362             resultBricks, distanceThresh);
00363   if (used>=0) {
00364     used1[i1] = true;
00365     used2[i2] = true;
00366     used3[used] = true;
00367     resultBricks[resultBricks.size()-1]->setParentId(blobs1[i1]->getViewableId());
00368   }
00369       }
00370     }
00371   }
00372 
00373   for (i1=0; i1<blobs1.size(); i1++){
00374     for (i2=0; i2<blobs3.size();i2++){
00375       if (!used1[i1] && !used3[i2]) {
00376   used = addBrickWithTwoSides(ShS, corners1[i1], corners3[i2], corners2, 
00377             resultBricks, distanceThresh);
00378   if (used>=0) {
00379     used1[i1] = true;
00380     used3[i2] = true;
00381     used2[used] = true;
00382     resultBricks[resultBricks.size()-1]->setParentId(blobs1[i1]->getViewableId());
00383   }
00384       }
00385     }
00386   }
00387 
00388   for (i1=0; i1<blobs3.size(); i1++){
00389     for (i2=0; i2<blobs2.size();i2++){
00390       if (!used3[i1] && !used2[i2]) {
00391   used = addBrickWithTwoSides(ShS, corners3[i1], corners2[i2], corners1, 
00392             resultBricks, distanceThresh);
00393   if (used>=0) {
00394     used3[i1] = true;
00395     used2[i2] = true;
00396     used1[used] = true;
00397     resultBricks[resultBricks.size()-1]->setParentId(blobs3[i1]->getViewableId());
00398   }
00399       }
00400     }
00401   }
00402 
00403 
00404   return resultBricks;
00405 }
00406 
00407 
00408 // Subroutine for blob brick detection
00409 // If the two sides match, finds the third side that matches
00410 // Extrapolates all the points and adds the brick to the vector
00411 // returns the index of the third face, or -1 if no match is found
00412 //
00413 // subroutine for an old version
00414 int BrickData::addBrickWithTwoSides(ShapeSpace &ShS,
00415             std::vector<Point>& corners1, 
00416             std::vector<Point>& corners2, 
00417             std::vector<std::vector<Point> >& blobs3, 
00418             std::vector<Shape<BrickData> >& result, 
00419             float distanceThresh)
00420 {
00421   unsigned int blobi;
00422   int i1=-1,j1=-1, i2=-1, j2=-1;
00423   for (int i=0; i<4 && i2<0; i++) {
00424     for (int j=0; j<4 && j2<0; j++) {
00425       if (corners1[i].distanceFrom(corners2[j]) < distanceThresh) {
00426   if (corners1[(i+1)%4].distanceFrom(corners2[(j+3)%4]) < distanceThresh) {
00427     i1 = i;
00428     i2 = (i+1)%4;
00429     j1 = (j+3)%4;
00430     j2 = j;
00431   }
00432   else if (corners1[(i+3)%4].distanceFrom(corners2[(j+1)%4]) < distanceThresh) {
00433     i1 = (i+3)%4;
00434     i2 = i;
00435     j1 = j;
00436     j2 = (j+1)%4;
00437   }
00438   if (i2>=0) {
00439     // Two matching corners have been found: (i1,j2), (i2, j1)
00440     // Look for the third side
00441     bool found = false;
00442     Point center, mid1, mid2, mid3, c1, c2, c3;
00443     for (blobi=0; blobi<blobs3.size() && !found; blobi++) {
00444       for(int k=0; k<4 && !found; k++) {
00445         if (((blobs3[blobi][k].distanceFrom(corners1[i1]) < distanceThresh) +
00446        (blobs3[blobi][(k+1)%4].distanceFrom(corners1[(i1+3)%4]) < distanceThresh) +
00447        (blobs3[blobi][(k+3)%4].distanceFrom(corners2[(j2+1)%4]) < distanceThresh)) > 1) {
00448     // (i1,j2, k) is the center
00449     found = true;
00450     mid1 = (corners1[i2]+corners2[j1])/2;
00451     if (blobs3[blobi][k].distanceFrom(corners1[i1]) < distanceThresh) 
00452       center = (corners1[i1]+corners2[j2]+blobs3[blobi][k])/3;
00453     else
00454       center = (corners1[i1]+corners2[j2])/2;
00455     if (blobs3[blobi][(k+1)%4].distanceFrom(corners1[(i1+3)%4]) < distanceThresh)
00456       mid2 = (blobs3[blobi][(k+1)%4] + corners1[(i1+3)%4])/2;
00457     else
00458       mid2 = corners1[(i1+3)%4];
00459     if (blobs3[blobi][(k+3)%4].distanceFrom(corners2[(j2+1)%4]) < distanceThresh)
00460       mid3 = (blobs3[blobi][(k+3)%4] + corners2[(j2+1)%4])/2;
00461     else
00462       mid3 = corners2[(j2+1)%4];
00463     c1 = corners1[(i1+2)%4];
00464     c2 = blobs3[blobi][(k+2)%4];
00465     c3 = corners2[(j2+2)%4];
00466         }
00467         else if (((blobs3[blobi][k].distanceFrom(corners1[i2]) < distanceThresh) +
00468        (blobs3[blobi][(k+1)%4].distanceFrom(corners2[(j1+3)%4]) < distanceThresh) +
00469        (blobs3[blobi][(k+3)%4].distanceFrom(corners1[(i2+1)%4]) < distanceThresh)) > 1) {
00470     // (i2, j1, k) is the center
00471     found = true;
00472     mid1 = (corners1[i1]+corners2[j2])/2;
00473     if (blobs3[blobi][k].distanceFrom(corners1[i2]) < distanceThresh) 
00474       center = (corners1[i2]+corners2[j1]+blobs3[blobi][k])/3;
00475     else
00476       center = (corners1[i2]+corners2[j1])/2;
00477     if (blobs3[blobi][(k+1)%4].distanceFrom(corners2[(j1+3)%4]) < distanceThresh)
00478       mid2 = (blobs3[blobi][(k+1)%4] + corners2[(j1+3)%4])/2;
00479     else
00480       mid2 = corners2[(j1+3)%4];
00481     if (blobs3[blobi][(k+3)%4].distanceFrom(corners1[(i2+1)%4]) < distanceThresh)
00482       mid3 = (blobs3[blobi][(k+3)%4] + corners1[(i2+1)%4])/2;
00483     else
00484       mid3 = corners1[(i2+1)%4];
00485     c1 = corners2[(j1+2)%4];
00486     c2 = blobs3[blobi][(k+2)%4];
00487     c3 = corners1[(i2+2)%4];
00488         }
00489       }
00490 
00491     }
00492 
00493     if (found) {
00494       // Found a brick, figure out where the top / left / etc sides are
00495 
00496       // Debug shapes
00497       NEW_SHAPE(centerp, PointData, new PointData(ShS, center));
00498       NEW_SHAPE(mid1p, PointData, new PointData(ShS, mid1));
00499       NEW_SHAPE(mid2p, PointData, new PointData(ShS, mid2));
00500       NEW_SHAPE(mid3p, PointData, new PointData(ShS, mid3));
00501       NEW_SHAPE(c1p, PointData, new PointData(ShS, c1));
00502       NEW_SHAPE(c2p, PointData, new PointData(ShS, c2));
00503       NEW_SHAPE(c3p, PointData, new PointData(ShS, c3));
00504       centerp->setColor(rgb(150,0,255));
00505       mid1p->setColor(rgb(150,0,255));
00506       mid2p->setColor(rgb(150,0,255));
00507       mid3p->setColor(rgb(150,0,255));
00508       c1p->setColor(rgb(150,0,255));
00509       c2p->setColor(rgb(150,0,255));
00510       c3p->setColor(rgb(150,0,255));
00511       
00512       Point GFL, GFR, GBL, GBR, TFL, TFR, TBL, TBR;
00513       
00514       TFR = center;
00515       if (mid1.isBelow(center, camcentric) && mid1.isBelow(mid2, camcentric) && mid1.isBelow(mid3, camcentric)) {
00516         GFR = mid1;
00517         GBR = c1;
00518         if (mid2.isRightOf(mid3, camcentric)) {
00519     TBR = mid2;
00520     TBL = c2;
00521     TFL = mid3;
00522     GFL = c3;
00523         }
00524         else {
00525     TBR = mid3;
00526     TBL = c3;
00527     TFL = mid2;
00528     GFL = c2;
00529         }
00530       }
00531       else if (mid2.isBelow(center, camcentric) && mid2.isBelow(mid1, camcentric) && mid2.isBelow(mid3, camcentric)) {
00532         GFR = mid2;
00533         GBR = c2;
00534         if (mid1.isRightOf(mid3, camcentric)) {
00535     TBR = mid1;
00536     TBL = c1;
00537     TFL = mid3;
00538     GFL = c3;
00539         }
00540         else {
00541     TBR = mid3;
00542     TBL = c3;
00543     TFL = mid1;
00544     GFL = c1;
00545         }
00546       }
00547       else {
00548         GFR = mid3;
00549         GBR = c3;
00550         if (mid2.isRightOf(mid1, camcentric)) {
00551     TBR = mid2;
00552     TBL = c2;
00553     TFL = mid1;
00554     GFL = c1;
00555         }
00556         else {
00557     TBR = mid1;
00558     TBL = c1;
00559     TFL = mid2;
00560     GFL = c2;
00561         }
00562       }
00563       
00564       GBL = GFL + (TBL - TFL);
00565 
00566       Shape<BrickData> newBrick(ShS, GFL, GFR, GBL, GBR, TFL, TFR, TBL, TBR);
00567       result.push_back(newBrick);
00568       centerp->setParentId(newBrick->getViewableId());
00569       mid1p->setParentId(newBrick->getViewableId());
00570       mid2p->setParentId(newBrick->getViewableId());
00571       mid3p->setParentId(newBrick->getViewableId());
00572       c1p->setParentId(newBrick->getViewableId());
00573       c2p->setParentId(newBrick->getViewableId());
00574       c3p->setParentId(newBrick->getViewableId());
00575       return blobi;
00576     }
00577     else {
00578       // What do we do if we get two good faces and no third?
00579     }
00580     
00581   }
00582       }
00583     }
00584   }
00585 
00586   return -1;
00587 
00588 }
00589 
00590 
00591 
00592 
00593 
00594 /* Unified brick extraction starting from 2 or 3 blobs, each of a different color 
00595  *
00596  *************    Final Version   ***************
00597  *
00598  * Checks the relative locations of the blobs (to ensure they are adjacent enough to 
00599  * be part of the same brick)
00600  *
00601  * Computes the interior edge lines from the regions close to two faces.
00602  *
00603  * Computes the corners of each face individually. 
00604  * This step is handled in blobData::findCorners
00605  *
00606  * Combine all the corner guesses together and extrapolate missing corners.
00607  *
00608  *
00609  * Some helper functions can be found in BrickOps.h 
00610  */
00611 Shape<BrickData> BrickData::extractBrick(ShapeSpace& space, vector<Shape<BlobData> > &blobs)
00612 {
00613   unsigned int nblobs = blobs.size();
00614   std::vector<Point> centroids;
00615 
00616   ShapeRoot invalid;
00617   if (nblobs > 3 || nblobs < 2) {
00618     return ShapeRootType(invalid,BrickData);
00619   }
00620 
00621   for (unsigned int i=0; i<nblobs; i++) {
00622     centroids.push_back(blobs[i]->getCentroid());
00623   }
00624 
00625   // Check inter-blob distance
00626   // If any pair of blobs are too far apart, this set is invalid
00627   for (unsigned int i=0; i<nblobs; i++) {
00628     for (unsigned int j=i+1; j<nblobs; j++) {
00629       if (centroids[i].distanceFrom(centroids[j]) >= 
00630     blobs[i]->topLeft.distanceFrom(centroids[i]) + 
00631     blobs[j]->topLeft.distanceFrom(centroids[j])){
00632   return ShapeRootType(invalid,BrickData);  
00633       }
00634     }
00635   }
00636 
00637 
00638   // arbitrary face assignment, aligned as the brick is in camera space
00639   // this will probably break down if the dog tilts its head
00640   // 
00641   // if we only have two faces, we're assuming only the top and "left" faces are visible
00642   // always assume a top face is visible, becuase any shape without a top face is much more likely
00643   // to be a pyramid than a tall brick
00644   int top=-1, left=-1, right=-1;
00645   
00646   if (centroids[0].isAbove(centroids[1], camcentric)) {
00647     top = 0;
00648   }
00649   else {
00650     top = 1;
00651   }
00652   if (nblobs > 2 && centroids[2].isAbove(centroids[top], camcentric)) {
00653     top = 2;
00654   }
00655 
00656   if ((top != 0 && centroids[0].isLeftOf(centroids[1], camcentric)) || top == 1) {
00657     left = 0;
00658   }
00659   else {
00660     left = 1;
00661   }
00662   if (nblobs>2 && top != 2 && centroids[2].isLeftOf(centroids[left], camcentric)) {
00663     left = 2;
00664   }
00665 
00666   if (nblobs > 2) {
00667     if (top != 0 && left != 0) {
00668       right = 0;
00669     }
00670     else if (top != 1 && left != 1) {
00671       right = 1;
00672     }
00673     else {
00674       right = 2;
00675     }
00676   }
00677 
00678   if (top == left || top == right || left == right){
00679     std::cout<<"ERROR: Brick side misclassification!"<<std::endl;
00680     return ShapeRootType(invalid,BrickData);  
00681   }
00682 
00683 
00684   // Compute two-blob boundary regions
00685   // This is used to find the interior edges of the brick
00686   // It is also used as a second check for inter-brick distance
00687   const int INTERSECT_DIST = 5, MIN_REQUIRED_DIST = 2;
00688   std::vector<Sketch<bool> > boundaries;
00689   NEW_SKETCH_N(topEdist, uchar, visops::edist(blobs[top]->getRendering()));
00690   NEW_SKETCH_N(leftEdist, uchar, visops::edist(blobs[left]->getRendering()));
00691   NEW_SKETCH(tlsmall, bool, topEdist<MIN_REQUIRED_DIST & leftEdist<MIN_REQUIRED_DIST);
00692   if (!tlsmall->max()){
00693     return ShapeRootType(invalid,BrickData);  
00694   }
00695   NEW_SKETCH(tl, bool, topEdist<INTERSECT_DIST & leftEdist<INTERSECT_DIST);
00696   rgb green_rgb(0,0,255);
00697   tl->setColor(green_rgb);
00698   boundaries.push_back(tl);
00699 
00700   if (nblobs>2) {
00701     NEW_SKETCH_N(rightEdist, uchar, visops::edist(blobs[right]->getRendering()));
00702     NEW_SKETCH(trsmall, bool, topEdist<MIN_REQUIRED_DIST & rightEdist<MIN_REQUIRED_DIST);
00703     if (!trsmall->max()){
00704       return ShapeRootType(invalid,BrickData);
00705     } 
00706     NEW_SKETCH(tr, bool, topEdist<INTERSECT_DIST & rightEdist<INTERSECT_DIST);
00707     tr->setColor(green_rgb);
00708     boundaries.push_back(tr);
00709 
00710     NEW_SKETCH(lrsmall, bool, leftEdist<MIN_REQUIRED_DIST & rightEdist<MIN_REQUIRED_DIST);
00711     if (!lrsmall->max()){
00712       return ShapeRootType(invalid,BrickData);
00713     } 
00714     NEW_SKETCH(lr, bool, leftEdist<INTERSECT_DIST & rightEdist<INTERSECT_DIST);
00715     lr->setColor(green_rgb);
00716     boundaries.push_back(lr);
00717   }
00718 
00719 
00720 
00721   // Begin gathering evidence for each point
00722   // This should probably be augmented by an extra vector of confidences
00723   std::vector<std::vector<Point> > points(8);
00724 
00725   // Arbitrary corner / face assignemt (in cam space)
00726   //
00727   //      5------6
00728   //     /   T  /|
00729   //    /(4)   / 7
00730   //   1------2 / <-- R
00731   //   |  L   |/
00732   //   0------3
00733   //    
00734 
00735 
00736   // Construct the interior lines
00737   // Add their guesses to the appropriate points on the brick
00738   NEW_SHAPE(tlline, LineData, LineData::extractLine(boundaries[0]));
00739   Shape<LineData> trline, lrline;
00740 
00741   if (right!=-1) {
00742     trline = LineData::extractLine(boundaries[1]);
00743     lrline = LineData::extractLine(boundaries[2]);
00744 
00745     // shorten interior lines based on their intersections
00746     bool on1=false, on2=false;
00747     if (tlline.isValid() && trline.isValid()) {
00748       Point isectPt = tlline->intersectionWithLine(trline, on1, on2);
00749       if (on1) {
00750   tlline->rightPt().setCoords(isectPt);
00751       }
00752       if (on2) {
00753   trline->leftPt().setCoords(isectPt);
00754       }
00755     }
00756 
00757     if (tlline.isValid() && lrline.isValid()) {
00758       Point isectPt = tlline->intersectionWithLine(lrline, on1, on2);
00759       if (on1) {
00760   tlline->rightPt().setCoords(isectPt);
00761       }
00762       if (on2) {
00763   lrline->topPt().setCoords(isectPt);
00764       }
00765     }
00766 
00767     if (trline.isValid() && lrline.isValid()) {
00768       Point isectPt = trline->intersectionWithLine(lrline, on1, on2);
00769       if (on1) {
00770   trline->leftPt().setCoords(isectPt);
00771       }
00772       if (on2) {
00773   lrline->topPt().setCoords(isectPt);
00774       }
00775     }
00776 
00777 
00778     if (trline.isValid()) {
00779       points[2].push_back(trline->leftPt());
00780       points[6].push_back(trline->rightPt());
00781     }
00782 
00783     if (lrline.isValid()) {
00784       points[2].push_back(lrline->topPt());
00785       points[3].push_back(lrline->bottomPt());
00786     }
00787   }
00788 
00789   if (tlline.isValid()) {
00790     points[1].push_back(tlline->leftPt());
00791     points[2].push_back(tlline->rightPt());
00792   }
00793 
00794 
00795 
00796   // Extract the corners of the blob using the derivative approach
00797   //
00798   // corners are coming out of findCorners() in counter-clock-wise order around the blob
00799   //  with unknown starting position
00800   //
00801   // the findCorners function in BlobData is the other important part of the algorithm
00802   // Several different methods of corner extraction can be connected there. 
00803   std::vector<Point> candidates;
00804 
00805   // top face
00806 
00807   // Compute an orthogonal bounding box from the top-left line
00808   if (tlline.isValid()) {
00809     if (trline.isValid() && trline->getLength() > tlline->getLength()) {
00810       candidates = findOrthogonalBoundingBox(space, blobs[top], centroids[top], trline);
00811     }
00812     else {
00813       candidates = findOrthogonalBoundingBox(space, blobs[top], centroids[top], tlline);      
00814     }
00815   }
00816   else if (trline.isValid()) {
00817     candidates = findOrthogonalBoundingBox(space, blobs[top], centroids[top], trline);
00818   }
00819   
00820   float cornerValue; 
00821 
00822   std::vector<Point> tcorners = blobs[top]->findCorners(4, candidates, cornerValue);
00823 
00824   if (tcorners.size() == 4) {
00825     
00826     // Sort out which corner is which
00827     // if there's a clear left-most corner, its corner 1
00828     // if two corners are close in left-ness, then they should be 1 and 5
00829     unsigned int leftCorner = 0;
00830     for (unsigned int i=1; i<4; i++){
00831       if (tcorners[i].isLeftOf(tcorners[leftCorner], camcentric)) {
00832   leftCorner = i;
00833       }
00834     }
00835     int closeCorner = -1;
00836     float mostLeft = tcorners[leftCorner].coordX();
00837     const int MAX_CLOSE_DIST = 5;
00838     for (unsigned int i=0; i<4; i++) {
00839       if (i != leftCorner && tcorners[i].coordX() - mostLeft < MAX_CLOSE_DIST) {
00840   closeCorner = i;
00841       }
00842     }
00843     
00844     if (closeCorner != -1) {
00845       // If 5 was actually left-most, switch leftCorner to 1
00846       if ((unsigned int)closeCorner == (leftCorner+1)%4) {
00847   leftCorner = closeCorner;
00848       }
00849       else if ((unsigned int)closeCorner != (leftCorner+3)%4) {
00850   std::cout<<"WARNING, opposite corners are close together!"<<std::endl;
00851       }
00852     }
00853     NEW_SHAPE(top1, PointData, PointData(space, tcorners[leftCorner]));
00854     points[1].push_back(tcorners[leftCorner]);
00855     points[2].push_back(tcorners[(leftCorner+1)%4]);
00856     points[6].push_back(tcorners[(leftCorner+2)%4]);
00857     points[5].push_back(tcorners[(leftCorner+3)%4]);
00858 
00859   }
00860   else {
00861     // Some versions of findCorners don't always return 4 corners. 
00862     // How to handle these cases is ambiguous at best. 
00863   }
00864 
00865   // repeat with other faces
00866 
00867   // left face
00868   candidates.clear();
00869   if (tlline.isValid()) {
00870     if (lrline.isValid() && lrline->getLength() > tlline->getLength()) {
00871       candidates = findOrthogonalBoundingBox(space, blobs[left], centroids[left], lrline);
00872     }
00873     else {
00874       candidates = findOrthogonalBoundingBox(space, blobs[left], centroids[left], tlline);      
00875     }
00876   }
00877   else if (trline.isValid()) {
00878     candidates = findOrthogonalBoundingBox(space, blobs[left], centroids[left], lrline);
00879   }
00880 
00881   for (unsigned int i=0; i<candidates.size(); i++) {
00882     NEW_SHAPE(candidate, PointData, PointData(space, candidates[i]));
00883     candidate->setParentId(blobs[left]->getViewableId());
00884   }
00885   std::vector<Point> lcorners = blobs[left]->findCorners(4, candidates, cornerValue);
00886   if (lcorners.size() == 4) {
00887     
00888     // if there's a clear right-most corner, its corner 3
00889     // if two corners are close in right-ness, then they should 3 and 2
00890     unsigned int rightCorner = 0;
00891     for (unsigned int i=1; i<4; i++){
00892       if (lcorners[i].isRightOf(lcorners[rightCorner], camcentric)) {
00893   rightCorner = i;
00894       }
00895     }
00896     int closeCorner = -1;
00897     float mostRight = lcorners[rightCorner].coordX();
00898     const int MAX_CLOSE_DIST = 5;
00899     for (unsigned int i=0; i<4; i++) {
00900       if (i != rightCorner && mostRight - lcorners[i].coordX() < MAX_CLOSE_DIST) {
00901   closeCorner = i;
00902       }
00903     }
00904     
00905     if (closeCorner != -1) {
00906       // If 2 was actually right-most, switch rightCorner to 3
00907       if ((unsigned int)closeCorner == (rightCorner+3)%4) {
00908   rightCorner = closeCorner;
00909       }
00910       else if ((unsigned int)closeCorner != (rightCorner+1)%4) {
00911   std::cout<<"WARNING, opposite corners are close together!"<<std::endl;
00912       }
00913     }
00914     NEW_SHAPE(left2, PointData, PointData(space, lcorners[rightCorner]));
00915     points[3].push_back(lcorners[rightCorner]);
00916     points[2].push_back(lcorners[(rightCorner+1)%4]);
00917     points[1].push_back(lcorners[(rightCorner+2)%4]);
00918     points[0].push_back(lcorners[(rightCorner+3)%4]);
00919 
00920   }
00921   else {
00922 
00923   }
00924 
00925   // right face
00926   if (right != -1) {
00927 
00928     candidates.clear();
00929     if (trline.isValid()) {
00930       if (lrline.isValid() && lrline->getLength() > trline->getLength()) {
00931   candidates = findOrthogonalBoundingBox(space, blobs[right], centroids[right], lrline);
00932       }
00933       else {
00934   candidates = findOrthogonalBoundingBox(space, blobs[right], centroids[right], trline);      
00935       }
00936     }
00937     else if (lrline.isValid()) {
00938       candidates = findOrthogonalBoundingBox(space, blobs[right], centroids[right], lrline);
00939     }
00940 
00941     std::vector<Point> rcorners = blobs[right]->findCorners(4, candidates, cornerValue);
00942     if (rcorners.size() == 4) {
00943     
00944       // if there's a clear bottom-most corner, its corner 3
00945       // if two corners are close in bottom-ness, then they should 3 and 7
00946       unsigned int bottomCorner = 0;
00947       for (unsigned int i=1; i<4; i++){
00948   if (rcorners[i].isBelow(rcorners[bottomCorner], camcentric)) {
00949     bottomCorner = i;
00950   }
00951       }
00952       int closeCorner = -1;
00953       float mostBottom = rcorners[bottomCorner].coordY();
00954       const int MAX_CLOSE_DIST = 5;
00955       for (unsigned int i=0; i<4; i++) {
00956   if (i != bottomCorner && mostBottom - rcorners[i].coordY() < MAX_CLOSE_DIST) {
00957     closeCorner = i;
00958   }
00959       }
00960     
00961       if (closeCorner != -1) {
00962   // If 7 was actually bottom-most, switch bottomCorner to 3
00963   if ((unsigned int)closeCorner == (bottomCorner+3)%4) {
00964     bottomCorner = closeCorner;
00965   }
00966   else if ((unsigned int)closeCorner != (bottomCorner+1)%4) {
00967     std::cout<<"WARNING, opposite corners are close together!"<<std::endl;
00968   }
00969       }
00970       NEW_SHAPE(right3, PointData, PointData(space,rcorners[bottomCorner]));
00971       points[3].push_back(rcorners[bottomCorner]);
00972       points[7].push_back(rcorners[(bottomCorner+1)%4]);
00973       points[6].push_back(rcorners[(bottomCorner+2)%4]);
00974       points[2].push_back(rcorners[(bottomCorner+3)%4]);
00975 
00976     }
00977   }
00978 
00979 
00980   // If we have additional ways of extracting candidate corners, 
00981   // add the corners they find to the vector of corners now.
00982 
00983 
00984   // debug output
00985   /*for (unsigned int i=0; i<8; i++){
00986     for (unsigned int j=0; j<points[i].size(); j++) {
00987       NEW_SHAPE(corner, PointData, PointData(space, points[i][j]));
00988       char name[10];
00989       sprintf(name, "corner%d%d",i,j);
00990       corner->setName(name);
00991     }
00992     }*/
00993 
00994 
00995   // Now merge all our candidate points to get the final brick
00996 
00997   vector<Point> finalCorners(8);
00998 
00999   // for now just average corners together to get the last corners
01000   // should at least weight the average based on confidence
01001   // could also do statistics
01002   // produce a final confidence based on the std. deviation
01003 
01004   // if we didn't get the center point of the brick then we don't have a shot
01005   // top-left line also is pretty critical, 
01006   // though I probably should re-work below to work in the case where top and right only work well
01007   if (points[2].size()==0 || points[1].size()==0){
01008     return ShapeRootType(invalid,BrickData);    
01009   }
01010 
01011   Point empty(0,0);
01012 
01013 
01014   // Lots of cases for which corners are visible and extrapolating the others
01015 
01016   for (unsigned int i=0; i<8; i++) {
01017     finalCorners[i] = empty;
01018     for (unsigned int j=0; j<points[i].size(); j++) {
01019       finalCorners[i]+=points[i][j];
01020     }
01021     if (points[i].size() > 0) {
01022       finalCorners[i]/=points[i].size();
01023     }
01024   }
01025 
01026   if (points[3].size()==0){
01027     // These should be easier cases
01028     return ShapeRootType(invalid,BrickData);        
01029   }
01030 
01031   if (points[6].size() == 0) {
01032     // If we have the top left line but not corner 6, infer corner 6 from the thickness of the top side
01033     if (tlline.isValid()) {
01034       NEW_SKETCH_N(topRendering, bool, blobs[top]->getRendering());
01035       Point topPt = (Region::extractRegion(topRendering)).mostDistantPtFrom(tlline.getData());
01036       NEW_SHAPE(intersectLine, LineData, LineData(space, topPt, tlline->getThetaNorm()));
01037       Point intersectPoint = intersectLine->intersectionWithLine(tlline);
01038       // lower confidence result
01039       finalCorners[6] = topPt+finalCorners[2]-intersectPoint;
01040     }
01041     // can also use right left line and the right side, but the top side is more likely to be good
01042     else {
01043       return ShapeRootType(invalid,BrickData);        
01044     }
01045  }
01046 
01047   if (points[0].size() == 0) {
01048     finalCorners[0] = finalCorners[3] + finalCorners[1]-finalCorners[2];
01049   }
01050 
01051   if (points[5].size() == 0) {
01052     finalCorners[5] = finalCorners[6] + finalCorners[1]-finalCorners[2];
01053   }
01054 
01055   if (points[7].size() == 0) {
01056     finalCorners[7] = finalCorners[3] + finalCorners[6]-finalCorners[2];
01057   }
01058 
01059   finalCorners[4] = finalCorners[5] + finalCorners[7] - finalCorners[6] +
01060     finalCorners[5] + finalCorners[0] - finalCorners[1] +
01061     finalCorners[0] + finalCorners[7] - finalCorners[3];
01062   finalCorners[4]/=3.0;
01063 
01064   // left == front for generalized brick
01065   NEW_SHAPE(returnbrick, BrickData, new BrickData(space, 
01066               finalCorners[0], finalCorners[3],
01067               finalCorners[4], finalCorners[7],
01068               finalCorners[1], finalCorners[2],
01069               finalCorners[5], finalCorners[6]));
01070   return returnbrick;
01071     
01072 }
01073 
01074 
01075 
01076   // Generates a wide bounding box around the blob from a parallel line along one edge
01077   // This bounding box will be used as a starting point for the quadrilateral-fitting algorithm
01078   // 
01079   // Generates 4 points based on the line, centroid of the blob, and parameters for extending the bounds
01080   // Then sorts the points into counter-clockwise order to satisfy the ordering constraints to make other 
01081   // extrapolations possible. 
01082 vector<Point> BrickData::findOrthogonalBoundingBox(ShapeSpace& space, Shape<BlobData> blob, Point centroid, Shape<LineData> parallel)
01083 {
01084   const float PARALLEL_EXTEND_FACTOR = .6, ORTHO_EXTEND_FACTOR = .6;
01085 
01086   vector<Point> candidates;
01087   LineData candidate1(space, parallel->end1Pt(), parallel->end2Pt());
01088 
01089   //candidate1.setEndPts(parallel->end1Pt(), parallel->end2Pt());
01090   Point parallelExtend(candidate1.end2Pt() - candidate1.end1Pt());
01091   parallelExtend *= PARALLEL_EXTEND_FACTOR;
01092   LineData ortho(space, centroid, candidate1.getThetaNorm());
01093   Point orthoIsect = candidate1.intersectionWithLine(ortho);
01094   Point candidate2Offset = (centroid - orthoIsect) * 2.0;
01095   Point orthoExtend = candidate2Offset * ORTHO_EXTEND_FACTOR;
01096 
01097   LineData candidate2(space, candidate1.end1Pt() + candidate2Offset + orthoExtend - parallelExtend,
01098           candidate1.end2Pt() + candidate2Offset + orthoExtend + parallelExtend);
01099   candidate1.setEndPts(candidate1.end1Pt() - orthoExtend - parallelExtend,
01100            candidate1.end2Pt() - orthoExtend + parallelExtend);
01101   candidates.push_back(candidate1.leftPt());
01102   candidates.push_back(candidate1.rightPt());
01103   candidates.push_back(candidate2.rightPt());
01104   candidates.push_back(candidate2.leftPt());
01105 
01106   // debug output
01107   for (unsigned int i=0; i<candidates.size(); i++) {
01108     NEW_SHAPE_N(candidate, PointData, PointData(space, candidates[i]));
01109     candidate->setParentId(blob->getViewableId());
01110   }
01111 
01112   // order counter-clockwise around the blob before returning
01113   Point centerPoint = (candidates[0] + candidates[1] + candidates[2] + candidates[3]) / 4.0;
01114   vector<float> centerAngles;
01115   for (unsigned int i=0; i<candidates.size(); i++) {
01116     NEW_SHAPE_N(centerLine, LineData, LineData(space, centerPoint, candidates[i]));
01117     centerLine->setParentId(blob->getViewableId());
01118     centerAngles.push_back(atan2(centerLine->end2Pt().coordY() - centerLine->end1Pt().coordY(),
01119          centerLine->end2Pt().coordX() - centerLine->end1Pt().coordX()));
01120   }
01121 
01122 
01123   vector<Point> orderedPoints;
01124   
01125   int maxi = 0, mini = 0;
01126   float maxAng = centerAngles[0];
01127   float minAng = maxAng;
01128   
01129   for (unsigned int i=1; i<candidates.size(); i++) {
01130     if (centerAngles[i] > maxAng) {
01131       maxAng = centerAngles[i];
01132       maxi = i;
01133     }
01134     if (centerAngles[i] < minAng) {
01135       minAng = centerAngles[i];
01136       mini = i;
01137     }
01138        
01139   }
01140 
01141   orderedPoints.push_back(candidates[maxi]);
01142   
01143   float lastMax = maxAng;
01144   for (unsigned int i=1; i<candidates.size(); i++) {
01145     maxi = mini;
01146     maxAng = minAng;
01147     for (unsigned int j=0; j<candidates.size(); j++) {
01148       float curAng = centerAngles[j];
01149       if (curAng > maxAng && curAng < lastMax) {
01150   maxi = j;
01151   maxAng = curAng;
01152       }
01153     }
01154     orderedPoints.push_back(candidates[maxi]);
01155     lastMax = maxAng;
01156   }
01157   
01158   // debug output
01159   for (unsigned int i=0; i<candidates.size(); i++) {
01160     NEW_SHAPE(orderedcandidate, PointData, PointData(space, orderedPoints[i]));
01161     orderedcandidate->setParentId(blob->getViewableId());
01162   }
01163 
01164   return orderedPoints;
01165 }
01166 
01167 
01168 
01169 } // namespace

DualCoding 4.0
Generated Thu Nov 22 00:52:36 2007 by Doxygen 1.5.4