Skip to content
Snippets Groups Projects
Commit 513cfa76 authored by Joan Vallvé Navarro's avatar Joan Vallvé Navarro
Browse files

creating/discarding lines properly

parent d660a6f7
No related branches found
No related tags found
No related merge requests found
...@@ -37,9 +37,18 @@ unsigned int LineFinderIterative::findLines( const Eigen::MatrixXs & _points, ...@@ -37,9 +37,18 @@ unsigned int LineFinderIterative::findLines( const Eigen::MatrixXs & _points,
//Find lines recursively //Find lines recursively
this->findLinesRecursive(_points, _line_list, 0, _points.cols()-1); 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 number of lines detected
return _line_list.size(); return _line_list.size();
} }
unsigned int LineFinderIterative::findLines( const laserscanutils::ScanSegment & _segment, unsigned int LineFinderIterative::findLines( const laserscanutils::ScanSegment & _segment,
...@@ -56,6 +65,15 @@ unsigned int LineFinderIterative::findLines( const laserscanutils::ScanSegment & ...@@ -56,6 +65,15 @@ unsigned int LineFinderIterative::findLines( const laserscanutils::ScanSegment &
//Find lines recursively //Find lines recursively
this->findLinesRecursive(_segment.points_, _line_list, 0, _segment.points_.cols()-1); 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 number of lines detected
return _line_list.size(); return _line_list.size();
} }
...@@ -192,8 +210,14 @@ unsigned int LineFinderIterative::findLinesRecursive( const Eigen::Matrix<Scalar ...@@ -192,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_dist = (_points.middleCols(_idx_o, _idx_e-_idx_o+1).transpose() * line.abc_).array().abs().maxCoeff(&max_ii);
max_ii += _idx_o; max_ii += _idx_o;
//check if farthest point at distance lower than threshold // n_points and length
if ( max_dist < ilf_params_.max_fit_error_ ) //straight line case 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 //Fit line
fitLine(_points.middleCols(_idx_o, _idx_e-_idx_o+1), line); fitLine(_points.middleCols(_idx_o, _idx_e-_idx_o+1), line);
...@@ -201,14 +225,17 @@ unsigned int LineFinderIterative::findLinesRecursive( const Eigen::Matrix<Scalar ...@@ -201,14 +225,17 @@ unsigned int LineFinderIterative::findLinesRecursive( const Eigen::Matrix<Scalar
//fill line segment attributes //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_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.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_first_ = _idx_o;
line.idx_last_ = _idx_e; line.idx_last_ = _idx_e;
// correct length
line.length_ = (line.point_first_ - line.point_last_).norm();
//push back the line //push back the line
_line_list.push_back(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 //split _points into two subsets and call findLinesRecursive again for each subset
this->findLinesRecursive(_points, _line_list, _idx_o, max_ii ); this->findLinesRecursive(_points, _line_list, _idx_o, max_ii );
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment