00001 #include <iostream>
00002 #include <vector>
00003 #include <list>
00004 #include <math.h>
00005
00006 #include "Motion/Kinematics.h"
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);
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
00059 void SphereData::printParams() const {
00060 cout << "Type = " << getTypeName();
00061 cout << "Shape ID = " << getId() << endl;
00062 cout << "Parent ID = " << getParentId() << endl;
00063
00064
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
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
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());
00097 Point cam_pt(cam_pos(1)/cam_pos(4), cam_pos(2)/cam_pos(4), cam_pos(3)/cam_pos(4));
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);
00111 LineData cam_center(getSpace(), cam_pt, centroid);
00112
00113
00114
00115
00116
00117
00118
00119
00120
00121
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;
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]));
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
00162
00163
00164
00165
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
00187
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
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
00209 for(cur_color = 0; cur_color < num_colors; cur_color++) {
00210
00211 if(Valid_Colors[cur_color] == true) {
00212
00213
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
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
00226 spheres_vec.push_back(spheresList[cur_sphere]);
00227 }
00228
00229 };
00230 }
00231 return(spheres_vec);
00232 }
00233
00234
00235
00236 Sketch<bool>* SphereData::render() const {
00237 const int cx = int(centerPt().getCoords()(1));
00238 const int cy = int(centerPt().getCoords()(2));
00239
00240
00241
00242
00243
00244
00245
00246
00247
00248
00249
00250
00251
00252
00253
00254
00255
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;
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 }