Tekkotsu Homepage
Demos
Overview
Downloads
Dev. Resources
Reference
Credits

SphereData.cc

Go to the documentation of this file.
00001 #include <iostream>
00002 #include <vector>
00003 #include <list>
00004 #include <math.h>
00005 
00006 #include "Motion/Kinematics.h"  // for kine
00007 
00008 #include "SketchSpace.h"
00009 #include "Sketch.h"
00010 #include "ShapeRoot.h"
00011 #include "LineData.h"
00012 #include "Region.h"
00013 #include "visops.h"
00014 
00015 #include "SphereData.h"
00016 #include "ShapeSphere.h"
00017 
00018 using namespace std;
00019 
00020 namespace DualCoding {
00021 
00022 SphereData::SphereData(ShapeSpace& _space, const Point &c) 
00023   : BaseData(_space,sphereDataType),
00024     centroid(c), radius(0)
00025   { mobile = SPHERE_DATA_MOBILE; }
00026   
00027 SphereData::SphereData(const SphereData& otherData)
00028   : BaseData(otherData),centroid(otherData.centroid),radius(otherData.radius) 
00029   { mobile = otherData.mobile; }
00030   
00031 DATASTUFF_CC(SphereData);
00032 
00033 bool SphereData::isMatchFor(const ShapeRoot& other) const {
00034   if (!(isSameTypeAs(other) && isSameColorAs(other)))
00035     return false;
00036   const Shape<SphereData>& other_sphere = ShapeRootTypeConst(other,SphereData);
00037   float dist = centroid.distanceFrom(other_sphere->centerPt());
00038   return dist < 2*max(radius,other_sphere->radius); // *** DST hack
00039 }
00040 
00041 void SphereData::mergeWith(const ShapeRoot& other) {
00042   const Shape<SphereData>& other_sphere = ShapeRootTypeConst(other,SphereData);
00043   if (other_sphere->confidence <= 0)
00044     return;
00045   const int other_conf = other_sphere->confidence;
00046   confidence += other_conf;
00047   centroid = (centroid*confidence + other_sphere->centerPt()*other_conf) / (confidence+other_conf);
00048   radius = (radius*confidence + other_sphere->getRadius()*other_conf) / (confidence+other_conf);
00049 }
00050 
00051 bool SphereData::updateParams(const ShapeRoot& other, bool) {
00052   const Shape<SphereData>& other_sphere = *static_cast<const Shape<SphereData>*>(&other);
00053   centroid = (centroid*(confidence-1) + other_sphere->getCentroid())/confidence;
00054   radius = (radius*(confidence-1) + other_sphere->getRadius())/confidence;
00055   return true;
00056 }
00057 
00058 //! Print information about this shape. (Virtual in BaseData.)
00059 void SphereData::printParams() const {
00060   cout << "Type = " << getTypeName();
00061   cout << "Shape ID = " << getId() << endl;
00062   cout << "Parent ID = " << getParentId() << endl;
00063   
00064   // Print critical points.
00065   cout << endl;
00066   cout << "center{" << centerPt().coordX() << ", " << centerPt().coordY() << "}" << endl;
00067   
00068   cout << "radius = " << getRadius() << endl;
00069   printf("color = %d %d %d\n",getColor().red,getColor().green,getColor().blue);
00070   cout << "mobile = " << getMobile() << endl;
00071   cout << "viewable = " << isViewable() << endl;
00072 }
00073 
00074 
00075 //! Transformations. (Virtual in BaseData.)
00076 void SphereData::applyTransform(const NEWMAT::Matrix& Tmat, const ReferenceFrameType_t newref) {
00077   centroid.applyTransform(Tmat,newref);
00078 }
00079 
00080 bool SphereData::isInside(const Point& pt) const {
00081   float dist = pt.distanceFrom(centerPt());
00082   return radius>dist;
00083 }
00084 
00085 
00086 void SphereData::projectToGround(const NEWMAT::Matrix& camToBase,
00087          const NEWMAT::ColumnVector& groundplane) {
00088 #ifdef TGT_HAS_CAMERA
00089   NEWMAT::ColumnVector cam_pos = (kine->jointToBase(CameraFrameOffset)).SubMatrix(1,4,4,4);
00090 #else
00091   // shouldn't we do it this way regardless of TGT_HAS_CAMERA?
00092   NEWMAT::ColumnVector cam_pos = camToBase.SubMatrix(1,4,4,4);
00093 #endif
00094   cout << "cam position (" << (cam_pos(1)/cam_pos(4)) << ","
00095        << (cam_pos(2)/cam_pos(4)) << "," << (cam_pos(3)/cam_pos(4)) << ")" << endl;
00096   Point tangent_pt(centroid.coordX(),centroid.coordY()+radius, centroid.coordZ()); // pick a tangent point from cam point.
00097   Point cam_pt(cam_pos(1)/cam_pos(4), cam_pos(2)/cam_pos(4), cam_pos(3)/cam_pos(4)); // position of camera w.r.t. base
00098   cout << "sphere in cam frame: centroid:" << "(" << centroid.coordX() 
00099        << "," << centroid.coordY() << "," << centroid.coordZ() << ");  tangent_pt:" 
00100        << "(" << tangent_pt.coordX() << "," << tangent_pt.coordY() << "," << tangent_pt.coordZ()
00101        << ")" << endl;
00102 
00103   centroid.projectToGround(camToBase,groundplane);
00104   tangent_pt.projectToGround(camToBase,groundplane);
00105   cout << "sphere projected to ground: centroid:" << "(" << centroid.coordX() 
00106        << "," << centroid.coordY() << "," << centroid.coordZ() << ");  tangent_pt:" 
00107        << "(" << tangent_pt.coordX() << "," << tangent_pt.coordY() << "," << tangent_pt.coordZ()
00108        << ")" << endl;
00109 
00110   LineData tangent_line(getSpace(), cam_pt, tangent_pt); // tangent line from camera to sphere
00111   LineData cam_center(getSpace(), cam_pt, centroid); // line from camera passing through center point of sphere
00112 
00113   // a line perpendicular to tangent_line should cross cam_center line at the center point of the sphere if it
00114   // crosses tangent_line at the tangent point. Distance b/w tangent point and center point is the radius of sphere
00115   // which should also equal the height of the sphere (coordZ = 1/groundplane(3) + radius)
00116   // line from tangent_pt to centroid: z = ax + b (a known, b unkown)
00117   // line from camera to centroid: z = cx + d (c,d known)
00118   // tangent_line: z = ex + f (e,f known)
00119   // tangent_pt: x = (f-b)/(a-e)
00120   // centroid: x = (d-b)/(a-c), z = d + c(d-b)/(a-c) = 1/groundplane(3) + radius = (radius above groud level)
00121   // solve for b and substitute it to get centroid and radius
00122 
00123   vector<float> t_abc_xz = tangent_line.lineEquation_abc_xz();
00124   vector<float> cc_abc_xz = cam_center.lineEquation_abc_xz();
00125   vector<float> cc_abc_xy = cam_center.lineEquation_abc();
00126 
00127   const float f = cc_abc_xz[2] / cc_abc_xz[1];
00128   const float e = - cc_abc_xz[0] / cc_abc_xz[1];
00129   const float d = t_abc_xz[2] / t_abc_xz[1];
00130   const float c = - t_abc_xz[0] / t_abc_xz[1];
00131   const float a = -1.0 / e; // perpendicular to e
00132   const float ground = 1.0/groundplane(3);
00133   const float DXtoR = 1/ cos(atan(a)) / cos(atan(-cc_abc_xy[0]/cc_abc_xy[1])); // radius = dx * DXtoR where dx is b/w center pt and tangent pt
00134   const float b = (-DXtoR*f*a+DXtoR*f*c+DXtoR*d*a-DXtoR*d*e+ground*a*a-ground*a*c-ground*e*a+ground*e*c-d*a*a+d*e*a)/(-a*c+e*c+DXtoR*c-DXtoR*e);
00135 
00136   cout << "ground level: " << ground << ", DXtoR: " << DXtoR << endl;
00137   cout << "tangent line: z = " << e << " * x + " << f << endl;
00138   cout << "perpendicular line: z = " << a << " * x + " << b << endl;
00139   cout << "center line: z = " << c << " * x + " << d << endl;
00140   cout << "dx b/w tangent pt and center pt: " << ((f-b)/(a-e)-(d-b)/(a-c)) << endl;
00141 
00142   const float x = (d-b)/(a-c);
00143   const float z = d + c*(d-b)/(a-c);
00144   const float y = (cc_abc_xy[2]-cc_abc_xy[0]*x) / cc_abc_xy[1];
00145 
00146   centroid.setCoords(x,y,z);
00147   centroid.setRefFrameType(egocentric);
00148   radius = z-ground;
00149 
00150   cout << " => (" << x << "," << y << "," << z << ");  radius: " << radius << endl;
00151 }
00152 
00153 void SphereData::setRadius(float _radius) {
00154   radius = _radius;
00155   deleteRendering();
00156 }
00157 //}
00158 
00159 
00160 // ==================================================
00161 // BEGIN SKETCH MANIPULATION AND LINE EXTRACTION CODE
00162 // ==================================================
00163 
00164 
00165 //! Extraction.
00166 //{
00167 std::vector<Shape<SphereData> > SphereData::extractSpheres(const Sketch<bool>& sketch)
00168 {
00169   const float AREA_TOLERANCE = 0.5;
00170   const int REGION_THRESH = 25;
00171   NEW_SKETCH_N(labels,uint,visops::oldlabelcc(sketch,visops::EightWayConnect));
00172   list<Region> regionlist = Region::extractRegions(labels,REGION_THRESH);
00173   std::vector<Shape<SphereData> > spheres;
00174   
00175   if(regionlist.empty())
00176     return spheres;
00177   
00178   typedef list<Region>::iterator R_IT;
00179   for (R_IT it = regionlist.begin(); it != regionlist.end(); ++it) {
00180     float ratio = it->findSemiMajorAxisLength()/(float)(it->findSemiMinorAxisLength());
00181     if((ratio < 2.0) && (ratio > 1.0/(float)2.0)
00182        && (it->findArea() > M_PI*2.0*(it->findSemiMajorAxisLength())
00183      *2.0*(it->findSemiMinorAxisLength())*AREA_TOLERANCE/4.0)) {
00184       Shape<SphereData> temp_sphere(*it);
00185       temp_sphere->inheritFrom(*sketch.operator->());
00186       //   temp_sphere->setParentId(sketch->getViewableId());
00187       // temp_sphere->setColor(sketch->getColor());
00188       spheres.push_back(Shape<SphereData>(temp_sphere));
00189     };
00190   }
00191   return spheres;
00192 }
00193 
00194 std::vector<Shape<SphereData> > SphereData::get_spheres(const Sketch<CMVision::uchar>& cam) {
00195   //! Declare all colors as valid.
00196   std::vector<bool> Valid_Colors;
00197   Valid_Colors.resize(ProjectInterface::getNumColors(),true);
00198   return(get_spheres(cam,Valid_Colors));
00199 }
00200 
00201 std::vector<Shape<SphereData> > SphereData::get_spheres(const Sketch<CMVision::uchar>& cam,
00202              std::vector<bool>& Valid_Colors) {
00203   std::vector<Shape<SphereData> > spheres_vec;
00204   uchar cur_color;
00205   uchar num_colors = (uchar)Valid_Colors.size();
00206   char *pmask_name_chr = (char *)malloc(128*sizeof(char));
00207   
00208   // Loop through all valid colors.
00209   for(cur_color = 0; cur_color < num_colors; cur_color++) {
00210     
00211     if(Valid_Colors[cur_color] == true) {
00212       
00213       // Segment color pixels.
00214       NEW_SKETCH_N(pmask, bool, visops::colormask(cam,cur_color));
00215       sprintf(pmask_name_chr, "pmask_%d",cur_color);
00216       pmask->setName(pmask_name_chr);
00217       
00218       // Extract spheres.
00219       std::vector<Shape<SphereData> > spheresList = SphereData::extractSpheres(pmask);
00220       
00221       int num_spheres = (int)spheresList.size();
00222       int cur_sphere;
00223       
00224       for(cur_sphere = 0; cur_sphere < num_spheres; cur_sphere++) {
00225   //  spheresList[cur_sphere]->setColor(ProjectInterface::getColorRGB(cur_color));
00226   spheres_vec.push_back(spheresList[cur_sphere]); 
00227       }
00228       
00229     };
00230   }
00231   return(spheres_vec);
00232 }
00233 
00234 
00235 //! Render into a sketch space and return reference. (Private.)
00236 Sketch<bool>* SphereData::render() const {
00237   const int cx = int(centerPt().getCoords()(1));
00238   const int cy = int(centerPt().getCoords()(2));
00239   /*  
00240   // Sure the sphere rendering is terribly inefficient, but it works
00241   float a = getRadius();
00242   float x_skip = atan(1/(0.5*a)); // minimum x-diff w/o gaps 
00243   for( float x = (cx-a); x<(cx+a); x+=x_skip) {
00244     float y_y0_sq = 1 - (x-cx)*(x-cx);
00245     if(y_y0_sq > 0) {
00246       int y_bot = cy + (int)(sqrt(y_y0_sq));
00247       int y_top = cy - (int)(sqrt(y_y0_sq));
00248       draw_result((int)x,y_bot) = true;
00249       draw_result((int)x,y_top) = true;
00250     }
00251   }
00252   draw_result(cx-(int)a,cy) = true; // fill in "holes" at ends
00253   draw_result(cx+(int)a,cy) = true;
00254   */  
00255   // Fill the sphere.
00256 
00257   Sketch<bool> result(space->getDualSpace(), "render("+getName()+")");
00258   result = 0;
00259   const int rad =(int) floor(getRadius()+0.5);
00260   const int radSq = rad*rad + rad/10; // rad/10 added to make sphere look nicer
00261   const int minX = (rad > cx) ? 0 : cx-rad;
00262   const int maxX = ((unsigned int) (rad+cx) > getSpace().getDualSpace().getWidth()-1)
00263     ? getSpace().getDualSpace().getWidth()-1 : cx+rad;
00264   for (int x = minX; x <= maxX; x++) {
00265     const int yRange = (int) sqrt((float) (radSq-(cx-x)*(cx-x))); 
00266     const int minY = (yRange > cy) ? 0 : cy-yRange;
00267     const int maxY = ((unsigned int) yRange+cy > getSpace().getDualSpace().getHeight()-1)
00268       ? getSpace().getDualSpace().getHeight()-1 : cy+yRange;
00269     for (int y = minY; y <= maxY; y++)
00270       result(x,y) = true;
00271   }
00272   return new Sketch<bool>(result);
00273 }
00274 
00275 
00276 } // namespace

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