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