changed polylines defined extremes condition and implementation of

several functions

src/CMakeLists.txt

@@ -77,5 +77,5 @@ INSTALL(FILES ${HDRS}
 INSTALL(FILES ../Findlaser_scan_utils.cmake DESTINATION ${CMAKE_ROOT}/Modules/)
 
 #Build examples & tests
-# MESSAGE("Building examples and tests.")
-# ADD_SUBDIRECTORY(examples)
+MESSAGE("Building tests.")
+ADD_SUBDIRECTORY(test)

src/laser_scan.cpp

@@ -86,7 +86,7 @@ bool LaserScan::checkScanJumps(unsigned int _idx, unsigned int _idx_range) const
 //{
 //}
 
-void LaserScan::ranges2xy(const LaserScanParams& _scan_params, Eigen::Matrix4s _device_T, const ScalarT& _kr)
+void LaserScan::ranges2xy(const LaserScanParams& _scan_params, Eigen::Matrix4s _device_T, const ScalarT& _jump_th)
 {
     //local vars
     ScalarT azimuth = _scan_params.angle_min_;
@@ -120,22 +120,20 @@ void LaserScan::ranges2xy(const LaserScanParams& _scan_params, Eigen::Matrix4s _
             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;
+            points_all_.col(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;
+            points_.col(ii_ok) << point_ref(0), point_ref(1), 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) > fabs(_kr * ranges_[ii] * _scan_params.angle_step_)) //jump condition (kr*r*sin(a) ~ kr*r*a)
+            if (fabs(ranges_[ii] - prev_range) > _jump_th) //jump condition
             {
                 jumps_indexes_.push_back(ii_ok); //indexes over points_
                 jumps_mask_[ii] = true; //masks over ranges_
             }
             else
-            {
                 jumps_mask_[ii] = false;
-            }
             
             //increment ok counter
             ii_ok++;
@@ -147,7 +145,7 @@ void LaserScan::ranges2xy(const LaserScanParams& _scan_params, Eigen::Matrix4s _
         {
             ranges_[ii] = -1.;
             jumps_mask_[ii] = true;
-            points_all_.block<3, 1>(0, ii) << 0, 0, 0;
+            points_all_.col(ii) << 0, 0, 0;
             //prev_range = 0;
         }
        
@@ -165,60 +163,44 @@ void LaserScan::ranges2xy(const LaserScanParams& _scan_params, Eigen::Matrix4s _
     is_raw_processed_ = true;
 }
 
-void LaserScan::findSegments(const LaserScanParams& _scan_params, std::list<laserscanutils::ScanSegment>& _segment_list, bool _compute_params, const ScalarT& _kr_segment, const ScalarT& _kr_defined)
+void LaserScan::findSegments(const LaserScanParams& _scan_params, std::list<laserscanutils::ScanSegment>& _segment_list, bool _compute_params, const ScalarT& _jump_th_segment)
 {
     //local vars
-    std::list<unsigned int>::iterator jumps_it, next_jumps_it, jumps_last;
     unsigned int num_points;
-    ScalarT d1sq, d2sq, d12sq;
 
     //in case ths input scan is not yet raw processed, do it
-    if (!is_raw_processed_) ranges2xy(_scan_params, Eigen::Matrix4s::Identity(), _kr_segment);
+    if (!is_raw_processed_)
+        ranges2xy(_scan_params, Eigen::Matrix4s::Identity(), _jump_th_segment);
     
-    //set jumps_last to the last valid element of jumps
-    jumps_last = std::prev(jumps_indexes_.end());
-
     //run over all jumps (except the last, which indicates the closing index) 
-    for (jumps_it = jumps_indexes_.begin(); jumps_it != jumps_last; jumps_it++)
+    for (auto jumps_it = jumps_indexes_.begin(); jumps_it != std::prev(jumps_indexes_.end()); jumps_it++)
     {
         //new segment in the list
         _segment_list.push_back(ScanSegment());
- 
+
         //check how many points 
-        next_jumps_it = std::next(jumps_it);
-        //next_jumps_it++;
-        num_points = (*next_jumps_it) - (*jumps_it);
- 
+        num_points = (*std::next(jumps_it)) - (*jumps_it);
+
         //fill points
         _segment_list.back().points_.resize(3, num_points);
         for (unsigned int ii = 0; ii < num_points; ii++)
-        {
-            _segment_list.back().points_.block<3, 1>(0, ii) << this->points_.block<3, 1>(0, (*jumps_it) + ii);
-        }
+            _segment_list.back().points_.col(ii) << this->points_.col((*jumps_it) + ii);
         
         //set segment attributes
-         _segment_list.back().idx_first_ = (*jumps_it); 
-         _segment_list.back().idx_last_ = (*jumps_it + num_points -1); 
+        _segment_list.back().idx_first_ = (*jumps_it);
+        _segment_list.back().idx_last_ = (*jumps_it + num_points -1);
+
+        // prev point
         if ( _segment_list.back().idx_first_ == 0)
-            _segment_list.back().first_defined_ = false;
+            _segment_list.back().prev_point_ = points_.col(_segment_list.back().idx_first_); // no previous point: first
         else
-        {
-            d1sq = points_.block<2,1>(0, _segment_list.back().idx_first_-1).squaredNorm();
-            d2sq = points_.block<2,1>(0, _segment_list.back().idx_first_).squaredNorm();
-            //_segment_list.back().first_defined_ = ( d1sq > d2sq ); //true if previous scan point is further
-            d12sq = (points_.block<2,1>(0, _segment_list.back().idx_first_)-points_.block<2,1>(0, _segment_list.back().idx_first_-1)).squaredNorm();
-            _segment_list.back().first_defined_ = ( d2sq * _scan_params.angle_step_ * _scan_params.angle_step_ * _kr_defined * _kr_defined > d12sq && d1sq > d2sq ); //true if previous scan point is further than _kr times the corresponding distance to the angle increment
-        }
+            _segment_list.back().prev_point_ = points_.col(_segment_list.back().idx_first_-1);
+
+        // next point
         if ( _segment_list.back().idx_last_ == points_.cols()-1)
-            _segment_list.back().last_defined_ = false;
+            _segment_list.back().next_point_ = points_.col(_segment_list.back().idx_last_); // no next point: last
         else
-        {
-            d1sq = points_.block<2,1>(0, _segment_list.back().idx_last_).squaredNorm();
-            d2sq = points_.block<2,1>(0, _segment_list.back().idx_last_+1).squaredNorm();
-            //_segment_list.back().last_defined_ = ( d2sq > d1sq ); //true if next scan point is further
-            d12sq = (points_.block<2,1>(0, _segment_list.back().idx_first_)-points_.block<2,1>(0, _segment_list.back().idx_first_+1)).squaredNorm();
-            _segment_list.back().last_defined_ = ( d1sq * _scan_params.angle_step_ * _scan_params.angle_step_ * _kr_defined * _kr_defined > d12sq && d2sq > d1sq ); //true if previous scan point is further than _kr times the corresponding distance to the angle increment
-        }
+            _segment_list.back().next_point_ = points_.col(_segment_list.back().idx_last_+1);
 
         //check _compute_params to compute all segment params
         if (_compute_params)

src/laser_scan.h

@@ -60,13 +60,13 @@ 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::Matrix<ScalarT, 3, Eigen::Dynamic> 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:
         //          - not necessarily regular angular increments between consecutive points
         //          - size equal or smaller than ranges_raw_
-        Eigen::MatrixXs points_;
+        Eigen::Matrix<ScalarT, 3, Eigen::Dynamic> points_;
 
         //For each element in ranges_, r_i, indicates if there is a jump (true) between that element and the previouos. 
         //Same size as ranges_raw_
@@ -125,7 +125,7 @@ class LaserScan
          * Set also the jumps_ vector, which after the call holds the indexes to points_ where a scan segment starts
          *
          **/
-        void ranges2xy(const LaserScanParams& _scan_params, Eigen::Matrix4s _device_T = Eigen::Matrix4s::Identity(), const ScalarT& _kr = 10);
+        void ranges2xy(const LaserScanParams& _scan_params, Eigen::Matrix4s _device_T = Eigen::Matrix4s::Identity(), const ScalarT& _jump_th = 1);
         //TODO void processRaw()-> from ranges_raw_ fills: ranges_, points_, jumps_indexes and jumps_mask
 
         /** \brief Check for scan correctness 
@@ -153,7 +153,7 @@ class LaserScan
          * If _compute_params set to true, this method calls ScanSegment::computeAll() for each segment in the output list
          *
          **/
-        void findSegments(const LaserScanParams& _scan_params, std::list<laserscanutils::ScanSegment>& _segment_list, bool _compute_params = true, const ScalarT& _kr_segment = 10, const ScalarT& _kr_defined = 20);
+        void findSegments(const LaserScanParams& _scan_params, std::list<laserscanutils::ScanSegment>& _segment_list, bool _compute_params = true, const ScalarT& _jump_th_segment = 1);
 
 };
 

src/line_finder_iterative.cpp

@@ -24,48 +24,11 @@ void LineFinderIterative::setIlfParams(const LineFinderIterativeParams & _params
     ilf_params_.min_supports_ = _params.min_supports_;
 }
 
-unsigned int LineFinderIterative::findLines( const Eigen::MatrixXs & _points, 
+unsigned int LineFinderIterative::findLines( const Eigen::Matrix<ScalarT, 3, Eigen::Dynamic> & _points,
                                              std::list<laserscanutils::LineSegment> & _line_list) const
 {
-    //local vars
-    std::list<laserscanutils::LineSegment>::const_iterator l1_it; //, l2_it; 
-    double dsq1,dsq2; 
-
     //Find lines recursively
-    this->findLinesRecursive(_points, _line_list, 0, _points.cols()-1, false, false);
-    
-    //loop over all lines to set extreme points to true or false
-    /*
-    for (l1_it = _line_list.begin(); l1_it != _line_list.end(); l1_it++)
-    {
-        std::cout << __FILE__ << ": " << __LINE__ << std::endl; 
-        //first extreme line point. Check if it is also extreme of the scan
-        if ( l1_it->idx_first_ == 0 ) l1_it->is_first_extreme_ == false; 
-        else
-        {
-            std::cout << __FILE__ << ": " << __LINE__ << "idx_first_: " << l1_it->idx_first_ << "; _points.cols(): " << _points.cols() << std::endl; 
-            dsq1 = _points.block<3,1>(0,l1_it->idx_first_).head(2).squaredNorm();
-            dsq2 = _points.block<3,1>(0,l1_it->idx_first_-1).head(2).squaredNorm();
-            if ( dsq2 > dsq1 ) l1_it->is_first_extreme_ == true; //previous scan point is further
-            else l1_it->is_first_extreme_ == false; 
-            std::cout << __FILE__ << ": " << __LINE__ << std::endl; 
-        }
-        
-        //last extreme line point. Check if it is also extreme of the scan
-        std::cout << __FILE__ << ": " << __LINE__ << std::endl; 
-        if ( l1_it->idx_last_ >= _points.cols()-1 ) l1_it->is_last_extreme_ == false; 
-        else
-        {
-            std::cout << __FILE__ << ": " << __LINE__ << std::endl; 
-            dsq1 = _points.block<3,1>(0,l1_it->idx_last_).head(2).squaredNorm();
-            dsq2 = _points.block<3,1>(0,l1_it->idx_last_+1).head(2).squaredNorm();
-            if ( dsq2 > dsq1 ) l1_it->is_last_extreme_ == true;//next scan point is further
-            else l1_it->is_last_extreme_ == false;             
-            std::cout << __FILE__ << ": " << __LINE__ << std::endl; 
-        }
-            
-    }
-    */
+    this->findLinesRecursive(_points, _line_list, 0, _points.cols()-1);
     
     //return number of lines detected
     return _line_list.size(); 
@@ -74,16 +37,13 @@ unsigned int LineFinderIterative::findLines( const Eigen::MatrixXs & _points,
 unsigned int LineFinderIterative::findLines( const laserscanutils::ScanSegment & _segment,  
                                              std::list<laserscanutils::LineSegment> & _line_list) const
 {
-
     //Find lines recursively
-    this->findLinesRecursive(_segment.points_, _line_list, 0, _segment.points_.cols()-1, _segment.first_defined_, _segment.last_defined_); 
-    
+    this->findLinesRecursive(_segment.points_, _line_list, 0, _segment.points_.cols()-1);
     
     //return number of lines detected
     return _line_list.size(); 
 }
 
-
 unsigned int LineFinderIterative::linesToPolylines( const std::list<laserscanutils::LineSegment> & _line_list, 
                                                     std::list<laserscanutils::Polyline> & _polyline_list, 
                                                     double _dist_threshold_sq) const
@@ -96,7 +56,7 @@ unsigned int LineFinderIterative::linesToPolylines( const std::list<laserscanuti
     
     //add first line
     l1_it = _line_list.begin();
-    _polyline_list.push_back( Polyline(l1_it->point_first_,l1_it->point_last_,l1_it->first_defined_,l1_it->last_defined_) );
+    _polyline_list.push_back( Polyline(l1_it->point_first_,l1_it->point_last_, false, false) );
 
     //check consecutive lines , if they are close or not. 
     l2_it = l1_it; 
@@ -107,11 +67,10 @@ unsigned int LineFinderIterative::linesToPolylines( const std::list<laserscanuti
         if ( (l1_it->point_last_-l2_it->point_first_).squaredNorm() < _dist_threshold_sq ) //consecutive lines are close -> connected
         {
             _polyline_list.back().addPoint(l2_it->point_last_);
-            _polyline_list.back().last_defined_ = l2_it->last_defined_; 
         }
         else //not connected , so start a new polyline
         {
-            _polyline_list.push_back( Polyline(l2_it->point_first_,l2_it->point_last_,l2_it->first_defined_,l2_it->last_defined_) );
+            _polyline_list.push_back( Polyline(l2_it->point_first_,l2_it->point_last_, false, false) );
         }
     }
     
@@ -119,7 +78,7 @@ unsigned int LineFinderIterative::linesToPolylines( const std::list<laserscanuti
     return _polyline_list.size(); 
 }
 
-unsigned int LineFinderIterative::findPolylines( const Eigen::MatrixXs & _points,
+unsigned int LineFinderIterative::findPolylines( const Eigen::Matrix<ScalarT, 3, Eigen::Dynamic> & _points,
                                                  std::list<laserscanutils::Polyline> & _polyline_list,
                                                  double _dist_threshold_sq) const
 {
@@ -143,17 +102,32 @@ unsigned int LineFinderIterative::findPolylines(laserscanutils::LaserScan & _sca
     std::list<laserscanutils::LineSegment> line_list;
     
     //get scan segments
-    _scan.findSegments(_scan_params, segment_list, true, ilf_params_.range_k_segment_, ilf_params_.range_k_defined_);
+    _scan.findSegments(_scan_params, segment_list, true, ilf_params_.range_jump_segment_);
     
     //for each scan segment, gets line segments
     for ( segment_it = segment_list.begin(); segment_it != segment_list.end(); segment_it++ ) 
     {
-        //this->findLines(segment_it->points_, line_list);
+        // find lines in segment
+        line_list.clear();
         this->findLines(*segment_it, line_list);
+
+        // create polyline with first line points
+        laserscanutils::Polyline polyline(line_list.front().point_first_, line_list.front().point_last_, false, false);
+
+        // add rest of the lines points
+        for (auto line_it = std::next(line_list.begin()); line_it != line_list.end(); line_it++)
+            polyline.addPoint(line_it->point_last_);
+
+        // Set defined extremes
+        polyline.first_defined_ = line_list.front().point_first_.head(2).squaredNorm() < segment_it->prev_point_.head(2).squaredNorm() &&
+                                  segment_it->prev_point_.transpose() * line_list.front().abc_ > ilf_params_.dist_th_defined_;
+        polyline.last_defined_  = line_list.back().point_last_.head(2).squaredNorm() < segment_it->next_point_.head(2).squaredNorm() &&
+                                  segment_it->next_point_.transpose() * line_list.back().abc_ > ilf_params_.dist_th_defined_;
+
+        _polyline_list.push_back(polyline);
     }
 
-    //from the line list, compute polylines, and return
-    return this->linesToPolylines(line_list, _polyline_list); 
+    return _polyline_list.size();
 }
 
 
@@ -165,39 +139,30 @@ void LineFinderIterative::print() const
 //************************************************************
 //PROTECTED METHODS
 
-unsigned int LineFinderIterative::findLinesRecursive( const Eigen::MatrixXs & _points, 
+unsigned int LineFinderIterative::findLinesRecursive( const Eigen::Matrix<ScalarT, 3, Eigen::Dynamic> & _points,
                                                       std::list<laserscanutils::LineSegment> & _line_list, 
                                                       unsigned int _idx_o, 
-                                                      unsigned int _idx_e,
-                                                      bool _first_defined,
-                                                      bool _last_defined ) const
+                                                      unsigned int _idx_e) const
 {
     LineSegment line;//, line1, line2; 
-    ScalarT max_dist, dist, r_prev_sq, r_sq, r_next_sq;
+    ScalarT max_dist, dist;
     unsigned int max_ii; 
     bool split_point_defined; 
     
     //If not enough points, return, nothing to do. 
-    if ( (_idx_e-_idx_o + 1) < ilf_params_.min_supports_ ) return 0; 
+    if ( (_idx_e-_idx_o + 1) < ilf_params_.min_supports_ )
+        return 0;
     
     //fits a line between points at _idx_o and _idx_e
-    line.abc_ = _points.block<3,1>(0,_idx_o).cross(_points.block<3,1>(0,_idx_e));
+    line.abc_ = _points.col(_idx_o).cross(_points.col(_idx_e));
     
     //normalize the line
     line.normalize(); 
 
-    //for each point in (_idx_o,_idx_e), check distance to line, and get the farthest point to line
-    max_dist = 0; 
-    for (unsigned int ii=_idx_o + 1; ii<_idx_e; ii++)
-    {
-        dist = fabs( line.abc_.dot(_points.block<3,1>(0,ii)) );
-        if ( dist > max_dist )
-        {
-            max_dist = dist; 
-            max_ii = ii; 
-        }
-    }
-    
+    // get the farthest point to line
+    max_dist = (_points.middleCols(_idx_o, _idx_e-_idx_o+1).transpose() * line.abc_).array().abs().maxCoeff(&max_ii);
+    max_ii += _idx_o;
+
     //check if farthest point at distance lower than threshold
     if ( max_dist < ilf_params_.max_fit_error_ ) //straight line case
     {
@@ -205,43 +170,20 @@ unsigned int LineFinderIterative::findLinesRecursive( const Eigen::MatrixXs & _p
         fitLine(_points, line);
         
         //fill line segment attributes
-        line.point_first_ << _points.block<3,1>(0,_idx_o);
-        line.point_last_ << _points.block<3,1>(0,_idx_e); 
+        line.point_first_ << _points.col(_idx_o);
+        line.point_last_ << _points.col(_idx_e);
         line.np_ = _idx_e - _idx_o + 1;
         line.idx_first_ = _idx_o;
         line.idx_last_ = _idx_e;
-        line.first_defined_ = _first_defined; 
-        line.last_defined_ = _last_defined; 
-        /*
-        if (_idx_o == 0) line.first_defined_ = false; 
-        else
-        {
-            d1sq = _points.block<3,1>(0,_idx_o-1).head(2).squaredNorm();
-            d2sq = _points.block<3,1>(0,_idx_o).head(2).squaredNorm();
-            line.first_defined_ = ( d1sq > d2sq ); //true if previous scan point is further
-        }
-        if (_idx_e == _points.cols()-1) line.last_defined_ = false; 
-        else
-        {
-            d1sq = _points.block<3,1>(0,_idx_e).head(2).squaredNorm();
-            d2sq = _points.block<3,1>(0,_idx_e+1).head(2).squaredNorm();
-            line.last_defined_ = ( d2sq > d1sq ); //true if next scan point is further
-        }
-        */
         
         //push back the line
         _line_list.push_back(line);
     }
     else //non straight line case -> carry on recursivity
     {
-        //check if split point (max_ii) is defined or not, from the range scan perspective
-        r_prev_sq = _points.block<3,1>(0,max_ii-1).head(2).squaredNorm(); //squared range of previous point 
-        r_sq = _points.block<3,1>(0,max_ii).head(2).squaredNorm();//squared range of max_ii point 
-        r_next_sq = _points.block<3,1>(0,max_ii+1).head(2).squaredNorm();//squared range of next point 
-        
         //split _points into two subsets and call findLinesRecursive again for each subset
-        this->findLinesRecursive(_points, _line_list, _idx_o, max_ii, _first_defined, (r_sq < r_next_sq) );
-        this->findLinesRecursive(_points, _line_list, max_ii, _idx_e, (r_sq < r_prev_sq), _last_defined);
+        this->findLinesRecursive(_points, _line_list, _idx_o, max_ii );
+        this->findLinesRecursive(_points, _line_list, max_ii, _idx_e);
     }
     
     //return number of lines detected

src/line_finder_iterative.h

@@ -17,8 +17,8 @@ struct LineFinderIterativeParams
 {
     ScalarT max_fit_error_; //maximum allowed fit error to consider a straight line
     unsigned int min_supports_; //Min supports at the hough grid to consider a cell as a line
-    ScalarT range_k_segment_; //range factor (w.r.t. range*ang_step) threshold for considering a new segment
-    ScalarT range_k_defined_; //range factor (w.r.t. range*ang_step) threshold for considering a defined line extreme point (bigger than range_k_segment_)
+    ScalarT range_jump_segment_; //range difference (w.r.t previous range) threshold for considering a new segment
+    ScalarT dist_th_defined_; //range difference (w.r.t previous range) for considering a defined line extreme point (bigger than range_k_segment_)
 };
 
     
@@ -77,7 +77,7 @@ class LineFinderIterative : public LineFinder
         *    \return Number of lines extracted.
         *
         */
-        unsigned int findLines( const Eigen::MatrixXs & _points, 
+        unsigned int findLines( const Eigen::Matrix<ScalarT, 3, Eigen::Dynamic> & _points,
                                 std::list<laserscanutils::LineSegment> & _line_list) const;
         
         //2nd version of findLines() required to correctly fill if end points are defined or not
@@ -113,7 +113,7 @@ class LineFinderIterative : public LineFinder
         *    \param _polyline_list list of polylines
         * 
         */        
-        unsigned int findPolylines( const Eigen::MatrixXs & _points,
+        unsigned int findPolylines( const Eigen::Matrix<ScalarT, 3, Eigen::Dynamic> & _points,
                                     std::list<laserscanutils::Polyline> & _polyline_list, 
                                     double _dist_threshold_sq = 0.01) const;        
         
@@ -157,15 +157,10 @@ class LineFinderIterative : public LineFinder
         *    \return Number of lines extracted.
         * 
         */
-        unsigned int findLinesRecursive( const Eigen::MatrixXs & _points, 
+        unsigned int findLinesRecursive( const Eigen::Matrix<ScalarT, 3, Eigen::Dynamic> & _points,
                                          std::list<laserscanutils::LineSegment> & _line_list, 
                                          unsigned int _idx_o, 
-                                         unsigned int _idx_e,
-                                         bool _first_defined,
-                                         bool _last_defined ) const;
-//         unsigned int findLinesRecursive( const Eigen::MatrixXs & _points, 
-//                                          std::list<laserscanutils::LineSegment> & _line_list, 
-//                                          unsigned int _idx_offset = 0 ) const;
+                                         unsigned int _idx_e) const;
 
 };
 

src/line_segment.cpp

@@ -13,9 +13,9 @@ LineSegment::LineSegment() :
     np_(0),
     range_(0.),
     theta_(0.), 
-    length_(0.), 
-    first_defined_(false), 
-    last_defined_(false)
+    length_(0.)
+    //first_defined_(false),
+    //last_defined_(false)
 {
 
 }
@@ -122,9 +122,9 @@ void LineSegment::print() const
               << "\tnumber of points: " << np_ << std::endl
               << "\trange: " << range_ << std::endl
               << "\ttheta: " << theta_ << std::endl
-              << "\tlength: " << length_ << std::endl
-              << "\tfirst_defined: " << first_defined_ << std::endl
-              << "\tlast_defined: " << last_defined_ << std::endl;        
+              << "\tlength: " << length_ << std::endl;
+              //<< "\tfirst_defined: " << first_defined_ << std::endl
+              //<< "\tlast_defined: " << last_defined_ << std::endl;
 }
 
 }//namespace

src/line_segment.h

@@ -35,8 +35,8 @@ class LineSegment
         ScalarT range_; //range component in polar parameterization
         ScalarT theta_; //theta component in polar parameterization.
         ScalarT length_; //theta component in polar parameterization.
-        bool first_defined_; //indicates if point_first_ is a well defined extreme of the line segment
-        bool last_defined_; //indicates if point_last_ is a well defined extreme of the line segment
+        //bool first_defined_; //indicates if point_first_ is a well defined extreme of the line segment
+        //bool last_defined_; //indicates if point_last_ is a well defined extreme of the line segment
 
     public:
         //constructor
@@ -86,4 +86,4 @@ class LineSegment
         void print() const;    
 };
 }//namespace
-#endif
\ No newline at end of file
+#endif

src/polyline.h

@@ -19,9 +19,9 @@ namespace laserscanutils
 class Polyline
 {
     public:
-        Eigen::MatrixXs points_;//set of ordered points forming the polyline. Homogeneous coordinates
-        Eigen::MatrixXs covs_;//set of concatenated ordered point covariances (2x(2*npoints)).
-        Eigen::MatrixXs lines_;//set of ordered lines between consecutive points. Homogeneous coordinates
+        Eigen::Matrix<ScalarT, 3, Eigen::Dynamic> points_;//set of ordered points forming the polyline. Homogeneous coordinates
+        Eigen::Matrix<ScalarT, 2, Eigen::Dynamic> covs_;//set of concatenated ordered point covariances (2x(2*npoints)).
+        Eigen::Matrix<ScalarT, 3, Eigen::Dynamic> lines_;//set of ordered lines between consecutive points. Homogeneous coordinates
         bool first_defined_; //if true, indicates that the first point is a defined extreme of the polyline
         bool last_defined_; //if true, indicates that the last point is a defined extreme of the polyline
         

src/scan_segment.h

@@ -29,6 +29,8 @@ class ScanSegment : public PointSet
         unsigned int idx_last_; //index of the last point of this segment to the correponding scan
         bool first_defined_; //true when previous point in the scan to the first point is further
         bool last_defined_; //true when next point in the scan to the last point is further
+        Eigen::Vector3s prev_point_; //The previous point of the segment in the scan
+        Eigen::Vector3s next_point_; //The next point of the segment in the scan
     
     public:
         //constructor
@@ -44,4 +46,4 @@ class ScanSegment : public PointSet
         virtual void print() const;    
 };
 }//namespace
-#endif
\ No newline at end of file
+#endif