Merge branch 'fix_laser_scan_utils' into 'master'

Fix laser scan utils

See merge request mobile_robotics/wolf!196

src/processor_tracker_feature_corner.cpp

@@ -12,10 +12,15 @@ namespace wolf
 {
 
 ProcessorTrackerFeatureCorner::ProcessorTrackerFeatureCorner(
-        const laserscanutils::LineFinderIterativeParams& _line_finder_params, const unsigned int& _n_corners_th) :
-        ProcessorTrackerFeature("TRACKER FEATURE CORNER", 0), line_finder_(_line_finder_params), n_tracks_th_(
-                _n_corners_th), R_world_sensor_(Eigen::Matrix3s::Identity()), R_robot_sensor_(
-                Eigen::Matrix3s::Identity()), extrinsics_transformation_computed_(false)
+        const laserscanutils::LineFinderIterativeParams& _line_finder_params,
+        const Scalar& _time_tolerance,
+        const unsigned int& _n_corners_th) :
+                ProcessorTrackerFeature("TRACKER FEATURE CORNER", _time_tolerance, 0),
+                line_finder_(_line_finder_params),
+                n_tracks_th_(_n_corners_th),
+                R_world_sensor_(Eigen::Matrix3s::Identity()),
+                R_robot_sensor_(Eigen::Matrix3s::Identity()),
+                extrinsics_transformation_computed_(false)
 {
     //
 }

src/processor_tracker_feature_corner.h

@@ -65,6 +65,7 @@ class ProcessorTrackerFeatureCorner : public ProcessorTrackerFeature
         EIGEN_MAKE_ALIGNED_OPERATOR_NEW; // to guarantee alignment (see http://eigen.tuxfamily.org/dox-devel/group__TopicStructHavingEigenMembers.html)
 
         ProcessorTrackerFeatureCorner(const laserscanutils::LineFinderIterativeParams& _line_finder_params,
+                                      const Scalar& _time_tolerance,
                                       const unsigned int& _n_corners_th);
         virtual ~ProcessorTrackerFeatureCorner();
 
@@ -87,7 +88,7 @@ class ProcessorTrackerFeatureCorner : public ProcessorTrackerFeature
          * \param _incoming_feature input/output feature in incoming capture to be corrected
          * \return false if the the process discards the correspondence with origin's feature
          */
-        virtual bool correctFeatureDrift(const FeatureBasePtr _last_feature, FeatureBasePtr _incoming_feature);
+        virtual bool correctFeatureDrift(const FeatureBasePtr _origin_feature, const FeatureBasePtr _last_feature, FeatureBasePtr _incoming_feature);
 
         /** \brief Vote for KeyFrame generation
          *
@@ -118,7 +119,7 @@ class ProcessorTrackerFeatureCorner : public ProcessorTrackerFeature
 
 };
 
-inline bool ProcessorTrackerFeatureCorner::correctFeatureDrift(const FeatureBasePtr _last_feature,
+inline bool ProcessorTrackerFeatureCorner::correctFeatureDrift(const FeatureBasePtr _origin_feature, const FeatureBasePtr _last_feature,
                                                               FeatureBasePtr _incoming_feature)
 {
     return true;

src/processor_tracker_landmark_corner.cpp

@@ -359,7 +359,7 @@ Eigen::VectorXs ProcessorTrackerLandmarkCorner::computeSquaredMahalanobisDistanc
 ProcessorBasePtr ProcessorTrackerLandmarkCorner::create(const std::string& _unique_name, const ProcessorParamsBasePtr _params, const SensorBasePtr sensor_ptr)
 {
     ProcessorParamsLaserPtr params = std::static_pointer_cast<ProcessorParamsLaser>(_params);
-    ProcessorTrackerLandmarkCornerPtr prc_ptr = std::make_shared<ProcessorTrackerLandmarkCorner>(params->line_finder_params_, params->new_corners_th, params->loop_frames_th);
+    ProcessorTrackerLandmarkCornerPtr prc_ptr = std::make_shared<ProcessorTrackerLandmarkCorner>(params->line_finder_params_, _params->time_tolerance,  params->new_corners_th, params->loop_frames_th);
     prc_ptr->setName(_unique_name);
     return prc_ptr;
 }
@@ -407,12 +407,18 @@ ProcessorTrackerLandmarkCorner::~ProcessorTrackerLandmarkCorner()
 }
 
 ProcessorTrackerLandmarkCorner::ProcessorTrackerLandmarkCorner(
-        const laserscanutils::LineFinderIterativeParams& _line_finder_params, const unsigned int& _new_corners_th,
+        const laserscanutils::LineFinderIterativeParams& _line_finder_params,
+        const Scalar& _time_tolerance,
+        const unsigned int& _new_corners_th,
         const unsigned int& _loop_frames_th) :
-        ProcessorTrackerLandmark("TRACKER LANDMARK CORNER", 0), line_finder_(_line_finder_params), new_corners_th_(
-                _new_corners_th), loop_frames_th_(_loop_frames_th), R_sensor_world_(Eigen::Matrix3s::Identity()), R_world_sensor_(
-                Eigen::Matrix3s::Identity()), R_robot_sensor_(Eigen::Matrix3s::Identity()), extrinsics_transformation_computed_(
-                false)
+                ProcessorTrackerLandmark("TRACKER LANDMARK CORNER", _time_tolerance, 0),
+                line_finder_(_line_finder_params),
+                new_corners_th_(_new_corners_th),
+                loop_frames_th_(_loop_frames_th),
+                R_sensor_world_(Eigen::Matrix3s::Identity()),
+                R_world_sensor_(Eigen::Matrix3s::Identity()),
+                R_robot_sensor_(Eigen::Matrix3s::Identity()),
+                extrinsics_transformation_computed_(false)
 {
 }
 

src/processor_tracker_landmark_corner.h

@@ -70,9 +70,9 @@ class ProcessorTrackerLandmarkCorner : public ProcessorTrackerLandmark
 
         // These values are constant -- provide a setter or accept them at construction time if you need to configure them
         Scalar aperture_error_th_ = 20.0 * M_PI / 180.; //20 degrees
-        Scalar angular_error_th_ = 10.0 * M_PI / 180.; //10 degrees;
-        Scalar position_error_th_ = 1;
-        Scalar min_features_ratio_th_ = 0.5;
+//        Scalar angular_error_th_ = 10.0 * M_PI / 180.; //10 degrees;
+//        Scalar position_error_th_ = 1;
+//        Scalar min_features_ratio_th_ = 0.5;
 
         Eigen::Matrix3s R_sensor_world_, R_world_sensor_;
         Eigen::Matrix3s R_robot_sensor_;
@@ -87,7 +87,9 @@ class ProcessorTrackerLandmarkCorner : public ProcessorTrackerLandmark
         EIGEN_MAKE_ALIGNED_OPERATOR_NEW; // to guarantee alignment (see http://eigen.tuxfamily.org/dox-devel/group__TopicStructHavingEigenMembers.html)
 
         ProcessorTrackerLandmarkCorner(const laserscanutils::LineFinderIterativeParams& _line_finder_params,
-                                       const unsigned int& _new_corners_th, const unsigned int& _loop_frames_th);
+                                       const Scalar& _time_tolerance,
+                                       const unsigned int& _new_corners_th,
+                                       const unsigned int& _loop_frames_th);
 
         virtual ~ProcessorTrackerLandmarkCorner();
 

src/processor_tracker_landmark_polyline.cpp

@@ -477,7 +477,7 @@ Scalar ProcessorTrackerLandmarkPolyline::sqDistPointToLine(const Eigen::Vector3s
     return (_A.head<2>() - _B.head<2>()).squaredNorm();
 }
 
-void ProcessorTrackerLandmarkPolyline::createNewLandmarks(LandmarkBaseList& _new_landmarks)
+void ProcessorTrackerLandmarkPolyline::createNewLandmarks()
 {
     //std::cout << "ProcessorTrackerLandmarkPolyline::createNewLandmarks" << std::endl;
     FeaturePolyline2DPtr polyline_ft_ptr;
@@ -488,7 +488,7 @@ void ProcessorTrackerLandmarkPolyline::createNewLandmarks(LandmarkBaseList& _new
         new_lmk_ptr = createLandmark(new_feature_ptr);
         //std::cout << "\tfeature: " << new_feature_ptr->id() << std::endl;
         //std::cout << "\tnew_landmark: " << new_lmk_ptr->id() << ": "<< ((LandmarkPolyline2D*)new_lmk_ptr)->getNPoints() << " points" << std::endl;
-        _new_landmarks.push_back(new_lmk_ptr);
+        new_landmarks_.push_back(new_lmk_ptr);
         // cast
         polyline_ft_ptr = std::static_pointer_cast<FeaturePolyline2D>(new_feature_ptr);
         // create new correspondence

src/processor_tracker_landmark_polyline.h

@@ -160,7 +160,7 @@ class ProcessorTrackerLandmarkPolyline : public ProcessorTrackerLandmark
 
         /** \brief Creates a landmark for each of new_features_last_
          **/
-        virtual void createNewLandmarks(LandmarkBaseList& _new_landmarks);
+        virtual void createNewLandmarks();
 
         /** \brief Create one landmark
          *

src/sensor_laser_2D.cpp

@@ -51,7 +51,7 @@ void SensorLaser2D::setDefaultScanParams()
     scan_params_.range_max_ = 100;
     scan_params_.range_std_dev_ = 0.01;
     
-    setNoise(Eigen::VectorXs::Constant(1,scan_params_.range_std_dev_));
+    setNoiseStd(Eigen::VectorXs::Constant(1,scan_params_.range_std_dev_));
 }
 
 void SensorLaser2D::setScanParams(const laserscanutils::LaserScanParams & _params)