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

DualCoding 5.1CVS
Generated Mon May 9 04:56:25 2016 by Doxygen 1.6.3