considering jumps in line concatenation

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++)