First version of corner detection. Builds ok, tested with real data , but too...

First version of corner detection. Builds ok, tested with real data , but too many corners appera... still debugging

src/corner_finder_range_diff.cpp

@@ -38,10 +38,12 @@ int CornerFinderRangeDiff::findCorners(const laserscanutils::LaserScan & _scan,
              ( _scan.checkScanJumps(ii, HALF_WINDOW_SIZE) == false ) )
         {
             //fit lines at left and right of the point
-            line_finder_.fitLine(_scan.points_.block(0,ii-HALF_WINDOW_SIZE,3,HALF_WINDOW_SIZE+1), line1);
-            line_finder_.fitLine(_scan.points_.block(0,ii,3,HALF_WINDOW_SIZE+1), line2);    
+//             std::cout << "findCorners(): " << __LINE__ << ": ii: " << ii << "/" << _scan.ranges_.size() << std::endl; 
+            line_finder_.fitLine(_scan.points_all_.block(0,ii-HALF_WINDOW_SIZE,3,HALF_WINDOW_SIZE+1), line1);
+            line_finder_.fitLine(_scan.points_all_.block(0,ii,3,HALF_WINDOW_SIZE+1), line2);    
             
             //check line error 
+//             std::cout << "findCorners(): " << __LINE__ << std::endl; 
             e1 = line1.fit_error_ / ( (ScalarT)HALF_WINDOW_SIZE * 2 + 1);
             e2 = line2.fit_error_ / ( (ScalarT)HALF_WINDOW_SIZE * 2 + 1);
             if ( ( e1 < LINE_FIT_ERROR ) && ( e2 < LINE_FIT_ERROR ) )
@@ -50,11 +52,12 @@ int CornerFinderRangeDiff::findCorners(const laserscanutils::LaserScan & _scan,
                 angle = line1.angleToLine(line2); //angle is in [0,2pi]
                 if ( ( angle < M_PI-MIN_ANGLE ) || ( angle > M_PI+MIN_ANGLE ) )
                 {
-//                     //corner found !!
-//                     new_corner.point_ << _scan.points_all_.block<1,3>(0,);
-//                     //new_corner.orientation_ = ;
-//                     new_corner.aperture_ = angle; 
-//                     _corner_list.push_back(new_corner); 
+                    //corner found !!
+                    std::cout << "findCorners(): " << __LINE__ << std::endl; 
+                    new_corner.point_ << _scan.points_all_.block<3,1>(0,ii);//TODO crossing point between lines
+                    new_corner.orientation_ = 0;//TODO
+                    new_corner.aperture_ = angle; 
+                    _corner_list.push_back(new_corner); 
                 }
             }
         }

src/laser_scan.cpp

@@ -35,6 +35,7 @@ void LaserScan::ranges2xy(Eigen::Matrix4s _device_T)
 
     //resize onvolved vectors for all points case
     ranges_.resize(ranges_raw_.size());
+    points_all_.resize(3,ranges_raw_.size());
     points_.resize(3,ranges_raw_.size());
     //jumps_indexes_.resize(ranges_raw_.size());
     jumps_mask_.resize(ranges_raw_.size());
@@ -58,6 +59,9 @@ void LaserScan::ranges2xy(Eigen::Matrix4s _device_T)
             //apply device mounting point calibration (p_r = T * p_l)
             point_ref = _device_T*point_laser; 
 
+            //set to points_all_ as a 2D homogeneous
+            points_all_.block<3,1>(0,ii) << point_ref(0),point_ref(1),1;
+
             //set to points_ as a 2D homogeneous
             points_.block<3,1>(0,ii_ok) << point_ref(0),point_ref(1),1;
                         
@@ -83,6 +87,7 @@ void LaserScan::ranges2xy(Eigen::Matrix4s _device_T)
         {
             ranges_[ii] = -1.; 
             jumps_mask_[ii] = true; 
+            points_all_.block<3,1>(0,ii) << 0,0,0;
             prev_range = 0; 
         }
         
@@ -108,8 +113,8 @@ bool LaserScan::checkScanCorrectness(unsigned int _idx, unsigned int _idx_range)
     if ( ! is_raw_processed_ ) return false;
 
     //set loop bounds
-    unsigned int ii_init = std::max( 0 , (_idx-_idx_range) );
-    unsigned int ii_end = std::min( ranges_.size()-1 , (_idx+_idx_range) );
+    unsigned int ii_init = std::max( (unsigned int)0 , (_idx-_idx_range) );
+    unsigned int ii_end = std::min( (unsigned int)(ranges_.size()-1) , (_idx+_idx_range) );
     
     //proceed
     for (unsigned int ii=ii_init; ii<=ii_end; ii++ )
@@ -133,8 +138,8 @@ bool LaserScan::checkScanJumps(unsigned int _idx, unsigned int _idx_range) const
     if ( ! is_raw_processed_ ) return true;
     
     //set loop bounds
-    unsigned int ii_init = std::max( 0 , (_idx-_idx_range) );
-    unsigned int ii_end = std::min( ranges_.size()-1 , (_idx+_idx_range) );
+    unsigned int ii_init = std::max( (unsigned int)0 , (_idx-_idx_range) );
+    unsigned int ii_end = std::min( (unsigned int)(ranges_.size()-1) , (_idx+_idx_range) );
     
     //proceed
     for (unsigned int ii=ii_init; ii<=ii_end; ii++ )

src/laser_scan.h

@@ -59,7 +59,7 @@ class LaserScan
         //Ordered, Marked 2D points, each one expressed in homogeneous coordinates (x,y,1)^T. 
         //NaN's, inf's and out of range are marked as points at infty->last component set to 0.
         //Same size as ranges_raw_
-        //Eigen::MatrixXs points_all_;
+        Eigen::MatrixXs points_all_;
         
         //Ordered, Correct 2D points, each one expressed in homogeneous coordinates (x,y,1)^T. 
         //NaN's, inf's and out of range are filtered out, implying:

src/line_segment.cpp

@@ -58,7 +58,7 @@ void LineSegment::pointProjectionToLine(const Eigen::Vector3s & _in_pt, Eigen::V
     _out_pt(0) = -( c + b*_out_pt(1) ) / a; 
 }
 
-double angleToLine(const laserscanutils::LineSegment & _line) const
+double LineSegment::angleToLine(const laserscanutils::LineSegment & _line) const
 {
     //compute each angle at 4-quadrant in [-pi,pi]
     ScalarT angle_this = atan2(abc_(1)/abc_(2), abc_(0)/abc_(2));