00001
00002 #include <iostream>
00003 #include <vector>
00004 #include <list>
00005 #include <math.h>
00006
00007 #include "BaseData.h"
00008 #include "Point.h"
00009 #include "Shared/Measures.h"
00010 #include "ShapeTypes.h"
00011
00012 #include "SketchSpace.h"
00013 #include "Sketch.h"
00014 #include "Region.h"
00015 #include "visops.h"
00016
00017 #include "ShapeSpace.h"
00018 #include "ShapeRoot.h"
00019
00020 #include "EllipseData.h"
00021 #include "ShapeEllipse.h"
00022
00023 using namespace std;
00024
00025 namespace DualCoding {
00026
00027 inline int round(float x) { return (int) ceil((double)x-0.5f); }
00028
00029 EllipseData::EllipseData(ShapeSpace& _space, const Point &c,
00030 const float _semimajor, const float _semiminor, const float _orientation)
00031 : BaseData(_space, getStaticType()),
00032 center_pt(c), semimajor(_semimajor), semiminor(_semiminor), orientation(_orientation)
00033 { center_pt.setRefFrameType(getRefFrameType());
00034 mobile = ELLIPSE_DATA_MOBILE; }
00035
00036 DATASTUFF_CC(EllipseData);
00037
00038 BoundingBox EllipseData::getBoundingBox() const {
00039 float o_sin = sin(orientation);
00040 float o_cos = cos(orientation);
00041 Point major_tip(semimajor*o_cos, semimajor*o_sin);
00042 Point minor_tip(-semiminor*o_sin, semiminor*o_cos);
00043 return BoundingBox(BoundingBox(BoundingBox(center_pt+major_tip),
00044 BoundingBox(center_pt-major_tip)),
00045 BoundingBox(BoundingBox(center_pt+minor_tip),
00046 BoundingBox(center_pt-minor_tip)));
00047 }
00048
00049 bool EllipseData::isMatchFor(const ShapeRoot& other) const {
00050 if (!(isSameTypeAs(other) && isSameColorAs(other)))
00051 return false;
00052 const Shape<EllipseData>& other_ellipse = ShapeRootTypeConst(other,EllipseData);
00053 float dist = center_pt.distanceFrom(other_ellipse->centerPt());
00054 return dist < 2*max(semimajor,other_ellipse->semimajor);
00055 }
00056
00057 bool EllipseData::updateParams(const ShapeRoot& other, bool) {
00058 const Shape<EllipseData>& other_ellipse = ShapeRootTypeConst(other,EllipseData);
00059 if (other_ellipse->confidence <= 0)
00060 return false;
00061 const int other_conf = other_ellipse->confidence;
00062 center_pt = (center_pt*confidence + other_ellipse->centerPt()*other_conf) / (confidence+other_conf);
00063 semimajor = (semimajor*confidence + other_ellipse->getSemimajor()*other_conf) / (confidence+other_conf);
00064 semiminor = (semiminor*confidence + other_ellipse->getSemiminor()*other_conf) / (confidence+other_conf);
00065 orientation = orientation*((orientation_t)confidence/(confidence+other_conf))
00066 + other_ellipse->getOrientation()*((orientation_t)confidence/(confidence+other_conf));
00067 return true;
00068 }
00069
00070
00071 void EllipseData::printParams() const {
00072 cout << "Type = " << getTypeName() << " ID=" << getId() << " ParentID=" << getParentId() << endl;
00073 printf(" color = %d %d %d\n",getColor().red,getColor().green,getColor().blue);
00074 cout << " center = " << centerPt() << endl;
00075 cout << " semimajor = " << getSemimajor()
00076 << ", semiminor = " << getSemiminor()
00077 << ", orientation = " << getOrientation() << endl;
00078 cout << " mobile = " << getMobile() << ", viewable = " << isViewable() << endl;
00079 }
00080
00081 pair<Point,Point> EllipseData::findFeaturePoints() const {
00082 AngPi theta = getOrientation();
00083 NEWMAT::ColumnVector from_center(4);
00084
00085 float dl = getSemimajor();
00086 from_center << dl*cos(theta) << dl*sin(theta) << 0 << 0;
00087 Point majorPt(from_center);
00088 majorPt += center_pt;
00089
00090 dl = getSemiminor();
00091 theta = theta + (AngPi) (M_PI/2);
00092 from_center << dl*sin(theta) << dl*cos(theta) << 0 << 0;
00093 Point minorPt(from_center);
00094 minorPt += center_pt;
00095 return pair<Point,Point>(majorPt,minorPt);
00096 }
00097
00098
00099 void EllipseData::applyTransform(const NEWMAT::Matrix& Tmat, const ReferenceFrameType_t newref) {
00100 pair<Point,Point> featurePts = findFeaturePoints();
00101 center_pt.applyTransform(Tmat,newref);
00102
00103 featurePts.first.applyTransform(Tmat,newref);
00104 featurePts.second.applyTransform(Tmat,newref);
00105 updateProperties(featurePts.first, featurePts.second);
00106 }
00107
00108 void EllipseData::updateProperties(const Point& majorPt, const Point& minorPt) {
00109 setSemiminor(minorPt.xyDistanceFrom(center_pt));
00110 setSemimajor(majorPt.xyDistanceFrom(center_pt));
00111 setOrientation(atan2(majorPt.coords(2)-center_pt.coords(2),majorPt.coords(1)-center_pt.coords(1)));
00112 }
00113
00114 void EllipseData::projectToGround(const NEWMAT::Matrix& camToBase,
00115 const NEWMAT::ColumnVector& groundplane) {
00116 pair<Point,Point> featurePts = findFeaturePoints();
00117 center_pt.projectToGround(camToBase,groundplane);
00118 featurePts.first.projectToGround(camToBase,groundplane);
00119 featurePts.second.projectToGround(camToBase,groundplane);
00120 updateProperties(featurePts.first, featurePts.second);
00121 }
00122
00123
00124
00125 void EllipseData::setOrientation(const AngPi _orientation) {
00126 orientation = AngPi(_orientation);
00127 deleteRendering();
00128 }
00129
00130 void EllipseData::setSemimajor(float _semimajor) {
00131 semimajor = _semimajor;
00132 deleteRendering();
00133 }
00134
00135 void EllipseData::setSemiminor(float _semiminor) {
00136 semiminor = _semiminor;
00137 deleteRendering();
00138 }
00139
00140
00141
00142
00143
00144
00145
00146
00147
00148
00149 std::vector<Shape<EllipseData> > EllipseData::extractEllipses(const Sketch<bool>& sketch)
00150 {
00151 const float AREA_TOLERANCE = 0.5;
00152 const int REGION_THRESH = 25;
00153 NEW_SKETCH_N(labels,uint,visops::oldlabelcc(sketch,visops::EightWayConnect));
00154 list<Region> regionlist = Region::extractRegions(labels,REGION_THRESH);
00155 std::vector<Shape<EllipseData> > ellipses;
00156
00157 if(regionlist.empty())
00158 return ellipses;
00159
00160 typedef list<Region>::iterator R_IT;
00161 for (R_IT it = regionlist.begin(); it != regionlist.end(); ++it) {
00162 float ratio = it->findSemiMajorAxisLength()/(float)(it->findSemiMinorAxisLength());
00163 if((ratio < 2.0) && (ratio > 1.0/(float)2.0)
00164 && (it->findArea() > M_PI*2.0*(it->findSemiMajorAxisLength())
00165 *2.0*(it->findSemiMinorAxisLength())*AREA_TOLERANCE/4.0)) {
00166 Shape<EllipseData> temp_ellipse(*it);
00167 temp_ellipse->setParentId(sketch->getViewableId());
00168 temp_ellipse->setColor(sketch->getColor());
00169 ellipses.push_back(Shape<EllipseData>(temp_ellipse));
00170 };
00171 }
00172 return ellipses;
00173 }
00174
00175
00176
00177 Sketch<bool>* EllipseData::render() const {
00178 SketchSpace &SkS = space->getDualSpace();
00179 NEWMAT::ColumnVector ctr(centerPt().getCoords());
00180 SkS.applyTmat(ctr);
00181 const float &cx = ctr(1);
00182 const float &cy = ctr(2);
00183 const NEWMAT::Matrix &Tmat = SkS.getTmat();
00184 NEWMAT::ColumnVector ori(2);
00185 ori << cos(orientation) << sin(orientation);
00186 NEWMAT::Matrix rot(2,2);
00187 rot(1,1) = Tmat(1,1);
00188 rot(1,2) = Tmat(1,2);
00189 rot(2,1) = Tmat(2,1);
00190 rot(2,2) = Tmat(2,2);
00191 ori = rot * ori;
00192 const float &cosT = ori(1);
00193 const float &sinT = ori(2);
00194 const float xRange = semimajor / Tmat(4,4);
00195 const float majorSq = xRange*xRange;
00196 const float mnrDevMjr = semiminor/semimajor;
00197 Sketch<bool> result(SkS, "render("+getName()+")");
00198 result = 0;
00199 for (float xDist = -xRange; xDist <= xRange; xDist+=0.2) {
00200 const float yRange = sqrt(max((float)0, majorSq - xDist*xDist)) * mnrDevMjr;
00201 for (float yDist = -yRange; yDist <= yRange; yDist+=0.2) {
00202 int const px = round(cx+xDist*cosT-yDist*sinT);
00203 int const py = round(cy+yDist*cosT+xDist*sinT);
00204 if ( px >= 0 && px < result.width &&
00205 py >= 0 && py < result.height )
00206 result(px,py) = true;
00207 }
00208 }
00209 return new Sketch<bool>(result);
00210 }
00211
00212 }