Tekkotsu Homepage
Demos
Overview
Downloads
Dev. Resources
Reference
Credits

LineData.cc

Go to the documentation of this file.
00001 //-*-c++-*-
00002 #include <iostream>
00003 #include <math.h>
00004 #include <vector>
00005 #include <list>
00006 
00007 #include "Macrodefs.h"
00008 
00009 #include "SketchSpace.h"
00010 #include "Sketch.h"
00011 #include "Region.h"
00012 #include "visops.h"
00013 
00014 #include "ShapeSpace.h"
00015 #include "ShapeRoot.h"
00016 
00017 #include "PointData.h"
00018 #include "LineData.h"
00019 #include "ShapeLine.h"
00020 
00021 using namespace std;
00022 
00023 namespace DualCoding {
00024 
00025 DATASTUFF_CC(LineData);
00026 
00027 const Point LineData::origin_pt = Point(0,0);
00028 
00029 LineData::LineData(ShapeSpace& _space, const Point &p1, orientation_t orient)
00030   : BaseData(_space,getStaticType()), end1_pt(p1), end2_pt(), 
00031     rho_norm(0), theta_norm(0), orientation(0), length(0) {
00032   int const width = space->getDualSpace().getWidth();
00033   int const height = space->getDualSpace().getHeight();
00034   // Use a large offset from p1 to p2 because SketchGUI must calculate
00035   // the line slope from p1/p2 coords transmitted as strings; we don't
00036   // transmit the orientation.  But don't use an offset so large that the line
00037   // goes off-screen.
00038   float p2x=0, p2y=0;
00039   if ( fabs(orient-M_PI/2) < 0.001 ) {
00040     p2x = p1.coordX();
00041     p2y = p1.coordY() > height/2 ? 0 : height-1;
00042   } else {
00043     float slope = tan(orient);
00044     float intcpt = p1.coordY() - p1.coordX()*slope;
00045     p2x = p1.coordX() >= width/2 ? 0.0 : width-1;
00046     p2y = p2x * slope + intcpt;
00047     if ( p2y > height-1 ) {
00048       p2x = (height - intcpt) / slope;
00049       p2y = height;
00050     } else if ( p2y < 0 ) {
00051       p2x = -intcpt / slope;
00052       p2y = 0;
00053     }
00054   }
00055   end2_pt = Point(p2x,p2y);
00056   end1_pt.setValid(false);
00057   end1_pt.setActive(false);
00058   end2_pt.setValid(false);
00059   end2_pt.setActive(false);
00060   update_derived_properties();
00061 }
00062 
00063 Point LineData::getCentroid() const { return (end1Pt()+end2Pt())*0.5; }
00064 
00065 void LineData::setInfinite(bool value) {
00066   end1_pt.setActive(!value);
00067   end2_pt.setActive(!value);
00068 }
00069 
00070 #define NORMPOINT_MATCH_DISTSQ 3500
00071 #define LINE_MATCH_OVERLAP -10
00072 #define LINE_ORIENTATION_MATCH M_PI/6
00073 
00074 bool LineData::isMatchFor(const ShapeRoot& other) const {
00075   if (!(isSameTypeAs(other) && isSameColorAs(other)))
00076     return false;
00077   else {
00078     const Shape<LineData>& other_ln = ShapeRootTypeConst(other,LineData);
00079     return isMatchFor(other_ln.getData());
00080   }
00081 }
00082 
00083 bool LineData::isMatchFor(const LineData& other_line) const {
00084   float const px = rho_norm*cos(theta_norm), 
00085     py = rho_norm*sin(theta_norm);
00086   float const qx = other_line.rho_norm*cos(other_line.theta_norm),
00087     qy = other_line.rho_norm*sin(other_line.theta_norm);
00088   AngPi theta_diff = float(theta_norm) - float(other_line.theta_norm);
00089   if ( theta_diff > M_PI/2 )
00090     theta_diff = M_PI - theta_diff;
00091   float normpointdistsq = (px-qx)*(px-qx) + (py-qy)*(py-qy);
00092   // cout << "px=" << px << "  py=" << py << "  qx=" << qx << "  qy=" << qy << "  normpointdistsq=" << normpointdistsq 
00093   //  << "  theta_diff=" << theta_diff
00094   //   << "  perpdist=" << perpendicularDistanceFrom(other_line.getCentroid())
00095   //   << "  isoverlapped=" << isOverlappedWith(other_line,LINE_MATCH_OVERLAP) << endl;
00096   return normpointdistsq < NORMPOINT_MATCH_DISTSQ  // *** DST hack
00097     && theta_diff < LINE_ORIENTATION_MATCH
00098     && perpendicularDistanceFrom(other_line.getCentroid()) < 100
00099     && isOverlappedWith(other_line,LINE_MATCH_OVERLAP);
00100 }
00101 
00102   bool LineData::isOverlappedWith(const LineData& otherline, int amount) const {
00103   if ( firstPtCoord() <= otherline.firstPtCoord() )
00104     return secondPtCoord()-amount >= otherline.firstPtCoord();
00105   else
00106     return firstPtCoord()+amount <= otherline.secondPtCoord();
00107 }
00108 
00109 
00110 void LineData::mergeWith(const ShapeRoot& other) {
00111   const Shape<LineData>& other_line = ShapeRootTypeConst(other,LineData);
00112   if (other_line->confidence <= 0)
00113     return;
00114   const int other_conf = other_line->confidence;
00115   confidence += other_conf;
00116   end1_pt = (end1_pt*confidence + other_line->end1Pt()*other_conf) / (confidence+other_conf);
00117   end2_pt = (end2_pt*confidence + other_line->end2Pt()*other_conf) / (confidence+other_conf);
00118   update_derived_properties();
00119 }
00120 
00121 
00122 bool LineData::isValidUpdate(coordinate_t c1_cur, coordinate_t c2_cur, coordinate_t c1_new, coordinate_t c2_new) {
00123   const float c1_noise = 10.0 + fabs(c1_cur+c1_new) / 20.0; // allow larger error for shapes far from the robot
00124   const float c2_noise = 10.0 + fabs(c2_cur+c2_new) / 20.0;
00125   return (c1_new-c1_noise < c1_cur && c2_cur < c2_new+c2_noise);
00126 }
00127 
00128 
00129 //! Update a line in the local map with info from a matching line in the ground space.
00130 bool LineData::updateParams(const ShapeRoot& ground_root, bool force) {
00131   const Shape<LineData>& ground_line = ShapeRootTypeConst(ground_root,LineData);
00132   //  cout << "updating local Line " << getId() << " with data from ground line " << ground_line->getId() << ":" << endl;
00133   //  ground_line->printEnds();
00134   //  cout << "Update from " << endl;
00135   //  printEnds();
00136   
00137   const coordinate_t c1_cur = firstPtCoord();
00138   const coordinate_t c2_cur = secondPtCoord();
00139 
00140   Point _end1_pt = firstPt();
00141   Point _end2_pt = secondPt();
00142   
00143   updateLinePt(firstPt(), firstPtCoord(), firstPt(ground_line), firstPtCoord(ground_line), -1);
00144   updateLinePt(secondPt(), secondPtCoord(), secondPt(ground_line), secondPtCoord(ground_line), +1);
00145   //  cout << "to" << endl;
00146   //  printEnds();
00147   
00148   const coordinate_t c1_new = firstPtCoord();
00149   const coordinate_t c2_new = secondPtCoord();
00150   
00151   if (isValidUpdate(c1_cur, c2_cur, c1_new, c2_new) || force){
00152     //    cout << "was accepted, line updated" << endl;
00153     //    ++nsamples;
00154     update_derived_properties();
00155     return true;
00156   }
00157   
00158   //  cout << "was denied, line not updated" << endl;
00159   setEndPts(_end1_pt, _end2_pt);
00160   return false;
00161 }
00162 
00163 void LineData::updateLinePt(EndPoint& localPt, coordinate_t local_coord,
00164           const EndPoint& groundPt, coordinate_t ground_coord,
00165           int sign) {
00166   if ( groundPt.isValid() ) {
00167     if ( localPt.isValid() )
00168       localPt.updateParams(groundPt);
00169     else
00170       localPt = groundPt;
00171   }
00172   else if ( (ground_coord - local_coord)*sign > 0 )
00173     localPt = groundPt;
00174 }
00175 
00176 bool LineData::isAdmissible() const {
00177   if (end1Pt().isValid() && end2Pt().isValid())
00178     return length >= 70.0;  // *** DST hack: lines should be at least 70 mm long
00179   else
00180     return length >= 40.0;
00181 }
00182 
00183 //! Print information about this shape. (Virtual in BaseData.)
00184 void LineData::printParams() const {
00185   cout << "Type = " << getTypeName() << "  ID=" << getId() << "  ParentID=" << getParentId() << endl;
00186   cout << "  end1{" << end1Pt().coordX() << ", " << end1Pt().coordY()  << "}"
00187        << " active=" << end1Pt().isActive()
00188        << " valid=" << end1Pt().isValid() << endl;
00189   
00190   cout << "  end2{" << end2Pt().coordX() << ", " << end2Pt().coordY() <<  "}"
00191        << " active=" << end2Pt().isActive()
00192        << " valid=" << end2Pt().isValid() << std::endl;
00193   
00194   cout << "  rho_norm=" << rho_norm
00195        << ", theta_norm=" << theta_norm
00196        << ", orientation=" << getOrientation() 
00197        << ", length=" << getLength() << endl;
00198   
00199   printf("  color = %d %d %d\n",getColor().red,getColor().green,getColor().blue);
00200   
00201   cout << "  mobile=" << getMobile()
00202        << ", viewable=" << isViewable() << endl;
00203   
00204   vector<float> abc = lineEquation_abc();
00205   printf("  equ = %f %f %f\n",abc[0],abc[1],abc[2]);
00206 }
00207 
00208 void LineData::printEnds() const {
00209   cout << "  end1{" << end1Pt().coordX() << ", " << end1Pt().coordY()  << "}";
00210   cout << "  active=" << end1Pt().isActive() << ", valid=" << end1Pt().isValid() << endl;
00211   cout << "  end2{" << end2Pt().coordX() << ", " << end2Pt().coordY() <<  "}";
00212   cout << "  active=" << end2Pt().isActive() << ", valid=" << end2Pt().isValid() << endl;
00213   // cout << endl;
00214 }
00215 
00216 
00217 
00218 // *** Transformations. *** //
00219 
00220 //! Apply a transformation to this shape.
00221 void LineData::applyTransform(const NEWMAT::Matrix& Tmat, const ReferenceFrameType_t newref) {
00222   end1Pt().applyTransform(Tmat,newref);
00223   end2Pt().applyTransform(Tmat,newref);
00224   update_derived_properties();
00225 }
00226 
00227 void LineData::projectToGround(const NEWMAT::Matrix& camToBase,
00228              const NEWMAT::ColumnVector& groundplane) {
00229   end1Pt().projectToGround(camToBase,groundplane);
00230   end2Pt().projectToGround(camToBase,groundplane);
00231   update_derived_properties();
00232 }
00233 
00234 
00235 // *** Logical endpoints *** //
00236 
00237 EndPoint& LineData::leftPt() { return end1Pt().isLeftOf(end2Pt()) ? end1_pt : end2_pt; }
00238 EndPoint& LineData::rightPt() { return end1Pt().isLeftOf(end2Pt()) ? end2_pt : end1_pt; }
00239 EndPoint& LineData::topPt() { return end1Pt().isAbove(end2Pt()) ? end1_pt : end2_pt; }
00240 EndPoint& LineData::bottomPt() { return end1Pt().isAbove(end2Pt()) ? end2_pt : end1_pt; }
00241 
00242 Shape<PointData> LineData::leftPtShape() {
00243   Shape<PointData> result(new PointData(*space, leftPt()));
00244   result->setName("leftPt");
00245   result->inheritFrom(*this);
00246   result->setViewable(false);
00247   return result;
00248 }
00249 
00250 Shape<PointData> LineData::rightPtShape() {
00251   Shape<PointData> result(new PointData(*space, leftPt()));
00252   result->setName("rightPt");
00253   result->inheritFrom(*this);
00254   result->setViewable(false);
00255   return result;
00256 }
00257 
00258 Shape<PointData> LineData::topPtShape() {
00259   Shape<PointData> result(new PointData(*space, leftPt()));
00260   result->setName("topPt");
00261   result->inheritFrom(*this);
00262   result->setViewable(false);
00263   return result;
00264 }
00265 
00266 Shape<PointData> LineData::bottomPtShape() {
00267   Shape<PointData> result(new PointData(*space, leftPt()));
00268   result->setName("bottomPt");
00269   result->inheritFrom(*this);
00270   result->setViewable(false);
00271   return result;
00272 }
00273 
00274 EndPoint& LineData::firstPt() {
00275   if ( isNotVertical() )
00276     if ( end1Pt().coordX() < end2Pt().coordX() )
00277       return end1Pt();
00278     else return end2Pt();
00279   else
00280     if ( end1Pt().coordY() < end2Pt().coordY() )
00281       return end1Pt();
00282     else return end2Pt();
00283 }
00284     
00285 EndPoint& LineData::firstPt(const Shape<LineData> &otherline) const {
00286   if ( isNotVertical() )
00287     if ( otherline->end1Pt().coordX() < otherline->end2Pt().coordX() )
00288       return otherline->end1Pt();
00289     else return otherline->end2Pt();
00290   else
00291     if ( otherline->end1Pt().coordY() < otherline->end2Pt().coordY() )
00292       return otherline->end1Pt();
00293     else return otherline->end2Pt();
00294 }
00295     
00296 EndPoint& LineData::secondPt() {
00297   if ( isNotVertical() )
00298     if ( end1Pt().coordX() > end2Pt().coordX() )
00299       return end1Pt();
00300     else return end2Pt();
00301   else
00302     if ( end1Pt().coordY() > end2Pt().coordY() )
00303       return end1Pt();
00304     else return end2Pt();
00305 }
00306     
00307 EndPoint& LineData::secondPt(const Shape<LineData> &otherline) const {
00308   if ( isNotVertical() )
00309     if ( otherline->end1Pt().coordX() > otherline->end2Pt().coordX() )
00310       return otherline->end1Pt();
00311     else return otherline->end2Pt();
00312   else
00313     if ( otherline->end1Pt().coordY() > otherline->end2Pt().coordY() )
00314       return otherline->end1Pt();
00315     else return otherline->end2Pt();
00316 }
00317     
00318 Shape<PointData> LineData::firstPtShape() {
00319   Shape<PointData> result(new PointData(*space, firstPt()));
00320   result->setName("firstPt");
00321   result->inheritFrom(*this);
00322   result->setViewable(false);
00323   return result;
00324 }
00325 
00326 Shape<PointData> LineData::secondPtShape() {
00327   Shape<PointData> result(new PointData(*space, secondPt()));
00328   result->setName("secondPt");
00329   result->inheritFrom(*this);
00330   result->setViewable(false);
00331   return result;
00332 }
00333 
00334 coordinate_t LineData::firstPtCoord() const {
00335   return  isNotVertical() ?
00336     const_cast<LineData*>(this)->firstPt().coordX() :
00337     const_cast<LineData*>(this)->firstPt().coordY();
00338 }
00339 
00340 coordinate_t LineData::firstPtCoord(const Shape<LineData> &otherline) const {
00341   return  isNotVertical() ?
00342     firstPt(otherline).coordX() :
00343     firstPt(otherline).coordY();
00344 }
00345 
00346 coordinate_t LineData::secondPtCoord() const {
00347   return  isNotVertical() ?
00348     const_cast<LineData*>(this)->secondPt().coordX() :
00349     const_cast<LineData*>(this)->secondPt().coordY();
00350 }
00351 
00352 coordinate_t LineData::secondPtCoord(const Shape<LineData> &otherline) const {
00353   return  isNotVertical() ?
00354     secondPt(otherline).coordX() :
00355     secondPt(otherline).coordY();
00356 }
00357 
00358 
00359 // *** Functions to set endpoints. *** //
00360 
00361 void LineData::setEndPts(const EndPoint& _end1_pt, const EndPoint& _end2_pt) {
00362   end1_pt.setCoords(_end1_pt);
00363   end1_pt.setActive(_end1_pt.isActive());
00364   end1_pt.setValid(_end1_pt.isValid());
00365   end1_pt.setNumUpdates(_end1_pt.numUpdates());
00366 
00367   end2_pt.setCoords(_end2_pt);
00368   end2_pt.setActive(_end2_pt.isActive());
00369   end2_pt.setValid(_end2_pt.isValid());
00370   end2_pt.setNumUpdates(_end2_pt.numUpdates());
00371 
00372   update_derived_properties();
00373 }
00374 
00375 
00376 // *** Properties functions. *** //
00377 
00378 std::pair<float,float> LineData::lineEquation_mb() const {
00379   float m;
00380   if ((fabs(end2Pt().coordX() - end1Pt().coordX()) * BIG_SLOPE)
00381       <= fabs(end2Pt().coordY() - end2Pt().coordY()))
00382     m = BIG_SLOPE;
00383   else 
00384     m = (end2Pt().coordY() - end1Pt().coordY())/(end2Pt().coordX() - end1Pt().coordX());
00385   float b = end1Pt().coordY() - m * end1Pt().coordX();
00386   return pair<float,float>(m,b);
00387 }
00388 
00389 
00390 //! Determine parameters a, b, c, d satisfying the equation ax + bz = c.
00391 std::vector<float> LineData::lineEquation_abc_xz() const {
00392   float dx = end2Pt().coordX() - end1Pt().coordX();
00393   float dz = end2Pt().coordZ() - end1Pt().coordZ();
00394 
00395   std::vector<float> abc;
00396   abc.resize(3, 1.0);
00397   float& a = abc[0];
00398   float& b = abc[1];
00399   float& c = abc[2];
00400 
00401   // If vertical...b = 0
00402   if((dx == 0.0)
00403      || (dz/dx > BIG_SLOPE)) {
00404     a = 1.0;
00405     b = 0.0;
00406     c = end1Pt().coordX();
00407   }
00408   
00409   // If horizontal...a = 0
00410   else if((dz == 0.0)
00411     || (dx/dz > BIG_SLOPE)) {
00412     a = 0.0;
00413     b = 1.0;
00414     c = end1Pt().coordZ();
00415   }
00416   
00417   // If not horizontal or vertical...a = 1.0
00418   else {
00419     a = 1.0;
00420     b = (end1Pt().coordX() - end2Pt().coordX()) 
00421       / (end2Pt().coordZ() - end1Pt().coordZ());
00422     c = end1Pt().coordX() + b*end1Pt().coordZ();
00423   }
00424   
00425   return(abc);
00426 
00427 }
00428 
00429 //! Determine parameters a, b, c satisfying the equation ax + by = c.
00430 std::vector<float> LineData::lineEquation_abc() const {
00431   float dx = end2Pt().coordX() - end1Pt().coordX();
00432   float dy = end2Pt().coordY() - end1Pt().coordY();
00433   
00434   std::vector<float> abc;
00435   abc.resize(3, 1.0);
00436   float& a = abc[0];
00437   float& b = abc[1];
00438   float& c = abc[2];
00439 
00440   // If vertical...b = 0
00441   if( fabs(dx) < 1.0e-6 || dy/dx > BIG_SLOPE) {
00442     a = 1.0;
00443     b = 0.0;
00444     c = end1Pt().coordX();
00445   }
00446   
00447   // If horizontal...a = 0
00448  else if ( fabs(dy) < 1.0e-6 || dx/dy > BIG_SLOPE ) {
00449     a = 0.0;
00450     b = 1.0;
00451     c = end1Pt().coordY();
00452   }
00453   
00454   // If not horizontal or vertical...a = 1.0
00455   else {
00456     a = 1.0;
00457     //    b = (end1Pt().coordX() - end2Pt().coordX()) 
00458     // / (end2Pt().coordY() - end1Pt().coordY());
00459     b = -dx / dy;
00460     c = end1Pt().coordX() + b*end1Pt().coordY();
00461   }
00462   
00463   return(abc);
00464 }
00465 
00466 
00467 // *** Functions to set values dealing with orientation. *** //
00468 
00469 void LineData::update_derived_properties() {
00470   rho_norm = perpendicularDistanceFrom(origin_pt);
00471   const vector<float> abc = lineEquation_abc();
00472   const float& a1 = abc[0];
00473   const float& b1 = abc[1];
00474   const float& c1 = abc[2];
00475   const float c1sign = (c1 >= 0) ? 1.0 : -1.0;
00476   theta_norm = atan2(b1*c1sign, a1*c1sign);
00477   orientation = theta_norm + AngPi(M_PI/2);
00478   length = end1Pt().distanceFrom(end2Pt());
00479   const ReferenceFrameType_t ref = getRefFrameType();
00480   end1_pt.setRefFrameType(ref);
00481   end2_pt.setRefFrameType(ref);
00482   deleteRendering();
00483 }
00484 
00485 bool LineData::isNotVertical() const {
00486   const AngPi threshold = M_PI / 3;
00487   const AngPi orient = getOrientation();
00488   return (orient <= threshold) || (orient >= M_PI - threshold);
00489 }
00490 
00491 /*
00492 bool LineData::isRoughlyPerpendicularTo(Shape<LineData>& other) {
00493   AngPi threshold = M_PI_4;
00494   AngPi orientation_diff = getOrientation() - other->getOrientation();
00495   if((orientation_diff >= threshold) && (orientation_diff < (M_PI-threshold)))
00496     return true;
00497   else
00498     return false;
00499 }
00500 
00501 bool LineData::isExactlyPerpendicularTo(Shape<LineData>& other) {
00502   AngPi orientation_diff = getOrientation() - other->getOrientation();
00503   return (orientation_diff == M_PI_2);
00504 }
00505 
00506 */
00507 
00508 
00509 // *** These functions are true based on line length. *** //
00510 
00511 bool LineData::isLongerThan(const Shape<LineData>& other) const {
00512   return length > other->length; }
00513 
00514 bool LineData::isLongerThan(float ref_length) const {
00515   return length > ref_length; }
00516 
00517 bool LineData::isShorterThan(const Shape<LineData>& other) const {
00518   return length < other->length; }
00519 
00520 bool LineData::isShorterThan(float ref_length) const {
00521   return length < ref_length; }
00522 
00523 bool LineData::isBetween(const Point &p, const LineData &other) const {
00524   if (getOrientation() == other.getOrientation()) { // parallel lines
00525     float dl = perpendicularDistanceFrom(other.end1Pt()); // distance between the lines
00526     return (perpendicularDistanceFrom(p) <= dl && other.perpendicularDistanceFrom(p) <= dl);
00527   }
00528   else {
00529     bool b;
00530     const LineData p_line (*space,  p,  // line from intersection of this and other to p
00531          intersectionWithLine(other, b, b));
00532     const AngPi theta_pline = p_line.getOrientation();
00533     const AngPi theta_greater = 
00534       (getOrientation() > other.getOrientation()) ? getOrientation() : other.getOrientation();
00535     const AngPi theta_smaller = 
00536       (getOrientation() < other.getOrientation()) ? getOrientation() : other.getOrientation();
00537     if (theta_greater - theta_smaller > M_PI/2)
00538       return (theta_pline >= theta_greater || theta_pline <= theta_smaller);
00539     else
00540       return (theta_pline <= theta_greater && theta_pline >= theta_smaller);
00541   }
00542 }
00543 
00544   
00545 // ***  Check intersection. *** //
00546 
00547 bool
00548 LineData::intersectsLine(const Shape<LineData>& other) const {
00549   return intersectsLine(other.getData());
00550 }
00551 
00552 bool
00553 LineData::intersectsLine(const LineData& other) const {
00554   // Calculate F(x,y) = 0 for this line (x1,y1) to (x2,y2).
00555   pair<float,float> F = lineEquation_mb();
00556   
00557   // Calculate G(x,y) = 0 for L (x3,y3) to (x4,y4).
00558   pair<float,float> G = other.lineEquation_mb();
00559   
00560   // NOTE: These tests are assumed to be taking place in the space of
00561   // "this" line.  Therefore, the limits of line extent (for lines
00562   // with inactive endpoints) are calculated in the space of "this"
00563   // line.
00564   
00565   // JJW *** YOU NEED TO TAKE ACCOUNT OF END POINTS BEING TURNED OFF.
00566   
00567   //  float end1x, end1y, end2x, end2y, other_end1x, other_end1y, other_end2x, other_end2y;
00568   //  if(end1Pt().isActive()) {
00569   //    end1x = end1Pt().coordX();
00570   //    end1y = end1Pt().coordY();
00571   //  } else {
00572   //    end1x = 
00573   
00574   
00575   // TEST 1
00576   
00577   // Calculate r3 = F(x3,y3)
00578   float r3 = F.first * other.end1Pt().coordX() + F.second - other.end1Pt().coordY();
00579   
00580   // Calculate r4 = F(x4,y4)
00581   float r4 = F.first * other.end2Pt().coordX() + F.second - other.end2Pt().coordY();
00582   
00583   // If r3 != 0...
00584   // ...AND r4 != 0...
00585   // ...AND SGN(r3) == SGN(r4)...
00586   // ...THEN the lines do not intersect.
00587   
00588   if((r3 != 0)
00589      && (r4 != 0)
00590      && (SGN(r3) == SGN(r4))
00591      )
00592     return false;
00593   
00594   
00595   // TEST 2
00596   
00597   // Calculate r1 = G(x1,y1)
00598   float r1 = G.first * end1Pt().coordX() + G.second - end1Pt().coordY();
00599   
00600   // Calculate r2 = G(x2,y2)
00601   float r2 = G.first * end2Pt().coordX() + G.second - end2Pt().coordY(); 
00602   
00603   // If r1 != 0...
00604   // ...AND r2 != 0...
00605   // ...AND SGN(r1) == SGN(r2)...
00606   // ...THEN the lines do not intersect.
00607   
00608   if((r1 != 0)
00609      && (r2 != 0)
00610      && (SGN(r1) == SGN(r2))
00611      )
00612     return false;
00613   
00614   // Otherwise, the lines DO intersect.
00615   return true;
00616 }
00617 
00618 
00619 Point LineData::intersectionWithLine(const Shape<LineData>& other,
00620              bool& intersection_on_this,
00621              bool& intersection_on_other) const { 
00622   return intersectionWithLine(other.getData(), intersection_on_this,intersection_on_other); 
00623 }
00624 
00625 Point
00626 LineData::intersectionWithLine(const LineData& other, 
00627              bool& intersection_on_this, bool& intersection_on_other) const {
00628 
00629   // Based upon algorithm written by Paul Bourke, April 1989.
00630   // http://astronomy.swin.edu/~pbourke/geometry/lineline2d/  
00631   // Accessed July 20th 2004
00632   
00633   // Points 1 and 2 are on "this" line. Points 3 and 4 define the "other" line.
00634   //  float x1, x2, x3, x4, y1, y2, y3, y4;
00635   float x1, x2, x3, x4, y1, y2, y3, y4;
00636   x1 = end1Pt().coordX();
00637   x2 = end2Pt().coordX();
00638   x3 = other.end1Pt().coordX();
00639   x4 = other.end2Pt().coordX();
00640   // x3 = other->end1Pt().coordX();
00641   // x4 = other->end2Pt().coordX();
00642   y1 = end1Pt().coordY();
00643   y2 = end2Pt().coordY();
00644   y3 = other.end1Pt().coordY();
00645   y4 = other.end2Pt().coordY();
00646   // y3 = other->end1Pt().coordY();
00647   // y4 = other->end2Pt().coordY();
00648   
00649   // x1 + u_a(x2-x1) = x3 + u_b(x4-x3)
00650   // y1 + u_a(y2-y1) = y3 + u_b(y4-y3)
00651   
00652   // u_a = (x4-x3)(y1-y3) - (y4-y3)(x1-x3)
00653   //       -------------------------------
00654   //       (y4-y3)(x2-x1) - (x4-x3)(y2-y1)
00655   
00656   // u_b = (x2-x1)(y1-y3) - (y2-y1)(x1-x3)
00657   //       -------------------------------
00658   //       (y4-y3)(x2-x1) - (x4-x3)(y2-y1)
00659   
00660   float denom = ((y4-y3)*(x2-x1) - (x4-x3)*(y2-y1));
00661   float u_a_numerator = ((x4-x3)*(y1-y3) - (y4-y3)*(x1-x3));
00662   float u_b_numerator = ((x2-x1)*(y1-y3) - (y2-y1)*(x1-x3));
00663   
00664   // If denominators of u_a and u_b are zero, then lines are parallel.
00665   if(denom == 0.0) {
00666     if (u_a_numerator == 0.0 && u_b_numerator == 0.0) {
00667       PRINTF("intersectionWithLine: lines are coincident!\n");
00668       return(end1Pt());
00669     }
00670     else {
00671       cout << x1 << " " << x2 << " " << x3 << " " << x4 << " "
00672      << y1 << " " << y2 << " " << y3 << " " << y4 << endl;
00673       cout << "this theta; " << getOrientation() << ", other theta: " << other.getOrientation() << endl;
00674       PRINTF("ERROR in intersectionWithLine: lines are parallel!\n");
00675       return(Point(-9999.0,-99999.0));
00676     }
00677   }
00678   
00679   else {
00680     float u_a = u_a_numerator / denom;
00681     float u_b = u_b_numerator / denom;
00682     
00683     // If 0 <= u_a <=1  then intersection point is on segment a.
00684     if(0.0 <= u_a <=1.0)
00685       intersection_on_this = true;
00686     else
00687       intersection_on_this = false;
00688     
00689     // If 0 <= u_b <=1  then intersection point is on segment b.
00690     if(0.0 <= u_b <=1.0)
00691       intersection_on_other = true;
00692     else
00693       intersection_on_other = false;
00694     
00695     return(Point((x1+u_a*(x2-x1)),
00696      (y1+u_a*(y2-y1))));
00697   }
00698 }
00699 
00700 
00701 // *** Distance. *** //
00702 
00703 float LineData::perpendicularDistanceFrom(const Point& otherPt) const {
00704   // NOTE that this formula is rather slow, as it involves the
00705   // calculation of the line equation in addition to a square root here.
00706   
00707   // Using the formula from http://www.tpub.com/math2/8.htm
00708   vector<float> abc = lineEquation_abc();
00709   float& a = abc[0];
00710   float& b = abc[1];
00711   float& c = abc[2];
00712   
00713   // Distance...
00714   //    (x*A + y*B - C  )
00715   // abs(-------------  )
00716   //    (sqrt(a^2 + b^2)) 
00717   float d = fabs((a * otherPt.coordX() + b * otherPt.coordY() - c)/sqrt(a*a + b*b));
00718   //  cout << "abcd: " << a << ", " << b << ", " << c << ", " << d << endl;
00719   return(d);
00720 }
00721 
00722 
00723 
00724 // ==================================================
00725 // BEGIN LINE EXTRACTION CODE
00726 // ==================================================
00727 
00728 #define BEG_DIST_THRESH 2
00729 #define END_DIST_THRESH 2
00730 
00731 Shape<LineData> LineData::extractLine(Sketch<bool>& sketch) {
00732   NEW_SKETCH_N(fatmask,bool,visops::fillin(sketch,1,2,8));
00733   NEW_SKETCH_N(skel,bool,visops::skel(fatmask));
00734   return extractLine(skel, sketch);
00735 }
00736 
00737 Shape<LineData> LineData::extractLine(Sketch<bool>& skeleton, 
00738               const Sketch<bool>& occlusions) {
00739   const int width = skeleton->getWidth(), height = skeleton->getHeight();
00740   SketchSpace &SkS = skeleton->getSpace();
00741   ShapeSpace &ShS = SkS.getDualSpace();
00742 
00743   // approximate largest skel region with line
00744   NEW_SKETCH_N(labels,uint,visops::oldlabelcc(skeleton,visops::EightWayConnect));
00745   list<Region> regions = Region::extractRegions(labels,EXTRACT_LINE_MIN_AREA);
00746   if ( regions.empty() ) {
00747     ShapeRoot invalid; // need to define a named variable to avoid warning on next line
00748     return ShapeRootType(invalid,LineData);
00749   }
00750   Region skelchunk = regions.front();  // regions not empty, so use the largest....
00751   AngPi orientation(skelchunk.findPrincipalAxisOrientation());
00752   Point centroid(skelchunk.findCentroid());
00753   // cout << " orientation=" << orientation << "  centroid=" << centroid.getCoords() << endl;
00754   if (! skelchunk.isContained(centroid, 3) ) {   //  region does not look like a straight line
00755     Shape<LineData> result(splitLine(ShS, skelchunk, skeleton, occlusions));
00756     if ( result.isValid() )
00757       return result;
00758   }
00759   
00760   // Create a symbolic line object.
00761   Shape<LineData> extracted_line(ShS, centroid, orientation);
00762   extracted_line->setColor(skeleton->getColor());
00763   extracted_line->setParentId(skeleton->getViewableId());
00764 
00765   // rho and theta describe the normal line.
00766   // normpoint is where the normal line intersects with this line.
00767   // normpoint may actually be off-image.
00768   float x_normpoint = extracted_line->rho_norm*cos(extracted_line->theta_norm);
00769   float y_normpoint = extracted_line->rho_norm*sin(extracted_line->theta_norm);
00770   
00771   // m is slope of this line (not the normal line)
00772   // float m = (y_normpoint != 0) ? -(x_normpoint/y_normpoint) : BIG_SLOPE;
00773   float m = max(min(tan(extracted_line->theta_norm+AngPi(M_PI/2)), (float)BIG_SLOPE), (float)(-BIG_SLOPE));
00774   float b = y_normpoint - m*x_normpoint;
00775   // cout << "  x_normpoint=" << x_normpoint << "  y_normpoint=" << y_normpoint
00776   // << "  m=" << m << "  b=" << b <<std::endl;
00777   
00778   NEW_SKETCH_N(skelDist,uint,visops::edist(skeleton));
00779   if ( abs(m) <= 1 )  // when |slope| <= 1, scan along x, else scan along y
00780     extracted_line->scanHorizForEndPts(skelDist,occlusions,m,b);
00781   else
00782     extracted_line->scanVertForEndPts(skelDist,occlusions,m,b);
00783 
00784   // Check to see if any endpoints are near any edge of the screen.
00785   // If they are, invalidate them, assuming that line continues beyond screen.
00786   extracted_line->end1_pt.checkValidity(width,height,BEG_DIST_THRESH);
00787   extracted_line->end2_pt.checkValidity(width,height,BEG_DIST_THRESH);
00788 
00789   // Transform from SketchSpace coordinates to ShapeSpace coordinates
00790   SkS.applyTmatinv(extracted_line->end1_pt.getCoords());
00791   SkS.applyTmatinv(extracted_line->end2_pt.getCoords());
00792   extracted_line->update_derived_properties();
00793   return extracted_line;
00794 }
00795 
00796 
00797   // hack to split a non-straight region by kei
00798 Shape<LineData> LineData::splitLine(ShapeSpace &ShS, Region &skelchunk, 
00799         Sketch<bool> &skeleton, const Sketch<bool> &occlusions) {
00800   //  cout << "this region is not straight, needs be split into two regions" << endl;
00801   Point bounds[4] = {skelchunk.findTopBoundPoint(), skelchunk.findLeftBoundPoint(),
00802          skelchunk.findRightBoundPoint(), skelchunk.findBotBoundPoint()};
00803   //  cout << "top(0): " << bounds[0] << endl;
00804   //  cout << "left(1): " << bounds[1] << endl;
00805   //  cout << "right(2): " << bounds[2] << endl;
00806   //  cout << "bottom(3): " << bounds[3] << endl;
00807   for (int i = 0; i < 4; i++) {
00808     for (int j = i+1; j < 4; j++) {
00809       if (bounds[i].distanceFrom(bounds[j]) > 20 && ! skelchunk.isContained((bounds[i]+bounds[j])/2.0, 3)) {
00810   //  cout << "[" << i << "," << j << "] form a line from which most distant point is where the region should split " << endl;
00811   LineData ln(ShS,bounds[i],bounds[j]);
00812   PointData most_distant_pt(ShS, skelchunk.mostDistantPtFrom(ln));
00813   //  cout << "Point of split: " << most_distant_pt.getCentroid() << endl;
00814   const Sketch<bool>& point_rendering = most_distant_pt.getRendering();
00815   uint const clear_dist = 10;
00816   NEW_SKETCH_N(not_too_close, bool, visops::edist(point_rendering) >= clear_dist);
00817   skeleton &= not_too_close;
00818   return extractLine(skeleton, occlusions);
00819       }
00820     }
00821   }
00822   ShapeRoot invalid; // need to define a named variable to avoid warning on next line
00823   return ShapeRootType(invalid,LineData);
00824 }
00825 
00826 void LineData::scanHorizForEndPts(const Sketch<uint>& skelDist, 
00827           const Sketch<bool>& occlusions, 
00828           float m, float b) {
00829   // Scan along the infinite line, looking for segments in the image.
00830   bool on_line = false;        // true if tracing a line skeleton
00831   int beg_dist_thresh = BEG_DIST_THRESH;   // skel has to be <= this close to start segment
00832   int end_dist_thresh = END_DIST_THRESH;   // how far line should extend from skel
00833   int curxstart = -1;    // start of current segment we're examining
00834   int possxstop = -1;    // end of current segment unless we bridge a gap
00835   int bestxstart = -1;   // start of best (longest) segment seen so far
00836   int bestxstop = -1;    // end of best segment seen so far
00837   int bestlength = -1;   // length of best segment seen so far
00838   for (int x = 0, y, dist=0; x < skelDist.width; x++) {
00839     y = int(m*x+b);
00840     
00841     // If y is on-screen...
00842     if (y >= 0 && y < skelDist.height) {
00843       dist = skelDist(x,y);
00844 
00845       if (on_line == false) {   // not currently on a line segment
00846   if (dist <= beg_dist_thresh) {
00847     // start a new segment
00848     on_line = true;
00849     curxstart = x - dist;
00850     // if new segment begins at an occluder, back up over the occluder
00851     int curystart;
00852     bool backupflag = false;
00853     while ( curxstart >= 0 && (curystart=int(m*curxstart+b)) >= 0 && curystart < skelDist.height )
00854       if ( occlusions(curxstart,curystart) || skelDist(curxstart,curystart) == 0 ) {
00855         --curxstart;
00856         backupflag = true;
00857       } else { // if we backed up past the occluder, go one step forward
00858         curxstart += backupflag;
00859         break;
00860       }
00861     if ( curxstart < 0) // occluder extended to left edge of image
00862       curxstart = 0;
00863   }
00864       }
00865       else {   // on_line == true:  currently on a line segment
00866   const bool occ = occlusions(x,y);
00867   // cout << "x=" << x << "  dist=" << dist <<"  occ=" << occ;
00868   if ( dist <= end_dist_thresh || occ ) {
00869     if ( occ )
00870       possxstop = x;
00871     else
00872       possxstop = x - dist;
00873     // cout << "  possxstop=" << possxstop;
00874     if ( possxstop-curxstart > bestlength ) {
00875       bestxstart = curxstart;
00876       bestxstop = possxstop;
00877       bestlength = bestxstop-bestxstart;
00878       // cout << "  bestxstop=" << bestxstop << "  bestlength=" << bestlength;
00879     }
00880   }
00881   else if ( dist > extractorGapTolerance ) {
00882     // we're traversing a gap, and it just got too long
00883     on_line = false;
00884     // cout << "  on_line=" << on_line << "  dist=" << dist;
00885   };  
00886   // cout << endl;
00887       }
00888     }
00889   }
00890 
00891   // cout << "xstart:" << bestxstart << "xstop:" << bestxstop << endl;
00892   float y1 = m*bestxstart + b;
00893   float y2 = m*bestxstop + b;
00894   setEndPts(Point(bestxstart,y1), Point(bestxstop,y2));
00895   // cout << "m = " << m << "   b = " << b << endl;
00896   balanceEndPointHoriz(end1_pt,occlusions,m,b);
00897   balanceEndPointHoriz(end2_pt,occlusions,m,b);
00898 }
00899 
00900 void LineData::scanVertForEndPts(const Sketch<uint>& skelDist, 
00901           const Sketch<bool>& occlusions, 
00902           float m, float b) {
00903   // Scan along the infinite line, looking for segments in the image.
00904   bool on_line = false;        // true if tracing a line skeleton
00905   int beg_dist_thresh = BEG_DIST_THRESH;   // skel has to be <= this close to start segment
00906   int end_dist_thresh = END_DIST_THRESH;   // how far line should extend from skel
00907   int curystart = -1;    // start of current segment we're examining
00908   int possystop = -1;    // end of current segment unless we bridge a gap
00909   int bestystart = -1;   // start of best (longest) segment seen so far
00910   int bestystop = -1;    // end of best segment seen so far
00911   int bestlength = -1;   // length of best segment seen so far
00912   for (int x, y = 0, dist=0; y < skelDist.height; y++) {
00913     x = int((y-b)/m);
00914     
00915     // If x is on-screen...
00916     if (x >= 0 && x < skelDist.width) {
00917       dist = int(skelDist(x,y));
00918 
00919       if (on_line == false) {   // not currently on a line segment
00920   if (dist <= beg_dist_thresh) {
00921     // start a new segment
00922     on_line = true;
00923     curystart = y - dist;
00924     // if new segment begins at an occluder, back up over the occluder
00925     int curxstart;
00926     bool backupflag = false;
00927     while ( curystart >= 0 && (curxstart=int((curystart-b)/m)) >= 0 && curxstart < skelDist.width )
00928       if ( occlusions(curxstart,curystart) || skelDist(curxstart,curystart) == 0 ){
00929         --curystart;
00930         backupflag = true;
00931       } else { // if we backed up past the occluder, go one step forward
00932         curystart += backupflag;
00933         break;
00934       }
00935     if ( curystart < 0) // occluder extended to top edge of image
00936       curystart = 0;
00937   }
00938       }
00939       else {   // on_line == true:  currently on a line segment
00940   const bool occ = occlusions(x,y);
00941   // cout << "y=" << y << "  dist=" << dist <<"  occ=" << occ;
00942   if ( dist <= end_dist_thresh || occ ) {
00943     if ( occ )
00944       possystop = y;
00945     else
00946       possystop = y - dist;
00947     // cout << "  possystop=" << possystop;
00948     if ( possystop-curystart > bestlength ) {
00949       bestystart = curystart;
00950       bestystop = possystop;
00951       bestlength = bestystop-bestystart;
00952       // cout << "  bestystop=" << bestystop << "  bestlength=" << bestlength;
00953     }
00954   }
00955   else if ( dist > extractorGapTolerance ) {
00956     // we're traversing a gap, and it just got too long
00957     on_line = false;
00958     // cout << "  on_line=" << on_line;
00959   };  
00960   // cout << endl;
00961       }
00962     }
00963   }
00964 
00965   float x1 = (bestystart-b)/m;
00966   float x2 = (bestystop-b)/m;
00967   // cout << "x1=" << x1 << ", ystart=" << bestystart << "   x2=" << x2 << ", ystop=" << bestystop << endl;
00968   setEndPts(Point(x1,bestystart), Point(x2,bestystop));
00969   // cout << "m = " << m << "   b = " << b << endl;
00970   balanceEndPointVert(end1_pt,occlusions,m,b);
00971   balanceEndPointVert(end2_pt,occlusions,m,b);
00972 }
00973 
00974 void LineData::balanceEndPointHoriz(EndPoint &, Sketch<bool> const &, float, float) {
00975   /*
00976   int xstep = ( pt.coordX() < max(end1_pt.coordX(),end2_pt.coordX()) ? +1 : -1 );
00977   int toppix = 0, bottompix = 0;
00978   int const balance_rows = 5;    // check this many rows from the line endpoint inward
00979   int const row_samples = 10; // for each row, check this many pixels each side of the line
00980   */
00981   return;
00982 }
00983 
00984 void LineData::balanceEndPointVert(EndPoint &pt, Sketch<bool> const &occluders, float m, float b) {
00985   int ystep = ( pt.coordY() < max(end1_pt.coordY(),end2_pt.coordY()) ? +1 : -1 );
00986   int leftpix = 0, rightpix = 0;
00987   int const balance_rows = 8;    // check this many rows from the line endpoint inward
00988   int const row_samples = 10; // for each row, check this many pixels each side of the line
00989   // cout << endl << " ystep=" << ystep << " coords= ( " << pt.coordX() << " , " << pt.coordY() << ")"  << endl;
00990   for ( int y = (int)pt.coordY(), ycnt=balance_rows ;
00991   y>= 0 && y<occluders.height && ycnt-- > 0 ; y+=ystep ) {
00992     int const xstart = (int) ((y-b)/m);
00993     for ( int x = xstart-row_samples; x < xstart; x++ )
00994       if ( x >= 0 )
00995   leftpix += occluders(x,y);
00996     for ( int x = xstart+row_samples; x > xstart; x-- )
00997       if ( x < occluders.width )
00998   rightpix += occluders(x,y);
00999     // cout << "   " << xstart << "/" << y << "  =  " << leftpix << " " << rightpix << endl;;
01000   }
01001    float const new_x = pt.coordX() + (rightpix-leftpix)/(2*balance_rows);
01002    // cout << "    leftpix=" << leftpix <<"  rightpix=" << rightpix << endl;
01003    pt.setCoords(new_x, pt.coordY());
01004 }
01005 
01006 vector<Shape<LineData> > LineData::extractLines(Sketch<bool> const& sketch,
01007             const int num_lines) {
01008   NEW_SKETCH_N(fatmask,bool,visops::fillin(sketch,1,2,8));
01009   NEW_SKETCH_N(skel,bool,visops::skel(fatmask));
01010   return extractLines(skel, sketch, num_lines);
01011 }
01012 
01013 vector<Shape<LineData> > LineData::extractLines(Sketch<bool> const& skel,
01014             Sketch<bool> const& occluders,
01015             const int num_lines) {
01016   vector<Shape<LineData> > lines_vec;
01017   NEW_SKETCH_N(temp,bool,visops::copy(skel))
01018   for (int gloop = 0; gloop<num_lines; gloop++) {  // <---- HACK TO LIMIT NUMBER OF LINES RETURNED
01019     Shape<LineData> newline(extractLine(temp,occluders));
01020     if ( !newline.isValid() ) break;
01021     newline->clearLine(temp);
01022     if (newline->isLongerThan(DEFAULT_MIN_LENGTH)) {
01023       lines_vec.push_back(newline);
01024     }
01025   }
01026   // std::cout << "extractLines returning " << lines_vec.size() << " lines." << std::endl;
01027   
01028   return lines_vec;
01029 }
01030 
01031 //! Clear out pixels that are on or close to this line.
01032 void LineData::clearLine(Sketch<bool>& sketch) {
01033   const Sketch<bool>& line_rendering = getRendering();
01034   uint const clear_dist = 5;
01035   Sketch<bool> not_too_close = (visops::edist(line_rendering) >= clear_dist);
01036   sketch &= not_too_close;
01037 } 
01038 
01039   
01040 
01041 // ==================================================
01042 // BEGIN LINE RENDERING CODE
01043 // ==================================================
01044 
01045 Sketch<bool>& LineData::getRendering() {
01046   if ( ! end1Pt().rendering_valid || ! end2Pt().rendering_valid )
01047     deleteRendering();
01048   if ( rendering_sketch == NULL )
01049     rendering_sketch = render();
01050   return *rendering_sketch;
01051 }
01052 
01053 //! Render line to SketchSpace and return reference.
01054 //! This function does not link the Sketch<bool>* in the shape to the sketch returned.
01055 Sketch<bool>* LineData::render() const {
01056   SketchSpace &renderspace = space->getDualSpace();
01057   int const width = renderspace.getWidth();
01058   int const height = renderspace.getHeight();
01059   float x1,y1,x2,y2;
01060   setDrawCoords(x1, y1, x2, y2, width, height);
01061   Sketch<bool>* draw_result = 
01062     new Sketch<bool>(renderspace, "render("+getName()+")");
01063   (*draw_result)->setParentId(getViewableId());
01064   (*draw_result)->setColor(getColor());
01065   *draw_result = 0;
01066   drawline2d(*draw_result, (int)x1, (int)y1, (int)x2, (int)y2);
01067   const_cast<LineData*>(this)->end1Pt().rendering_valid = true;
01068   const_cast<LineData*>(this)->end2Pt().rendering_valid = true;
01069   return draw_result;
01070 }  
01071  
01072   
01073 // This function will be called by both LineData and PolygonData renderers
01074 void LineData::setDrawCoords(float& x1,float& y1, float& x2, float& y2,
01075            const int width, const int height) const {
01076   EndPoint e1(end1Pt());
01077   EndPoint e2(end2Pt());
01078   space->getDualSpace().applyTmat(e1.getCoords());
01079   space->getDualSpace().applyTmat(e2.getCoords());
01080   const EndPoint &left_point = e1.coordX() <= e2.coordX() ? e1 : e2;
01081   const EndPoint &right_point = e1.coordX() <= e2.coordX() ? e2 : e1;
01082 
01083   // Check if horizontal
01084   if((int)left_point.coordY() == (int)right_point.coordY()) { 
01085     if (!left_point.isActive())
01086       x1 = 0;
01087     else x1 = max(0,(int)left_point.coordX());
01088     
01089     if (!right_point.isActive())
01090       x2 = width-1;
01091     else x2 = min(width-1, (int)right_point.coordX());
01092     
01093     y1 = (int)left_point.coordY();
01094     y2 = y1;
01095   } 
01096   else // Check if vertical...
01097     if ((int)left_point.coordX() == (int)right_point.coordX()) { 
01098     
01099     x1 = (int)left_point.coordX();
01100     x2 = x1;
01101     
01102     const EndPoint &top_point = end1Pt().coordY() <= end2Pt().coordY() ? end1Pt() : end2Pt();
01103     const EndPoint &bottom_point = end1Pt().coordY() <= end2Pt().coordY() ? end2Pt() : end1Pt();
01104 
01105     if(!top_point.isActive())
01106       y1 = 0;
01107     else y1 = max(0,(int)top_point.coordY());
01108     
01109     if (!bottom_point.isActive())
01110       y2 = height-1;
01111     else y2 = min(height-1,(int)bottom_point.coordY());
01112   } 
01113 
01114   else {   // Neither horizontal nor vertical...
01115     float m = (right_point.coordY()-left_point.coordY())/(right_point.coordX()-left_point.coordX());
01116     float b = left_point.coordY() - m*left_point.coordX();
01117     
01118     // find image edge intersections
01119     int i0x = (int)((0-b)/m);
01120     int ihx = (int)(((height-1)-b)/m);
01121     int i0y = (int)(m*0+b);
01122     int iwy = (int)(m*(width-1)+b);
01123     
01124     // If left point is active, set starting x and y accordingly.
01125     if(left_point.isActive()) {
01126       x1 = (int)left_point.coordX();
01127       y1 = (int)left_point.coordY();
01128     } 
01129     
01130     // If endpoint 1 extends past image edge...
01131     else { 
01132       
01133       // intersects left edge
01134       if(i0y >= 0 && i0y < height) { 
01135   x1 = 0;
01136   y1 = i0y;
01137       } 
01138       
01139       // intersects top or bottom edge
01140       else { 
01141   
01142   // intersects top first
01143   if (i0x < ihx) { 
01144     x1 = i0x;
01145     y1 = 0;
01146   } 
01147   
01148   // intersects bottom first
01149   else { 
01150     x1 = ihx;
01151     y1 = height-1;
01152   }
01153       }
01154     }
01155     
01156     // If right point is active, set starting x and y accordingly.
01157     if(right_point.isActive()) {
01158       x2 = (int)right_point.coordX();
01159       y2 = (int)right_point.coordY();
01160     } 
01161     else { // endpoint extends to image edge
01162       if(iwy >= 0 && iwy < height) { // intersects right edge
01163   x2 = width-1;
01164   y2 = iwy;
01165       } 
01166       else { // intersects top or bottom edge
01167   if (i0x > ihx) { // intersects top last 
01168     x2 = i0x;
01169     y2 = 0;
01170   } 
01171   else { // intersects bottom last
01172     x2 = ihx;
01173     y2 = height-1;
01174   }
01175       }
01176     }
01177   }
01178 }
01179 
01180 void LineData::drawline2d(Sketch<bool>& canvas, int x0, int y0, int x1, int y1) {
01181   int width = canvas->getWidth();
01182   int height = canvas->getHeight();
01183   
01184   // code below from free Graphics Gems repository, graphicsgems.org
01185   int d, x, y, ax, ay, sx, sy, dx, dy;
01186   dx = x1-x0;  ax = abs(dx)<<1;  sx = SGN(dx);
01187   dy = y1-y0;  ay = abs(dy)<<1;  sy = SGN(dy);
01188   
01189   x = x0;
01190   y = y0;
01191   if ( ax > ay ) {    /* x dominant */
01192     d = ay-(ax>>1);
01193     for (;;) {
01194       if (x >= 0 && y >= 0 && x < width && y < height)
01195   canvas(x,y) = true;
01196       
01197       if (x==x1)
01198   return;
01199       
01200       if ( d >= 0 ) {
01201   y += sy;
01202   d -= ax;
01203       }
01204       
01205       x += sx;
01206       d += ay;
01207     }
01208   }
01209   else {      /* y dominant */
01210     d = ax-(ay>>1);
01211     for (;;) {
01212       if (x >= 0 && y >= 0 && x < width && y < height)
01213   canvas(x,y) = true;
01214       
01215       if ( y == y1 )
01216   return;
01217       
01218       if ( d >= 0 ) {
01219   x += sx;
01220   d -= ay;
01221       }
01222       
01223       y += sy;
01224       d += ax;
01225     }
01226   }
01227 }
01228 //}
01229 
01230 
01231 std::vector<Shape<LineData> > LineData::houghTransform(const Sketch<bool>& fat, 
01232             const Sketch<bool>& skinny,
01233             const Sketch<bool>& occlusions,
01234             const size_t num_lines, int minLength)
01235 {
01236   std::vector<Shape<LineData> > result;
01237   ShapeSpace& ShS = fat->getSpace().getDualSpace();
01238 
01239   const int width = fat->getWidth(), height = fat->getHeight();
01240   const int numR = 120, numTheta = 120;
01241   const int numRf = 40, numThetaf = 40;
01242   int hsize = numR*numTheta, hsizef = numRf * numThetaf;
01243   int htab[hsize], hfine[hsizef]; // Hough accumulator table
01244   float minTheta = 0.0, maxTheta = 2*M_PI; //0.9999*M_PI;
01245   float minR = 0.000, maxR = sqrt((float)width*width+(float)height*height); //240.0;
01246   float thetaSpread = maxTheta - minTheta;
01247   float rSpread = maxR - minR;
01248   //float bestR = 0, bestTheta = 0;
01249 
01250   for (int i = 0; i < hsize; i++)
01251     htab[i] = 0; // zero accumulator table
01252 
01253   // build accumulator table
01254   float theta, r;
01255   int ridx;
01256   for (int x = 0; x < width; x++) {
01257     for (int y = 0; y < height; y++) {
01258       if (fat(x,y) == true) {
01259   for (int tidx = 0; tidx < numTheta ; tidx++) {
01260     theta = minTheta + tidx*thetaSpread/numTheta;
01261     r = (float)x*cos(theta)+(float)y*sin(theta);
01262     ridx = (int)((r-minR)*(float)numR/rSpread); // Is this right calc?
01263     if (ridx >= 0 && ridx < numR) // check ridx bounds
01264       htab[ridx+tidx*numR]++; 
01265   }
01266       }
01267     }
01268   }
01269 
01270 
01271   /*float lineLen = 500;
01272   for (int i = 0; i < numR*numTheta; i++) {
01273     if (htab[i] >= minCount)
01274       {
01275   const float curR = (i%numR)*rSpread/numR + minR;
01276   const float curTheta = (i/numR)*thetaSpread/numTheta + minTheta;
01277   const float x1 = curR*cos(curTheta), y1 = curR*sin(curTheta);
01278   const float x2 = x1+lineLen*cos(curTheta+M_PI/2), y2 = y1+lineLen*sin(curTheta+M_PI/2);
01279   const float x3 = x1-lineLen*cos(curTheta+M_PI/2), y3 = y1-lineLen*sin(curTheta+M_PI/2);
01280 
01281   Point e1(x3,y3), e2(x2,y2);
01282   Shape<LineData> line(ShS,e1,e2);
01283   line->end1Pt().setValid(false);
01284   line->end2Pt().setValid(false);
01285   result.push_back(line);
01286       }
01287       }*/
01288 
01289   // Finely explore the first n best lines in the rough table
01290   int max_val = -1, max_i = 0;
01291   while(result.size() < num_lines) {
01292 
01293     // find maximum value in accumulator table
01294     max_val = -1;
01295     max_i = 0;
01296     for (int i = 0; i < numR*numTheta; i++) {
01297       if (max_val < htab[i]) {
01298   max_val = htab[i];
01299   max_i = i;
01300       }
01301     }
01302 
01303     // Give up if the lines start getting too small
01304     if (max_val < minLength)
01305       break;
01306     
01307 
01308     float bestR = (max_i%numR)*rSpread/numR + minR;
01309     float bestTheta = (max_i/numR)*thetaSpread/numTheta + minTheta;
01310 
01311     // determine parameters for the fine hough iteration
01312     const float fthetaSpread = M_PI/40.0; 
01313     const float frSpread = maxR/40.0;
01314     float fminTheta = bestTheta-fthetaSpread/2.0;
01315     float fminR = bestR-frSpread/2.0;
01316 
01317     for (int i = 0; i < hsizef; i++)
01318       hfine[i] = 0; // zero fine table
01319     
01320     // build fine table
01321     float thetaf, rf;
01322     int ridxf;
01323     for (int x = 0; x < width; x++) {
01324       for (int y = 0; y < height; y++) {
01325   if (skinny(x,y) == true) {
01326     for (int tidx = 0; tidx < numThetaf ; tidx++) {
01327       thetaf = fminTheta + tidx*fthetaSpread/numThetaf;
01328       rf = (float)x*cos(thetaf)+(float)y*sin(thetaf);
01329       ridxf = (int)((rf-fminR)*(float)numRf/frSpread); // Is this right calc?
01330       if (ridxf >= 0 && ridxf < numRf) // check ridx bounds
01331         hfine[ridxf+tidx*numRf]++;  
01332     }
01333   }
01334       }
01335     }
01336 
01337     
01338     // Pull the best line out of the fine table
01339     int max_val_f = -1, max_i_f = 0;
01340     for (int i = 0; i < numRf*numThetaf; i++) {
01341       if (max_val_f < hfine[i]) {
01342   max_val_f = hfine[i];
01343   max_i_f = i;
01344       }
01345     }
01346     float bestRf = (max_i_f%numRf)*frSpread/numRf + fminR;
01347     float bestThetaf = (max_i_f/numRf)*fthetaSpread/numThetaf + fminTheta;
01348 
01349     // Add viable segments to the vector
01350     // Step along the line, looking for segments. 
01351     float lineLen = 500;
01352     // Can be simpler
01353     const float x1 = bestRf*cos(bestThetaf), y1 = bestRf*sin(bestThetaf);
01354     const float x2 = x1+lineLen*cos(bestThetaf+M_PI/2), y2 = y1+lineLen*sin(bestThetaf+M_PI/2);
01355     const float x3 = x1-lineLen*cos(bestThetaf+M_PI/2), y3 = y1-lineLen*sin(bestThetaf+M_PI/2);
01356     
01357     Point e1(x3,y3), e2(x2,y2);
01358     Shape<LineData> line(ShS,e1,e2);
01359 
01360 
01361     line->setColor(skinny->getColor());
01362     line->setParentId(skinny->getViewableId());
01363 
01364     // m is slope of this line (not the normal line)
01365     float m = (y1 != 0) ? -(x1/y1) : BIG_SLOPE;
01366     float b = y1 - m*x1;
01367     
01368     NEW_SKETCH_N(skelDist,uint,visops::edist(skinny));
01369     if ( abs(m) <= 1 )  // when |slope| <= 1, scan along x, else scan along y
01370       line->scanHorizForEndPts(skelDist,occlusions,m,b);
01371     else
01372       line->scanVertForEndPts(skelDist,occlusions,m,b);
01373     
01374     //! Check to see if any endpoints are near any edge of the screen.
01375     //! If they are, invalidate them, assuming that line continues beyond screen.
01376     line->end1_pt.checkValidity(width,height,BEG_DIST_THRESH);
01377     line->end2_pt.checkValidity(width,height,BEG_DIST_THRESH);
01378 
01379 
01380     // Experimental: check how much of the line actually lies along edge pixels
01381     // only include lines that are at least 50% above edge pixels. 
01382     
01383     float len = line->getLength();
01384     EndPoint start = line->end1_pt;
01385     EndPoint dx = (line->end2_pt - start)/len;
01386     float onCount = 0;
01387     for (float i=0; i<len; i++) {
01388       EndPoint cur = start+dx*i;
01389       if (skinny((int)(cur.coordX()), (int)(cur.coordY()))) {
01390   onCount++;
01391       }
01392     }
01393 
01394 
01395     std::cout<<"Line with "<<onCount<<" / "<<len<<" pixels";
01396     if (line->isLongerThan(minLength) && (onCount / len) > .5 )
01397       {
01398   std::cout<<" accepted";
01399   result.push_back(line);
01400       }
01401     std::cout<<std::endl;
01402 
01403 
01404     // Empty the corresponding bins in the rough hough table
01405     //std::cout<<max_i%numR<<","<<max_i/numR<<std::endl;
01406     for(int row = -5; row <= 5; row++)
01407       {
01408   for (int col = -(5-abs(row)); col <=5-abs(row); col++)
01409     {
01410       //int index = (max_i - (max_i%numR) + ((max_i+col)%numR)+row*numR + numR*numTheta)%(numR*numTheta);
01411       //std::cout<<index%numR<<","<<index/numR<<" ";
01412       htab[(max_i - (max_i%numR) + ((max_i+col)%numR)+row*numR + numR*numTheta)%(numR*numTheta)] = 0;
01413     }
01414   //std::cout<<std::endl;
01415       }
01416     //std::cout<<std::endl;
01417   }
01418   
01419   return result;
01420 }
01421 
01422 bool LineData::linesParallel(Shape<LineData> l1, Shape<LineData>l2)
01423 {
01424   const float maxDTheta = .15, minDThetaR = 10.0;
01425   float dTheta = l1->getOrientation() - l2->getOrientation(); 
01426   if (dTheta > M_PI_2)
01427     dTheta = dTheta - M_PI;
01428   if (abs(dTheta) < maxDTheta)
01429     {
01430       if (abs (100*dTheta + (l1->rho_norm - l2->rho_norm)) > minDThetaR)
01431   return true;
01432     }
01433   return false;
01434 }
01435 
01436 LineData& LineData::operator=(const LineData& other) {
01437   if (&other == this)
01438     return *this;
01439   BaseData::operator=(other);
01440   end1_pt = other.end1_pt;
01441   end2_pt = other.end2_pt;
01442   rho_norm = other.rho_norm;
01443   theta_norm = other.theta_norm;
01444   orientation = other.orientation;
01445   length = other.length;
01446   return *this;
01447 }
01448 
01449 
01450 // Compute if two points are on the same side of the line
01451 bool LineData::pointsOnSameSide(const Point& p1, const Point& p2)
01452 {
01453   float dx = end2_pt.coordX() - end1_pt.coordX();
01454   float dy = end2_pt.coordY() - end1_pt.coordY();
01455   
01456   float p1val = (p1.coordY() - end1_pt.coordY())*dx - (p1.coordX() - end1_pt.coordX())*dy;
01457   float p2val = (p2.coordY() - end1_pt.coordY())*dx - (p2.coordX() - end1_pt.coordX())*dy;
01458 
01459   return (p1val>0) == (p2val>0);
01460 }
01461 
01462 // Checks if the distance from the line to the point is less than 1 pixel,
01463 // and checks that the point is within the end points of the line.
01464 bool LineData::pointOnLine(const Point& p)
01465 {
01466   const float BOUNDS_EXTEND = 1.0;
01467   float dx = end2_pt.coordX() - end1_pt.coordX();
01468   float dy = end2_pt.coordY() - end1_pt.coordY();
01469   float val = (p.coordY() - end1_pt.coordY())*dx - (p.coordX() - end1_pt.coordX())*dy;
01470   val /= length;
01471   bool inBounds = (p.coordX() >= (leftPt().coordX() - BOUNDS_EXTEND)) &&
01472     (p.coordX() <= (rightPt().coordX() + BOUNDS_EXTEND)) &&
01473     (p.coordY() >= (topPt().coordY() - BOUNDS_EXTEND)) &&
01474     (p.coordY() <= (bottomPt().coordY() + BOUNDS_EXTEND));
01475   return (val > -1) && (val < 1) && inBounds;
01476 }
01477 
01478 
01479 // Comparison predicates
01480 
01481 bool LineData::LengthLessThan::operator() (const Shape<LineData> &line1, const Shape<LineData> &line2) const {
01482       return line1->getLength() < line2->getLength();
01483 }
01484 
01485 bool LineData::ParallelTest::operator() (const Shape<LineData> &line1, const Shape<LineData> &line2) const {
01486   return angdist(line1->getOrientation(),line2->getOrientation()) <= tolerance;
01487 }
01488 
01489 bool LineData::PerpendicularTest::operator() (const Shape<LineData> &line1, const Shape<LineData> &line2) const {
01490   return angdist(angdist(line1->getOrientation(),line2->getOrientation()), M_PI/2) <= tolerance;
01491 }
01492 
01493 bool LineData::ColinearTest::operator() (const Shape<LineData> &line1, const Shape<LineData> &line2) const {
01494   return ParallelTest(ang_tol)(line1,line2) && 
01495     abs( line1->getRhoNorm() - line2->getRhoNorm() ) <= dist_tol;
01496 }
01497 
01498 bool LineData::IsHorizontal::operator() (const Shape<LineData> &line) const {
01499       const AngPi orient = line->getOrientation();
01500       return  (orient <= threshold) || (orient >= M_PI - threshold);
01501     }
01502 
01503 bool LineData::IsVertical::operator() (const Shape<LineData> &line) const {
01504       const AngPi orient = line->getOrientation();
01505       return  (orient >= threshold) && (orient <= M_PI - threshold);
01506     }
01507 
01508 } // namespace

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