Tekkotsu Homepage
Demos
Overview
Downloads
Dev. Resources
Reference
Credits

BlobData.cc

Go to the documentation of this file.
00001 //-*-c++-*-
00002 #include <iostream>
00003 #include <vector>
00004 
00005 #include "Vision/cmvision.h"
00006 
00007 #include "SketchSpace.h"
00008 #include "Sketch.h"
00009 #include "ShapeSpace.h"
00010 #include "ShapeRoot.h"
00011 
00012 #include "BlobData.h"
00013 #include "LineData.h"  // for drawline2d
00014 #include "ShapeBlob.h"
00015 #include "visops.h"
00016 #include "Region.h"
00017 #include "ShapeLine.h"
00018 #include "ShapePoint.h"
00019 
00020 #include "BrickOps.h" 
00021 
00022 using namespace std;
00023 
00024 namespace DualCoding {
00025 
00026 BlobData::BlobData(ShapeSpace& _space,
00027        const Point &_topLeft, const Point &_topRight,
00028        const Point &_bottomLeft, const Point &_bottomRight,
00029        const float _area, const std::vector<run> &_runvec,
00030        const BlobOrientation_t _orientation, coordinate_t _assumedHeight,
00031        rgb _rgbvalue) :
00032   BaseData(_space, getStaticType()),
00033   orientation(_orientation), assumedHeight(_assumedHeight),
00034   topLeft(_topLeft), topRight(_topRight),
00035   bottomLeft(_bottomLeft), bottomRight(_bottomRight),
00036   area(_area), runvec(_runvec)
00037 {
00038   setColor(_rgbvalue);
00039 }
00040       
00041 DATASTUFF_CC(BlobData);
00042 
00043 //! return the centroid of the shape in point format
00044 Point BlobData::getCentroid() const {
00045   return Point((topLeft.coords + topRight.coords + bottomLeft.coords + bottomRight.coords) / 4,
00046          getRefFrameType());
00047 }
00048   
00049 void BlobData::printParams() const {
00050   cout << "Type = " << getTypeName() << "  ID=" << getId() << "  ParentID=" << getParentId() << endl;
00051   printf("  color = %d %d %d\n",getColor().red,getColor().green,getColor().blue);
00052   cout << "  tl=" << NEWMAT::printmat(topLeft.coords) << endl
00053        << "  tr=" << NEWMAT::printmat(topRight.coords) << endl
00054        << "  bl=" << NEWMAT::printmat(bottomLeft.coords) << endl
00055        << "  br=" << NEWMAT::printmat(bottomRight.coords) << endl
00056        << "  area=" << area << ", assumedHeight=" << assumedHeight
00057        << endl;
00058 }
00059 
00060 Sketch<bool>* BlobData::render() const {
00061   SketchSpace &SkS = space->getDualSpace();
00062   Sketch<bool>* result = new Sketch<bool>(SkS, "render("+getName()+")");
00063   *result = false;
00064   switch ( orientation ) {
00065   case groundplane:
00066     if ( space->getRefFrameType() == camcentric ) {
00067       for ( std::vector<BlobData::run>::const_iterator it = runvec.begin();
00068       it != runvec.end(); it++ ) {
00069   const BlobData::run &r = *it;
00070   int const xstop = r.x + r.width;
00071   for ( int xi=r.x; xi<xstop; xi++ )
00072     (*result)(xi, r.y) = true;
00073       }
00074     } else {
00075       NEWMAT::ColumnVector tl(topLeft.getCoords()), tr(topRight.getCoords()),
00076   bl(bottomLeft.getCoords()), br(bottomRight.getCoords());
00077       SkS.applyTmat(tl); SkS.applyTmat(tr); SkS.applyTmat(bl); SkS.applyTmat(br);
00078       LineData::drawline2d(*result, (int)tl(1), (int)tl(2), (int)tr(1), (int)tr(2));
00079       LineData::drawline2d(*result, (int)tr(1), (int)tr(2), (int)br(1), (int)br(2));
00080       LineData::drawline2d(*result, (int)br(1), (int)br(2), (int)bl(1), (int)bl(2));
00081       LineData::drawline2d(*result, (int)bl(1), (int)bl(2), (int)tl(1), (int)tl(2));
00082     }
00083     break;
00084   case pillar:
00085   case poster:
00086     cout << "BlobData::render() : Can't yet render blobs in pillar/poster orientations" << endl;
00087     break;
00088   }
00089   return result;
00090 }
00091 
00092 //! Transformations. (Virtual in BaseData.)
00093 void BlobData::applyTransform(const NEWMAT::Matrix& Tmat, const ReferenceFrameType_t newref) {
00094   topLeft.applyTransform(Tmat,newref);
00095   topRight.applyTransform(Tmat,newref);
00096   bottomLeft.applyTransform(Tmat,newref);
00097   bottomRight.applyTransform(Tmat,newref);
00098 }
00099 
00100 void BlobData::projectToGround(const NEWMAT::Matrix& camToBase,
00101              const NEWMAT::ColumnVector& gndplane) {
00102   float const camblob_area = area;
00103   switch ( orientation ) {
00104   case groundplane:
00105     topLeft.projectToGround(camToBase,gndplane);
00106     topRight.projectToGround(camToBase,gndplane);
00107     bottomLeft.projectToGround(camToBase,gndplane);
00108     bottomRight.projectToGround(camToBase,gndplane);
00109     break;
00110   case pillar:
00111     bottomLeft.projectToGround(camToBase,gndplane);
00112     bottomRight.projectToGround(camToBase,gndplane);
00113     topLeft = bottomLeft;
00114     topRight = bottomRight;
00115     topLeft.coords(3) += assumedHeight;
00116     topRight.coords(3) += assumedHeight;
00117     break;
00118   case poster:
00119     // create an elevated plane by shifting the ground plane by assumedHeight
00120     NEWMAT::ColumnVector elevatedPlane = gndplane;
00121     float displacement = assumedHeight * sqrt(elevatedPlane(1) * elevatedPlane(1) + elevatedPlane(2) * elevatedPlane(2) + elevatedPlane(3) * elevatedPlane(3));
00122     elevatedPlane(4) += elevatedPlane(3) >= 0 ? displacement : -displacement;
00123     
00124     // Project the centroid onto the elevated plane and set all 4 points to the new centroid
00125     Point centroid = getCentroid();
00126     centroid.projectToGround(camToBase, elevatedPlane);
00127     bottomLeft = centroid;
00128     bottomRight = centroid;
00129     topLeft = centroid;
00130     topRight = centroid;
00131     break;
00132   }
00133   update_derived_properties();
00134   area = camblob_area;  // stick with cam area for now
00135 }
00136 
00137 void BlobData::update_derived_properties() {
00138   switch ( orientation ) {
00139 
00140   case groundplane:
00141   case pillar:
00142   case poster:
00143     // DST HACK -- should project each run to ground and integrate,
00144     // or use the formula for area of a quadrilateral
00145     area = fabs((topRight+bottomRight-topLeft-bottomLeft).coords(1)) *
00146            fabs((bottomLeft+bottomRight-topLeft-topRight).coords(2)) / 4;
00147     break;
00148   }
00149 }
00150 
00151 bool BlobData::isMatchFor(const ShapeRoot& other) const {
00152   if (!(isSameTypeAs(other) && isSameColorAs(other)))
00153     return false;
00154   else {
00155     const Shape<BlobData>& other_blob = ShapeRootTypeConst(other,BlobData);
00156     float dist = getCentroid().distanceFrom(other_blob->getCentroid());
00157     return dist < 2*sqrt(area);
00158   }
00159 }
00160 
00161 bool BlobData::updateParams(const ShapeRoot& other, bool forceUpdate) {
00162   const Shape<BlobData>& other_blob = ShapeRootTypeConst(other,BlobData);
00163   int other_conf = other_blob->confidence;
00164   if (other_conf <= 0) {
00165     if (forceUpdate)
00166       other_conf = 1;
00167     else
00168       return false;
00169   }
00170   confidence += other_conf;
00171   const int sumconf = confidence + other_conf;
00172   topLeft = (topLeft*confidence + other_blob->topLeft*other_conf) / sumconf;
00173   topRight = (topRight*confidence + other_blob->topRight*other_conf) / sumconf;
00174   bottomLeft = (bottomLeft*confidence + other_blob->bottomLeft*other_conf) / sumconf;
00175   bottomRight = (bottomRight*confidence + other_blob->bottomRight*other_conf) / sumconf;
00176   float const newArea = (area*confidence + other_blob->area*other_conf) / sumconf;
00177   update_derived_properties();
00178   area = newArea; // fixup because update_derived_properties currently messes up on pillar and poster areas
00179   return true;
00180 }
00181 
00182 //================================================================
00183 // Blob extraction
00184 
00185 std::vector<Shape<BlobData> >
00186 BlobData::extractBlobs(const Sketch<bool> &sketch, 
00187            int minarea,
00188            BlobOrientation_t orient, 
00189            coordinate_t height,
00190            int maxblobs) {
00191   // We could multiply sketch*color_index to convert to uchar and
00192   // spare ourselves the for loop that follows for setting blob
00193   // colors, but this way is actually much faster because we skip all
00194   // the pixel-wise multiplications.  Casting Sketch<bool> to
00195   // Sketch<uchar> is done with a simple reinterpret_cast; no copying.
00196   set<color_index> dummyColors;
00197   dummyColors.insert(1); // converting bool to uchar always yields color index 1
00198   map<color_index,int> minareas;
00199   minareas[1] = minarea;
00200   map<color_index,BlobOrientation_t> orients;
00201   orients[1] = orient;
00202   map<color_index,coordinate_t> heights;
00203   heights[1] = height;
00204   std::vector<Shape<BlobData> > result(extractBlobs((Sketch<uchar>&)sketch,dummyColors,minareas,orients,heights,maxblobs));
00205   rgb sketchColor(sketch->getColor());
00206   for ( std::vector<Shape<BlobData> >::iterator it = result.begin();
00207   it != result.end(); it++ )
00208     (*it)->setColor(sketchColor);
00209   return result;
00210 }
00211 
00212 std::vector<Shape<BlobData> > 
00213 BlobData::extractBlobs(const Sketch<uchar> &sketch,
00214            int minarea,
00215            BlobOrientation_t orient,
00216            coordinate_t height,
00217            int maxblobs) {
00218   const int numColors = ProjectInterface::getNumColors();
00219   set<color_index> colors;
00220   map<color_index,int> minareas;
00221   map<color_index,BlobOrientation_t> orients;
00222   map<color_index,coordinate_t> heights;
00223   for (int i = 1; i < numColors; i++) {
00224     colors.insert(i);
00225     minareas[i] = minarea;
00226     orients[i] = orient;
00227     heights[i] = height;
00228   }
00229   return extractBlobs(sketch, colors, minareas, orients, heights, maxblobs);
00230 }
00231 
00232 std::vector<Shape<BlobData> >
00233 BlobData::extractBlobs(const Sketch<uchar> &sketch, 
00234            const std::set<color_index>& colors,
00235            const std::map<color_index,int>& minareas,
00236            const std::map<color_index,BlobOrientation_t>& orients,
00237            const std::map<color_index,coordinate_t>& heights,
00238            int maxblobs) {
00239   int parent = sketch->getId();
00240   uchar *pixels = &((*sketch.pixels)[0]);
00241 
00242   // convert pixel array to RLE
00243   int const maxRuns = (sketch.width * sketch.height) / 8;
00244   CMVision::run<uchar> *rle_buffer = new CMVision::run<uchar>[maxRuns];
00245   unsigned int const numRuns = CMVision::EncodeRuns(rle_buffer, pixels, sketch.width, sketch.height, maxRuns);
00246 
00247   // convert RLE to region list
00248   CMVision::ConnectComponents(rle_buffer,numRuns);
00249   int const maxRegions = (sketch.width * sketch.height) / 16;   // formula from RegionGenerator.h
00250   CMVision::region *regions = new CMVision::region[maxRegions];
00251   unsigned int numRegions = CMVision::ExtractRegions(regions, maxRegions, rle_buffer, numRuns);
00252   unsigned int const numColors = ProjectInterface::getNumColors();
00253   CMVision::color_class_state *ccs = new CMVision::color_class_state[numColors];
00254   unsigned int const maxArea = CMVision::SeparateRegions(ccs, numColors, regions, numRegions);
00255   CMVision::SortRegions(ccs, numColors, maxArea);
00256   CMVision::MergeRegions(ccs, int(numColors), rle_buffer);
00257 
00258   // extract blobs from region list
00259   std::vector<Shape<BlobData> > result(20);
00260   result.clear();
00261   ShapeSpace &ShS = sketch->getSpace().getDualSpace();
00262   //  for ( size_t color=1; color < numColors; color++ ) {
00263   for (set<color_index>::const_iterator it = colors.begin();
00264        it != colors.end(); it++) {
00265     int const color = *it;
00266     const CMVision::region* list_head = ccs[color].list;
00267     if ( list_head != NULL ) {
00268       const rgb rgbvalue = ProjectInterface::getColorRGB(color);
00269       int const minarea = (minareas.find(color)==minareas.end()) ? 50 : const_cast<map<color_index,int>&>(minareas)[color];
00270       BlobOrientation_t const orient = (orients.find(color)==orients.end()) ? 
00271   groundplane : const_cast<map<color_index,BlobOrientation_t>&>(orients)[color];
00272       coordinate_t const height = (heights.find(color)==heights.end()) ? 0 : const_cast<map<color_index,coordinate_t>&>(heights)[color];
00273       for (int i=0; list_head!=NULL && i<maxblobs && list_head->area >= minarea;
00274      list_head = list_head->next, i++) {
00275   BlobData* blobdat = new_blob(ShS,*list_head, rle_buffer, orient, height, rgbvalue);
00276   blobdat->setParentId(parent);
00277   result.push_back(Shape<BlobData>(blobdat));
00278       }
00279     }
00280   }
00281   delete[] ccs;
00282   delete[] regions;
00283   delete[] rle_buffer;
00284   return result;
00285 }
00286 
00287 BlobData* BlobData::new_blob(ShapeSpace& space,
00288            const CMVision::region &reg,
00289            const CMVision::run<CMVision::uchar> *rle_buff, 
00290            const BlobOrientation_t orient,
00291            const coordinate_t height,
00292            const rgb rgbvalue) {
00293   int const x1 = reg.x1;
00294   int const y1 = reg.y1;
00295   int const x2 = reg.x2;
00296   int const y2 = reg.y2;
00297   // Count the number of runs so we can allocate a vector of the right size.
00298   // The first run might be numbered 0, so we must use -1 as end of list indicator.
00299   int numruns = 0;
00300   for (int runidx = reg.run_start; runidx != -1; 
00301        runidx = rle_buff[runidx].next ? rle_buff[runidx].next : -1)
00302     ++numruns;
00303   std::vector<BlobData::run> runvec(numruns);
00304   runvec.clear();
00305   // now fill in the run vector
00306   for (int runidx = reg.run_start; runidx != -1; 
00307        runidx = rle_buff[runidx].next ? rle_buff[runidx].next : -1) {
00308     const CMVision::run<uchar> &this_run = rle_buff[runidx];
00309     runvec.push_back(BlobData::run(this_run.x, this_run.y, this_run.width));
00310   }
00311   if ( space.getRefFrameType() == camcentric )
00312     return new BlobData(space,
00313       Point(x1,y1),Point(x2,y1),
00314       Point(x1,y2),Point(x2,y2),
00315       reg.area, runvec, orient, height, rgbvalue);
00316   else
00317     return new BlobData(space,
00318       Point(x2,y2),Point(x1,y2),
00319       Point(x2,y1),Point(x1,y1),
00320       reg.area, runvec, orient, height, rgbvalue);
00321 }
00322 
00323 
00324 //================================================================
00325 // Corner extraction: General call to find the corners of a blob.
00326 // Used in brick / pyramid extraction
00327 //
00328 //
00329 // Requires the number of corners you expect to find.
00330 // Currently just uses shape fitting to find the corners
00331 // Originally used either the derivative or diagonal approaches to finding corners
00332 //
00333 // Shape fitting works very well, but is very slow.
00334 // (no attempt was made to optimize it though)
00335 //
00336 // The old diagonal/derivative approach is at the bottom
00337 // It is much faster, but less robust.
00338 
00339   std::vector<Point> BlobData::findCorners(unsigned int nExpected, 
00340              std::vector<Point>& candidates,
00341              float &bestValue) {
00342 
00343     std::vector<Point> fitCorners = findCornersShapeFit(nExpected, candidates, bestValue);
00344 
00345     // debug output
00346     for (unsigned int i=0; i<fitCorners.size(); i++){
00347       NEW_SHAPE(fitline, LineData, LineData(*space, fitCorners[i], fitCorners[(i+1)%nExpected]));
00348       fitline->setParentId(getViewableId());
00349     }
00350 
00351     // Handling off-screen bricks:
00352     // If our fit of corners is close to the edge of the image,
00353     // re-try fitting 3 or 5 corners to the brick
00354     bool onEdge = false;
00355     int width = space->getDualSpace().getWidth();
00356     int height = space->getDualSpace().getHeight();
00357     for (unsigned int i=0; i<nExpected; i++) {
00358       if (fitCorners[i].coordX() < 5 || fitCorners[i].coordX() > width - 5 ||
00359     fitCorners[i].coordY() < 5 || fitCorners[i].coordY() > height - 5) {
00360   onEdge = true;
00361   break;
00362       }
00363     }
00364     if (onEdge && nExpected == 4) {
00365       std::vector<int> outsideCandidates;
00366       for (unsigned int i=0; i<nExpected; i++) {
00367   if (candidates[i].coordX() < 5 || candidates[i].coordX() > width - 5 ||
00368       candidates[i].coordY() < 5 || candidates[i].coordY() > height - 5) {
00369     outsideCandidates.push_back(i);
00370   }
00371       }
00372 
00373 
00374       std::vector<Point> candidates3(candidates), candidates5;
00375 
00376       if (outsideCandidates.size() == 0) {
00377   std::cout<<"Err? final points are near the edge, but the candidates aren't?"<<std::endl;
00378       }
00379       
00380       // If just one candidate is outside the scene, 
00381       // try removing it for 3 points
00382       // and adding the points where it crosses the border of the image for 5 points
00383       if (outsideCandidates.size() == 1) {
00384   int outC = outsideCandidates[0];
00385   candidates3.erase(candidates3.begin() + outC);
00386       
00387   Point p1, p2;
00388   if (candidates[outC].coordX() < 5) {
00389     p1.setCoords(0,0);
00390     p2.setCoords(0,height);
00391   }
00392   else if (candidates[outC].coordX() > width - 5) {
00393     p1.setCoords(width,0);
00394     p2.setCoords(width,height);
00395   }
00396   else if (candidates[outC].coordY() < 5) {
00397     p1.setCoords(0,0);
00398     p2.setCoords(width,0);
00399   }
00400   else {
00401     p1.setCoords(0,height);
00402     p2.setCoords(width,height);
00403   }
00404   LineData edgeLine(*space, p1,p2);
00405   LineData l1(*space, candidates[(outC+3)%4], candidates[outC]);
00406   LineData l2(*space, candidates[(outC+1)%4], candidates[outC]);
00407   candidates5.push_back(candidates[(outC+3)%4]);
00408   candidates5.push_back(l1.intersectionWithLine(edgeLine));
00409   candidates5.push_back(l2.intersectionWithLine(edgeLine));
00410   candidates5.push_back(candidates[(outC+1)%4]);
00411   candidates5.push_back(candidates[(outC+2)%4]);
00412       }
00413       
00414       if (outsideCandidates.size() == 2) {
00415   Point betweenOutside = (candidates[outsideCandidates[0]] + candidates[outsideCandidates[1]])/2;
00416   candidates3[outsideCandidates[0]].setCoords(betweenOutside);
00417   candidates3.erase(candidates3.begin() + outsideCandidates[1]);
00418   
00419   int dC = outsideCandidates[1] - outsideCandidates[0];
00420   int c1 = outsideCandidates[1];
00421   candidates5.push_back(candidates[outsideCandidates[0]]);
00422   candidates5.push_back(betweenOutside);
00423   candidates5.push_back(candidates[outsideCandidates[1]]);
00424   candidates5.push_back(candidates[(c1+dC)%4]);
00425   candidates5.push_back(candidates[(c1+2*dC)%4]);
00426       }
00427       
00428       if (outsideCandidates.size() > 2) {
00429   // Not gonna get very good points out of this, probably
00430       }
00431     
00432       float value3, value5;
00433       for (unsigned int i=0; i<candidates3.size(); i++) {
00434   NEW_SHAPE(candidate3, PointData, PointData(*space, candidates3[i]));
00435   candidate3->setParentId(getViewableId());
00436       }
00437       for (unsigned int i=0; i<candidates5.size(); i++) {
00438   NEW_SHAPE(candidate5, PointData, PointData(*space, candidates5[i]));
00439   candidate5->setParentId(getViewableId());
00440       }
00441       std::vector<Point> fitcorners3 = findCornersShapeFit(3, candidates3, value3);
00442       std::vector<Point> fitcorners5 = findCornersShapeFit(5, candidates5, value5);
00443       for (unsigned int i=0; i<fitcorners3.size(); i++) {
00444   NEW_SHAPE(fit3, PointData, PointData(*space, fitcorners3[i]));
00445   fit3->setParentId(getViewableId());
00446       }
00447       for (unsigned int i=0; i<fitcorners5.size(); i++) {
00448   NEW_SHAPE(fit5, PointData, PointData(*space, fitcorners5[i]));
00449   fit5->setParentId(getViewableId());
00450       }
00451     }
00452 
00453     // right now just take the corners from quadrilateral fitting
00454     return fitCorners;
00455 
00456 
00457     // Old method, used the diagonal and derivative approaches and took the better results
00458     // Was generally much faster, but broke down in some special cases
00459     /* 
00460     const float MIN_BOUNDING_SCORE = .75;
00461 
00462     float derivScore = -1, diagScore = -1;
00463 
00464     std::vector<Point> derivCorners = findCornersDerivative();
00465 
00466     if (derivCorners.size() == nExpected) {
00467 
00468       derivScore = getBoundingQuadrilateralInteriorPointRatio(derivCorners);
00469       std::cout<<"Derivative score for ("<<getViewableId()<<") = "<<derivScore<<std::endl;
00470       if (derivScore > MIN_BOUNDING_SCORE) {
00471   return derivCorners;
00472       }
00473     }
00474     
00475     std::vector<Point> diagCorners = findCornersDiagonal();
00476 
00477     if (diagCorners.size() == nExpected) {
00478       diagScore = getBoundingQuadrilateralInteriorPointRatio(diagCorners);
00479       std::cout<<"Diagonal score for ("<<getViewableId()<<") = "<<diagScore<<std::endl;
00480       if (diagScore > derivScore) {
00481   return diagCorners;
00482       }
00483       else if (derivScore > .5) {
00484   return derivCorners;
00485       }
00486     }
00487 
00488     // can we integrate sets of incomplete / overcomplete points?
00489 
00490     std::vector<Point> result;
00491 
00492     return result;
00493     */
00494   }
00495 
00496 
00497   /*
00498    * Derivative approach to finding the corners of the blobs. 
00499    * Computes the distance from the center to the edge in a circle around the blob
00500    * Takes a couple derivatives, and looks for peaks.
00501    *
00502    * Works well on big shapes, poorly on small ones
00503    * Doesn't make any guarantees as to how many points are returned. 
00504    */
00505   std::vector<Point> BlobData::findCornersDerivative()
00506   {
00507     std::vector<Point> corners;
00508 
00509     float radius = sqrt((topRight.coordX()-topLeft.coordX())*(topRight.coordX()-topLeft.coordX()) + 
00510       (bottomLeft.coordY()-topLeft.coordY())*(bottomLeft.coordY()-topLeft.coordY()))/2 + 1;
00511     int len = (int)(2*M_PI*radius + 1);
00512     float distances[len];
00513     Point points[len];
00514     Point centroid = getCentroid();
00515     NEW_SKETCH(rendering, bool, getRendering());
00516     
00517     int i=0, maxi = 0;
00518 
00519     maxi = findRadialDistancesFromPoint(centroid, radius, rendering, distances, points);
00520     float gdist[len], ddist[len], d2dist[len], d3dist[len];
00521     applyGaussian(distances, gdist, len);
00522     takeDerivative(gdist, ddist, len);
00523     takeDerivative(ddist, d2dist, len);
00524     takeDerivative(d2dist, d3dist, len);
00525 
00526     drawHist(gdist, len, rendering);
00527     drawHist(ddist, len, rendering);
00528     drawHist(d2dist, len, rendering);
00529     
00530 
00531     // Corners are negative peaks in the second derivative of the distance from the center
00532     // Zero-crossings in the first derivative should work, but we want to capture contour changes that 
00533     // don't necessarily cross zero (steep increase -> shallow increase could be a corner)
00534 
00535     const float MIN_D2 = 5.0;
00536 
00537     float curmin = -MIN_D2;
00538     int curi = -1;
00539     int curonpeak = 0;
00540     for (i=0; i<len; i++) {
00541       if (d2dist[i]  < curmin) {
00542   curmin = d2dist[i];
00543   curi = i;
00544   curonpeak = 1;
00545       }
00546       else if (curonpeak) {
00547   if (d2dist[i] > -MIN_D2 || (d3dist[i-1] > 0 && d3dist[i] <= 0)) {
00548     curonpeak = 0;
00549     curmin = -MIN_D2;
00550     corners.push_back(points[curi]);
00551     NEW_SHAPE(cornerpoint, PointData, Shape<PointData>(*space, points[curi])); 
00552     cornerpoint->setParentId(rendering->getViewableId());
00553   } 
00554       }
00555     }
00556 
00557 
00558     // Normalize returned corners to counter-clock-wise order;
00559     vector<Point> reversedCorners;
00560     for (i=corners.size()-1; i>=0; i--){
00561       reversedCorners.push_back(corners[i]);
00562     }
00563 
00564     return reversedCorners;
00565   }
00566 
00567 
00568   /*
00569    * Diagonal approach to finding corners
00570    * ad-hoc heuristic works well for normal parallelograms 
00571    *
00572    * fails on trapezoids and triangles, and where nCorners != 4. 
00573    *
00574    *
00575    * Computes radial distance from the center like the derivative method
00576    * Finds the peak in distance (furthest point is once corner)
00577    * Finds the peak in the opposite region (another corner)
00578    * 
00579    * At this point it can draw the diagonal. The breakdown occurs when 
00580    * it thinks it has a diagonal at this point but it isn't an actual diagonal
00581    *
00582    * Then split the quadrilateral into two triangles, and the furthest points from the 
00583    * diagonal are the two remaining corners. 
00584    */
00585   std::vector<Point> BlobData::findCornersDiagonal()
00586   {
00587     std::vector<Point> corners;
00588 
00589     float radius = sqrt((topRight.coordX()-topLeft.coordX())*(topRight.coordX()-topLeft.coordX()) + 
00590       (bottomLeft.coordY()-topLeft.coordY())*(bottomLeft.coordY()-topLeft.coordY()))/2 + 1;
00591     int len = (int)(2*M_PI*radius + 1);
00592     float distances[len];
00593     Point points[len];
00594     Point centroid = getCentroid();
00595     NEW_SKETCH(rendering, bool, getRendering());
00596     
00597     int i=0;
00598     int maxi = 0, origmaxi = 0;
00599     bool stillmax = false;
00600 
00601     maxi = findRadialDistancesFromPoint(centroid, radius, rendering, distances, points);
00602 
00603     // Find second max
00604     int maxi2 = 0;
00605     float max2 = 0;
00606     stillmax = false;
00607     origmaxi = -1;
00608     for (i=0; i<len; i++) {
00609       if (distances[i] >= max2 && 
00610     abs(i-maxi) > len*3/8 && 
00611     abs(i-maxi) < len*5/8) {
00612   if (distances[i] > max2) {
00613     maxi2 = i;
00614     max2 = distances[i];
00615     origmaxi = maxi2;
00616     stillmax = true;
00617   }
00618   else if (stillmax){
00619     maxi2 = (origmaxi+i)/2;
00620   }
00621       }
00622       else {
00623   stillmax = false;
00624       }
00625     }
00626     
00627     corners.push_back(points[maxi]);
00628     corners.push_back(points[maxi2]);
00629     std::cout<<"Corners: ("<<corners[0].coordX()<<","<<corners[0].coordY()<<")  ("<<
00630       corners[1].coordX()<<","<<corners[1].coordY()<<")\n";
00631 
00632     // Get the regions on either side of the line made by the two corners
00633     // The most distant points in those regions are the other two corners
00634     NEW_SHAPE(diag, LineData, Shape<LineData>(*space, corners[0], corners[1]));
00635     diag->firstPt().setActive(false);
00636     diag->secondPt().setActive(false);
00637     diag->setParentId(rendering->getViewableId());
00638 
00639     NEW_SKETCH_N(filled, bool, visops::topHalfPlane(diag));
00640     NEW_SKETCH(side1, bool, filled & rendering);
00641     NEW_SKETCH(side2, bool, !filled & rendering);
00642     
00643     const float MIN_PT_DIST = 3.0;
00644 
00645     Point pt3 = (Region::extractRegion(side1)).mostDistantPtFrom(diag.getData());
00646     Point pt4 = (Region::extractRegion(side2)).mostDistantPtFrom(diag.getData());
00647     if (diag->perpendicularDistanceFrom(pt3) > MIN_PT_DIST)
00648       corners.push_back(pt3);
00649     if (diag->perpendicularDistanceFrom(pt4) > MIN_PT_DIST)
00650       corners.push_back(pt4);
00651     
00652 
00653     // Sort the corners into order going around the brick
00654     std::vector<Point> resultCorners;
00655     std::vector<float> angles;
00656 
00657     float ta;
00658     Point tp;
00659     for (i=0; i<(int)corners.size(); i++) {
00660       Point di = corners[i] - centroid;
00661       angles.push_back(atan2(di.coordY(), di.coordX()));
00662       resultCorners.push_back(corners[i]);
00663       for (int j=i-1; j>=0; j--) {
00664   if (angles[j+1] > angles[j]) {
00665     ta = angles[j];
00666     angles[j] = angles[j+1];
00667     angles[j+1] = ta;
00668     tp = resultCorners[j];
00669     resultCorners[j] = resultCorners[j+1];
00670     resultCorners[j+1] = tp;
00671   }
00672   else{
00673     break;
00674   }
00675       }
00676     }
00677 
00678     NEW_SHAPE(cornerline1, LineData, Shape<LineData>(*space, resultCorners[0], resultCorners[1])); 
00679     cornerline1->setParentId(rendering->getViewableId());
00680     if (resultCorners.size() > 3) {
00681       NEW_SHAPE(cornerline2, LineData, Shape<LineData>(*space, resultCorners[2], resultCorners[3]));
00682       cornerline2->setParentId(rendering->getViewableId());
00683     }
00684     
00685      return resultCorners;
00686   }
00687 
00688 
00689 
00690 
00691 
00692   // find corners by fitting a quadrilateral to the blob
00693   //
00694   // Its expecting a set of candidate points that are roughly aligned with one set of edges and are
00695   // significantly wider than the blob itself. 
00696   // It contracts the candidate points to form a rough bounding box, 
00697   // then does simulated annealing on random perturbations of the end points to get a good fit
00698   //
00699   // Potential quadrilateral fits are scored by maximizing the number of edge points of the blob that 
00700   // lie under one of the lines, and minimizing the total area. 
00701   // A fixed minimum edge length constraint is also enforced. 
00702   std::vector<Point> BlobData::findCornersShapeFit(unsigned int ncorners, std::vector<Point>& candidates, 
00703                float &bestValue)
00704   { 
00705     NEW_SKETCH(rendering, bool, getRendering());
00706 
00707     std::vector<Point> bestPoints(ncorners), curTest(ncorners);
00708     float bestScore;
00709     int bestEdgeCount;
00710     std::vector<std::vector<Point> > testPoints;
00711     std::vector<float> testScores;
00712     std::vector<int> testEdgeCounts;
00713 
00714     if (candidates.size() == ncorners) {
00715       for (unsigned int i=0; i<ncorners; i++) {
00716   bestPoints[i].setCoords(candidates[i]);
00717   curTest[i].setCoords(candidates[i]);
00718       }
00719     }
00720     else {
00721       std::cout<<"Warning: incorrect number of candidates provided"<<std::endl;
00722       return bestPoints;
00723     }
00724 
00725     NEW_SKETCH_N(nsum, uchar, visops::neighborSum(getRendering()));
00726     NEW_SKETCH(borderPixels, bool, nsum > 0 & nsum < 8 & getRendering());
00727     int edgeTotal = 0;
00728     for (unsigned int x=0; x<borderPixels->getWidth(); x++) {
00729       borderPixels(x,0) = 0;
00730       borderPixels(x,borderPixels->getHeight() - 1) = 0;
00731     }
00732     for (unsigned int y=0; y<borderPixels->getHeight(); y++) {
00733       borderPixels(0,y) = 0;
00734       borderPixels(borderPixels->getWidth() - 1, y) = 0;
00735     }
00736     for (unsigned int i=0; i<borderPixels->getNumPixels(); i++) {
00737       if (borderPixels->at(i))
00738   edgeTotal++;
00739     }
00740 
00741     bestScore = getBoundingQuadrilateralScore(*this, bestPoints, borderPixels, bestEdgeCount, *space);
00742 
00743     int testCount = 0, testEdgeCount;
00744     Point dp;
00745     float dpDist, testRatio;
00746     bool hasMoved;
00747 
00748     float annealingScalar = 1.0;
00749     bool doingRandomMovement = false;
00750     const float ANNEALING_CAP = 25.0;
00751     const float WEIGHT_SCALAR = .2;
00752     
00753     const float MIN_DISTANCE = 10.0;
00754     const float MIN_BOUNDING_RATIO = 0.8;
00755 
00756     int iterationCount = 0, annealingStart = 0;
00757     // Right now it just keeps going until the annealing weight gets to a set threshold
00758     // Should probably be improved by checking the quality of the fit at intervals
00759     // especially if the points have stopped moving
00760     while (annealingScalar < ANNEALING_CAP) {
00761 
00762       hasMoved = false;
00763       
00764       // Test each corner in succession
00765       for (unsigned int i=0; i<ncorners; i++) {
00766 
00767   testScores.clear();
00768   testPoints.clear();
00769   testEdgeCounts.clear();
00770   testCount = 0;
00771   // Try moving the corner toward or away from either adjacent corner by 1 pixel
00772   // Only look at moving toward adjacent corners until we start doing random movement
00773 
00774   for (unsigned int j=0; j<ncorners; j++)
00775     curTest[j].setCoords(bestPoints[j]);
00776   dp.setCoords(curTest[(i+1)%ncorners] - curTest[i]);
00777   dpDist = curTest[i].distanceFrom(curTest[(i+1)%ncorners]);
00778   // Don't allow corners to get too close to each other
00779   if (dpDist > MIN_DISTANCE) {
00780 
00781     dp/=dpDist;
00782     curTest[i]+=dp;
00783     testRatio = getBoundingQuadrilateralInteriorPointRatio(*this, curTest, *space);
00784     if (testRatio > MIN_BOUNDING_RATIO) {
00785       testScores.push_back(getBoundingQuadrilateralScore(*this, curTest, borderPixels, 
00786                      testEdgeCount, *space));
00787       testPoints.push_back(curTest);
00788       testEdgeCounts.push_back(testEdgeCount);
00789       testCount++;
00790     }
00791 
00792     // Look outward too if we're in the annealing stage
00793     if (doingRandomMovement) {
00794       curTest[i].setCoords(bestPoints[i]);
00795       curTest[i]-=dp;
00796       testRatio = getBoundingQuadrilateralInteriorPointRatio(*this, curTest, *space);
00797       if (testRatio > MIN_BOUNDING_RATIO) {
00798         testScores.push_back(getBoundingQuadrilateralScore(*this, curTest, borderPixels, 
00799                  testEdgeCount, *space));
00800         testPoints.push_back(curTest);
00801         testEdgeCounts.push_back(testEdgeCount);
00802         testCount++;
00803       }
00804 
00805     }
00806     
00807   }
00808 
00809   curTest[i].setCoords(bestPoints[i]);
00810   dp.setCoords(curTest[(i+ncorners-1)%ncorners] - curTest[i]);
00811   dpDist = curTest[i].distanceFrom(curTest[(i+ncorners-1)%ncorners]);
00812   // Don't allow corners to get too close to each other
00813   if (dpDist > MIN_DISTANCE) {
00814 
00815     dp/=dpDist;
00816     curTest[i]+=dp;
00817     testRatio = getBoundingQuadrilateralInteriorPointRatio(*this, curTest, *space);
00818     if (testRatio > MIN_BOUNDING_RATIO) {
00819       testScores.push_back(getBoundingQuadrilateralScore(*this, curTest, borderPixels, 
00820                      testEdgeCount, *space));
00821       testPoints.push_back(curTest);
00822       testEdgeCounts.push_back(testEdgeCount);
00823       testCount++;
00824     }
00825 
00826     // Look outward too if we're in the annealing stage
00827     if (doingRandomMovement) {
00828       curTest[i].setCoords(bestPoints[i]);
00829       curTest[i]-=dp;
00830       testRatio = getBoundingQuadrilateralInteriorPointRatio(*this, curTest, *space);
00831       if (testRatio > MIN_BOUNDING_RATIO) {
00832         testScores.push_back(getBoundingQuadrilateralScore(*this, curTest, borderPixels, 
00833                  testEdgeCount, *space));
00834         testPoints.push_back(curTest);
00835         testEdgeCounts.push_back(testEdgeCount);
00836         testCount++;
00837       }
00838     }
00839     
00840   }
00841   
00842 
00843   testScores.push_back(bestScore);
00844   testPoints.push_back(bestPoints);
00845   testEdgeCounts.push_back(bestEdgeCount);
00846   testCount++;
00847 
00848   int move = -1;
00849   if (doingRandomMovement) {
00850     move = pickMove(testScores, annealingScalar);
00851   }
00852   else {
00853     move = 0;
00854     for (int j=0; j<testCount; j++) {
00855       if (testScores[j] < testScores[move])
00856         move = j;
00857     }
00858     if (move != testCount-1) {
00859       hasMoved = true;
00860     }
00861   }
00862 
00863   if (move < 0 || move >= testCount)
00864     std::cout<<"Hmm, picked a bad move somewhere ("<<move<<")\n";
00865   else {
00866     bestPoints[i].setCoords(testPoints[move][i]);
00867     bestScore = testScores[move];
00868     bestEdgeCount = testEdgeCounts[move];
00869   }
00870 
00871       }
00872 
00873       // Increase the weight proportional to how much of the edges are covered
00874       // Only increase it if we're at the annealing stage though. 
00875       if (doingRandomMovement) {
00876   annealingScalar += bestEdgeCount*WEIGHT_SCALAR/edgeTotal;
00877       }
00878       else if (!hasMoved) {
00879   doingRandomMovement = true;
00880 
00881   // debug output
00882   annealingStart = iterationCount;
00883   for (unsigned int z=0; z<ncorners; z++) {
00884     NEW_SHAPE(preannealing, PointData, PointData(*space, bestPoints[z]));
00885     preannealing->setParentId(borderPixels->getViewableId());
00886   }
00887       }
00888 
00889       iterationCount++;
00890       if (iterationCount > 500) {
00891   std::cout<<"Warning, annealing stopped by max iteration count\n"<<std::endl;
00892   break;
00893       }
00894       
00895     }
00896 
00897     std::cout<<"Shape fit took "<<iterationCount<<" iterations ("<<iterationCount - annealingStart<<" of annealing)\n";
00898     bestValue = bestScore;
00899     return bestPoints;
00900 
00901   }
00902 
00903 
00904 // Comparison predicates
00905 
00906 bool BlobData::AreaLessThan::operator() (const Shape<BlobData> &b1, const Shape<BlobData> &b2) const {
00907       return b1->getArea() < b2->getArea();
00908 }
00909 
00910 } // namespace

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