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 "PyramidData.h"
00018 #include "ShapePyramid.h"
00019 #include "ShapePoint.h"
00020 #include "Region.h"
00021
00022 using namespace std;
00023
00024 namespace DualCoding {
00025
00026 PyramidData::PyramidData(ShapeSpace& _space,
00027 const EndPoint &_FL, const EndPoint &_FR,
00028 const EndPoint &_BL, const EndPoint &_BR,
00029 const EndPoint &_Top)
00030 : BaseData(_space,pyramidDataType),
00031 FL(_FL), FR(_FR), BL(_BL), BR(_BR), Top(_Top),
00032 centroid((FL + FR + BL + BR + Top) / 5)
00033 {
00034 }
00035
00036
00037 DATASTUFF_CC(PyramidData);
00038
00039 bool PyramidData::isMatchFor(const ShapeRoot& other) const {
00040 if (!(isSameTypeAs(other) && isSameColorAs(other)))
00041 return false;
00042
00043
00044 float dist = 0;
00045 return dist < 20;
00046 }
00047
00048 void PyramidData::mergeWith(const ShapeRoot& other) {
00049 const Shape<PyramidData>& other_pyramid = ShapeRootTypeConst(other,PyramidData);
00050 if (other_pyramid->confidence <= 0)
00051 return;
00052 }
00053
00054 bool PyramidData::updateParams(const ShapeRoot& other, bool) {
00055 const Shape<PyramidData>& other_pyramid = *static_cast<const Shape<PyramidData>*>(&other);
00056
00057 FL = (FL*(confidence-1) + other_pyramid->getFL())/confidence;
00058 FR = (FR*(confidence-1) + other_pyramid->getFR())/confidence;
00059 BL = (BL*(confidence-1) + other_pyramid->getBL())/confidence;
00060 BR = (BR*(confidence-1) + other_pyramid->getBR())/confidence;
00061 Top = (Top*(confidence-1) + other_pyramid->getTop())/confidence;
00062 deleteRendering();
00063 return true;
00064 }
00065
00066
00067 void PyramidData::printParams() const {
00068 cout << "Type = " << getTypeName();
00069 cout << "Shape ID = " << getId() << endl;
00070 cout << "Parent ID = " << getParentId() << endl;
00071
00072 printf("color = %d %d %d\n",getColor().red,getColor().green,getColor().blue);
00073 cout << "viewable = " << isViewable() << endl;
00074 }
00075
00076
00077
00078 void PyramidData::applyTransform(const NEWMAT::Matrix& Tmat, const ReferenceFrameType_t newref) {
00079 FL.applyTransform(Tmat,newref);
00080 FR.applyTransform(Tmat,newref);
00081 BL.applyTransform(Tmat,newref);
00082 BR.applyTransform(Tmat,newref);
00083 Top.applyTransform(Tmat,newref);
00084 }
00085
00086
00087
00088
00089
00090
00091
00092
00093
00094
00095
00096
00097
00098
00099
00100
00101
00102 void PyramidData::projectToGround(const NEWMAT::Matrix & camToBase,
00103 const NEWMAT::ColumnVector& groundplane) {
00104 FL.projectToGround(camToBase, groundplane);
00105 FR.projectToGround(camToBase, groundplane);
00106 BL.projectToGround(camToBase, groundplane);
00107 BR.projectToGround(camToBase, groundplane);
00108
00109 Point bottomCenter = (FL + FR + BL + BR) / 4.0;
00110 bottomCenter.setRefFrameType(egocentric);
00111 float height;
00112 Top.projectToGround(camToBase, groundplane);
00113 height = Top.getHeightAbovePoint(bottomCenter, groundplane);
00114 Top.setCoords(bottomCenter.coordX(), bottomCenter.coordY(), bottomCenter.coordZ() + height);
00115 centroid = bottomCenter;
00116 centroid.setCoords(centroid.coordX(), centroid.coordY(), centroid.coordZ() + height/5);
00117 std::cout << "New centroid: " << centroid << endl;
00118 }
00119
00120
00121
00122
00123
00124
00125
00126
00127
00128
00129 Sketch<bool>* PyramidData::render() const {
00130 SketchSpace &renderspace = space->getDualSpace();
00131
00132
00133
00134 Sketch<bool>* draw_result =
00135 new Sketch<bool>(renderspace, "render("+getName()+")");
00136 (*draw_result)->setParentId(getViewableId());
00137 (*draw_result)->setColor(getColor());
00138 *draw_result = 0;
00139 LineData F(*space, FL, FR);
00140 *draw_result = *draw_result & F.getRendering();
00141 LineData L(*space, FL, BL);
00142 *draw_result = *draw_result & L.getRendering();
00143 LineData B(*space, BL, BR);
00144 *draw_result = *draw_result & B.getRendering();
00145 LineData R(*space, BR, FR);
00146 *draw_result = *draw_result & R.getRendering();
00147 LineData FT(*space, FL, Top);
00148 *draw_result = *draw_result & FT.getRendering();
00149 LineData LT(*space, FR, Top);
00150 *draw_result = *draw_result & LT.getRendering();
00151 LineData BT(*space, BL, Top);
00152 *draw_result = *draw_result & BT.getRendering();
00153 LineData RT(*space, BR, Top);
00154 *draw_result = *draw_result & RT.getRendering();
00155
00156 return draw_result;
00157 }
00158
00159
00160
00161
00162
00163
00164 Shape<PyramidData> PyramidData::extractPyramid(ShapeSpace& space, vector<Shape<BlobData> > &blobs)
00165 {
00166 unsigned int nblobs = blobs.size();
00167 std::vector<Point> centroids;
00168
00169 ShapeRoot invalid;
00170 if (nblobs != 2) {
00171 return ShapeRootType(invalid,PyramidData);
00172 }
00173
00174 for (unsigned int i=0; i<nblobs; i++) {
00175 centroids.push_back(blobs[i]->getCentroid());
00176 }
00177
00178
00179 if (centroids[0].distanceFrom(centroids[1]) >=
00180 blobs[0]->topLeft.distanceFrom(centroids[0]) +
00181 blobs[1]->topLeft.distanceFrom(centroids[1])){
00182 return ShapeRootType(invalid,PyramidData);
00183 }
00184
00185 int left=-1, right=-1;
00186
00187 if (centroids[0].isLeftOf(centroids[1], camcentric)) {
00188 left = 0;
00189 right = 1;
00190 }
00191 else {
00192 left = 1;
00193 right = 0;
00194 }
00195
00196 const int INTERSECT_DIST = 5, MIN_REQUIRED_DIST = 2;
00197 NEW_SKETCH_N(leftEdist, uchar, visops::edist(blobs[left]->getRendering()));
00198 NEW_SKETCH_N(rightEdist, uchar, visops::edist(blobs[right]->getRendering()));
00199 NEW_SKETCH(small, bool, leftEdist<MIN_REQUIRED_DIST & rightEdist<MIN_REQUIRED_DIST);
00200 if (!small->max()){
00201 return ShapeRootType(invalid,PyramidData);
00202 }
00203 NEW_SKETCH(boundary, bool, leftEdist<INTERSECT_DIST & rightEdist<INTERSECT_DIST);
00204 rgb green_rgb(0, 255, 0);
00205 boundary->setColor(green_rgb);
00206
00207
00208 std::vector<std::vector<Point> > points(5);
00209
00210
00211
00212
00213
00214
00215
00216
00217
00218
00219
00220
00221 NEW_SHAPE(boundaryline, LineData, LineData::extractLine(boundary));
00222
00223 if (boundaryline.isValid()) {
00224 points[0].push_back(boundaryline->topPt());
00225 points[2].push_back(boundaryline->bottomPt());
00226 }
00227
00228
00229
00230
00231
00232
00233 std::vector<Point> candidates;
00234
00235 candidates = findBoundingTriangle(space, blobs[left],
00236 centroids[left], boundaryline);
00237
00238 float cornerValue;
00239
00240 std::vector<Point> lcorners = blobs[left]->findCorners(3, candidates, cornerValue);
00241
00242 if (lcorners.size() == 3) {
00243
00244 unsigned int leftCorner = 0;
00245 for (unsigned int i=1; i<3; i++){
00246 if (lcorners[i].isLeftOf(lcorners[leftCorner], camcentric)) {
00247 leftCorner = i;
00248 }
00249 }
00250
00251 points[1].push_back(lcorners[leftCorner]);
00252 points[2].push_back(lcorners[(leftCorner+1)%3]);
00253 points[0].push_back(lcorners[(leftCorner+2)%3]);
00254
00255 }
00256 else {
00257 return ShapeRootType(invalid,PyramidData);
00258 }
00259
00260
00261
00262
00263 candidates.clear();
00264 candidates = findBoundingTriangle(space, blobs[right],
00265 centroids[right], boundaryline);
00266
00267 std::vector<Point> rcorners = blobs[right]->findCorners(3, candidates, cornerValue);
00268
00269 if (rcorners.size() == 3) {
00270
00271 unsigned int rightCorner = 0;
00272 for (unsigned int i=1; i<3; i++){
00273 if (rcorners[i].isRightOf(rcorners[rightCorner], camcentric)) {
00274 rightCorner = i;
00275 }
00276 }
00277
00278 points[3].push_back(rcorners[rightCorner]);
00279 points[0].push_back(rcorners[(rightCorner+1)%3]);
00280 points[2].push_back(rcorners[(rightCorner+2)%3]);
00281
00282 }
00283 else {
00284 return ShapeRootType(invalid,PyramidData);
00285 }
00286
00287
00288 for (unsigned int i=0; i<5; i++){
00289 for (unsigned int j=0; j<points[i].size(); j++) {
00290 NEW_SHAPE(corner, PointData, PointData(space, points[i][j]));
00291 char name[10];
00292 sprintf(name, "corner%d%d",i,j);
00293 corner->setName(name);
00294 }
00295 }
00296
00297
00298 vector<Point> finalCorners(5);
00299
00300 Point empty(0,0);
00301
00302 for (unsigned int i=0; i<5; i++) {
00303 finalCorners[i] = empty;
00304 for (unsigned int j=0; j<points[i].size(); j++) {
00305 finalCorners[i]+=points[i][j];
00306 }
00307 if (points[i].size() > 0) {
00308 finalCorners[i]/=points[i].size();
00309 }
00310 }
00311
00312
00313
00314
00315 finalCorners[4] = finalCorners[1] + finalCorners[3] - finalCorners[2];
00316
00317
00318 NEW_SHAPE(returnPyramid, PyramidData, new PyramidData(space,
00319 finalCorners[1],
00320 finalCorners[2],
00321 finalCorners[4],
00322 finalCorners[3],
00323 finalCorners[0]));
00324
00325 return returnPyramid;
00326 }
00327
00328
00329
00330
00331
00332 vector<Point> PyramidData::findBoundingTriangle(ShapeSpace& space, Shape<BlobData> blob,
00333 Point , Shape<LineData> parallel)
00334 {
00335
00336 const float PARALLEL_EXTEND_FACTOR = .4, ORTHO_EXTEND_FACTOR = .4;
00337
00338 vector<Point> candidates;
00339
00340 Point thirdPoint = (Region::extractRegion(blob->getRendering())).mostDistantPtFrom(parallel.getData());
00341
00342 LineData perpendicular(space, thirdPoint, parallel->getThetaNorm());
00343
00344 Point isect(perpendicular.intersectionWithLine(parallel));
00345
00346 Point orthoExtend((thirdPoint - isect)*ORTHO_EXTEND_FACTOR);
00347 Point parallelExtend((parallel->end2Pt() - parallel->end1Pt())*PARALLEL_EXTEND_FACTOR);
00348
00349 Point pt1(parallel->end1Pt() - parallelExtend - orthoExtend);
00350 Point pt2(parallel->end2Pt() + parallelExtend - orthoExtend);
00351
00352 if (pt1.isAbove(pt2, camcentric) && thirdPoint.isLeftOf(pt1, camcentric) ||
00353 !pt1.isAbove(pt2, camcentric) && !thirdPoint.isLeftOf(pt1, camcentric)){
00354 candidates.push_back(pt2);
00355 candidates.push_back(pt1);
00356 }
00357 else {
00358 candidates.push_back(pt1);
00359 candidates.push_back(pt2);
00360 }
00361 candidates.push_back(thirdPoint + orthoExtend);
00362
00363
00364 NEW_SHAPE(candidate31, PointData, PointData(space, candidates[0]));
00365 NEW_SHAPE(candidate32, PointData, PointData(space, candidates[1]));
00366 NEW_SHAPE(candidate33, PointData, PointData(space, candidates[2]));
00367 candidate31->setParentId(blob->getViewableId());
00368 candidate32->setParentId(blob->getViewableId());
00369 candidate33->setParentId(blob->getViewableId());
00370
00371 return candidates;
00372 }
00373
00374
00375 }