Line first and last points are now set as the projection of first and last supporter

src/laser_scan.cpp

@@ -24,7 +24,7 @@ void LaserScan::ranges2xy(Eigen::Matrix4s _device_T)
     ScalarT prev_range = 0;
     unsigned int ii = 0;
     unsigned int ii_ok = 0;
-    ScalarT kr = 2; //TODO: as a parameters somewhere. 
+    ScalarT kr = 10; //TODO: as a parameters somewhere. 
     Eigen::Vector4s point_laser, point_ref;
 
     //resize to all points case
@@ -57,7 +57,7 @@ void LaserScan::ranges2xy(Eigen::Matrix4s _device_T)
 //             points_(2,ii_ok) = 1;
             
             //check jump. Min dist between consecutive points is r*sin(angle_step_). A jump occurs when this min dist is overpassed by kr times
-            if ( fabs(ranges_[ii]-prev_range) > kr*ranges_[ii]*params_.angle_step_) //jump condition (kr*r*sin(a) ~ kr*r*a)
+            if ( fabs(ranges_[ii]-prev_range) > fabs(kr*ranges_[ii]*params_.angle_step_)) //jump condition (kr*r*sin(a) ~ kr*r*a)
             //if ( fabs(ranges_[ii]-prev_range) > 0.5) //jump condition >0.5m
             {
                 jumps_.push_back(ii_ok);

src/line_finder.cpp

@@ -30,6 +30,42 @@ void LineFinder::fitLine(const Eigen::MatrixXs & _points, LineSegment & _line) c
     //_line.fit_error_ = (_points.transpose() * _line.abc_).array().abs().sum() / (_line.abc_.head(2).norm()*_points.cols());
 }
 
+void LineFinder::pointProjectionToLine(const Eigen::Vector3s & _line, const Eigen::Vector3s & _in_pt, Eigen::Vector3s & _out_pt) const
+{
+    ScalarT a,b,c,qx,qy,dd;
+    
+    //inits
+    a = _line(0); 
+    b = _line(1); 
+    c = _line(2); 
+    qx = _in_pt(0)/_in_pt(2);
+    qy = _in_pt(1)/_in_pt(2); 
+    
+    //compute point projection to line. The formula comes from frocing: 
+    //      1) _out_pt to lie on _lines, and f
+    //      2) lines l2 through _in_pt and _out_pt perpendicular to _line -> normal vectors perpendicular
+    
+    if (a == 0. ) //case a=0 -> vertical line (parallel to y axis)
+    {
+        _out_pt(0) = _in_pt(0);
+        _out_pt(1) = -c/b;
+        return;
+    }
+    
+    if ( b == 0. ) //case b=0 -> vertical line (parallel to y axis)
+    {
+        _out_pt(0) = -c/a;
+        _out_pt(1) = _in_pt(1);
+        return; 
+    }
+    
+    //general case
+    dd = -(a*a + b*b); //denominator. Can't be 0 because either a or b have to be non-zero
+    _out_pt(1) = (b*c + a*b*qx - a*a*qy) / dd;
+    _out_pt(0) = -( c + b*_out_pt(1) ) / a; 
+    
+}
+
 // unsigned int laserscanutils::LineFinder::mergeLines(std::list<LineSegment> & _line_list)
 // {
 //     //TODO

src/line_finder.h

@@ -45,7 +45,16 @@ class LineFinder
         * \param _line: a laserscanutils::Line object of the best fitting line in the Least Squares sense
         * 
         **/
-        void fitLine(const Eigen::MatrixXs & _points, LineSegment & _line) const;        
+        void fitLine(const Eigen::MatrixXs & _points, LineSegment & _line) const;    
+        
+        /** \brief Computes projection of point to line
+         * 
+         * Computes the projection of _in_pt to _line. Result at _out_pt
+         * Everything in homogeneous coordinates
+         * TODO: To be moved elsewhere. No reason why this methos has to be member of LineFinder
+         * 
+         **/
+        void pointProjectionToLine(const Eigen::Vector3s & _line, const Eigen::Vector3s & _in_pt, Eigen::Vector3s & _out_pt) const;
                 
         /** \brief Merges lines of a list
          * 

src/line_finder_hough.cpp

@@ -19,7 +19,7 @@ void LineFinderHough::setHoughParams(const LineFinderHoughParams & _params)
     this->hough_params_ = _params; 
     
     //resize hough grid accordingly
-    hough_grid_rows = (unsigned int)ceil(M_PI/hough_params_.theta_step_);//[0,PI]
+    hough_grid_rows = (unsigned int)ceil(M_PI/hough_params_.theta_step_);//theta in [0,PI]
     hough_grid_rows_half = (unsigned int)ceil(0.5*M_PI/hough_params_.theta_step_);//middle row index
     hough_grid_cols = (unsigned int)ceil(2*hough_params_.range_max_/hough_params_.range_step_);//[-rmax,+rmax]
     hough_grid_cols_half = (unsigned int)ceil(hough_params_.range_max_/hough_params_.range_step_);//middle col index
@@ -33,11 +33,11 @@ void LineFinderHough::setHoughParams(const LineFinderHoughParams & _params)
 unsigned int LineFinderHough::findLines( const Eigen::MatrixXs & _points, 
                                          std::list<laserscanutils::LineSegment> & _line_list)
 {
-    double theta, range; 
+    ScalarT theta, range; 
     int kr; 
     LineSegment line;
-    double xmax, xmin, ymax, ymin;
-    Eigen::Matrix3s pt; //auxiliary point
+    ScalarT xmax, xmin, ymax, ymin;
+    Eigen::Vector3s q_pt, p_pt; //auxiliary points
     Eigen::MatrixXs pts; //auxiliar array of points
     std::list<Eigen::Vector3s>::iterator pts_it; 
     
@@ -105,7 +105,7 @@ unsigned int LineFinderHough::findLines( const Eigen::MatrixXs & _points,
                     line.range_ = jj*hough_params_.range_step_; //range
 
                     //set starting and ending points of the line 
-                    //TODO: take into account normal convention. abc_ should point to the free space
+                    //TODO: apply convention: (point_first_ - point_last_ , 0)^T x (a/c, b/c, 0)^T = UP ! (z>0)
                     xmax=-100; xmin=100; ymax=-100; ymin=100; 
                     for (pts_it = hough_grid_.at(ii).at(jj).begin(); pts_it != hough_grid_.at(ii).at(jj).end(); pts_it++)
                     {
@@ -115,22 +115,23 @@ unsigned int LineFinderHough::findLines( const Eigen::MatrixXs & _points,
                         if (pts_it->x() < xmin) xmin = pts_it->x(); 
                         if (pts_it->y() < ymin) ymin = pts_it->y(); 
                     }
-                    if (ii < hough_grid_rows_half) //first and third quartile of r-theta plane
+                    if (ii < hough_grid_rows_half) //theta in [0,PI/2]  
                     {
-                        //TODO
-                        //pt << xmin,ymax,1;
-                        //line.point_first_ << projection of pt to line.abc_
-                        //pt << xmax,ymin,1;
-                        //line.point_last_ << projection of pt to line.abc_                        
-                        line.point_first_ << xmin,ymax,1;
-                        line.point_last_ << xmax,ymin,1;
-                        
+                        q_pt << xmin,ymax,1;
+                        pointProjectionToLine(line.abc_, q_pt, p_pt); 
+                        line.point_first_ << p_pt; 
+                        q_pt << xmax,ymin,1;
+                        pointProjectionToLine(line.abc_, q_pt, p_pt); 
+                        line.point_last_ << p_pt; 
                     }
-                    else //second and fourth quartile of r-theta plane
+                    else //theta in [PI/2,PI]
                     {
-                        //TODO same as above
-                        line.point_first_ << xmin,ymin,1;
-                        line.point_last_ << xmax,ymax,1;                    
+                        q_pt << xmin,ymin,1;
+                        pointProjectionToLine(line.abc_, q_pt, p_pt); 
+                        line.point_first_ << p_pt; 
+                        q_pt << xmax,ymax,1;
+                        pointProjectionToLine(line.abc_, q_pt, p_pt); 
+                        line.point_last_ << p_pt; 
                     }
                     
                     //push back the line to the list

src/line_segment.h

@@ -21,7 +21,7 @@ class LineSegment
         /** \brief Homogeneous parameterization of the line to which the segment lies on. 
          * 
          * Homogeneous parameterization of the line to which the segment lies on: (a,b,c)^T -> ax+by+c=0. 
-         * Vector (a,b,c)^T is the normal vector, pointing to a free surface
+         * Vector (a/c,b/c)^T is the normal vector, pointing to a free surface
          * 
          */
         Eigen::Vector3s abc_;