Multi-hypothesis Tree for data association ready. Tested with iri_wolf ros node

src/capture_laser_2D.cpp

@@ -7,19 +7,24 @@
 // unsigned int CaptureLaser2D::max_beam_distance = 5;//max number of beams of distance between lines to consider corner or concatenation
 // double CaptureLaser2D::max_distance = 0.5;//max distance between line ends to consider corner or concatenation
 
-//CaptureLaser2D::CaptureLaser2D(const TimeStamp & _ts, SensorBase* _sensor_ptr, const std::vector<float>& _ranges):
+//CaptureLaser2D::CaptureLaser2D(const TimeStamp & _ts, SensorBase* _sensor_ptr, const std::vector<WolfScalar>& _ranges):
 //	CaptureBase(_ts, _sensor_ptr, _ranges),
 //	ranges_(data_.data(), _ranges.size()),
 //	intensities_(data_.data(), 0)
 //{
 //    laser_ptr_ = (SensorLaser2D*)sensor_ptr_;
 //}
-CaptureLaser2D::CaptureLaser2D(const TimeStamp & _ts, SensorBase* _sensor_ptr, const std::vector<float>& _ranges) :
-        CaptureBase(_ts, _sensor_ptr), ranges_(_ranges)
+CaptureLaser2D::CaptureLaser2D(const TimeStamp & _ts, 
+                               SensorBase* _sensor_ptr, 
+                               const std::vector<float>& _ranges) 
+                               :
+                               CaptureBase(_ts, _sensor_ptr), 
+                               ranges_(_ranges)
 {
     laser_ptr_ = (SensorLaser2D*) sensor_ptr_;
 }
 
+
 //CaptureLaser2D::CaptureLaser2D(const TimeStamp & _ts, SensorBase* _sensor_ptr, const Eigen::VectorXs& _ranges, const Eigen::VectorXs& _intensities):
 //    CaptureBase(_ts, _sensor_ptr, _ranges),
 //    ranges_(data_.data(), _ranges.size()),
@@ -27,12 +32,19 @@ CaptureLaser2D::CaptureLaser2D(const TimeStamp & _ts, SensorBase* _sensor_ptr, c
 //{
 //      laser_ptr_ = (SensorLaser2D*)sensor_ptr_;
 //}
-CaptureLaser2D::CaptureLaser2D(const TimeStamp & _ts, SensorBase* _sensor_ptr, const std::vector<float>& _ranges, const std::vector<float>& _intensities) :
-        CaptureBase(_ts, _sensor_ptr), ranges_(_ranges), intensities_(_intensities)
+CaptureLaser2D::CaptureLaser2D(const TimeStamp & _ts, 
+                               SensorBase* _sensor_ptr, 
+                               const std::vector<float>& _ranges, 
+                               const std::vector<float>& _intensities) 
+                               :
+                               CaptureBase(_ts, _sensor_ptr),
+                               ranges_(_ranges), 
+                               intensities_(_intensities)
 {
     laser_ptr_ = (SensorLaser2D*) sensor_ptr_;
 }
 
+
 CaptureLaser2D::~CaptureLaser2D()
 {
     //
@@ -41,8 +53,7 @@ CaptureLaser2D::~CaptureLaser2D()
 void CaptureLaser2D::processCapture()
 {
     //variables
-    //std::list<Eigen::Vector4s> corners;
-    std::list < laserscanutils::Corner > corners;
+    std::list <laserscanutils::Corner> corners;
 
     //extract corners from range data
     this->extractCorners(corners);
@@ -60,15 +71,16 @@ void CaptureLaser2D::processCapture()
 
 unsigned int CaptureLaser2D::extractCorners(std::list<laserscanutils::Corner> & _corner_list) const
 {
-    std::list < laserscanutils::Line > line_list;
-    
-    laserscanutils::extractLines(laser_ptr_->getScanParams(), laser_ptr_->getCornerAlgParams(), ranges_, line_list);
-    return laserscanutils::extractCorners(laser_ptr_->getScanParams(), laser_ptr_->getCornerAlgParams(), line_list, _corner_list);
+//     std::list < laserscanutils::Line > line_list;
+//     
+//     laserscanutils::extractLines(laser_ptr_->getScanParams(), laser_ptr_->getCornerAlgParams(), ranges_, line_list);
+//     return laserscanutils::extractCorners(laser_ptr_->getScanParams(), laser_ptr_->getCornerAlgParams(), line_list, _corner_list);
+    return laserscanutils::extractCorners(laser_ptr_->getScanParams(), laser_ptr_->getCornerAlgParams(), ranges_, _corner_list);
 }
 
 unsigned int CaptureLaser2D::extractLines(std::list<laserscanutils::Line> & _line_list) const
 {
-    return laserscanutils::extractLines(laser_ptr_->getScanParams(), laser_ptr_->getCornerAlgParams(), ranges_, _line_list);
+    return laserscanutils::extractLines(laser_ptr_->getScanParams(), laser_ptr_->getCornerAlgParams().line_params_, ranges_, _line_list);
 }
 
 void CaptureLaser2D::createFeatures(std::list<laserscanutils::Corner> & _corner_list)
@@ -257,9 +269,9 @@ void CaptureLaser2D::establishConstraintsMHTree()
     tree.solve(ft_lk_pairs,associated_mask);
     
     //print tree & score table 
-    std::cout << "-------------" << std::endl; 
-    tree.printTree();
-    tree.printScoreTable();
+//     std::cout << "-------------" << std::endl; 
+//     tree.printTree();
+//     tree.printScoreTable();
 
     //loop over all features
     ii = 0; //runs over features

src/capture_laser_2D.h

@@ -12,9 +12,10 @@
 #include <eigen3/Eigen/Geometry>
 
 //laser_scan_utils
+#include "iri-algorithms/laser_scan_utils/entities.h"
 #include "iri-algorithms/laser_scan_utils/scan_basics.h"
+#include "iri-algorithms/laser_scan_utils/line_detector.h"
 #include "iri-algorithms/laser_scan_utils/corner_detector.h"
-#include "iri-algorithms/laser_scan_utils/entities.h"
 
 //wolf includes
 #include "constraint_corner_2D_theta.h"
@@ -45,9 +46,9 @@ class CaptureLaser2D : public CaptureBase
 //         static double max_distance;//max distance between line ends to consider corner or concatenation
 
         //Eigen::Map<Eigen::VectorXs> ranges_; // a map to the ranges inside de data vector
-        std::vector<float> ranges_; // ranges vector
+        std::vector<float> ranges_; // ranges vector. Type float to match ROS LaserScan message 
         //Eigen::Map<Eigen::VectorXs> intensities_; // a map to the intensities inside the data vector
-        std::vector<float> intensities_; // intensities vector
+        std::vector<float> intensities_; // intensities vector. Type float to match ROS LaserScan message 
         SensorLaser2D* laser_ptr_; //specific pointer to sensor laser 2D object
         
     public:
@@ -56,7 +57,6 @@ class CaptureLaser2D : public CaptureBase
          * Constructor with ranges
          * 
          **/
-        //CaptureLaser2D(const TimeStamp & _ts, SensorBase* _sensor_ptr, const Eigen::VectorXs& _ranges);
         CaptureLaser2D(const TimeStamp & _ts, SensorBase* _sensor_ptr, const std::vector<float>& _ranges);
 
         /** \brief Constructor with ranges and intensities
@@ -64,7 +64,6 @@ class CaptureLaser2D : public CaptureBase
          * Constructor with ranges and intensities
          *
          **/
-        //CaptureLaser2D(const TimeStamp & _ts, SensorBase* _sensor_ptr, const Eigen::VectorXs& _ranges, const Eigen::VectorXs& _intensities);
         CaptureLaser2D(const TimeStamp & _ts, SensorBase* _sensor_ptr, const std::vector<float>& _ranges, const std::vector<float>& _intensities);
 
         /** \brief Destructor

src/examples/test_ceres_2_lasers.cpp

@@ -135,8 +135,8 @@ int main(int argc, char** argv)
     string modelFileName;
 
     //model and initial view point
-    modelFileName = "/home/jvallve/iri-lab/faramotics/models/campusNordUPC.obj";
-    //modelFileName = "/home/acoromin/dev/br/faramotics/models/campusNordUPC.obj";
+    //modelFileName = "/home/jvallve/iri-lab/faramotics/models/campusNordUPC.obj";
+    modelFileName = "/home/acoromin/dev/br/faramotics/models/campusNordUPC.obj";
     //modelFileName = "/home/andreu/dev/faramotics/models/campusNordUPC.obj";
     devicePose.setPose(2, 8, 0.2, 0, 0, 0);
     viewPoint.setPose(devicePose);

src/sensor_laser_2D.cpp

@@ -59,13 +59,13 @@ void SensorLaser2D::setScanParams(const laserscanutils::ScanParams & _params)
 void SensorLaser2D::setDefaultCornerAlgParams()
 {
     //default parameters for corner extraction. TODO: get them from a file or static constants
-    corners_alg_params_.segment_window_size = 8;
-    corners_alg_params_.theta_min = 0.4;
-    corners_alg_params_.theta_max_parallel = 0.1;
-    corners_alg_params_.k_sigmas = 3;
-    corners_alg_params_.max_beam_distance = 5;
-    corners_alg_params_.max_distance = 1;
-
+    corners_alg_params_.theta_min_ = 0.4;
+    corners_alg_params_.max_distance_ = 1;
+    corners_alg_params_.line_params_.jump_dist_ut_ = 1;
+    corners_alg_params_.line_params_.window_sz_ = 8;
+    corners_alg_params_.line_params_.k_sigmas_ut_ = 3; 
+    corners_alg_params_.line_params_.concatenate_angle_ut_ = 0.1; 
+    corners_alg_params_.line_params_.concatenate_ii_ut_ = 5;
 }
 
 void SensorLaser2D::setCornerAlgParams(const laserscanutils::ExtractCornerParams & _corner_alg_params)