diff --git a/src/line_detector.cpp b/src/line_detector.cpp index 14409d157a89e1c31fd936d0bfd141e9c96c1945..4827be2ee7459c97e11468385f552e65d7bbde93 100644 --- a/src/line_detector.cpp +++ b/src/line_detector.cpp @@ -94,7 +94,7 @@ unsigned int laserscanutils::extractLines(const laserscanutils::ScanParams & _pa //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 @@ -103,54 +103,59 @@ unsigned int laserscanutils::extractLines(const laserscanutils::ScanParams & _pa line_it2++; last_it = _line_list.end(); last_it--; + jj_jump = jumps.begin(); while (line_it1 != last_it && line_it1 != _line_list.end()) { - // 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 - theta = angleBetweenLines(*line_it1, *line_it2); - - //Check angle threshold - if ( theta < _alg_params.concatenate_angle_ut_ ) //fabs(theta) not required since theta>0 - { - //compute a new line with all involved points - Line new_line; - fitLine(points.block(0,line_it1->first_, 3, line_it2->last_-line_it1->first_+1), new_line); - - //check if error below a threshold to effectively concatenate - if ( new_line.error_ < _params.range_std_dev_*_alg_params.k_sigmas_ut_ ) - { - //update line1 as the concatenation of l1 and l2 - new_line.first_ = line_it1->first_; - new_line.last_ = line_it2->last_; - 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 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_it2++; - } - else - line_it2++; + while (jj_jump != jumps.end() && line_it1->first_ > (*jj_jump)) + jj_jump++; - if (line_it2 == _line_list.end()) //no more lines to be checked with line 1, next line 1 (and 2 is the next one) - { - //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++; - } + // check number of beams and jumps between lines + if ( ( (int)line_it2->first_ - (int)line_it1->last_ ) <= (int)_alg_params.concatenate_ii_ut_ && //not too many points between lines + !(jj_jump != jumps.end() && line_it1->first_ < (*jj_jump) && line_it2->last_ >= (*jj_jump)) ) //not jumps between lines + { + //compute angle between lines 1 and 2 + theta = angleBetweenLines(*line_it1, *line_it2); + + //Check angle threshold + if ( theta < _alg_params.concatenate_angle_ut_ ) //fabs(theta) not required since theta>0 + { + //compute a new line with all involved points + Line new_line; + fitLine(points.block(0,line_it1->first_, 3, line_it2->last_-line_it1->first_+1), new_line); + + //check if error below a threshold to effectively concatenate + if ( new_line.error_ < _params.range_std_dev_*_alg_params.k_sigmas_ut_ ) + { + //update line1 as the concatenation of l1 and l2 + new_line.first_ = line_it1->first_; + new_line.last_ = line_it2->last_; + 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 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_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) + { + //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 // lines not close enought, next line 1 (and 2 is the next one) + else // lines not close enought or jumps between, next line 1 (and 2 is the next one) { //std::cout << "lines not close enought, next line 1 (and 2 is the next one) " << _line_list.size() << std::endl; line_it1++; @@ -158,7 +163,7 @@ unsigned int laserscanutils::extractLines(const laserscanutils::ScanParams & _pa 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++)