From 5a309cd83c174e15d881007620871c2bf5355dbf Mon Sep 17 00:00:00 2001
From: andreucm <acoromin@iri.upc.edu>
Date: Wed, 30 Mar 2016 18:05:35 +0200
Subject: [PATCH] Just a working copy of corner detection. Doesn't compilr, Not
tested yet
---
src/corner_finder_range_diff.cpp | 63 ++++++++++++++------------------
src/corner_finder_range_diff.h | 4 +-
src/laser_scan.cpp | 50 +++++++++++++++++++++++++
src/laser_scan.h | 38 +++++++++++++++++--
src/line_segment.cpp | 13 +++++++
src/line_segment.h | 8 ++++
6 files changed, 134 insertions(+), 42 deletions(-)
diff --git a/src/corner_finder_range_diff.cpp b/src/corner_finder_range_diff.cpp
index ac7419d..b97b1ad 100644
--- a/src/corner_finder_range_diff.cpp
+++ b/src/corner_finder_range_diff.cpp
@@ -17,57 +17,48 @@ int CornerFinderRangeDiff::findCorners(const laserscanutils::LaserScan & _scan,
std::list<laserscanutils::CornerPoint> & _corner_list)
{
//constants TODO: should be moved as a set algorithm params
- ScalarT ZERO_RANGE_DIFF = 0.02;
- unsigned int CORNER_WINDOW_SIZE = 5; //minimum size of line supproting corner
+ unsigned int HALF_WINDOW_SIZE = 5; //minimum size of line supproting corner
+ ScalarT LINE_FIT_ERROR = 0.05; //maximum allowed mean point-line error to consider a line is straight enough
+ ScalarT MIN_ANGLE = 30*M_PI/180.; //minimum allowed absoulte angle between lines around the corner
//variables
- ScalarT range_diff;
+ ScalarT e1,e2, angle;
LineSegment line1, line2;
+ CornerPoint new_corner;
//check if _scan is already raw processed
if ( !_scan.isRawProcessed() ) return -1;
//run over all scan data
- for (unsigned int ii = CORNER_WINDOW_SIZE; ii<_scan.ranges_.size()-CORNER_WINDOW_SIZE; ii++)
+ for (unsigned int ii = HALF_WINDOW_SIZE; ii<_scan.ranges_.size()-HALF_WINDOW_SIZE; ii++)
{
- //check if ranges ii and ii-1 are both correct and there is no jump between them
- if ( ( _scan.ranges_[ii-1] > 0 ) && ( _scan.ranges_[ii] > 0 ) && ( _scan.jumps_mask_[ii] == false ) )
+ //check correctness and jumps
+ if ( ( _scan.checkScanCorrectness(ii, HALF_WINDOW_SIZE) == true )
+ &&
+ ( _scan.checkScanJumps(ii, HALF_WINDOW_SIZE) == false ) )
{
- //compute range difference
- range_diff = _scan.ranges_[ii] - _scan.ranges_[ii-1];
+ //fit lines at left and right of the point
+ line_finder_.fitLine(_scan.points_.block(0,ii-HALF_WINDOW_SIZE,3,HALF_WINDOW_SIZE+1), line1);
+ line_finder_.fitLine(_scan.points_.block(0,ii,3,HALF_WINDOW_SIZE+1), line2);
- //corners have range diff close to zero TODO: this condition is to be discussed
- if(range_diff < ZERO_RANGE_DIFF)
+ //check line error
+ e1 = line1.fit_error_ / ( (ScalarT)HALF_WINDOW_SIZE * 2 + 1);
+ e2 = line2.fit_error_ / ( (ScalarT)HALF_WINDOW_SIZE * 2 + 1);
+ if ( ( e1 < LINE_FIT_ERROR ) && ( e2 < LINE_FIT_ERROR ) )
{
- //fit lines at eft and right of the point
- line_finder_.fitLine(_scan.points_.block(0,ii-CORNER_WINDOW_SIZE,3,CORNER_WINDOW_SIZE*2), line1);
- line_finder_.fitLine(_scan.points_.block(0,ii-CORNER_WINDOW_SIZE,3,CORNER_WINDOW_SIZE*2), line2);
-
- //check angle between lines
- //check fit errors
-
-// if ( fit errors < FIT_ERROR) && (angle in (0,135)U(225,360) ) //CORNER_MIN_ANGLE
-// {
-// //corner found
-// //set corner
-// // _corner_list.push_back();
-// }
+ //check angles between lines
+ angle = line1.angleToLine(line2); //angle is in [0,2pi]
+ if ( ( angle < M_PI-MIN_ANGLE ) || ( angle > M_PI+MIN_ANGLE ) )
+ {
+// //corner found !!
+// new_corner.point_ << _scan.points_all_.block<1,3>(0,);
+// //new_corner.orientation_ = ;
+// new_corner.aperture_ = angle;
+// _corner_list.push_back(new_corner);
+ }
}
}
}
-
-/*
- range_diff_i = range_i - range_(i-1)
-
- if(range_diff_i < ZERO_RANGE_DIFF)
- {
- v1,b1 = check {r_{i-CORNER_WINDOW_SIZE}, r_i} is line (with FIT_ERROR)
- v2,b2 = check {r_i, r_{i-CORNER_WINDOW_SIZE}} is line (with FIT_ERROR)
-
- angle = getAngle(b1,b2);
-
- }
-*/
}
void CornerFinderRangeDiff::print() const
diff --git a/src/corner_finder_range_diff.h b/src/corner_finder_range_diff.h
index 1a3e14f..4c8466a 100644
--- a/src/corner_finder_range_diff.h
+++ b/src/corner_finder_range_diff.h
@@ -61,10 +61,10 @@ class CornerFinderRangeDiff : public CornerFinder
* Returns corners as a std::list<CornerPoint>
*
* \Requires:
- * \param _points: 3xN matrix, set of points. Each column is a 2D point in homogeneous (x,y,1). Ordering is not required.
+ * \param _scan: A LaserScan object, already raw processed. If not raw processed, this method returns -1
*
* \Provides:
- * \param _corner_list set of corners extracted from _points
+ * \param _corner_list set of corners extracted from _scan
* \return Number of corners extracted.
*
*/
diff --git a/src/laser_scan.cpp b/src/laser_scan.cpp
index 0e3729f..3c4c1de 100644
--- a/src/laser_scan.cpp
+++ b/src/laser_scan.cpp
@@ -100,6 +100,56 @@ void LaserScan::ranges2xy(Eigen::Matrix4s _device_T)
is_raw_processed_ = true;
}
+bool LaserScan::checkScanCorrectness(unsigned int _idx, unsigned int _idx_range) const
+{
+ bool correct = true;
+
+ //first of all check if scan has been raw processed
+ if ( ! is_raw_processed_ ) return false;
+
+ //set loop bounds
+ unsigned int ii_init = std::max( 0 , (_idx-_idx_range) );
+ unsigned int ii_end = std::min( ranges_.size()-1 , (_idx+_idx_range) );
+
+ //proceed
+ for (unsigned int ii=ii_init; ii<=ii_end; ii++ )
+ {
+ if (ranges_[ii] < 0)
+ {
+ correct = false;
+ break;
+ }
+ }
+
+ //return
+ return correct;
+}
+
+bool LaserScan::checkScanJumps(unsigned int _idx, unsigned int _idx_range) const
+{
+ bool jump = false;
+
+ //first of all check if scan has been raw processed
+ if ( ! is_raw_processed_ ) return true;
+
+ //set loop bounds
+ unsigned int ii_init = std::max( 0 , (_idx-_idx_range) );
+ unsigned int ii_end = std::min( ranges_.size()-1 , (_idx+_idx_range) );
+
+ //proceed
+ for (unsigned int ii=ii_init; ii<=ii_end; ii++ )
+ {
+ if ( jumps_mask_[ii] )
+ {
+ jump = true;
+ break;
+ }
+ }
+
+ //return
+ return jump;
+}
+
void LaserScan::findSegments(std::list<laserscanutils::ScanSegment> & _segment_list)
{
std::list<unsigned int>::iterator jumps_it, next_jumps_it, jumps_last;
diff --git a/src/laser_scan.h b/src/laser_scan.h
index 415bfd7..9b6c39c 100644
--- a/src/laser_scan.h
+++ b/src/laser_scan.h
@@ -9,6 +9,7 @@
#include <vector>
#include <list>
#include <iostream>
+#include <algorithm> //std::min, max
namespace laserscanutils
{
@@ -51,16 +52,27 @@ class LaserScan
//Ordered raw range data
std::vector<float> ranges_raw_;
- //Ordered and marked range data. Bad values (NaN's, Inf's, out of range) are indicated with -1
+ //Ordered and marked range data. Bad values (NaN's, Inf's, out of range) are indicated with -1.
+ //Same size as ranges_raw_
std::vector<float> ranges_;
+
+ //Ordered, Marked 2D points, each one expressed in homogeneous coordinates (x,y,1)^T.
+ //NaN's, inf's and out of range are marked as points at infty->last component set to 0.
+ //Same size as ranges_raw_
+ //Eigen::MatrixXs points_all_;
- //ordered 2D points, each one expressed in homogeneous coordinates (x,y,1)^T. NaN and inf's are filtered out.
+ //Ordered, Correct 2D points, each one expressed in homogeneous coordinates (x,y,1)^T.
+ //NaN's, inf's and out of range are filtered out, implying:
+ // - not necessarily regular angular increments between consecutive points
+ // - size equal or smaller than ranges_raw_
Eigen::MatrixXs points_;
- //For each element in ranges_, r_i, indicates if there is a jump (true) between that element and the previouos.
+ //For each element in ranges_, r_i, indicates if there is a jump (true) between that element and the previouos.
+ //Same size as ranges_raw_
std::vector<bool> jumps_mask_;
//list of indexes over points_ where a scan jump is found. Indexes indicate the second point of the jump (start of a scan segment)
+ //size smaller than ranges_raw_
std::list<unsigned int> jumps_indexes_;
@@ -108,8 +120,26 @@ class LaserScan
**/
void ranges2xy(Eigen::Matrix4s _device_T = Eigen::Matrix4s::Identity());
//TODO void processRaw()-> from ranges_raw_ fills: ranges_, points_, jumps_indexes and jumps_mask
+
+ /** \brief Check for scan correctness
+ *
+ * Check for scan correctness
+ * Returns True if all scan points in [ _idx-_idx_range , _idx+_idx_range ] are ok
+ * Otherwise returns false.
+ *
+ **/
+ bool checkScanCorrectness(unsigned int _idx, unsigned int _idx_range) const;
- /** \brief Find segments based on jumps of consecutive scan points
+ /** \brief Check for scan jumps
+ *
+ * Check for scan jumps
+ * Returns True if there is at least one jump in some point in [ _idx-_idx_range , _idx+_idx_range ]
+ * Otherwise returns false.
+ *
+ **/
+ bool checkScanJumps(unsigned int _idx, unsigned int _idx_range) const;
+
+ /** \brief Find segments based on jumps of consecutive scan points TODO. MOve away this method to anoter class SegmentFinder
*
* Find segments based on jumps of consecutive scan points
* Do not compute segment parameters, just fill ScanSegment.points_
diff --git a/src/line_segment.cpp b/src/line_segment.cpp
index 4978256..8b0fcec 100644
--- a/src/line_segment.cpp
+++ b/src/line_segment.cpp
@@ -58,6 +58,19 @@ void LineSegment::pointProjectionToLine(const Eigen::Vector3s & _in_pt, Eigen::V
_out_pt(0) = -( c + b*_out_pt(1) ) / a;
}
+double angleToLine(const laserscanutils::LineSegment & _line) const
+{
+ //compute each angle at 4-quadrant in [-pi,pi]
+ ScalarT angle_this = atan2(abc_(1)/abc_(2), abc_(0)/abc_(2));
+ ScalarT angle_line = atan2(_line.abc_(1)/_line.abc_(2), _line.abc_(0)/_line.abc_(2));
+
+ //Fit angles to [0,2pi] domain
+ if ( angle_this < 0 ) angle_this = 2*M_PI + angle_this;
+ if ( angle_line < 0 ) angle_line = 2*M_PI + angle_line;
+
+ //return angle from this to _line, in [0,2pi]
+ return (angle_line-angle_this);
+}
void LineSegment::merge(const LineSegment & _segment)
{
diff --git a/src/line_segment.h b/src/line_segment.h
index 585c778..9214900 100644
--- a/src/line_segment.h
+++ b/src/line_segment.h
@@ -50,6 +50,14 @@ class LineSegment
**/
void pointProjectionToLine(const Eigen::Vector3s & _in_pt, Eigen::Vector3s & _out_pt) const;
+ /** \brief Angle from this to line, in [0,2pi]
+ *
+ * Returns the angle from this vector abc_ to the argument line's vector abc_
+ * Return value in [0,2pi]
+ *
+ **/
+ double angleToLine(const laserscanutils::LineSegment & _line) const;
+
//merges this LineSegment with the argument.
void merge(const LineSegment & _segment);
--
GitLab