afegit parametre threshold angle per detectar jumps

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;

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; 

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;
     

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

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 ?
+}
+
 

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