divided extract corners in extract lines from ranges and extract corners from lines

src/corner_detector.cpp

@@ -12,6 +12,256 @@ void laserscanutils::ExtractCornerParams::print() const
                 << "   max_distance: " << max_distance << std::endl;
 }
 
+unsigned int laserscanutils::extractLines(const laserscanutils::ScanParams & _params, const ExtractCornerParams & _alg_params, const std::vector<float> & _ranges, std::list<laserscanutils::Line> & _line_list)
+{
+  //local variables
+  ScalarT cos_theta, theta;
+  Line line;
+  std::list<Line>::iterator line_it1, line_it2;
+  std::queue<unsigned int> jumps;
+  bool update_iterators;
+  ScalarT max_distance_sq(_alg_params.max_distance*_alg_params.max_distance); //init max distance as the squared value to avoid sqroot on comparisons
+
+  //init random generator
+  std::default_random_engine generator(1);
+  std::uniform_int_distribution<int> rand_window_overlapping(1,_alg_params.segment_window_size);
+
+  //STEP 0: convert range polar data to cartesian points, and keep range jumps
+//    for (unsigned int ii = 0; ii<_ranges.size(); ii++)
+//    {
+//        //range 2 polar
+//        azimuth = _params.angle_max_ - _params.angle_step_ * ii;
+//        points.col(ii) << _ranges(ii)*cos(azimuth), _ranges(ii)*sin(azimuth), 1; //points.row0-> x coordinate, points.row1->y coordinate
+//
+//        //check range jump and keep it
+//        if (ii > 0 && fabs(_ranges(ii)-_ranges(ii-1)) > _alg_params.max_distance) jumps.push(ii);
+//    }
+  Eigen::MatrixXs points = laserPolar2xy(_params, _ranges, _alg_params.max_distance, jumps);
+  std::cout << "jumps: " << jumps.size() << std::endl;
+  std::cout << "points: " << points.rows() << "x" << points.cols()<< std::endl;
+
+  // STEP 1: find line segments running over the scan
+  for (unsigned int ii = _alg_params.segment_window_size-1; ii<points.cols(); ii=ii+rand_window_overlapping(generator) )
+  {
+      unsigned int i_from = ii - _alg_params.segment_window_size + 1;
+
+      // update the jump to be checked
+      while (!jumps.empty() && i_from > jumps.front())
+          jumps.pop();
+
+      // check if there is a jump inside the window (not fitting if it is the case)
+      if (jumps.front() > i_from && jumps.front() <= ii)
+          continue;
+
+      //Found the best fitting line over points within the window [ii - segment_window_size + 1, ii]
+      //fitLine(i_from, ii, points, line);
+      fitLine(points.block(0,i_from, 3, ii-i_from+1), 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 )
+      {
+          line.first_ = i_from;
+          line.last_ = ii;
+          line.point_first_ = points.col(line.first_);
+          line.point_last_ = points.col(line.last_);
+          line.np_ = line.last_ - line.first_ + 1;
+          _line_list.push_back(line);
+      }
+  }
+  std::cout << "Lines fitted: " << _line_list.size() << std::endl;
+
+  //In case just less than two lines found, return with 0 corners
+  if ( _line_list.size() < 2 ) return 0;
+
+  //STEP 2: concatenate lines
+  line_it1 = _line_list.begin();
+  line_it2 = line_it1;
+  line_it2 ++;
+  while ( line_it1 != _line_list.end() )
+  {
+      //if no concatenation found, iterators will run forward
+      update_iterators = true;
+
+      // check if last of line1 and first of line2 are within the threshold
+      if ( ( line_it2->first_ - line_it1->last_ ) <=  _alg_params.max_beam_distance )
+      {
+          //compute angle between lines 1 and 2
+          cos_theta = (line_it1->vector_).dot(line_it2->vector_) / ( (line_it1->vector_).norm()*(line_it2->vector_).norm() );
+          theta = acos (cos_theta); //theta is in [0,PI]
+          if (theta > M_PI/2) theta = M_PI - theta;//assures theta>0 AND either theta close to 0 or close to PI will always map to theta close to 0.
+//            std::cout << std::endl << "cos_theta: " << cos_theta << std::endl << "theta: " << theta << std::endl <<
+//                                      "*index_it1: " << *index_it1 << std::endl << "*index_it2: " << *index_it2 << std::endl;
+//                                      "   (*line_it1).dot(*line_it2): " << (*line_it1).dot(*line_it2) << std::endl <<
+//                                      "   (*line_it1).norm()*(*line_it2).norm(): " << (*line_it1).norm()*(*line_it2).norm() << std::endl;
+
+          //Check angle threshold
+          if ( theta < _alg_params.theta_max_parallel ) //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 )
+              {
+                  //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_ = points.col(new_line.first_);
+                  new_line.point_last_ = points.col(new_line.last_);
+                  new_line.np_ = new_line.last_ - new_line.first_ + 1;
+                  *line_it1 = new_line;
+
+                  //remove l2 from the list
+                  line_it2 = _line_list.erase(line_it2);
+
+                  //in that case do not update iterators to allow concatenation of the new l1 with the next line in the list, if any
+                  update_iterators = false;
+
+//                      std::cout << "lines concatenated" << std::endl;
+//                      std::cout << "line 1: " << std::endl << line_it1->first << std::endl <<
+//                           line_it1->last << std::endl << line_it1->vector_.transpose() << std::endl << line_it1->error << std::endl;
+//                      std::cout << "line 2: " << std::endl << line_it2->first << std::endl <<
+//                           line_it2->last << std::endl << line_it2->vector_.transpose() << std::endl << line_it2->error << std::endl;
+//                      std::cout << "line resultant: "<< std::endl << new_line.first << std::endl <<
+//                          new_line.last << std::endl << new_line.vector_.transpose() << std::endl << new_line.error << std::endl;
+              }
+          }
+      }
+
+      //update iterators
+      if (update_iterators)
+      {
+          line_it1 ++;
+          line_it2 = line_it1;
+          line_it2 ++;
+      }
+  }
+  std::cout << "Lines after concatenation: " << _line_list.size() << std::endl;
+
+  return _line_list.size();
+}
+
+unsigned int laserscanutils::extractCorners(const laserscanutils::ScanParams & _params, const ExtractCornerParams & _alg_params, const std::list<laserscanutils::Line> & _line_list, std::list<laserscanutils::Corner> & _corner_list)
+{
+  //local variables
+  ScalarT cos_theta, theta;
+  Corner corner;
+  //std::list<Line>::iterator line_it1, line_it2;
+  bool update_iterators;
+  ScalarT max_distance_sq(_alg_params.max_distance*_alg_params.max_distance); //init max distance as the squared value to avoid sqroot on comparisons
+
+  //STEP 3:  find corners over the line list
+  auto line_it1 = _line_list.begin();
+  auto line_it2 = line_it1;
+  line_it2 ++;
+  while ( line_it1 != _line_list.end() )
+  {
+      //if no concatenation found, iterators will run forward
+      update_iterators = true;
+
+      // check if last of line1 and first of line2 are within the threshold
+      if ( ( line_it2->first_ - line_it1->last_ ) <=  _alg_params.max_beam_distance )
+      {
+          //compute angle between lines 1 and 2
+          cos_theta = (line_it1->vector_).dot(line_it2->vector_) / ( (line_it1->vector_).norm()*(line_it2->vector_).norm() );
+          theta = acos (cos_theta); //theta is in [0,PI]
+          if (theta > M_PI/2) theta = M_PI - theta;//assures theta>0 AND either theta close to 0 or close to PI will always map to theta close to 0.
+//             std::cout << std::endl << "cos_theta: " << cos_theta << std::endl << "theta: " << theta << std::endl <<
+//                     "line 1: " << line_it1->first << "-" << line_it1->last << std::endl <<
+//                     "line 2: " << line_it2->first << "-" << line_it2->last << std::endl <<
+//                     "   (*line_it1).dot(*line_it2): " << (line_it1->vector_).dot(line_it2->vector_) << std::endl <<
+//                     "   (*line_it1).norm()*(*line_it2).norm(): " << (line_it1->vector_).norm()*(line_it2->vector_).norm() << std::endl;
+
+          //Check angle threshold
+          if ( theta > _alg_params.theta_min ) //fabs(theta) not required since theta>0
+          {
+              // Find the corner ad the cross product between lines (intersection)
+              //corner = (line_it1->vector_).cross(line_it2->vector_);
+              corner.pt_ = (line_it1->vector_).cross(line_it2->vector_);
+
+              // normalize corner point to set last component to 1
+              corner.pt_ = corner.pt_ / corner.pt_(2);
+
+              // Check if the corner is close to both lines ends. TODO: Move this check before computing intersection
+              if ( (line_it1->point_last_ - corner.pt_).head(2).squaredNorm() < max_distance_sq
+                      &&
+                 (line_it2->point_first_ - corner.pt_).head(2).squaredNorm() < max_distance_sq )
+              {
+
+                  //vector from corner to first point of l1
+                  Eigen::Vector2s v1 = line_it1->point_first_.head(2) - corner.pt_.head(2);
+
+                  //compute corner orientation as the angle between v1 and local X, in [-pi,pi]
+                  corner.orientation_ = atan2(v1(1),v1(0));
+
+                  //Compute corner aperture with line_it1->first, corner and line_it2->last
+                  corner.aperture_ = cornerAperture(line_it1->point_first_, corner.pt_, line_it2->point_last_);
+
+                  //set other descriptors
+                  corner.line_1_ = *line_it1;
+                  corner.line_2_ = *line_it2;
+//                    corner.np_l1_ = line_it1->last_ - line_it1->first_;//>0 assured due to scanning order
+//                    corner.np_l2_ = line_it2->last_ - line_it2->first_;//>0 assured due to scanning order
+//                    corner.error_l1_ = line_it1->error_;
+//                    corner.error_l2_ = line_it2->error_;
+
+                  //add the corner to the list
+                  _corner_list.push_back(corner);
+              }
+          }
+
+          //update iterators //TODO: QUESTION: Are we checking each line against all ? Why if they are find consecutively?
+          if (line_it2 != _line_list.end() )
+          {
+              line_it2 ++;
+              update_iterators = false;
+          }
+      }
+
+      //update iterators
+      if (update_iterators)
+      {
+          line_it1 ++;
+          line_it2 = line_it1;
+          line_it2 ++;
+      }
+
+  }
+  std::cout << "Corners extracted: " << _corner_list.size() << std::endl;
+
+  // Erase duplicated corners
+  if ( _corner_list.size() > 1 )
+  {
+      // concatenate lines
+      std::list<laserscanutils::Corner>::iterator corner_it1 = _corner_list.begin();
+      std::list<laserscanutils::Corner>::iterator corner_it2 = corner_it1;
+      corner_it2 ++;
+      while ( corner_it2 != _corner_list.end() )
+      {
+          // Check if two corners are close enough. TODO: Check othe attributes also (orientation and aperture) !
+          if ( ( corner_it1->pt_ - corner_it2->pt_ ).head(2).squaredNorm() < max_distance_sq )
+          {
+              // Keep the one with bigger product of number of points of each line
+              (*corner_it1) = (corner_it1->line_1_.np_*corner_it1->line_2_.np_ > corner_it2->line_1_.np_*corner_it2->line_2_.np_? *corner_it1 : *corner_it2);
+              corner_it2 = _corner_list.erase(corner_it2);
+          }
+          else
+          {
+              corner_it1 ++;
+              corner_it2 = corner_it1;
+              corner_it2 ++;
+          }
+      }
+  }
+  std::cout << "Corners after removing duplicates: " << _corner_list.size() << std::endl;
+
+  for(std::list<laserscanutils::Corner>::iterator corner_it1 = _corner_list.begin(); corner_it1 != _corner_list.end(); corner_it1++)
+    corner_it1->print();
+
+  return _corner_list.size();
+}
+
 //unsigned int laserscanutils::extractCorners(const laserscanutils::ScanParams & _params, const ExtractCornerParams & _alg_params, const Eigen::VectorXs & _ranges, std::list<Eigen::Vector4s> & _corner_list)
 //unsigned int laserscanutils::extractCorners(const laserscanutils::ScanParams & _params, const ExtractCornerParams & _alg_params, const Eigen::VectorXs & _ranges, std::list<laserscanutils::Corner> & _corner_list)
 unsigned int laserscanutils::extractCorners(const laserscanutils::ScanParams & _params, const ExtractCornerParams & _alg_params, const std::vector<float> & _ranges, std::list<laserscanutils::Corner> & _corner_list)

src/corner_detector.h

@@ -50,6 +50,23 @@ namespace laserscanutils
      */
     //unsigned int extractCorners(const laserscanutils::ScanParams & _params, const ExtractCornerParams & _alg_params, const Eigen::VectorXs & _ranges, std::list<Eigen::Vector4s> & _corner_list);
 
+
+    /** \brief Extract lines from a given scan. Result as a list of Line
+     *
+     * Extract lines from a given scan.
+     * Returns Lines as a list of laserscanutils::Line
+     *
+     */
+    unsigned int extractLines(const laserscanutils::ScanParams & _params, const ExtractCornerParams & _alg_params, const std::vector<float> & _ranges, std::list<laserscanutils::Line> & _line_list);
+
+    /** \brief Extract corners from a given lines list. Result as a list of Corners
+     *
+     * Extract corners from a given lines list.
+     * Returns corners as a list of laserscanutils::Corners
+     *
+     */
+    unsigned int extractCorners(const laserscanutils::ScanParams & _params, const ExtractCornerParams & _alg_params, const std::list<laserscanutils::Line> & _line_list, std::list<laserscanutils::Corner> & _corner_list);
+
     /** \brief Extract corners from a given scan. Result as a list of Corners
      * 
      * Extract corners from a given scan.