From 8d0354a2855513fe6bda6beb6c06501826af9016 Mon Sep 17 00:00:00 2001
From: Joan Sola <jsola@iri.upc.edu>
Date: Thu, 8 Mar 2018 12:20:52 +0100
Subject: [PATCH] Manage time_tolerance in processor base and tracker ...

... not yet in processor motion.
---
 src/processor_base.h                            | 5 +----
 src/processor_frame_nearest_neighbor_filter.cpp | 2 +-
 src/processor_loopclosure_base.cpp              | 4 ++--
 src/processor_loopclosure_base.h                | 2 +-
 src/processor_tracker.cpp                       | 2 +-
 src/processor_tracker.h                         | 2 +-
 src/processor_tracker_feature.cpp               | 4 ++--
 src/processor_tracker_feature.h                 | 2 +-
 src/processor_tracker_feature_dummy.h           | 6 +++---
 src/processor_tracker_landmark.cpp              | 4 ++--
 src/processor_tracker_landmark.h                | 2 +-
 src/processors/processor_diff_drive.cpp         | 7 +------
 src/processors/processor_diff_drive.h           | 7 +++----
 src/test/gtest_problem.cpp                      | 6 +++---
 src/test/gtest_processor_base.cpp               | 3 +--
 15 files changed, 24 insertions(+), 34 deletions(-)

diff --git a/src/processor_base.h b/src/processor_base.h
index f9b33533b..0d10721f9 100644
--- a/src/processor_base.h
+++ b/src/processor_base.h
@@ -106,9 +106,6 @@ class KFPackBuffer
  */
 struct ProcessorParamsBase
 {
-    ProcessorParamsBase()          {time_tolerance = 0;};
-    virtual ~ProcessorParamsBase() = default;
-
     std::string type;
     std::string name;
     Scalar time_tolerance;
@@ -129,7 +126,7 @@ class ProcessorBase : public NodeBase, public std::enable_shared_from_this<Proce
         static unsigned int processor_id_count_;
 
     public:
-        ProcessorBase(const std::string& _type, const Scalar& _time_tolerance = 0);
+        ProcessorBase(const std::string& _type, const Scalar& _time_tolerance);
         virtual ~ProcessorBase();
         void remove();
 
diff --git a/src/processor_frame_nearest_neighbor_filter.cpp b/src/processor_frame_nearest_neighbor_filter.cpp
index a9c120565..8d23b6d4e 100644
--- a/src/processor_frame_nearest_neighbor_filter.cpp
+++ b/src/processor_frame_nearest_neighbor_filter.cpp
@@ -4,7 +4,7 @@ namespace wolf
 {
 
 ProcessorFrameNearestNeighborFilter::ProcessorFrameNearestNeighborFilter(const Params& _params):
-    ProcessorLoopClosureBase("FRAME NEAREST NEIGHBOR FILTER"),
+    ProcessorLoopClosureBase("FRAME NEAREST NEIGHBOR FILTER", _params.time_tolerance),
     params_(_params)
 {
   // area of ellipse based on the Chi-Square Probabilities
diff --git a/src/processor_loopclosure_base.cpp b/src/processor_loopclosure_base.cpp
index f1a2a3c64..3369b5bc5 100644
--- a/src/processor_loopclosure_base.cpp
+++ b/src/processor_loopclosure_base.cpp
@@ -10,8 +10,8 @@
 namespace wolf
 {
 
-ProcessorLoopClosureBase::ProcessorLoopClosureBase(const std::string& _type):
-  ProcessorBase(_type)
+ProcessorLoopClosureBase::ProcessorLoopClosureBase(const std::string& _type, const Scalar _time_tolerance):
+  ProcessorBase(_type, _time_tolerance)
 {
   //
 }
diff --git a/src/processor_loopclosure_base.h b/src/processor_loopclosure_base.h
index 8c5f14938..8de42fa59 100644
--- a/src/processor_loopclosure_base.h
+++ b/src/processor_loopclosure_base.h
@@ -45,7 +45,7 @@ protected:
 
 public:
 
-  ProcessorLoopClosureBase(const std::string& _type);
+  ProcessorLoopClosureBase(const std::string& _type, const Scalar _time_tolerance);
 
   virtual ~ProcessorLoopClosureBase() = default;
 
diff --git a/src/processor_tracker.cpp b/src/processor_tracker.cpp
index 1268c2deb..45718021a 100644
--- a/src/processor_tracker.cpp
+++ b/src/processor_tracker.cpp
@@ -14,7 +14,7 @@
 namespace wolf
 {
 
-ProcessorTracker::ProcessorTracker(const std::string& _type, const unsigned int _max_new_features, const Scalar& _time_tolerance) :
+ProcessorTracker::ProcessorTracker(const std::string& _type, const Scalar& _time_tolerance, const unsigned int _max_new_features) :
         ProcessorBase(_type, _time_tolerance),
         processing_step_(FIRST_TIME_WITHOUT_PACK),
         origin_ptr_(nullptr),
diff --git a/src/processor_tracker.h b/src/processor_tracker.h
index d4c63b71f..d8b39bf39 100644
--- a/src/processor_tracker.h
+++ b/src/processor_tracker.h
@@ -87,7 +87,7 @@ class ProcessorTracker : public ProcessorBase
         FeatureBaseList new_features_incoming_; ///< list of the new features of \b last successfully tracked in \b incoming
 
     public:
-        ProcessorTracker(const std::string& _type, const unsigned int _max_new_features = 0, const Scalar& _time_tolerance = 0.1);
+        ProcessorTracker(const std::string& _type, const Scalar& _time_tolerance, const unsigned int _max_new_features);
         virtual ~ProcessorTracker();
 
         /** \brief Full processing of an incoming Capture.
diff --git a/src/processor_tracker_feature.cpp b/src/processor_tracker_feature.cpp
index e7da3b557..9c1efa233 100644
--- a/src/processor_tracker_feature.cpp
+++ b/src/processor_tracker_feature.cpp
@@ -10,8 +10,8 @@
 namespace wolf
 {
 
-ProcessorTrackerFeature::ProcessorTrackerFeature(const std::string& _type, const unsigned int _max_new_features) :
-        ProcessorTracker(_type, _max_new_features)
+ProcessorTrackerFeature::ProcessorTrackerFeature(const std::string& _type, const Scalar _time_tolerance, const unsigned int _max_new_features) :
+        ProcessorTracker(_type, _time_tolerance, _max_new_features)
 {
 }
 
diff --git a/src/processor_tracker_feature.h b/src/processor_tracker_feature.h
index 302d5f416..f196ddfa7 100644
--- a/src/processor_tracker_feature.h
+++ b/src/processor_tracker_feature.h
@@ -76,7 +76,7 @@ class ProcessorTrackerFeature : public ProcessorTracker
 
         /** \brief Constructor with type
          */
-        ProcessorTrackerFeature(const std::string& _type, const unsigned int _max_new_features = 0);
+        ProcessorTrackerFeature(const std::string& _type, const Scalar _time_tolerance, const unsigned int _max_new_features);
         virtual ~ProcessorTrackerFeature();
 
     protected:
diff --git a/src/processor_tracker_feature_dummy.h b/src/processor_tracker_feature_dummy.h
index 6fa449695..f6a917cf1 100644
--- a/src/processor_tracker_feature_dummy.h
+++ b/src/processor_tracker_feature_dummy.h
@@ -22,7 +22,7 @@ class ProcessorTrackerFeatureDummy : public ProcessorTrackerFeature
 {
 
     public:
-        ProcessorTrackerFeatureDummy(const unsigned int _max_new_features, const unsigned int _min_feat_for_keyframe);
+        ProcessorTrackerFeatureDummy(const Scalar _time_tolerance, const unsigned int _max_new_features, const unsigned int _min_feat_for_keyframe);
         virtual ~ProcessorTrackerFeatureDummy();
 
     protected:
@@ -73,8 +73,8 @@ class ProcessorTrackerFeatureDummy : public ProcessorTrackerFeature
 
 };
 
-inline ProcessorTrackerFeatureDummy::ProcessorTrackerFeatureDummy(const unsigned int _max_new_features, const unsigned int _min_feat_for_keyframe) :
-        ProcessorTrackerFeature("TRACKER FEATURE DUMMY", _max_new_features),
+inline ProcessorTrackerFeatureDummy::ProcessorTrackerFeatureDummy(const Scalar _time_tolerance, const unsigned int _max_new_features, const unsigned int _min_feat_for_keyframe) :
+        ProcessorTrackerFeature("TRACKER FEATURE DUMMY", _time_tolerance, _max_new_features),
         n_feature_(0), min_feat_for_keyframe_(_min_feat_for_keyframe)
 {
     //
diff --git a/src/processor_tracker_landmark.cpp b/src/processor_tracker_landmark.cpp
index 9c0603659..9fd9bd499 100644
--- a/src/processor_tracker_landmark.cpp
+++ b/src/processor_tracker_landmark.cpp
@@ -14,8 +14,8 @@
 namespace wolf
 {
 
-ProcessorTrackerLandmark::ProcessorTrackerLandmark(const std::string& _type, const unsigned int& _max_new_features, const Scalar& _time_tolerance) :
-    ProcessorTracker(_type, _max_new_features, _time_tolerance)
+ProcessorTrackerLandmark::ProcessorTrackerLandmark(const std::string& _type, const Scalar& _time_tolerance, const unsigned int& _max_new_features) :
+    ProcessorTracker(_type, _time_tolerance, _max_new_features)
 {
 }
 
diff --git a/src/processor_tracker_landmark.h b/src/processor_tracker_landmark.h
index 44db01813..8f6c5c3e6 100644
--- a/src/processor_tracker_landmark.h
+++ b/src/processor_tracker_landmark.h
@@ -72,7 +72,7 @@ WOLF_PTR_TYPEDEFS(ProcessorTrackerLandmark);
 class ProcessorTrackerLandmark : public ProcessorTracker
 {
     public:
-        ProcessorTrackerLandmark(const std::string& _type, const unsigned int& _max_new_features = 0, const Scalar& _time_tolerance = 0.1);
+        ProcessorTrackerLandmark(const std::string& _type, const Scalar& _time_tolerance, const unsigned int& _max_new_features);
         virtual ~ProcessorTrackerLandmark();
 
     protected:
diff --git a/src/processors/processor_diff_drive.cpp b/src/processors/processor_diff_drive.cpp
index 43eb7fed0..535cd5c17 100644
--- a/src/processors/processor_diff_drive.cpp
+++ b/src/processors/processor_diff_drive.cpp
@@ -242,16 +242,11 @@ FeatureBasePtr ProcessorDiffDrive::createFeature(CaptureMotionPtr _capture_motio
     return key_feature_ptr;
 }
 
-//bool ProcessorDiffDrive::keyFrameCallback(FrameBasePtr _keyframe_ptr, const Scalar& _time_tol)
-//{
-//  return ProcessorMotion::keyFrameCallback(_keyframe_ptr, _time_tol);
-//}
-
 ProcessorBasePtr ProcessorDiffDrive::create(const std::string& _unique_name,
                                             const ProcessorParamsBasePtr _params,
                                             const SensorBasePtr _sensor_ptr)
 {
-  const auto params = std::dynamic_pointer_cast<ProcessorParamsDiffDrive>(_params);
+  const auto params = std::static_pointer_cast<ProcessorParamsDiffDrive>(_params);
 
   if (params == nullptr)
   {
diff --git a/src/processors/processor_diff_drive.h b/src/processors/processor_diff_drive.h
index 030cb9516..6daba6031 100644
--- a/src/processors/processor_diff_drive.h
+++ b/src/processors/processor_diff_drive.h
@@ -17,7 +17,8 @@ WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsDiffDrive);
 
 struct ProcessorParamsDiffDrive : public ProcessorParamsBase
 {
-  ProcessorParamsDiffDrive(const Scalar _dist_travel_th,
+  ProcessorParamsDiffDrive(const Scalar _time_tolerance,
+                           const Scalar _dist_travel_th,
                            const Scalar _theta_traveled_th,
                            const Scalar _cov_det_th,
                            const Scalar _unmeasured_perturbation_std = 0.0001) :
@@ -26,7 +27,7 @@ struct ProcessorParamsDiffDrive : public ProcessorParamsBase
     cov_det_th_(_cov_det_th),
     unmeasured_perturbation_std_(_unmeasured_perturbation_std)
   {
-    //
+      time_tolerance = _time_tolerance;
   }
 
   Scalar dist_traveled_th_;
@@ -110,8 +111,6 @@ protected:
 
   virtual FeatureBasePtr createFeature(CaptureMotionPtr _capture_motion) override;
 
-//  bool keyFrameCallback(FrameBasePtr _keyframe_ptr, const Scalar& _time_tol) override;
-
 public:
 
   /// @brief Factory method
diff --git a/src/test/gtest_problem.cpp b/src/test/gtest_problem.cpp
index 0bbcf9d18..98459c49d 100644
--- a/src/test/gtest_problem.cpp
+++ b/src/test/gtest_problem.cpp
@@ -79,7 +79,7 @@ TEST(Problem, Installers)
     SensorBasePtr    S = P->installSensor   ("ODOM 3D", "odometer",        xs,         wolf_root + "/src/examples/sensor_odom_3D.yaml");
 
     // install processor tracker (dummy installation under an Odometry sensor -- it's OK for this test)
-    ProcessorBasePtr pt = std::make_shared<ProcessorTrackerFeatureDummy>(ProcessorTrackerFeatureDummy(5, 10));
+    ProcessorBasePtr pt = std::make_shared<ProcessorTrackerFeatureDummy>(ProcessorTrackerFeatureDummy(0.1, 5, 10));
     S->addProcessor(pt);
 
 
@@ -218,7 +218,7 @@ TEST(Problem, StateBlocks)
     ASSERT_EQ(P->getStateBlockList().size(),                2 + 3);
     ASSERT_EQ(P->getStateBlockNotificationList().size(),    2 + 3);
 
-    ProcessorBasePtr pt = std::make_shared<ProcessorTrackerFeatureDummy>(ProcessorTrackerFeatureDummy(5, 10));
+    ProcessorBasePtr pt = std::make_shared<ProcessorTrackerFeatureDummy>(ProcessorTrackerFeatureDummy(0.1, 5, 10));
     St->addProcessor(pt);
     ProcessorBasePtr pm = P->installProcessor("ODOM 3D",            "odom integrator",      "odometer", wolf_root + "/src/examples/processor_odom_3D.yaml");
 
@@ -247,7 +247,7 @@ TEST(Problem, Covariances)
 
     SensorBasePtr    Sm = P->installSensor   ("ODOM 3D", "odometer",xs, wolf_root + "/src/examples/sensor_odom_3D.yaml");
     SensorBasePtr    St = P->installSensor   ("CAMERA", "camera",   xs, wolf_root + "/src/examples/camera_params_ueye_sim.yaml");
-    ProcessorBasePtr pt = std::make_shared<ProcessorTrackerFeatureDummy>(ProcessorTrackerFeatureDummy(5, 10));
+    ProcessorBasePtr pt = std::make_shared<ProcessorTrackerFeatureDummy>(ProcessorTrackerFeatureDummy(0.1, 5, 10));
     St->addProcessor(pt);
     ProcessorBasePtr pm = P->installProcessor("ODOM 3D",            "odom integrator",      "odometer", wolf_root + "/src/examples/processor_odom_3D.yaml");
 
diff --git a/src/test/gtest_processor_base.cpp b/src/test/gtest_processor_base.cpp
index ce31eeed3..fd5648149 100644
--- a/src/test/gtest_processor_base.cpp
+++ b/src/test/gtest_processor_base.cpp
@@ -42,8 +42,7 @@ TEST(ProcessorBase, KeyFrameCallback)
     SensorBasePtr sens_trk = make_shared<SensorBase>("FEATURE", std::make_shared<StateBlock>(Eigen::VectorXs::Zero(2)),
                                                      std::make_shared<StateBlock>(Eigen::VectorXs::Zero(1)),
                                                      std::make_shared<StateBlock>(Eigen::VectorXs::Zero(2)), 2);
-    shared_ptr<ProcessorTrackerFeatureDummy> proc_trk = make_shared<ProcessorTrackerFeatureDummy>(5, 5);
-    proc_trk->setTimeTolerance(dt/2);
+    shared_ptr<ProcessorTrackerFeatureDummy> proc_trk = make_shared<ProcessorTrackerFeatureDummy>(dt/2, 5, 5);
 
     problem->addSensor(sens_trk);
     sens_trk->addProcessor(proc_trk);
-- 
GitLab