Homepage

Demos

Overview

Downloads

Tutorials

Reference

Credits

SphereData.cc Go to the documentation of this file. #include <iostream> #include <vector> #include <list> #include <math.h> #include "Motion/Kinematics.h" // for kine #include "SketchSpace.h" #include "Sketch.h" #include "ShapeRoot.h" #include "LineData.h" #include "Region.h" #include "visops.h" #include "SphereData.h" #include "ShapeSphere.h" namespace DualCoding { SphereData::SphereData(ShapeSpace& _space, const Point &c) : BaseData(_space,sphereDataType), centroid(c), radius(0) { mobile = SPHERE_DATA_MOBILE; } SphereData::SphereData(const SphereData& otherData) : BaseData(otherData),centroid(otherData.centroid),radius(otherData.radius) { mobile = otherData.mobile; } DATASTUFF_CC(SphereData); bool SphereData::isMatchFor(const ShapeRoot& other) const { if (!(isSameTypeAs(other) && isSameColorAs(other))) return false; const Shape<SphereData>& other_sphere = ShapeRootTypeConst(other,SphereData); float dist = centroid.distanceFrom(other_sphere->centerPt()); return dist < 2*max(radius,other_sphere->radius); // *** DST hack } void SphereData::mergeWith(const ShapeRoot& other) { const Shape<SphereData>& other_sphere = ShapeRootTypeConst(other,SphereData); if (other_sphere->confidence <= 0) return; const int other_conf = other_sphere->confidence; confidence += other_conf; centroid = (centroid*confidence + other_sphere->centerPt()*other_conf) / (confidence+other_conf); radius = (radius*confidence + other_sphere->getRadius()*other_conf) / (confidence+other_conf); } bool SphereData::updateParams(const ShapeRoot& other, bool) { const Shape<SphereData>& other_sphere = *static_cast<const Shape<SphereData>*>(&other); centroid = (centroid*(confidence-1) + other_sphere->getCentroid())/confidence; radius = (radius*(confidence-1) + other_sphere->getRadius())/confidence; return true; } //! Print information about this shape. (Virtual in BaseData.) void SphereData::printParams() const { cout << "Type = " << getTypeName(); cout << "Shape ID = " << getId() << endl; cout << "Parent ID = " << getParentId() << endl; // Print critical points. cout << endl; cout << "center{" << centerPt().coordX() << ", " << centerPt().coordY() << "}" << endl; cout << "radius = " << getRadius() << endl; printf("color = %d %d %d\n",getColor().red,getColor().green,getColor().blue); cout << "mobile = " << isMobile() << endl; cout << "viewable = " << isViewable() << endl; } //! Transformations. (Virtual in BaseData.) void SphereData::applyTransform(const NEWMAT::Matrix& Tmat) { centroid.applyTransform(Tmat); } bool SphereData::isInside(const Point& pt) const { float dist = pt.distanceFrom(centerPt()); return radius>dist; } void SphereData::projectToGround(const NEWMAT::Matrix& camToBase, const NEWMAT::ColumnVector& groundplane) { NEWMAT::ColumnVector cam_pos = (kine->jointToBase(CameraFrameOffset)).SubMatrix(1,4,4,4); cout << "cam position (" << (cam_pos(1)/cam_pos(4)) << "," << (cam_pos(2)/cam_pos(4)) << "," << (cam_pos(3)/cam_pos(4)) << ")" << endl; Point tangent_pt(centroid.coordX(),centroid.coordY()+radius, centroid.coordZ()); // pick a tangent point from cam point. 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 cout << "sphere in cam frame: centroid:" << "(" << centroid.coordX() << "," << centroid.coordY() << "," << centroid.coordZ() << "); tangent_pt:" << "(" << tangent_pt.coordX() << "," << tangent_pt.coordY() << "," << tangent_pt.coordZ() << ")" << endl; centroid.projectToGround(camToBase,groundplane); tangent_pt.projectToGround(camToBase,groundplane); cout << "sphere projected to ground: centroid:" << "(" << centroid.coordX() << "," << centroid.coordY() << "," << centroid.coordZ() << "); tangent_pt:" << "(" << tangent_pt.coordX() << "," << tangent_pt.coordY() << "," << tangent_pt.coordZ() << ")" << endl; LineData tangent_line(getSpace(), cam_pt, tangent_pt); // tangent line from camera to sphere LineData cam_center(getSpace(), cam_pt, centroid); // line from camera passing through center point of sphere // a line perpendicular to tangent_line should cross cam_center line at the center point of the sphere if it // crosses tangent_line at the tangent point. Distance b/w tangent point and center point is the radius of sphere // which should also equal the height of the sphere (coordZ = 1/groundplane(3) + radius) // line from tangent_pt to centroid: z = ax + b (a known, b unkown) // line from camera to centroid: z = cx + d (c,d known) // tangent_line: z = ex + f (e,f known) // tangent_pt: x = (f-b)/(a-e) // centroid: x = (d-b)/(a-c), z = d + c(d-b)/(a-c) = 1/groundplane(3) + radius = (radius above groud level) // solve for b and substitute it to get centroid and radius vector<float> t_abc_xz = tangent_line.lineEquation_abc_xz(); vector<float> cc_abc_xz = cam_center.lineEquation_abc_xz(); vector<float> cc_abc_xy = cam_center.lineEquation_abc(); const float f = cc_abc_xz[2] / cc_abc_xz[1]; const float e = - cc_abc_xz[0] / cc_abc_xz[1]; const float d = t_abc_xz[2] / t_abc_xz[1]; const float c = - t_abc_xz[0] / t_abc_xz[1]; const float a = -1.0 / e; // perpendicular to e const float ground = 1.0/groundplane(3); 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 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); cout << "ground level: " << ground << ", DXtoR: " << DXtoR << endl; cout << "tangent line: z = " << e << " * x + " << f << endl; cout << "perpendicular line: z = " << a << " * x + " << b << endl; cout << "center line: z = " << c << " * x + " << d << endl; cout << "dx b/w tangent pt and center pt: " << ((f-b)/(a-e)-(d-b)/(a-c)) << endl; const float x = (d-b)/(a-c); const float z = d + c*(d-b)/(a-c); const float y = (cc_abc_xy[2]-cc_abc_xy[0]*x) / cc_abc_xy[1]; centroid.setCoords(x,y,z); radius = z-ground; cout << " => (" << x << "," << y << "," << z << "); radius: " << radius << endl; } void SphereData::setRadius(float _radius) { radius = _radius; deleteRendering(); } //} // ================================================== // BEGIN SKETCH MANIPULATION AND LINE EXTRACTION CODE // ================================================== //! Extraction. //{ std::vector<ShapeRoot> SphereData::extractSpheres(const Sketch<bool>& sketch) { const float AREA_TOLERANCE = 0.5; const int REGION_THRESH = 25; NEW_SKETCH_N(labels,usint,visops::oldlabelcc(sketch,visops::EightWayConnect)); list<Region> regionlist = Region::extractRegions(labels,REGION_THRESH); std::vector<ShapeRoot> spheres; if(regionlist.empty()) return spheres; typedef list<Region>::iterator R_IT; for (R_IT it = regionlist.begin(); it != regionlist.end(); ++it) { float ratio = it->findSemiMajorAxisLength()/(float)(it->findSemiMinorAxisLength()); if((ratio < 2.0) && (ratio > 1.0/(float)2.0) && (it->findArea() > M_PI*2.0*(it->findSemiMajorAxisLength()) *2.0*(it->findSemiMinorAxisLength())*AREA_TOLERANCE/4.0)) { Shape<SphereData> temp_sphere(*it); temp_sphere->setParentId(sketch->getViewableId()); temp_sphere->setColor(sketch->getColor()); spheres.push_back(Shape<SphereData>(temp_sphere)); }; } return spheres; } std::vector<ShapeRoot> SphereData::get_spheres(const Sketch<uchar>& cam) { //! Declare all colors as valid. std::vector<bool> Valid_Colors; Valid_Colors.resize(ProjectInterface::getNumColors(),true); return(get_spheres(cam,Valid_Colors)); } std::vector<ShapeRoot> SphereData::get_spheres(const Sketch<uchar>& cam, std::vector<bool>& Valid_Colors) { std::vector<ShapeRoot> spheres_vec; uchar cur_color; uchar num_colors = (uchar)Valid_Colors.size(); char *pmask_name_chr = (char *)malloc(128*sizeof(char)); // Loop through all valid colors. for(cur_color = 0; cur_color < num_colors; cur_color++) { if(Valid_Colors[cur_color] == true) { // Segment color pixels. NEW_SKETCH_N(pmask, bool, cam == cur_color); sprintf(pmask_name_chr, "pmask_%d",cur_color); pmask->setName(pmask_name_chr); // Extract spheres. std::vector<ShapeRoot> spheresList = SphereData::extractSpheres(pmask); int num_spheres = (int)spheresList.size(); int cur_sphere; for(cur_sphere = 0; cur_sphere < num_spheres; cur_sphere++) { spheresList[cur_sphere]->setColor(ProjectInterface::getColorRGB(cur_color)); spheres_vec.push_back(spheresList[cur_sphere]); } }; } return(spheres_vec); } //! Render into a sketch space and return reference. (Private.) Sketch<bool>* SphereData::render() const { const int cx = int(centerPt().getCoords()(1)); const int cy = int(centerPt().getCoords()(2)); /* // Sure the sphere rendering is terribly inefficient, but it works float a = getRadius(); float x_skip = atan(1/(0.5*a)); // minimum x-diff w/o gaps for( float x = (cx-a); x<(cx+a); x+=x_skip) { float y_y0_sq = 1 - (x-cx)*(x-cx); if(y_y0_sq > 0) { int y_bot = cy + (int)(sqrt(y_y0_sq)); int y_top = cy - (int)(sqrt(y_y0_sq)); draw_result((int)x,y_bot) = true; draw_result((int)x,y_top) = true; } } draw_result(cx-(int)a,cy) = true; // fill in "holes" at ends draw_result(cx+(int)a,cy) = true; */ // Fill the sphere. Sketch<bool> result(space->getDualSpace(), "render("+getName()+")"); result = 0; const int rad =(int) floor(getRadius()+0.5); const int radSq = rad*rad + rad/10; // rad/10 added to make sphere look nicer const int minX = (rad > cx) ? 0 : cx-rad; const int maxX = ((unsigned int) (rad+cx) > getSpace().getDualSpace().getWidth()-1) ? getSpace().getDualSpace().getWidth()-1 : cx+rad; for (int x = minX; x <= maxX; x++) { const int yRange = (int) sqrt((float) (radSq-(cx-x)*(cx-x))); const int minY = (yRange > cy) ? 0 : cy-yRange; const int maxY = ((unsigned int) yRange+cy > getSpace().getDualSpace().getHeight()-1) ? getSpace().getDualSpace().getHeight()-1 : cy+yRange; for (int y = minY; y <= maxY; y++) result(x,y) = true; } return new Sketch<bool>(result); } } // namespace

DualCoding 3.0beta

Generated Wed Oct 4 00:01:54 2006 by Doxygen 1.4.7