src/laser_scan.cpp

@@ -91,48 +91,59 @@ void LaserScan::ranges2xy(const LaserScanParams& _scan_params, Eigen::Matrix4s _
     for (ii = 0; ii < ranges_raw_.size(); ii++)
     {
         //check raw range integrity
-        if ((ranges_raw_[ii] > _scan_params.range_min_) && (ranges_raw_[ii] < _scan_params.range_max_-0.2) && (!std::isnan(ranges_raw_[ii])) && (!std::isinf(ranges_raw_[ii])))
+		//invalid range
+        if (!isValidRange(ranges_raw_[ii], _scan_params))
         {
-            //set as valid range
-            ranges_[ii] = ranges_raw_[ii];
-    
-            //transform the laser hit from polar to 3D euclidean homogeneous
-            point_laser << ranges_[ii] * cos(azimuth), ranges_[ii] * sin(azimuth), 0, 1;
-            
-            //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_.col(ii) << point_ref(0), point_ref(1), 1;
-            
-            //set to points_ as a 2D homogeneous
-            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) > _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++;
-            
-            //keep current range as previous for the next iteration
-            prev_range = ranges_raw_[ii];
-        }
-        else //invalid raw value
-        {
-            ranges_[ii] = -1.;
-            jumps_mask_[ii] = true;
-            points_all_.col(ii) << 0, 0, 0;
-            //prev_range = 0;
+        	// fix if it is a single value between two valid values
+			if (ii > 0 && ii < ranges_raw_.size() && isValidRange(ranges_raw_[ii-1], _scan_params) && isValidRange(ranges_raw_[ii+1], _scan_params))
+			{
+				ranges_[ii] = (ranges_raw_[ii-1] + ranges_raw_[ii+1])/2.0;
+			}
+			// definitely invalid range
+			else
+			{
+				ranges_[ii] = -1.;
+				jumps_mask_[ii] = true;
+				points_all_.col(ii) << 0, 0, 0;
+				//prev_range = 0;
+
+			    //increment azimuth with angle step
+				azimuth += _scan_params.angle_step_;
+				continue;
+			}
         }
-       
-       //increment azimuth with angle step
+        // valid range
+		ranges_[ii] = ranges_raw_[ii];
+
+		//transform the laser hit from polar to 3D euclidean homogeneous
+		point_laser << ranges_[ii] * cos(azimuth), ranges_[ii] * sin(azimuth), 0, 1;
+
+		//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_.col(ii) << point_ref(0), point_ref(1), 1;
+
+		//set to points_ as a 2D homogeneous
+		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) > _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++;
+
+		//keep current range as previous for the next iteration
+		prev_range = ranges_raw_[ii];
+
+        //increment azimuth with angle step
         azimuth += _scan_params.angle_step_;
     }
     
@@ -172,117 +183,128 @@ void LaserScan::processRaw(const LaserScanParams& _scan_params, bool _compute_pa
     //for each range, check correctness of value and translate from polar to xy coordinates
     for (ii = 0; ii < ranges_raw_.size(); ii++)
     {
-        //check raw range integrity
-        if (!std::isnan(ranges_raw_[ii]) && !std::isinf(ranges_raw_[ii]))
-        {
-            // fill as valid range
-            ranges_[ii] = ranges_raw_[ii];
-
-            //transform the laser hit from polar to 3D euclidean homogeneous
-            point_laser << ranges_[ii] * cos(azimuth), ranges_[ii] * sin(azimuth), 0, 1;
-
-            //apply device mounting point calibration (p_r = T * p_l)
-            point_ref = _device_T * point_laser;
-
-            // out of range
-            prev_out_of_range = out_of_range;
-            out_of_range = ranges_raw_[ii] <= _scan_params.range_min_ || ranges_raw_[ii] >= _scan_params.range_max_;
-
-            // store in points_all_ as a 2D homogeneous
-            points_all_.col(ii) << point_ref(0), point_ref(1), 1;
-
-            // store in points_ as a 2D homogeneous if not out of range
-            if (!out_of_range)
-                points_.col(ii_ok) << point_ref(0), point_ref(1), 1;
-
-
-            // OPEN NEW SEGMENT (without closing any)
-            if ( prev_out_of_range && !out_of_range) // valid point after out of range
-            {
-                //std::cout << "OPEN NEW SEGMENT " << std::endl;
-                assert(open_segment == false && "opening a segment without closing the last one");
-
-                segment_list_.push_back(ScanSegment());
-
-                // first index
-                segment_list_.back().idx_first_ = ii_ok;
-
-                // prev point
-                if (prev_ii == -1)
-                    // no previous points: prev = current
-                    segment_list_.back().prev_point_ = points_all_.col(ii);
-                else
-                    // prev = previous point (out of range or not)
-                    segment_list_.back().prev_point_ = points_all_.col(prev_ii);
-
-                open_segment = true;
-            }
-            // CLOSE SEGMENT (without opening any)
-            else if ( out_of_range && !prev_out_of_range) // out of range after a ok point
-            {
-                //std::cout << "CLOSE SEGMENT " << std::endl;
-                assert(open_segment == true && "closing an already closed segment");
-
-                // last index
-                segment_list_.back().idx_last_ = ii_ok-1;
-
-                // points
-                unsigned int n_points = segment_list_.back().idx_last_ - segment_list_.back().idx_first_+1;
-                segment_list_.back().points_ = points_.middleCols(segment_list_.back().idx_first_, n_points);
-
-                // next point: current
-                segment_list_.back().next_point_ = points_all_.col(ii);
-
-                //check _compute_params to compute all segment params
-                if (_compute_params)
-                    segment_list_.back().computeAll();
-
-                open_segment = false;
-            }
-            // CLOSE SEGMENT & OPEN NEW SEGMENT
-            else if ( !out_of_range && !prev_out_of_range && fabs(ranges_[ii] - ranges_[prev_ii]) > _jump_th ) // jump from valid points
-            {
-                //std::cout << "CLOSE & OPEN SEGMENT " << std::endl;
-                assert(open_segment == true && "closing an already closed segment");
-
-                // CLOSE
-                // last index
-                segment_list_.back().idx_last_ = ii_ok-1;
-
-                // points
-                unsigned int n_points = segment_list_.back().idx_last_ - segment_list_.back().idx_first_+1;
-                segment_list_.back().points_ = points_.middleCols(segment_list_.back().idx_first_, n_points);
-
-                // next point: current
-                segment_list_.back().next_point_ = points_all_.col(ii);
-
-                //check _compute_params to compute all segment params
-                if (_compute_params)
-                    segment_list_.back().computeAll();
-
-                // OPEN
-                segment_list_.push_back(ScanSegment());
-
-                // first index
-                segment_list_.back().idx_first_ = ii_ok;
-
-                // prev point
-                segment_list_.back().prev_point_ = points_all_.col(prev_ii);
-            }
-
-            //increment ok counter
-            if (!out_of_range)
-                ii_ok++;
-
-            // keep previous non-nan non-inf index
-            prev_ii = ii;
-        }
-        else //invalid raw value
-        {
-            ranges_[ii] = -1.;
-            jumps_mask_[ii] = true;
-            points_all_.col(ii) << 0, 0, 0;
-        }
+		// prev out of range
+		prev_out_of_range = out_of_range;
+
+		//check raw range integrity
+    	if (!isValidRange(ranges_raw_[ii], _scan_params))
+		{
+			// fix if it is a single value between two valid values
+			if (ii > 0 && ii < ranges_raw_.size() && isValidRange(ranges_raw_[ii-1], _scan_params) && isValidRange(ranges_raw_[ii+1], _scan_params))
+			{
+				ranges_[ii] = (ranges_raw_[ii-1] + ranges_raw_[ii+1])/2.0;
+				out_of_range = false;
+			}
+			// definitely invalid range
+			else
+			{
+				ranges_[ii] = -1.;
+				jumps_mask_[ii] = true;
+				points_all_.col(ii) << 0, 0, 0;
+				out_of_range = true;
+			}
+		}
+		// valid range
+    	else
+    	{
+    		ranges_[ii] = ranges_raw_[ii];
+    		out_of_range = false;
+    	}
+
+		// store in points_ as a 2D homogeneous if not out of range
+		if (!out_of_range)
+		{
+			//transform the laser hit from polar to 3D euclidean homogeneous
+			point_laser << ranges_[ii] * cos(azimuth), ranges_[ii] * sin(azimuth), 0, 1;
+
+			//apply device mounting point calibration (p_r = T * p_l)
+			point_ref = _device_T * point_laser;
+
+			// store in points_all_ as a 2D homogeneous
+			points_all_.col(ii) << point_ref(0), point_ref(1), 1;
+			points_.col(ii_ok) << point_ref(0), point_ref(1), 1;
+		}
+
+		// OPEN NEW SEGMENT (without closing any)
+		if ( prev_out_of_range && !out_of_range) // valid point after out of range
+		{
+			//std::cout << "OPEN NEW SEGMENT " << std::endl;
+			assert(open_segment == false && "opening a segment without closing the last one");
+
+			segment_list_.push_back(ScanSegment());
+
+			// first index
+			segment_list_.back().idx_first_ = ii_ok;
+
+			// prev point
+			if (prev_ii == -1)
+				// no previous points: prev = current
+				segment_list_.back().prev_point_ = points_all_.col(ii);
+			else
+				// prev = previous point (out of range or not)
+				segment_list_.back().prev_point_ = points_all_.col(prev_ii);
+
+			open_segment = true;
+		}
+		// CLOSE SEGMENT (without opening any)
+		else if ( out_of_range && !prev_out_of_range) // out of range after a ok point
+		{
+			//std::cout << "CLOSE SEGMENT " << std::endl;
+			assert(open_segment == true && "closing an already closed segment");
+
+			// last index
+			segment_list_.back().idx_last_ = ii_ok-1;
+
+			// points
+			unsigned int n_points = segment_list_.back().idx_last_ - segment_list_.back().idx_first_+1;
+			segment_list_.back().points_ = points_.middleCols(segment_list_.back().idx_first_, n_points);
+
+			// next point: current
+			segment_list_.back().next_point_ = points_all_.col(ii);
+
+			//check _compute_params to compute all segment params
+			if (_compute_params)
+				segment_list_.back().computeAll();
+
+			open_segment = false;
+		}
+		// CLOSE SEGMENT & OPEN NEW SEGMENT
+		else if ( !out_of_range && !prev_out_of_range && fabs(ranges_[ii] - ranges_[prev_ii]) > _jump_th ) // jump from valid points
+		{
+			//std::cout << "CLOSE & OPEN SEGMENT " << std::endl;
+			assert(open_segment == true && "closing an already closed segment");
+
+			// CLOSE
+			// last index
+			segment_list_.back().idx_last_ = ii_ok-1;
+
+			// points
+			unsigned int n_points = segment_list_.back().idx_last_ - segment_list_.back().idx_first_+1;
+			segment_list_.back().points_ = points_.middleCols(segment_list_.back().idx_first_, n_points);
+
+			// next point: current
+			segment_list_.back().next_point_ = points_all_.col(ii);
+
+			//check _compute_params to compute all segment params
+			if (_compute_params)
+				segment_list_.back().computeAll();
+
+			// OPEN
+			segment_list_.push_back(ScanSegment());
+
+			// first index
+			segment_list_.back().idx_first_ = ii_ok;
+
+			// prev point
+			segment_list_.back().prev_point_ = points_all_.col(prev_ii);
+		}
+
+		//increment ok counter
+		if (!out_of_range)
+			ii_ok++;
+
+		// keep previous non-nan non-inf index
+		prev_ii = ii;
 
         //increment azimuth with angle step
         azimuth += _scan_params.angle_step_;

src/laser_scan.h

@@ -166,6 +166,11 @@ class LaserScan
          **/
         void findSegments(const LaserScanParams& _scan_params, std::list<laserscanutils::ScanSegment>& _segment_list, bool _compute_params = true, const ScalarT& _jump_th_segment = 1);
 
+        bool isValidRange(const ScalarT& range, const LaserScanParams& _scan_params) const
+        {
+        	return (range > _scan_params.range_min_) && (range < _scan_params.range_max_-0.2) && (!std::isnan(range)) && (!std::isinf(range));
+        }
+
 };
 
 }        //namespace

src/line_finder_iterative.cpp

@@ -21,25 +21,59 @@ LineFinderIterative::~LineFinderIterative()
 void LineFinderIterative::setIlfParams(const LineFinderIterativeParams & _params)
 {
     ilf_params_.max_fit_error_ = _params.max_fit_error_;
-    ilf_params_.min_supports_ = _params.min_supports_;
+    ilf_params_.min_segment_points_ = _params.min_segment_points_;
 }
 
 unsigned int LineFinderIterative::findLines( const Eigen::MatrixXs & _points,
                                              std::list<laserscanutils::LineSegment> & _line_list)
 {
+	//If not enough points, return, nothing to do.
+	if ( _points.cols() < ilf_params_.min_segment_points_ )
+		return 0;
+
+	//If segment too short, return, nothing to do.
+	if ((_points.col(0)-_points.col(_points.cols()-1)).head(2).norm() < ilf_params_.min_segment_dist_ )
+		return 0;
+
     //Find lines recursively
     this->findLinesRecursive(_points, _line_list, 0, _points.cols()-1);
-    
+
+    // discard polylines with any inner line (not considering first and last lines) that is too small (in points or distance)
+    if (_line_list.size() > 2)
+    	for (auto line_it = std::next(_line_list.begin()); line_it != std::prev(_line_list.end()); line_it++)
+    		if (line_it->np_ < ilf_params_.min_segment_points_ || line_it->length_ < ilf_params_.min_segment_dist_)
+    		{
+    			_line_list.clear();
+    			return 0;
+    		}
+
     //return number of lines detected
-    return _line_list.size(); 
+    return _line_list.size();
 }
 
 unsigned int LineFinderIterative::findLines( const laserscanutils::ScanSegment & _segment,  
                                              std::list<laserscanutils::LineSegment> & _line_list) const
 {
+	//If not enough points, return, nothing to do.
+	if ( _segment.points_.cols() < ilf_params_.min_segment_points_ )
+		return 0;
+
+	//If segment too short, return, nothing to do.
+	if ((_segment.points_.col(0)-_segment.points_.col(_segment.points_.cols()-1)).head(2).norm() < ilf_params_.min_segment_dist_ )
+		return 0;
+
     //Find lines recursively
     this->findLinesRecursive(_segment.points_, _line_list, 0, _segment.points_.cols()-1);
     
+    // discard polylines with any inner line (not considering first and last lines) that is too small (in points or distance)
+	if (_line_list.size() > 2)
+		for (auto line_it = std::next(_line_list.begin()); line_it != std::prev(_line_list.end()); line_it++)
+			if (line_it->np_ < ilf_params_.min_segment_points_ || line_it->length_ < ilf_params_.min_segment_dist_)
+			{
+				_line_list.clear();
+				return 0;
+			}
+
     //return number of lines detected
     return _line_list.size(); 
 }
@@ -161,10 +195,6 @@ unsigned int LineFinderIterative::findLinesRecursive( const Eigen::Matrix<Scalar
                                                       unsigned int _idx_o, 
                                                       unsigned int _idx_e) const
 {
-    //If not enough points, return, nothing to do.
-    if ( (_idx_e-_idx_o + 1) < ilf_params_.min_supports_ )
-        return 0;
-
     LineSegment line;//, line1, line2; 
     ScalarT max_dist, dist;
     unsigned int max_ii; 
@@ -180,8 +210,14 @@ unsigned int LineFinderIterative::findLinesRecursive( const Eigen::Matrix<Scalar
     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
+    // n_points and length
+    line.np_ = _idx_e - _idx_o + 1;
+	line.length_ = (_points.col(0)-_points.col(_points.cols()-1)).head(2).norm();
+
+    //check if farthest point at distance lower than threshold (or line too small to keep splitting)
+    if ( max_dist < ilf_params_.max_fit_error_ ||
+		 line.np_ < ilf_params_.min_segment_points_ ||
+		 line.length_ < ilf_params_.min_segment_dist_) //straight line case
     {
         //Fit line
         fitLine(_points.middleCols(_idx_o, _idx_e-_idx_o+1), line);
@@ -189,14 +225,17 @@ unsigned int LineFinderIterative::findLinesRecursive( const Eigen::Matrix<Scalar
         //fill line segment attributes
         line.pointProjectionToLine(_points.col(_idx_o), line.point_first_); //line.point_first_ << _points.col(_idx_o);
         line.pointProjectionToLine(_points.col(_idx_e), line.point_last_); //line.point_last_ << _points.col(_idx_e);
-        line.np_ = _idx_e - _idx_o + 1;
+
         line.idx_first_ = _idx_o;
         line.idx_last_ = _idx_e;
+        // correct length
+    	line.length_ = (line.point_first_ - line.point_last_).norm();
         
         //push back the line
         _line_list.push_back(line);
     }
-    else //non straight line case -> carry on recursivity
+    //non straight line case -> carry on recursivity
+    else
     {
         //split _points into two subsets and call findLinesRecursive again for each subset
         this->findLinesRecursive(_points, _line_list, _idx_o, max_ii );

src/line_finder_iterative.h

@@ -16,7 +16,8 @@ namespace laserscanutils
 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
+    unsigned int min_segment_points_; //Min points of a segment to be considered
+    ScalarT min_segment_dist_; //Min distance of a segment to be considered
     ScalarT range_jump_segment_; //range difference (w.r.t previous range) threshold for considering a new segment
     ScalarT dist_th_defined_; //distance of the next/prev point to the line for considering a defined line extreme point
     ScalarT std_dev_points_; // standard deviation of the detected points (TODO: modeling from sensor noise)
@@ -64,6 +65,10 @@ class LineFinderIterative : public LineFinder
          * 
          **/
         void setIlfParams(const LineFinderIterativeParams & _params);
+        LineFinderIterativeParams getIlfParams() const
+        {
+        	return ilf_params_;
+        }
                 
         /** \brief Find lines using Iterative line Fit. Result as a list of Line's
         *