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

No commit message

No commit message
parent 63509664
No related branches found
No related tags found
No related merge requests found
...@@ -90,8 +90,10 @@ unsigned int laserscanutils::extractCorners(const laserscanutils::ScanParams & _ ...@@ -90,8 +90,10 @@ unsigned int laserscanutils::extractCorners(const laserscanutils::ScanParams & _
corner_it2 ++; corner_it2 ++;
while (corner_it2 != _corner_list.end()) while (corner_it2 != _corner_list.end())
{ {
// Check if two corners are close enough. TODO: Check othe attributes also (orientation and aperture) ! // Check if two corners are close enough with similar orientation and aperture
if ( ( corner_it1->pt_ - corner_it2->pt_ ).head(2).squaredNorm() < max_distance_sq ) if ( ( corner_it1->pt_ - corner_it2->pt_ ).head(2).squaredNorm() < max_distance_sq &&
abs( pi2pi( corner_it1->aperture_ - corner_it2->aperture_) ) < _alg_params.theta_min_ &&
abs( pi2pi( corner_it1->orientation_ - corner_it2->orientation_) ) < _alg_params.theta_min_)
{ {
// Keep the one with bigger product of number of points of each line // Keep the one with bigger product of number of points of each line
(*corner_it1) = (corner_it1->line_1_.np_*corner_it1->line_2_.np_ > corner_it2->line_1_.np_*corner_it2->line_2_.np_? *corner_it1 : *corner_it2); (*corner_it1) = (corner_it1->line_1_.np_*corner_it1->line_2_.np_ > corner_it2->line_1_.np_*corner_it2->line_2_.np_? *corner_it1 : *corner_it2);
......
...@@ -92,4 +92,7 @@ void laserscanutils::ranges2xy(const ScanParams & _params, std::vector<float> & ...@@ -92,4 +92,7 @@ void laserscanutils::ranges2xy(const ScanParams & _params, std::vector<float> &
//_points.conservativeResize(Eigen::NoChange_t, ii_ok); //does not compile ... why ? //_points.conservativeResize(Eigen::NoChange_t, ii_ok); //does not compile ... why ?
} }
laserscanutils::ScalarT laserscanutils::pi2pi(const ScalarT& angle)
{
return (angle > 0 ? fmod(angle + M_PI, 2 * M_PI) - M_PI : fmod(angle - M_PI, 2 * M_PI) + M_PI);
}
...@@ -60,5 +60,11 @@ namespace laserscanutils ...@@ -60,5 +60,11 @@ namespace laserscanutils
* *
**/ **/
void ranges2xy(const ScanParams & _params, std::vector<float> & _ranges, const ScalarT& _jump_threshold, const ScalarT& _jump_angle_threshold, Eigen::MatrixXs & _points, std::list<unsigned int> & _jumps); void ranges2xy(const ScanParams & _params, std::vector<float> & _ranges, const ScalarT& _jump_threshold, const ScalarT& _jump_angle_threshold, Eigen::MatrixXs & _points, std::list<unsigned int> & _jumps);
/** \brief Fits angle in (-pi, pi]
*
* Fits angle in (-pi, pi]
**/
ScalarT pi2pi(const ScalarT& angle);
} }
#endif #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