From 19146aa84705265d6e2bfc81ff4ab26eb2652571 Mon Sep 17 00:00:00 2001
From: jvallve <jvallve@224674b8-e365-4e73-a4a8-558dbbfec58c>
Date: Thu, 15 Oct 2015 09:12:13 +0000
Subject: [PATCH] afegit parametre threshold angle per detectar jumps
---
src/examples/test_extract_corners.cpp | 1 +
src/examples/test_laser_detector.cpp | 1 +
src/line_detector.cpp | 3 +-
src/line_detector.h | 1 +
src/scan_basics.cpp | 41 ++++++++++++++++++++++++++-
src/scan_basics.h | 18 +++++++++++-
6 files changed, 62 insertions(+), 3 deletions(-)
diff --git a/src/examples/test_extract_corners.cpp b/src/examples/test_extract_corners.cpp
index eaa7a1e..ba71f98 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 c67f514..d069c71 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 1077e02..5ac8f4c 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 6abab37..f3a2949 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 7d075c5..0e1be80 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 77feab9..37c765a 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
--
GitLab