findCorners from lines Successfully tested with real data. Grid2D successfully...

findCorners from lines Successfully tested with real data. Grid2D successfully tested with real data

src/corner_finder.cpp

@@ -19,20 +19,42 @@ int CornerFinder::findCorners( const Eigen::MatrixXs & _points, std::list<lasers
     return -1; 
 }
 
-int CornerFinder::findCorners( const std::list<laserscanutils::LineSegment> & _lines, std::list<laserscanutils::CornerPoint> & _corner_list) const
+int CornerFinder::findCorners( std::list<laserscanutils::LineSegment> & _lines, std::list<laserscanutils::CornerPoint> & _corner_list) const
 {
-//     //local vars
-//     std::list<laserscanutils::LineSegment>::iterator line_i, line_j; 
-//     
-//     //loop over all lines , ij, and look for vertices greater than some threshold
-//     for (line_i = _lines.begin(); line_i != _lines.end(); line_i++)
-//     {
-//         for (line_j = _lines.begin(); line_j != _lines.end(); line_j++)
-//         {
-//             
-//         }
-//     }
+    //tunning
+    ScalarT th_ddsq = 0.05*0.05; //squared distance threshold
+    ScalarT th_aa = 45.*M_PI/180.; //minimum corner angle allowed
     
+    //local vars
+    std::list<laserscanutils::LineSegment>::iterator line_i, line_j, line_i_limit; 
+    ScalarT ddsq, aa; 
+    CornerPoint corner; 
+    bool corner_found = false; 
+    
+    
+    //loop over all lines , and look for neigbor vertices at a distance smaller than some threshold
+    line_i_limit = _lines.end(); 
+    line_i_limit --;
+    for ( line_i = _lines.begin(); line_i != line_i_limit; line_i++ ) //HERE!!! check end()-1!!
+    {
+        //set line_j as the next line
+        line_j = line_i; 
+        line_j ++; 
+        
+        //compute distance and angle bteween consecutive lines
+        ddsq = (line_i->point_last_ - line_j->point_first_).squaredNorm(); 
+        aa = fabs(line_i->angleToLine(*line_j));
+        if ( ( ddsq < th_ddsq ) && ( aa > th_aa) )
+        {
+            corner.point_ = 0.5 * (line_i->point_last_ + line_j->point_first_); //corner is set at the mean point
+            _corner_list.push_back(corner);
+        }
+    }
+}
+
+void CornerFinder::print() const
+{
+    //TODO
 }
 
 }//namespace

src/corner_finder.h

@@ -62,14 +62,14 @@ class CornerFinder
         * Returns corners as a std::list<CornerPoint>
         * 
         * \Requires: 
-        *    \param _lines: List of input lines
+        *    \param _lines: List of input lines. Assumed ordered, as provided by LineFinderIterative::findLines()
         * 
         * \Provides: 
         *    \param _corner_list set of corners extracted from _lines
         *    \return Number of corners extracted, or -1 if some error
         *
         */
-        virtual int findCorners( const std::list<laserscanutils::LineSegment> & _lines, 
+        virtual int findCorners( std::list<laserscanutils::LineSegment> & _lines, 
                                  std::list<laserscanutils::CornerPoint> & _corner_list) const;
                                                                   
         /** \brief Print things
@@ -77,7 +77,7 @@ class CornerFinder
          * Print things about this class
          * 
          **/
-        virtual void print() const = 0;
+        virtual void print() const;
 };
 }//namespace
 #endif

src/grid_2d.cpp

 
 #include "grid_2d.h"
 
+namespace laserscanutils
+{
+
 Grid2D::Grid2D(const double & _sz_x, const double & _sz_y, 
                const double & _dx, const double & _dy, 
                const double & _xmax, const double & _ymax) : 
@@ -175,7 +178,7 @@ void Grid2D::odometryUpdate(const std::vector<Eigen::Vector4d> & _odometry)
         
 }
 
-void Grid2D::lidarUpdate(const Eigen::MatrixXd & _scan_points)
+void Grid2D::lidarUpdate(const Eigen::MatrixXs & _scan_points)
 {
     unsigned int ii,jj;
     bool point_in_grid; 
@@ -259,3 +262,6 @@ void Grid2D::debugFill()
         }
     }
 }
+
+}//end of namespace
+

src/grid_2d.h

@@ -16,6 +16,9 @@
 #include <iostream>
 #include <math.h>
 
+namespace laserscanutils
+{
+
 /** \brief A 2D grid class
  *
  * A 2D grid class (yet another 2d grid class ...)
@@ -187,7 +190,7 @@ class Grid2D
          *  \param _scan_points homogeneous coordinates (x y 1)^T of the scan points,  with respect to the grid frame
          * 
          **/
-        void lidarUpdate(const Eigen::MatrixXd & _scan_points);
+        void lidarUpdate(const Eigen::MatrixXs & _scan_points);
 
         /** \brief Radar update
          * 
@@ -227,4 +230,7 @@ class Grid2D
          **/
         void debugFill(); 
 };
+
+}//end of namespace
+
 #endif

src/laser_scan.cpp

@@ -155,7 +155,7 @@ bool LaserScan::checkScanJumps(unsigned int _idx, unsigned int _idx_range) const
     return jump; 
 }
 
-void LaserScan::findSegments(std::list<laserscanutils::ScanSegment> & _segment_list)
+void LaserScan::findSegments(std::list<laserscanutils::ScanSegment> & _segment_list, bool _compute_params)
 {
     std::list<unsigned int>::iterator jumps_it, next_jumps_it, jumps_last; 
     unsigned int num_points; 
@@ -180,6 +180,9 @@ void LaserScan::findSegments(std::list<laserscanutils::ScanSegment> & _segment_l
         {
             _segment_list.back().points_.block<3,1>(0,ii) << this->points_.block<3,1>(0,(*jumps_it)+ii);
         }
+        
+        //check _compute_params to compute all segment params
+        if ( _compute_params ) _segment_list.back().computeAll(); 
     }
 }
 

src/laser_scan.h

@@ -143,9 +143,10 @@ class LaserScan
         *
         * Find segments based on jumps of consecutive scan points
         * Do not compute segment parameters, just fill ScanSegment.points_
+        * If _compute_params set to true, this method calls ScanSegment::computeAll() for each segment in the output list
         *
         **/
-        void findSegments(std::list<laserscanutils::ScanSegment> & _segment_list);        
+        void findSegments(std::list<laserscanutils::ScanSegment> & _segment_list, bool _compute_params = true);        
       
 };