Skip to content
Snippets Groups Projects
Commit ed6a5294 authored by jvallve's avatar jvallve
Browse files

bug in variable window size, new function windowPoints

parent bb04b7a4
No related branches found
No related tags found
No related merge requests found
......@@ -48,13 +48,13 @@ unsigned int laserscanutils::extractLines(const laserscanutils::ScanParams & _pa
//STEP 1: transform to euclidean coordinates and detect jumps
ranges2xy(_params, ranges, _alg_params.jump_dist_ut_, _alg_params.jump_angle_ut_, points, jumps);
// std::cout << "jumps: " << jumps.size() << std::endl;
// std::cout << "points: " << points.rows() << "x" << points.cols()<< std::endl;
// std::cout << "jumps: " << jumps.size() << std::endl;
// std::cout << "points: " << points.rows() << "x" << points.cols()<< std::endl;
// STEP 2: find line segments running over the scan
ii = 0;
//ii2=_alg_params.window_sz_;
window_points = std::max(_alg_params.min_window_points_, (unsigned int)(_alg_params.window_length_ / (_params.angle_step_ * ranges[ii])));
window_points = windowPoints(_params, _alg_params, _ranges, ii);
jj_jump = jumps.begin();
......@@ -85,7 +85,7 @@ unsigned int laserscanutils::extractLines(const laserscanutils::ScanParams & _pa
//increment window iterators
ii++;
}
window_points = std::max(_alg_params.min_window_points_, (unsigned int)(_alg_params.window_length_ / (_params.angle_step_ * ranges[ii])));
window_points = windowPoints(_params, _alg_params, _ranges, ii);
//std::cout << "min window points: " << _alg_params.min_window_points_ << std::endl;
//std::cout << "range: " << ranges[ii] << std::endl;
//std::cout << "l_step: " << _params.angle_step_ * ranges[ii] << std::endl;
......@@ -94,7 +94,7 @@ unsigned int laserscanutils::extractLines(const laserscanutils::ScanParams & _pa
//std::cout << "(unsigned int) window size / l_step: " << (unsigned int)(_alg_params.window_sz_ / (_params.angle_step_ * ranges[ii])) << std::endl;
//std::cout << "window_points: " << window_points << std::endl << std::endl;
}
//std::cout << "Lines fitted: " << _line_list.size() << std::endl;
std::cout << "Lines fitted: " << _line_list.size() << std::endl;
//STEP 3: concatenate lines
if ( _line_list.size() < 2 ) return _line_list.size(); //In case just less than two lines found, return
......@@ -158,7 +158,7 @@ unsigned int laserscanutils::extractLines(const laserscanutils::ScanParams & _pa
line_it2++;
}
}
//std::cout << "Lines after concatenation: " << _line_list.size() << std::endl;
std::cout << "Lines after concatenation: " << _line_list.size() << std::endl;
//STEP 4: removing outliers
for (line_it1 = _line_list.begin(); line_it1 != _line_list.end(); line_it1++)
......@@ -213,3 +213,14 @@ laserscanutils::ScalarT laserscanutils::angleBetweenLines(const laserscanutils::
//theta is in [0,PI/2]
return theta;
}
unsigned int laserscanutils::windowPoints(const laserscanutils::ScanParams & _params,
const ExtractLineParams & _alg_params,
const std::vector<float> & _ranges,
unsigned int& ii)
{
if (ii < _ranges.size() && _ranges[ii] !=0)
return std::max(_alg_params.min_window_points_, (unsigned int)(_alg_params.window_length_ / (_params.angle_step_ * _ranges[ii])));
else
return _alg_params.min_window_points_;
}
......@@ -61,5 +61,14 @@ namespace laserscanutils
*/
ScalarT angleBetweenLines(const laserscanutils::Line& line1, const laserscanutils::Line& line2);
/** \brief Returns the window points
*
* Returns the number of points in window depending on the ranges
*
*/
unsigned int windowPoints(const laserscanutils::ScanParams & _params,
const ExtractLineParams & _alg_params,
const std::vector<float> & _ranges,
unsigned int& ii);
}
#endif
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