From 38ed7592f89e4212375b485ba62e9210275935a7 Mon Sep 17 00:00:00 2001
From: jvallve <jvallve@224674b8-e365-4e73-a4a8-558dbbfec58c>
Date: Wed, 14 Oct 2015 16:12:34 +0000
Subject: [PATCH] line detector and corner detector improvements
---
src/corner_detector.cpp | 4 +-
src/examples/test_extract_corners.cpp | 2 +-
src/line_detector.cpp | 114 +++++++++++++++++++-------
src/line_detector.h | 2 +-
src/scan_basics.cpp | 9 +-
5 files changed, 92 insertions(+), 39 deletions(-)
diff --git a/src/corner_detector.cpp b/src/corner_detector.cpp
index 2786331..3267ffd 100644
--- a/src/corner_detector.cpp
+++ b/src/corner_detector.cpp
@@ -80,7 +80,7 @@ unsigned int laserscanutils::extractCorners(const laserscanutils::ScanParams & _
line_it1 ++;
}
-// std::cout << "Corners extracted: " << _corner_list.size() << std::endl;
+ // std::cout << "Corners extracted: " << _corner_list.size() << std::endl;
// Erase duplicated corners
corner_it1 = _corner_list.begin();
@@ -102,7 +102,7 @@ unsigned int laserscanutils::extractCorners(const laserscanutils::ScanParams & _
}
corner_it1 ++;
}
-// std::cout << "Corners after removing duplicates: " << _corner_list.size() << std::endl;
+ // std::cout << "Corners after removing duplicates: " << _corner_list.size() << std::endl;
return _corner_list.size();
}
diff --git a/src/examples/test_extract_corners.cpp b/src/examples/test_extract_corners.cpp
index 39fb708..eaa7a1e 100644
--- a/src/examples/test_extract_corners.cpp
+++ b/src/examples/test_extract_corners.cpp
@@ -95,7 +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_.window_sz_ = 0.5;
+ alg_params.line_params_.window_length_ = 0.5;
alg_params.line_params_.min_window_points_ = 5;
alg_params.line_params_.k_sigmas_ut_ = 3;
alg_params.line_params_.concatenate_ii_ut_ = 5;
diff --git a/src/line_detector.cpp b/src/line_detector.cpp
index ecb832f..1077e02 100644
--- a/src/line_detector.cpp
+++ b/src/line_detector.cpp
@@ -5,7 +5,7 @@ void laserscanutils::ExtractLineParams::print() const
{
std::cout << "Extract Line Algorithm Parameters : " << std::endl
<< " jump_dist_ut_: " << jump_dist_ut_ << std::endl
- << " window_sz: " << window_sz_ << std::endl
+ << " window_length: " << window_length_ << std::endl
<< " k_sigmas_ut_: " << k_sigmas_ut_ << std::endl
<< " concatenate_ii_ut_: " << concatenate_ii_ut_ << std::endl
<< " concatenate_angle_ut_: " << concatenate_angle_ut_ << std::endl;
@@ -20,8 +20,13 @@ void laserscanutils::fitLine(const Eigen::MatrixXs & _points, Line & _line)
//solve for line
_line.vector_ = AA.inverse().col(2);
+ // normalize the line
+ //std::cout << "line squaredNorm (before) " << _line.vector_.head(2).squaredNorm() << std::endl;
+ _line.vector_ /= _line.vector_.head(2).norm();
+ //std::cout << "line squaredNorm " << _line.vector_.head(2).squaredNorm() << std::endl;
+
// compute fitting error
- _line.error_ = (_points.transpose() * _line.vector_).array().abs().sum() / (_line.vector_.head(2).norm()*_points.cols());
+ _line.error_ = (_points.transpose() * _line.vector_).squaredNorm() / _points.cols(); //_line.error_ = (_points.transpose() * _line.vector_).array().abs().sum() / (_line.vector_.head(2).norm()*_points.cols());
}
unsigned int laserscanutils::extractLines(const laserscanutils::ScanParams & _params,
@@ -33,9 +38,9 @@ unsigned int laserscanutils::extractLines(const laserscanutils::ScanParams & _pa
Eigen::MatrixXs points;
ScalarT cos_theta, theta;
Line line;
- std::list<Line>::iterator line_it1, line_it2;
+ std::list<Line>::iterator line_it1, line_it2, last_it;
std::list<unsigned int> jumps;
- std::vector<float> ranges = _ranges;
+ std::vector<float> ranges(_ranges);
std::list<unsigned int>::iterator jj_jump;
bool update_iterators;
unsigned int ii, window_points;
@@ -48,7 +53,7 @@ unsigned int laserscanutils::extractLines(const laserscanutils::ScanParams & _pa
// STEP 2: find line segments running over the scan
ii = 0;
//ii2=_alg_params.window_sz_;
- window_points = std::max(_alg_params.min_window_points_, (unsigned int)(_alg_params.window_sz_ / (_params.angle_step_ * ranges[ii])));
+ window_points = std::max(_alg_params.min_window_points_, (unsigned int)(_alg_params.window_length_ / (_params.angle_step_ * ranges[ii])));
jj_jump = jumps.begin();
@@ -66,7 +71,7 @@ unsigned int laserscanutils::extractLines(const laserscanutils::ScanParams & _pa
fitLine(points.block(0,ii,3,window_points), line);
//if error below stdev, add line to ScalarT cornerAperture(const Eigen::Vector3s & _p1, const Eigen::Vector3s & _c, const Eigen::Vector3s & _p2);result set
- if ( line.error_ < _params.range_std_dev_*_alg_params.k_sigmas_ut_ )
+ if ( line.error_ < _params.range_std_dev_*_params.range_std_dev_*_alg_params.k_sigmas_ut_*_alg_params.k_sigmas_ut_ )
{
line.first_ = ii;
line.last_ = ii + window_points;
@@ -79,25 +84,27 @@ unsigned int laserscanutils::extractLines(const laserscanutils::ScanParams & _pa
//increment window iterators
ii++;
}
- window_points = std::max(_alg_params.min_window_points_, (unsigned int)(_alg_params.window_sz_ / (_params.angle_step_ * ranges[ii])));
- std::cout << "min window points: " << _alg_params.min_window_points_ << std::endl;
- std::cout << "range: " << ranges[ii] << std::endl;
- std::cout << "l_step: " << _params.angle_step_ * ranges[ii] << std::endl;
- std::cout << "window_sz: " << _alg_params.window_sz_ << std::endl;
- std::cout << "window size / l_step: " << (_alg_params.window_sz_ / (_params.angle_step_ * ranges[ii])) << std::endl;
- std::cout << "(unsigned int) window size / l_step: " << (unsigned int)(_alg_params.window_sz_ / (_params.angle_step_ * ranges[ii])) << std::endl;
- std::cout << "window_points: " << window_points << std::endl << std::endl;
+ window_points = std::max(_alg_params.min_window_points_, (unsigned int)(_alg_params.window_length_ / (_params.angle_step_ * ranges[ii])));
+ //std::cout << "min window points: " << _alg_params.min_window_points_ << std::endl;
+ //std::cout << "range: " << ranges[ii] << std::endl;
+ //std::cout << "l_step: " << _params.angle_step_ * ranges[ii] << std::endl;
+ //std::cout << "window_sz: " << _alg_params.window_sz_ << std::endl;
+ //std::cout << "window size / l_step: " << (_alg_params.window_sz_ / (_params.angle_step_ * ranges[ii])) << std::endl;
+ //std::cout << "(unsigned int) window size / l_step: " << (unsigned int)(_alg_params.window_sz_ / (_params.angle_step_ * ranges[ii])) << std::endl;
+ //std::cout << "window_points: " << window_points << std::endl << std::endl;
}
- std::cout << "Lines fitted: " << _line_list.size() << std::endl;
+ //std::cout << "Lines fitted: " << _line_list.size() << std::endl;
//STEP 3: concatenate lines
if ( _line_list.size() < 2 ) return _line_list.size(); //In case just less than two lines found, return
line_it1 = _line_list.begin();
line_it2 = line_it1;
- line_it2 ++;
- while (line_it2 != _line_list.end())
+ line_it2++;
+ last_it = _line_list.end();
+ last_it--;
+ while (line_it1 != last_it && line_it1 != _line_list.end())
{
- // check index threshold
+ // check number of beams between lines
if ( ( (int)line_it2->first_ - (int)line_it1->last_ ) <= (int)_alg_params.concatenate_ii_ut_ )
{
//compute angle between lines 1 and 2
@@ -119,40 +126,85 @@ unsigned int laserscanutils::extractLines(const laserscanutils::ScanParams & _pa
new_line.point_first_ = line_it1->point_first_;
new_line.point_last_ = line_it2->point_last_;
new_line.np_ = new_line.last_ - new_line.first_ + 1;
-
- //assign new values to l1. Removes l2
+
+ //assign new values to line 1. Remove line 2
*line_it1 = new_line;
line_it2 = _line_list.erase(line_it2);//iterator line_it2 points to the next line of the list
+ //Update iterator to last element
+ last_it = _line_list.end();
+ last_it--;
+ //std::cout << "lines concatenated " << _line_list.size() << std::endl;
}
else
- {
- line_it1++;
line_it2++;
- }
}
else
+ line_it2++;
+
+ if (line_it2 == _line_list.end()) //no more lines to be checked with line 1, next line 1 (and 2 is the next one)
{
- line_it1++;
+ //std::cout << "no more lines to be checked with line 1, next line 1 (and 2 is the next one) " << _line_list.size() << std::endl;
+ line_it1++;
+ line_it2 = line_it1;
line_it2++;
}
}
- else
+ else // lines not close enought, next line 1 (and 2 is the next one)
{
- line_it1++;
- line_it2++;
+ //std::cout << "lines not close enought, next line 1 (and 2 is the next one) " << _line_list.size() << std::endl;
+ line_it1++;
+ line_it2 = line_it1;
+ line_it2++;
}
}
- std::cout << "Lines after concatenation: " << _line_list.size() << std::endl;
+ //std::cout << "Lines after concatenation: " << _line_list.size() << std::endl;
+
+ //STEP 4: removing outliers
+ for (line_it1 = _line_list.begin(); line_it1 != _line_list.end(); line_it1++)
+ {
+ // Remove front and back points with high error
+ while (line_it1->last_ - line_it1->first_ > 1 && pow(points.col(line_it1->first_).transpose() * line_it1->vector_,2) > 2 * line_it1->error_)
+ line_it1->first_++;
+
+ while (line_it1->last_ - line_it1->first_ > 1 && pow(points.col(line_it1->last_).transpose() * line_it1->vector_,2) > 2 * line_it1->error_)
+ line_it1->last_--;
+
+ if (line_it1->last_ - line_it1->first_ +1 < line_it1->np_) // Only if any point removed
+ {
+ if (line_it1->last_ - line_it1->first_ < _alg_params.min_window_points_)
+ {
+ //std::cout << "line too short after removing outliers!" << std::endl;
+ line_it1 = _line_list.erase(line_it1);//iterator line_it points to the next line of the list
+ line_it1--;
+ }
+ else
+ {
+ //std::cout << "updating line after removing " << line_it1->np_ - (line_it1->last_ - line_it1->first_ +1) << " outliers!" << std::endl;
+ //update line
+ Line new_line;
+ new_line.first_ = line_it1->first_;
+ new_line.last_ = line_it1->last_;
+ new_line.point_first_ = points.col(line_it1->first_);
+ new_line.point_last_ = points.col(line_it1->last_);
+ new_line.np_ = new_line.last_ - new_line.first_ + 1;
+ fitLine(points.block(0,new_line.first_, 3, new_line.np_), new_line);
+ *line_it1 = new_line;
+ }
+ }
+ }
+
return _line_list.size();
}
laserscanutils::ScalarT laserscanutils::angleBetweenLines(const laserscanutils::Line& line1, const laserscanutils::Line& line2)
{
- ScalarT theta;
-
+ // lines should be normalized
+ assert(abs(line1.vector_.head(2).squaredNorm() - 1) < 1e-9 && "line 1 is not normalized!");
+ assert(abs(line2.vector_.head(2).squaredNorm() - 1) < 1e-9 && "line 2 is not normalized!");
+
//compute angle, result in [0,PI]
- theta = acos( (line1.vector_.head(2)).dot(line2.vector_.head(2)) / ( line1.vector_.head(2).norm()*line2.vector_.head(2).norm() ) );
+ ScalarT theta = acos( (line1.vector_.head(2)).dot(line2.vector_.head(2)) );
//returns the angle lower PI/2 of the crossing lines
if (theta > M_PI/2) theta = M_PI - theta;
diff --git a/src/line_detector.h b/src/line_detector.h
index 9acdceb..6abab37 100644
--- a/src/line_detector.h
+++ b/src/line_detector.h
@@ -18,7 +18,7 @@ namespace laserscanutils
{
//members
ScalarT jump_dist_ut_; //Upper threshold in consecutive ranges to consider a jump
- ScalarT window_sz_; // size (m) of the window of points to fit lines in the first pass
+ 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
unsigned int concatenate_ii_ut_;//Upper threshold for ray index difference between consecutive lines to consider concatenation
diff --git a/src/scan_basics.cpp b/src/scan_basics.cpp
index d60f81a..7d075c5 100644
--- a/src/scan_basics.cpp
+++ b/src/scan_basics.cpp
@@ -14,7 +14,7 @@ void laserscanutils::ScanParams::print() const
void laserscanutils::ranges2xy(const ScanParams & _params, std::vector<float> & _ranges, const ScalarT& _jump_threshold, Eigen::MatrixXs & _points, std::list<unsigned int> & _jumps)
{
- ScalarT azimuth = 0.0;
+ ScalarT azimuth = _params.angle_min_;
ScalarT prev_range;
unsigned int ii = 0;
@@ -22,7 +22,7 @@ void laserscanutils::ranges2xy(const ScanParams & _params, std::vector<float> &
_points.resize(3,_ranges.size());
//jumps_id_.clear(); //std::queue has no member clear(). why it is a queue ?
- _jumps.clear();
+ _jumps.clear();
//for each range, check correctness of value and translate from polar to xy coordinates
while (ii <_ranges.size())
@@ -30,7 +30,6 @@ void laserscanutils::ranges2xy(const ScanParams & _params, std::vector<float> &
if ( _ranges[ii] >_params.range_min_ && _ranges[ii] < _params.range_max_ && !isnan(_ranges[ii]) && !isinf(_ranges[ii]) )
{
//transform from polar to euclidean
- azimuth = _params.angle_min_+ _params.angle_step_*ii;
_points(0,ii) = _ranges[ii] * cos(azimuth);
_points(1,ii) = _ranges[ii] * sin(azimuth);
_points(2,ii) = 1;
@@ -41,10 +40,12 @@ void laserscanutils::ranges2xy(const ScanParams & _params, std::vector<float> &
//keep current range and update counter
prev_range = _ranges[ii];
- 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
--
GitLab