00001
00002 #include "ShapeLine.h"
00003 #include "ShapeRoot.h"
00004 #include "ShapeSpace.h"
00005 #include "Sketch.h"
00006 #include "SketchSpace.h"
00007 #include "visops.h"
00008
00009 #include "PolygonData.h"
00010 #include "ShapePolygon.h"
00011
00012 using namespace std;
00013
00014 namespace DualCoding {
00015
00016 DATASTUFF_CC(PolygonData);
00017
00018 PolygonData::PolygonData(const LineData& side)
00019 : BaseData(*side.space,polygonDataType), edges(), vertices()
00020 {
00021 edges.push_back(side);
00022 updateVertices();
00023 mobile = POLYGON_DATA_MOBILE;
00024 }
00025
00026 PolygonData::PolygonData(ShapeSpace& _space, const vector<Point>& pts,
00027 bool closed, bool end1Valid, bool end2Valid)
00028 : BaseData(_space, polygonDataType), edges(), vertices(pts.size()>1 ? pts : vector<Point>())
00029 {
00030 mobile = POLYGON_DATA_MOBILE;
00031 if (pts.empty()) return;
00032 for(vector<Point>::const_iterator vtx_it = pts.begin();
00033 vtx_it < pts.end()-1; vtx_it++)
00034 edges.push_back(LineData(_space, *vtx_it, *(vtx_it+1)));
00035 edges.front().end1Pt().setValid(end1Valid);
00036 edges.back().end2Pt().setValid(end2Valid);
00037 if (closed)
00038 tryClosePolygon();
00039 }
00040
00041
00042 vector<Shape<LineData> > PolygonData::extractPolygonEdges(Sketch<bool> const& sketch,
00043 Sketch<bool> const& occluder) {
00044 vector<Shape<LineData> > lines(LineData::extractLines(sketch, occluder,4));
00045 return lines;
00046 }
00047
00048 void PolygonData::updateVertices() {
00049 vertices.clear();
00050 if (edges.empty()) return;
00051
00052 if (isClosed())
00053 vertices.push_back(end1Ln().intersectionWithLine(end2Ln()));
00054 else
00055 vertices.push_back(end1Pt());
00056 if (edges.size() > 1)
00057 for (vector<LineData>::const_iterator it = edges.begin();
00058 it != edges.end()-1; it++)
00059 vertices.push_back(it->intersectionWithLine(*(it+1)));
00060 if (isClosed())
00061 vertices.push_back(vertices.front());
00062 else
00063 vertices.push_back(end2Pt());
00064 }
00065
00066 vector<ShapeRoot> PolygonData::formPolygons(const vector<LineData>& lines) {
00067 if (lines.empty()) return vector<ShapeRoot>();
00068
00069 vector<LineData> allEdges = lines;
00070 sort(allEdges.begin(),allEdges.end(), ptr_fun(isFirstLineLonger));
00071
00072 vector<ShapeRoot> newPolygons;
00073 for (vector<LineData>::iterator line_it = allEdges.begin();
00074 line_it != allEdges.end(); line_it++) {
00075 Shape<PolygonData> temp_polygon(*line_it);
00076 for (vector<LineData>::iterator line_it2 = line_it+1;
00077 line_it2 < allEdges.end(); line_it2++) {
00078 if (line_it2->getColor() == line_it->getColor()
00079 && (temp_polygon->tryUpdateEdge(Shape<LineData>(*line_it2))
00080 || temp_polygon->tryImportNewEndline(*line_it2))) {
00081 allEdges.erase(line_it2);
00082 line_it2 = line_it;
00083 }
00084 }
00085 temp_polygon->setColor(line_it->getColor());
00086 temp_polygon->updateVertices();
00087 temp_polygon->printParams();
00088 newPolygons.push_back(temp_polygon);
00089 }
00090 return newPolygons;
00091 }
00092
00093 vector<ShapeRoot> PolygonData::formPolygons(const vector<LineData>& lines,
00094 vector<Shape<PolygonData> >& existingPolygons,
00095 vector<ShapeRoot>& deletedPolygons) {
00096 if (lines.empty()) return vector<ShapeRoot>();
00097
00098 vector<LineData> allEdges = lines;
00099 for (vector<Shape<PolygonData> >::const_iterator pol_it = existingPolygons.begin();
00100 pol_it < existingPolygons.end(); pol_it++) {
00101 vector<LineData> existingEdges = (*pol_it)->getEdges();
00102 const int existingParentID = (*pol_it)->getId();
00103 for (vector<LineData>::iterator edge_it = existingEdges.begin();
00104 edge_it != existingEdges.end(); edge_it++)
00105 edge_it->setParentId(existingParentID);
00106 allEdges.insert(allEdges.end(), existingEdges.begin(),existingEdges.end());
00107 }
00108
00109 vector<ShapeRoot> newPolygons = formPolygons(allEdges);
00110
00111 for (vector<Shape<PolygonData> >::iterator existing_it = existingPolygons.begin();
00112 existing_it < existingPolygons.end(); existing_it++) {
00113 vector<ShapeRoot>::iterator new_it = newPolygons.begin();
00114 for (; new_it < newPolygons.end(); new_it++) {
00115 const Shape<PolygonData>& newPolygon = ShapeRootTypeConst(*new_it,PolygonData);
00116 if ((*existing_it)->getEdges().size() == newPolygon->getEdges().size()) {
00117 vector<LineData>::const_iterator edge_it = newPolygon->getEdges().begin();
00118 const int parentPolygonID = (*existing_it)->getId();
00119 for (;edge_it != newPolygon->getEdges().end(); edge_it++)
00120 if (parentPolygonID != edge_it->getParentId())
00121 break;
00122 if (edge_it == newPolygon->getEdges().end()) {
00123 (*existing_it)->edges = newPolygon->edges;
00124 (*existing_it)->vertices = newPolygon->vertices;
00125 new_it->deleteShape();
00126 newPolygons.erase(new_it--);
00127 cout << " => match for existing polygon " << parentPolygonID << endl;
00128 break;
00129 }
00130 }
00131 }
00132 if (new_it == newPolygons.end()) {
00133 deletedPolygons.push_back(*existing_it);
00134 existingPolygons.erase(existing_it--);
00135 }
00136 }
00137 return newPolygons;
00138 }
00139
00140
00141 BoundingBox PolygonData::getBoundingBox() const {
00142 BoundingBox result(vertices[0].coordX(), vertices[0].coordX(),
00143 vertices[0].coordY(), vertices[0].coordY());
00144 for (vector<Point>::const_iterator it = vertices.begin();
00145 it != vertices.end();
00146 it++) {
00147 if ( (*it).coordX() < result.xmin )
00148 result.xmin = (*it).coordX();
00149 else if ( (*it).coordX() > result.xmax )
00150 result.xmax = (*it).coordX();
00151 if ( (*it).coordY() < result.ymin )
00152 result.ymin = (*it).coordY();
00153 else if ( (*it).coordY() > result.ymax )
00154 result.ymax = (*it).coordY();
00155 }
00156 return result;
00157 }
00158
00159 bool PolygonData::formsNewEndline(const LineData& ln, bool useEnd1Pt, bool useEnd2Pt) const {
00160 if (edges.empty()) return true;
00161
00162 useEnd1Pt &= ln.end1Pt().isValid();
00163 useEnd2Pt &= ln.end2Pt().isValid();
00164
00165 if (!(useEnd1Pt || useEnd2Pt))
00166 return false;
00167
00168 if (end1Pt().isValid())
00169 if((useEnd1Pt && end1Pt().distanceFrom(ln.end1Pt()) < THRESH_DIST_VERTEX)
00170 || (useEnd2Pt && end1Pt().distanceFrom(ln.end2Pt()) < THRESH_DIST_VERTEX))
00171 return true;
00172 if (end2Pt().isValid())
00173 if((useEnd1Pt && end2Pt().distanceFrom(ln.end1Pt()) < THRESH_DIST_VERTEX)
00174 || (useEnd2Pt && end2Pt().distanceFrom(ln.end2Pt()) < THRESH_DIST_VERTEX))
00175 return true;
00176
00177 return false;
00178 }
00179
00180 bool PolygonData::tryImportNewEndline(const LineData& ln, bool useEnd1Pt, bool useEnd2Pt) {
00181 if (edges.empty()) {
00182 edges.push_back(ln);
00183 return true;
00184 }
00185
00186 useEnd1Pt &= ln.end1Pt().isValid();
00187 useEnd2Pt &= ln.end2Pt().isValid();
00188 if (!(useEnd1Pt || useEnd2Pt))
00189 return false;
00190
00191 if (end1Pt().isValid()) {
00192 if (useEnd1Pt &&
00193 end1Pt().distanceFrom(ln.end1Pt()) < THRESH_DIST_VERTEX) {
00194 LineData end1ln = ln;
00195 end1ln.setEndPts(ln.end2Pt(),ln.end1Pt());
00196 edges.insert(edges.begin(), end1ln);
00197 return true;
00198 }
00199 else if (useEnd2Pt &&
00200 end1Pt().distanceFrom(ln.end2Pt()) < THRESH_DIST_VERTEX) {
00201 edges.insert(edges.begin(), ln);
00202 return true;
00203 }
00204 }
00205
00206 if (end2Pt().isValid()) {
00207 if (useEnd1Pt &&
00208 end2Pt().distanceFrom(ln.end1Pt()) < THRESH_DIST_VERTEX) {
00209 edges.push_back(ln);
00210 return true;
00211 }
00212 else if (useEnd2Pt &&
00213 end2Pt().distanceFrom(ln.end2Pt()) < THRESH_DIST_VERTEX) {
00214 LineData end2ln = ln;
00215 end2ln.setEndPts(ln.end2Pt(),ln.end1Pt());
00216 edges.push_back(end2ln);
00217 return true;
00218 }
00219 }
00220 return false;
00221 }
00222
00223 bool PolygonData::tryClosePolygon() {
00224 if (vertices.size()<3 ||
00225 !end1Pt().isValid() || !end2Pt().isValid())
00226 return false;
00227 if (end1Ln().isMatchFor(end2Ln())) {
00228 cout << "end1 match for end2\n";
00229 end1Ln().printParams();
00230 end2Ln().printParams();
00231 edges.erase(edges.end());
00232 }
00233 edges.push_back(LineData(*space,end2Pt(),end1Pt()));
00234 edges.front().end1Pt().setValid(true);
00235 updateVertices();
00236 return true;
00237 }
00238
00239 bool PolygonData::isClosed() const {
00240 return (edges.size() > 2 &&
00241 (end1Ln().isMatchFor(end2Ln()) ||
00242 ((end1Pt().distanceFrom(end2Pt()) < THRESH_DIST_VERTEX)
00243 && end1Pt().isValid() && end2Pt().isValid())));
00244 }
00245
00246
00247 bool PolygonData::isMatchForEdge(const LineData& other) const {
00248 for(vector<LineData>::const_iterator this_it = edges.begin();
00249 this_it != edges.end(); this_it++)
00250 if (this_it->isMatchFor(other))
00251 return true;
00252 return false;
00253 }
00254
00255
00256
00257 vector<ShapeRoot> PolygonData::updateState() {
00258 bool breakPolygon = false;
00259 for (vector<LineData>::iterator it = edges.begin();
00260 it != edges.end(); it++) {
00261 if (it->getConfidence() < 0) {
00262 if (! (it == edges.begin() || it == edges.end()-1))
00263 breakPolygon = true;
00264 edges.erase(it--);
00265 }
00266 }
00267 if (breakPolygon && !edges.empty()) {
00268 vector<LineData> lines(edges);
00269 edges.clear();
00270 return formPolygons(lines);
00271 }
00272 else
00273 return vector<ShapeRoot>();
00274 }
00275
00276
00277 bool PolygonData::tryUpdateEdge(const ShapeRoot& ln) {
00278 for(vector<LineData>::iterator it = edges.begin();
00279 it != edges.end(); it++)
00280 if (it->isMatchFor(ln) && it->updateParams(ln)) {
00281 it->increaseConfidence(ln);
00282 if (it->getParentId() == 0 && ln->getParentId() != 0)
00283 it->setParentId(ln->getParentId());
00284 return true;
00285 }
00286 return false;
00287 }
00288
00289
00290
00291
00292
00293
00294
00295 bool PolygonData::isMatchFor(const ShapeRoot& other) const {
00296 if (other->isType(lineDataType) && isSameColorAs(other)) {
00297 const LineData& other_ln = ShapeRootTypeConst(other,LineData).getData();
00298 return (isMatchForEdge(other_ln));
00299 }
00300 if (other->isType(polygonDataType) && isSameColorAs(other)) {
00301 const PolygonData& other_polygon = ShapeRootTypeConst(other,PolygonData).getData();
00302 if (other_polygon.getEdges().size() == edges.size()) {
00303 vector<LineData>::const_iterator other_it = other_polygon.getEdges().begin();
00304 vector<LineData>::const_iterator this_it = getEdges().begin();
00305 for (; other_it != other_polygon.getEdges().end(); other_it++, this_it++)
00306 if (!this_it->isMatchFor(*other_it)) break;
00307 if (this_it == edges.end())
00308 return true;
00309 vector<LineData>::const_reverse_iterator other_rit = other_polygon.getEdges().rbegin();
00310 this_it = edges.begin();
00311 for (; other_rit != other_polygon.getEdges().rend(); other_it++, this_it++)
00312 if (!this_it->isMatchFor(*other_rit)) break;
00313 return (this_it == edges.end());
00314 }
00315 }
00316 return false;
00317 }
00318
00319 int PolygonData::getConfidence() const {
00320 switch (edges.size()) {
00321 case 0:
00322 return -1;
00323 case 1:
00324 return edges.front().getConfidence();
00325 default:
00326 vector<LineData>::const_iterator it = edges.begin();
00327 int conf = (it++)->getConfidence();
00328 for (; it != edges.end(); it++)
00329 if (conf > it->getConfidence())
00330 conf = it->getConfidence();
00331 return conf;
00332 };
00333 }
00334
00335 bool PolygonData::updateParams(const ShapeRoot& other, bool) {
00336 if (other->isType(lineDataType) && tryUpdateEdge(other)) {
00337 updateVertices();
00338 return true;
00339 }
00340 return false;
00341 }
00342
00343
00344
00345
00346
00347
00348
00349
00350
00351
00352
00353
00354
00355
00356
00357 bool PolygonData::isAdmissible() const {
00358 for (vector<LineData>::const_iterator it = edges.begin();
00359 it != edges.end(); it++) {
00360 if (it->isAdmissible())
00361 return true;
00362 }
00363 return false;
00364 }
00365
00366
00367
00368 bool PolygonData::isInside(const Point& pt) const {
00369 if (!isClosed()) return false;
00370 float mostDist = -1;
00371 unsigned int mostDistIndex = -1U;
00372 const unsigned int numVertices = vertices.size()-1;
00373 for (unsigned int i = 0; i < numVertices; i++) {
00374 float dist = pt.distanceFrom(vertices[i]);
00375 if (dist > mostDist) {
00376 mostDist = dist;
00377 mostDistIndex = i;
00378 }
00379 }
00380
00381 LineData ln(this->getSpace(),pt,(vertices[mostDistIndex]));
00382 unsigned int intersectionCount = 0;
00383
00384
00385 {
00386 float theta0 = (mostDistIndex == 0) ?
00387 ((vertices.front() == vertices.back()) ?
00388 atan2(vertices[vertices.size()-2].coordY()-vertices[mostDistIndex].coordY(),
00389 vertices[vertices.size()-2].coordX()-vertices[mostDistIndex].coordX()) :
00390 atan2(vertices[vertices.size()-1].coordY()-vertices[mostDistIndex].coordY(),
00391 vertices[vertices.size()-1].coordX()-vertices[mostDistIndex].coordX())) :
00392 atan2(vertices[mostDistIndex-1].coordY()-vertices[mostDistIndex].coordY(),
00393 vertices[mostDistIndex-1].coordX()-vertices[mostDistIndex].coordX());
00394 float theta1 = atan2(pt.coordY()-vertices[mostDistIndex].coordY(),pt.coordX()-vertices[mostDistIndex].coordX());
00395 float theta2 = (mostDistIndex == vertices.size()-1) ?
00396 ((vertices.front() == vertices.back()) ?
00397 atan2(vertices[1].coordY()-vertices[mostDistIndex].coordY(),
00398 vertices[1].coordX()-vertices[mostDistIndex].coordX()) :
00399 atan2(vertices[0].coordY()-vertices[mostDistIndex].coordY(),
00400 vertices[0].coordX()-vertices[mostDistIndex].coordX())) :
00401 atan2(vertices[mostDistIndex+1].coordY()-vertices[mostDistIndex].coordY(),
00402 vertices[mostDistIndex+1].coordX()-vertices[mostDistIndex].coordX());
00403 if ((theta2>theta0 && !((theta2-theta0<M_PI && theta2>theta1 && theta1>theta0) ||
00404 (2*M_PI-theta2+theta0<M_PI && (theta1>theta2 || theta0>theta1))))
00405 || (theta0>theta2 && !((theta0-theta2<M_PI && theta0>theta1 && theta1>theta2) ||
00406 (2*M_PI-theta0+theta2<M_PI && (theta1>theta0 || theta2>theta1))))) {
00407
00408 intersectionCount++;
00409 }
00410 }
00411
00412 for (unsigned int i = mostDistIndex+1; i < numVertices+mostDistIndex-1; i++) {
00413 LineData cleanEdge(*space,vertices[i%numVertices],vertices[(i+1)%numVertices]);
00414
00415 if (ln.getOrientation() != cleanEdge.getOrientation()
00416 && ln.intersectsLine(cleanEdge)) {
00417
00418 intersectionCount++;
00419 }
00420 }
00421 return (intersectionCount%2) == 0;
00422 }
00423
00424
00425 void PolygonData::printParams() const {
00426 cout << edges.size() << " edge(s)\n";
00427 for (vector<LineData>::const_iterator it = edges.begin();
00428 it != edges.end(); it++)
00429 cout << it->end1Pt() << " -> " << it->end2Pt() << endl;
00430
00431 cout << vertices.size () << " vertice(s):\n";
00432 for (vector<Point>::const_iterator it = vertices.begin();
00433 it != vertices.end(); it++)
00434 cout << (*it) << endl;
00435 }
00436
00437 void PolygonData::applyTransform(const NEWMAT::Matrix& Tmat, const ReferenceFrameType_t newref) {
00438 for (vector<LineData>::iterator it = edges.begin();
00439 it != edges.end(); it++)
00440 it->applyTransform(Tmat,newref);
00441 for (vector<Point>::iterator it = vertices.begin();
00442 it != vertices.end(); it++)
00443 it->applyTransform(Tmat,newref);
00444 }
00445
00446 void PolygonData::projectToGround(const NEWMAT::Matrix& camToBase,
00447 const NEWMAT::ColumnVector& groundplane) {
00448 for (vector<LineData>::iterator it = edges.begin();
00449 it != edges.end(); it++)
00450 it->projectToGround(camToBase,groundplane);
00451 }
00452
00453 Point PolygonData::getCentroid() const {
00454 if (edges.empty())
00455 return Point(0,0,0,getRefFrameType());
00456 vector<LineData>::const_iterator it = edges.begin();
00457 Point centroid = (it++)->getCentroid();
00458 for (; it != edges.end(); it++)
00459 centroid += it->getCentroid();
00460 return centroid/edges.size();
00461 }
00462
00463
00464 void PolygonData::setColor(const rgb &new_color) {
00465 color_rgb = new_color;
00466 for (vector<LineData>::iterator it = edges.begin();
00467 it != edges.end(); it++)
00468 it->setColor(color_rgb);
00469 deleteRendering();
00470 }
00471
00472 void PolygonData::setColor(const color_index cindex) {
00473 setColor(ProjectInterface::getColorRGB(cindex));
00474 }
00475
00476 void PolygonData::setColor(const std::string &color_name) {
00477 setColor(ProjectInterface::getColorRGB(color_name));
00478 }
00479
00480
00481 bool PolygonData::isFirstLineLonger(const Shape<LineData>& ln1,const Shape<LineData>& ln2) {
00482 return ln1->isLongerThan(ln2);
00483 }
00484
00485 Sketch<bool>* PolygonData::render() const {
00486 Sketch<bool> *canvas = new Sketch<bool>(space->getDualSpace(),"");
00487 (*canvas) = 0;
00488 const int width = space->getDualSpace().getWidth();
00489 const int height = space->getDualSpace().getHeight();
00490 float x1, y1, x2, y2;
00491 for (vector<LineData>::const_iterator it = edges.begin();
00492 it != edges.end(); it++) {
00493 it->setDrawCoords(x1, y1, x2, y2, width, height);
00494 it->drawline2d(*canvas, (int)x1, (int)y1, (int)x2, (int)y2);
00495 }
00496 return canvas;
00497 }
00498
00499
00500
00501 class convexHullPoint {
00502 public:
00503 int x, y;
00504 float angle;
00505
00506 convexHullPoint() : x(0), y(0), angle(0) {}
00507
00508 convexHullPoint(int _x, int _y, float _a) : x(_x), y(_y), angle(_a) {}
00509
00510 static int crossProduct(const convexHullPoint &p1, const convexHullPoint &p2, const convexHullPoint &p3) {
00511 return (p2.x - p1.x)*(p3.y - p1.y) - (p3.x - p1.x)*(p2.y - p1.y);
00512 }
00513
00514 class pointCompare : public binary_function<convexHullPoint, convexHullPoint, bool> {
00515 private:
00516 const convexHullPoint &pivot;
00517 public:
00518 pointCompare(const convexHullPoint &_pivot) : pivot(_pivot) {}
00519 bool operator() (const convexHullPoint &p1, const convexHullPoint &p2) {
00520 if ( p1.angle < p2.angle )
00521 return true;
00522 else if ( p1.angle > p2.angle )
00523 return false;
00524 else {
00525 int const d1x = pivot.x - p1.x;
00526 int const d1y = pivot.y - p1.y;
00527 int const d2x = pivot.x - p2.x;
00528 int const d2y = pivot.y - p2.y;
00529 return (d1x*d1x+d1y*d1y) < (d2x*d2x+d2y*d2y);
00530 }
00531 }
00532 }
00533 ;
00534 };
00535
00536 Shape<PolygonData> PolygonData::convexHull(const Sketch<bool> &sketch) {
00537 int const spts = sketch->sum();
00538 int const width = sketch.width;
00539 int const npix = sketch->getNumPixels();
00540
00541
00542
00543
00544 NEW_SKETCH_N(neighbs, uchar, visops::neighborSum(sketch));
00545 std::vector<convexHullPoint> points(spts);
00546 points.clear();
00547 int pivot_x = -1, pivot_y = width+1;
00548 for (int i=0; i<npix; i++) {
00549
00550 if ( sketch[i] && neighbs[i] < 8 ) {
00551 int const x = i % width;
00552 int const y = i / width;
00553 points.push_back(convexHullPoint(x,y,0));
00554 if ( y < pivot_y || (y == pivot_y && x > pivot_x) ) {
00555 pivot_x = x;
00556 pivot_y = y;
00557 };
00558 }
00559 }
00560 int const npts = points.size();
00561
00562
00563
00564 for (int i=0; i<npts; i++)
00565 points[i].angle = (float) -atan2((float) (pivot_y - points[i].y), (float) (pivot_x - points[i].x));
00566 std::sort(points.begin(),points.end(),convexHullPoint::pointCompare(convexHullPoint(pivot_x,pivot_y,0)));
00567
00568
00569 vector<convexHullPoint> hull(npts);
00570 hull.clear();
00571 hull.push_back(points[0]);
00572 hull.push_back(points[1]);
00573 for ( int i=2; i<npts; i++ ) {
00574 int last = hull.size() - 1;
00575 int o = convexHullPoint::crossProduct(hull[last-1],hull[last],points[i]);
00576 if ( o == 0 )
00577 hull[last] = points[i];
00578 else if ( o < 0 )
00579 hull.push_back(points[i]);
00580 else {
00581 while ( o >= 0 && hull.size() > 2 ) {
00582 hull.pop_back();
00583 last--;
00584 o = convexHullPoint::crossProduct(hull[last-1],hull[last],points[i]);
00585 }
00586 hull.push_back(points[i]);
00587 }
00588 }
00589 int const nhull = hull.size();
00590
00591 ShapeSpace &ShS = sketch->getDualSpace();
00592 vector<Point> vertices;
00593 for (int i=0; i<nhull; i++)
00594 vertices.push_back(Point(hull[i].x, hull[i].y));
00595 vertices.push_back(Point(hull[0].x, hull[0].y));
00596 return Shape<PolygonData>(new PolygonData(ShS,vertices,true));
00597 }
00598
00599
00600 }