diff --git a/src/corner_detector.cpp b/src/corner_detector.cpp index 278633195bb2255d93c38988c748fe112612eacc..3267ffdb5a1681813640f2fad7e4362dcb1e99d4 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 39fb7086adfedc4b898dd69e14f5d011f3c0a5ea..eaa7a1e3a1737159f4c984b4a37b7749ee7d0cfa 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 ecb832fcfd28dfb2698dbab5e207a88922298e89..1077e02d7d74405631b73487fb4bf92ddb34bd02 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 9acdcebbd71465a60bac3a9f2b9c57b774d21213..6abab37840d50e77b21d55edfbe08227dd56794d 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 d60f81adbf68f6808c7471915a831b046a873f54..7d075c52348c54118c045c287cf9d3f87191d870 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