Tekkotsu Homepage
Demos
Overview
Downloads
Dev. Resources
Reference
Credits

PyramidData.cc

Go to the documentation of this file.
00001 //-*-c++-*-
00002 
00003 #include <iostream>
00004 #include <vector>
00005 
00006 #include "BaseData.h"    // superclass
00007 #include "Point.h"       // Point data member
00008 #include "ShapeTypes.h"  // pyramidDataType
00009 
00010 #include "SketchSpace.h"
00011 #include "Sketch.h"
00012 #include "visops.h"
00013 
00014 #include "ShapeSpace.h"  // required by DATASTUFF_CC
00015 #include "ShapeRoot.h"   // required by DATASTUFF_CC
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     // needs implementation
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     //  ++confidence;
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   //! Print information about this shape. (Virtual in BaseData.)
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   //! Transformations. (Virtual in BaseData.)
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   /*void PyramidData::projectToGround(const NEWMAT::ColumnVector& groundplane) {
00087     FL.projectToGround(groundplane);
00088     FR.projectToGround(groundplane);
00089     BL.projectToGround(groundplane);
00090     BR.projectToGround(groundplane);
00091 
00092     Point bottomCenter = (FL + FR + BL + BR) / 4.0;
00093     float height;
00094     Top.projectToGround(groundplane);
00095     height = Top.getHeightAbovePoint(bottomCenter, groundplane);
00096     Top.setCoords(bottomCenter.coordX(), bottomCenter.coordY(), bottomCenter.coordZ() + height);
00097     centroid = bottomCenter;
00098     centroid.setCoords(centroid.coordX(), centroid.coordY(), centroid.coordZ() + height/5);
00099     std::cout<<"New centroid: ("<<centroid.coordX()<<","<<centroid.coordY()<<","<<centroid.coordZ()<<")\n";
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   // BEGIN SKETCH MANIPULATION AND PYRAMID EXTRACTION CODE
00122   // =====================================================
00123 
00124 
00125   //! Pyramid extraction.
00126 
00127 
00128   //! Render into a sketch space and return reference. (Private.)
00129   Sketch<bool>* PyramidData::render() const {
00130     SketchSpace &renderspace = space->getDualSpace();
00131     //int const width = renderspace.getWidth();
00132     //int const height = renderspace.getHeight();
00133     //float x1,y1,x2,y2;
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   /* Same idea as extractBrick, but simpler
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     // Check inter-blob distance
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     // Extract candidate points
00208     std::vector<std::vector<Point> > points(5);
00209 
00210     /*
00211      *          0
00212      *         /|\ 
00213      *        / | \  
00214      *       /  |  \ 
00215      *      / L | R \ 
00216      *     /    |    \ <--- 4
00217      *    1-----2-----3
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     // Find the corners for each face individually
00230     // blobData::findCorners takes the number of corners you expect, so this 
00231     // uses the same method as extractBrick
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     // find the corners of the right face.
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     // debug output
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   // A bounding triangle with room around it
00331   // contracted in quadrilateral (polygon) fitting
00332   vector<Point> PyramidData::findBoundingTriangle(ShapeSpace& space, Shape<BlobData> blob, 
00333               Point /*centroid*/, 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     // debug output
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 } // namespace

DualCoding 4.0
Generated Thu Nov 22 00:52:36 2007 by Doxygen 1.5.4