00001
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"
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
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
00053
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;
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;
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;
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
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
00201
00202 PlaneEquation elevatedPlane = gndplane;
00203 float const new_displacement = elevatedPlane.getDisplacement() + assumedHeight*elevatedPlane.getZsign();
00204 elevatedPlane.setDisplacement(new_displacement);
00205
00206
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
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;
00231 areatri1=sqrt(s*(s-side1)*(s-side2)*(s-side3));
00232
00233 s =(side3+side4+side5) / 2;
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
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;
00281 return true;
00282 }
00283
00284
00285
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
00294
00295
00296
00297
00298 set<color_index> dummyColors;
00299 dummyColors.insert(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
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
00351 CMVision::ConnectComponents(rle_buffer,numRuns);
00352 int const maxRegions = (sketch.width * sketch.height) / 16;
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
00362 std::vector<Shape<BlobData> > result(20);
00363 result.clear();
00364 ShapeSpace &ShS = sketch->getSpace().getDualSpace();
00365
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 ®,
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
00402
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
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
00445
00446
00447
00448
00449
00450
00451
00452
00453
00454
00455
00456
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
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
00471
00472
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
00500
00501
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
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
00573 return fitCorners;
00574
00575
00576
00577
00578
00579
00580
00581
00582
00583
00584
00585
00586
00587
00588
00589
00590
00591
00592
00593
00594
00595
00596
00597
00598
00599
00600
00601
00602
00603
00604
00605
00606
00607
00608
00609
00610
00611
00612
00613 }
00614
00615
00616
00617
00618
00619
00620
00621
00622
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
00651
00652
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
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
00689
00690
00691
00692
00693
00694
00695
00696
00697
00698
00699
00700
00701
00702
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
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
00752
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
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
00812
00813
00814
00815
00816
00817
00818
00819
00820
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
00877
00878
00879 while (annealingScalar < ANNEALING_CAP) {
00880
00881 hasMoved = false;
00882
00883
00884 for (unsigned int i=0; i<ncorners; i++) {
00885
00886 testScores.clear();
00887 testPoints.clear();
00888 testEdgeCounts.clear();
00889 testCount = 0;
00890
00891
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
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
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
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
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
00993
00994 if (doingRandomMovement) {
00995 annealingScalar += bestEdgeCount*WEIGHT_SCALAR/edgeTotal;
00996 }
00997 else if (!hasMoved) {
00998 doingRandomMovement = true;
00999
01000
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
01024
01025 bool BlobData::AreaLessThan::operator() (const Shape<BlobData> &b1, const Shape<BlobData> &b2) const {
01026 return b1->getArea() < b2->getArea();
01027 }
01028
01029 }