diff --git a/src/examples/test_extract_corners.cpp b/src/examples/test_extract_corners.cpp index eaa7a1e3a1737159f4c984b4a37b7749ee7d0cfa..ba71f98f646bc8e2c44c23b3891cdee3ae4c540c 100644 --- a/src/examples/test_extract_corners.cpp +++ b/src/examples/test_extract_corners.cpp @@ -95,6 +95,7 @@ int main(int argc, char** argv) laserscanutils::ExtractCornerParams alg_params; alg_params.line_params_.jump_dist_ut_ = 1.0; + alg_params.line_params_.jump_angle_ut_ = M_PI / 3; alg_params.line_params_.window_length_ = 0.5; alg_params.line_params_.min_window_points_ = 5; alg_params.line_params_.k_sigmas_ut_ = 3; diff --git a/src/examples/test_laser_detector.cpp b/src/examples/test_laser_detector.cpp index c67f51453a6f9d2980889584b4701e83b2bd91d1..d069c71af97228578eb1cdb8a100b0e05e2e1e32 100644 --- a/src/examples/test_laser_detector.cpp +++ b/src/examples/test_laser_detector.cpp @@ -111,6 +111,7 @@ int main() laserscanutils::ExtractCornerParams algExtractCornerParams; algExtractCornerParams.line_params_.jump_dist_ut_ = 500; + algExtractCornerParams.line_params_.jump_angle_ut_ = M_PI / 3; algExtractCornerParams.line_params_.window_sz_ = 5; algExtractCornerParams.line_params_.k_sigmas_ut_ = 3; algExtractCornerParams.line_params_.concatenate_ii_ut_ = 5; diff --git a/src/line_detector.cpp b/src/line_detector.cpp index 1077e02d7d74405631b73487fb4bf92ddb34bd02..5ac8f4c271b5ed9b81e9d95fdda84308ac60394f 100644 --- a/src/line_detector.cpp +++ b/src/line_detector.cpp @@ -5,6 +5,7 @@ void laserscanutils::ExtractLineParams::print() const { std::cout << "Extract Line Algorithm Parameters : " << std::endl << " jump_dist_ut_: " << jump_dist_ut_ << std::endl + << " jump_angle_ut_: " << jump_angle_ut_ << std::endl << " window_length: " << window_length_ << std::endl << " k_sigmas_ut_: " << k_sigmas_ut_ << std::endl << " concatenate_ii_ut_: " << concatenate_ii_ut_ << std::endl @@ -46,7 +47,7 @@ unsigned int laserscanutils::extractLines(const laserscanutils::ScanParams & _pa unsigned int ii, window_points; //STEP 1: transform to euclidean coordinates and detect jumps - ranges2xy(_params, ranges, _alg_params.jump_dist_ut_, points, 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; diff --git a/src/line_detector.h b/src/line_detector.h index 6abab37840d50e77b21d55edfbe08227dd56794d..f3a294955349f43047bbfce08d05c1949d93496c 100644 --- a/src/line_detector.h +++ b/src/line_detector.h @@ -18,6 +18,7 @@ namespace laserscanutils { //members ScalarT jump_dist_ut_; //Upper threshold in consecutive ranges to consider a jump + ScalarT jump_angle_ut_; //Upper threshold in angle of two consecutive ranges to consider a jump ScalarT window_length_; // length (m) of the window of points to fit lines in the first pass unsigned int min_window_points_; // minimum number of points to fit lines in the first pass ScalarT k_sigmas_ut_;//Uppet threshold of how many std_dev are tolerated to count that a point is supporting a line diff --git a/src/scan_basics.cpp b/src/scan_basics.cpp index 7d075c52348c54118c045c287cf9d3f87191d870..0e1be80c39b951111e5bfd0d72b778b1c9ee9af3 100644 --- a/src/scan_basics.cpp +++ b/src/scan_basics.cpp @@ -21,7 +21,6 @@ void laserscanutils::ranges2xy(const ScanParams & _params, std::vector<float> & //resize to all points case _points.resize(3,_ranges.size()); - //jumps_id_.clear(); //std::queue has no member clear(). why it is a queue ? _jumps.clear(); //for each range, check correctness of value and translate from polar to xy coordinates @@ -53,4 +52,44 @@ void laserscanutils::ranges2xy(const ScanParams & _params, std::vector<float> & //_points.conservativeResize(Eigen::NoChange_t, ii_ok); //does not compile ... why ? } +void laserscanutils::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) +{ + ScalarT azimuth = _params.angle_min_; + ScalarT prev_range; + unsigned int ii = 0; + + //resize to all points case + _points.resize(3,_ranges.size()); + + _jumps.clear(); + + //for each range, check correctness of value and translate from polar to xy coordinates + while (ii <_ranges.size()) + { + if ( _ranges[ii] >_params.range_min_ && _ranges[ii] < _params.range_max_ && !isnan(_ranges[ii]) && !isinf(_ranges[ii]) ) + { + //transform from polar to euclidean + _points(0,ii) = _ranges[ii] * cos(azimuth); + _points(1,ii) = _ranges[ii] * sin(azimuth); + _points(2,ii) = 1; + + //check jump + if ( (ii != 0) && (fabs(_ranges[ii]-prev_range) > _jump_threshold) && (fabs(_ranges[ii]-prev_range) > tan(_jump_angle_threshold) * prev_range * _params.angle_step_) ) + _jumps.push_back(ii); + + //keep current range and update counter + prev_range = _ranges[ii]; + ii++; + } + else + _ranges.erase(_ranges.begin()+ii); + + azimuth += _params.angle_step_; + } + + //resize the output matrix to the number of correct points, while keeping values + _points.conservativeResize(3, _ranges.size()); + //_points.conservativeResize(Eigen::NoChange_t, ii_ok); //does not compile ... why ? +} + diff --git a/src/scan_basics.h b/src/scan_basics.h index 77feab9bade2778a12029bceb9420f116c50d988..37c765a761ce8928daf365654d33c1616bf71aac 100644 --- a/src/scan_basics.h +++ b/src/scan_basics.h @@ -43,6 +43,22 @@ namespace laserscanutils * **/ void ranges2xy(const ScanParams & _params, std::vector<float> & _ranges, const ScalarT& _jump_threshold, Eigen::MatrixXs & _points, std::list<unsigned int> & _jumps); - + + /** \brief Transforms from ranges (polar) to euclidean (xy) detecting jumps not in absolute range threshold but in angle w.r.t the beams + * + * Transforms from polar (ranges) to euclidean (xy), while checking correctness of raw data. + * Invalid values (inf's and nan's) and out of range measurements are not transformed. + * Valid measurements are returned, by reference, in homogeneous coordinates. + * Ranges vector is returned with all not correct elements removed. + * A list of jumps indices is also returned by reference. + * \param _params laser_scan_utils structure of the laser scan parameters. + * \param _ranges raw range measurements from the laser scanner device. Size Nr. Type float to match ROS LaserScan message + * \param _jump_theshold distance threshold to classify a jump in the range. + * \param _jump_angle_theshold angle threshold to classify a jump in the range. + * \param _points matrix of homogeneous coordinates of hit points. Size 3xNp, where Np can be <= than Nr (due to possible inf's and nan's from raw data) + * \param jumps_id_ list of indexes of jumps in range measurements. Indexes correpond to _points vector. + * + **/ + 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); } #endif