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 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
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
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
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
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;
00135 }
00136
00137 void BlobData::update_derived_properties() {
00138 switch ( orientation ) {
00139
00140 case groundplane:
00141 case pillar:
00142 case poster:
00143
00144
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;
00179 return true;
00180 }
00181
00182
00183
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
00192
00193
00194
00195
00196 set<color_index> dummyColors;
00197 dummyColors.insert(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
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
00248 CMVision::ConnectComponents(rle_buffer,numRuns);
00249 int const maxRegions = (sketch.width * sketch.height) / 16;
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
00259 std::vector<Shape<BlobData> > result(20);
00260 result.clear();
00261 ShapeSpace &ShS = sketch->getSpace().getDualSpace();
00262
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 ®,
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
00298
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
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
00326
00327
00328
00329
00330
00331
00332
00333
00334
00335
00336
00337
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
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
00352
00353
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
00381
00382
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
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
00454 return fitCorners;
00455
00456
00457
00458
00459
00460
00461
00462
00463
00464
00465
00466
00467
00468
00469
00470
00471
00472
00473
00474
00475
00476
00477
00478
00479
00480
00481
00482
00483
00484
00485
00486
00487
00488
00489
00490
00491
00492
00493
00494 }
00495
00496
00497
00498
00499
00500
00501
00502
00503
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
00532
00533
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
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
00570
00571
00572
00573
00574
00575
00576
00577
00578
00579
00580
00581
00582
00583
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
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
00633
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
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
00693
00694
00695
00696
00697
00698
00699
00700
00701
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
00758
00759
00760 while (annealingScalar < ANNEALING_CAP) {
00761
00762 hasMoved = false;
00763
00764
00765 for (unsigned int i=0; i<ncorners; i++) {
00766
00767 testScores.clear();
00768 testPoints.clear();
00769 testEdgeCounts.clear();
00770 testCount = 0;
00771
00772
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
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
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
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
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
00874
00875 if (doingRandomMovement) {
00876 annealingScalar += bestEdgeCount*WEIGHT_SCALAR/edgeTotal;
00877 }
00878 else if (!hasMoved) {
00879 doingRandomMovement = true;
00880
00881
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
00905
00906 bool BlobData::AreaLessThan::operator() (const Shape<BlobData> &b1, const Shape<BlobData> &b2) const {
00907 return b1->getArea() < b2->getArea();
00908 }
00909
00910 }