processorLandmarkExternal is PT now

include/core/processor/processor_tracker_feature_landmark_external.h → include/core/processor/processor_landmark_external.h

@@ -23,14 +23,15 @@
 #pragma once
 
 #include "core/common/wolf.h"
-#include "core/processor/processor_tracker_feature.h"
+#include "core/processor/processor_tracker.h"
+#include "core/processor/track_matrix.h"
 
 namespace wolf
 {
 
-WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorTrackerFeatureLandmarkExternal);
+WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorLandmarkExternal);
 
-struct ParamsProcessorTrackerFeatureLandmarkExternal : public ParamsProcessorTrackerFeature
+struct ParamsProcessorLandmarkExternal : public ParamsProcessorTracker
 {
     bool use_orientation;                 ///< use orientation measure or not when emplacing factors
     double filter_quality_th;             ///< min quality to consider the detection
@@ -38,10 +39,10 @@ struct ParamsProcessorTrackerFeatureLandmarkExternal : public ParamsProcessorTra
     unsigned int filter_track_length_th;  ///< length of the track necessary to consider the detection
     double time_span;                     ///< for keyframe voting: time span since last frame
 
-    ParamsProcessorTrackerFeatureLandmarkExternal() = default;
-    ParamsProcessorTrackerFeatureLandmarkExternal(std::string _unique_name,
+    ParamsProcessorLandmarkExternal() = default;
+    ParamsProcessorLandmarkExternal(std::string _unique_name,
                                        const wolf::ParamsServer & _server):
-        ParamsProcessorTrackerFeature(_unique_name, _server)
+        ParamsProcessorTracker(_unique_name, _server)
     {
         use_orientation         = _server.getParam<bool>        (prefix + _unique_name + "/use_orientation");
         filter_quality_th       = _server.getParam<double>      (prefix + _unique_name + "/filter_quality_th");
@@ -51,50 +52,45 @@ struct ParamsProcessorTrackerFeatureLandmarkExternal : public ParamsProcessorTra
     }
 };
 
-WOLF_PTR_TYPEDEFS(ProcessorTrackerFeatureLandmarkExternal);
+WOLF_PTR_TYPEDEFS(ProcessorLandmarkExternal);
 
 //Class
-class ProcessorTrackerFeatureLandmarkExternal : public ProcessorTrackerFeature
+class ProcessorLandmarkExternal : public ProcessorTracker
 {
     public:
-        ProcessorTrackerFeatureLandmarkExternal(ParamsProcessorTrackerFeatureLandmarkExternalPtr _params_tracker_feature);
-        ~ProcessorTrackerFeatureLandmarkExternal() override;
+        ProcessorLandmarkExternal(ParamsProcessorLandmarkExternalPtr _params_tracker_feature);
+        ~ProcessorLandmarkExternal() override;
 
         // Factory method for high level API
-        WOLF_PROCESSOR_CREATE(ProcessorTrackerFeatureLandmarkExternal, ParamsProcessorTrackerFeatureLandmarkExternal);
+        WOLF_PROCESSOR_CREATE(ProcessorLandmarkExternal, ParamsProcessorLandmarkExternal);
 
         void configure(SensorBasePtr _sensor) override { };
 
     protected:
 
-        ParamsProcessorTrackerFeatureLandmarkExternalPtr params_tfle_;
-        //std::set<FeatureBasePtr> unmatched_detections_incoming_, unmatched_detections_last_;
-        std::list<FeatureBasePtr> unmatched_detections_incoming_;
+        ParamsProcessorLandmarkExternalPtr params_tfle_;
+        TrackMatrix track_matrix_;
 
-        /** \brief Track provided features in \b _capture
-         * \param _features_in input list of features in \b last to track
-         * \param _capture the capture in which the _features_in should be searched
-         * \param _features_out returned list of features found in \b _capture
-         * \param _feature_correspondences returned map of correspondences: _feature_correspondences[feature_out_ptr] = feature_in_ptr
+        /** Pre-process incoming Capture
+         *
+         * This is called by process() just after assigning incoming_ptr_ to a valid Capture.
          *
-         * IMPORTANT: The features in _features_out should be emplaced. Don't use `make_shared`, use `FeatureBase::emplace` instead.
-         * Then, they will be already linked to the _capture.
+         * Extract the detections and fill unmatched_detections_incoming_ (FeaturePtrList)
+         */
+        void preProcess() override;
+
+
+        /** \brief Process known Features
+         * \return The number of successful matches.
          *
-         * \return the number of features tracked
+         * This function tracks features from last to incoming and starts new tracks for new (not tracked) features in incoming
          */
-        unsigned int trackFeatures(const FeatureBasePtrList& _features_in,
-                                   const CaptureBasePtr& _capture,
-                                   FeatureBasePtrList& _features_out,
-                                   FeatureMatchMap& _feature_correspondences) override;
-
-        /** \brief Correct the drift in incoming feature by re-comparing against the corresponding feature in origin.
-         * \param _last_feature input feature in last capture tracked
-         * \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
+        unsigned int processKnown() override;
+
+        /**\brief Process new Features
+         *
          */
-        bool correctFeatureDrift(const FeatureBasePtr _origin_feature,
-                                 const FeatureBasePtr _last_feature,
-                                 FeatureBasePtr _incoming_feature) override;
+        unsigned int processNew(const int& _max_features) override { return 0;};
 
         /** \brief Vote for KeyFrame generation
          *
@@ -104,40 +100,28 @@ class ProcessorTrackerFeatureLandmarkExternal : public ProcessorTrackerFeature
          * WARNING! This function only votes! It does not create KeyFrames!
          */
         bool voteForKeyFrame() const override;
-
-        /** \brief Detect new Features
-         * \param _max_features maximum number of features detected (-1: unlimited. 0: none)
-         * \param _capture The capture in which the new features should be detected.
-         * \param _features_out The list of detected Features in _capture.
-         * \return The number of detected Features.
-         *
-         * This function detects Features that do not correspond to known Features/Landmarks in the system.
-         *
-         * IMPORTANT: The features in _features_out should be emplaced. Don't use `make_shared`, use `FeatureBase::emplace` instead.
-         * Then, they will be already linked to the _capture.
-         *
-         * The function is called in ProcessorTrackerFeature::processNew() to set the member new_features_last_,
-         * the list of newly detected features of the capture last_ptr_.
+        
+        /** \brief Emplaces a new factor for each known feature in Capture \b last
          */
-        unsigned int detectNewFeatures(const int& _max_new_features,
-                                       const CaptureBasePtr& _capture,
-                                       FeatureBasePtrList& _features_out) override;
+        void establishFactors() override;
+        
         /** \brief Emplaces a new factor
-         * \param _feature_ptr pointer to the parent Feature
-         * \param _feature_other_ptr pointer to the other feature constrained.
-         *
-         * This function emplaces a factor of the appropriate type for the derived processor.
+         * \param _feature feature pointer
+         * \param _landmark pointer to the landmark
+         */
+        FactorBasePtr emplaceFactor(FeatureBasePtr _feature, LandmarkBasePtr _landmark);
+        
+        /** \brief Emplaces a landmark or modifies (if needed) a landmark
+         * \param _feature_ptr pointer to the Feature
          */
-        FactorBasePtr emplaceFactor(FeatureBasePtr _feature_ptr,
-                                    FeatureBasePtr _feature_other_ptr) override;
+        LandmarkBasePtr emplaceLandmark(FeatureBasePtr _feature_ptr);
 
-        /** Pre-process incoming Capture
-         *
-         * This is called by process() just after assigning incoming_ptr_ to a valid Capture.
-         *
-         * extract the detections and fill detections_incoming_ (FeaturePtrList)
+        /** \brief Modifies (if needed) a landmark
+         * \param _landmark pointer to the landmark
+         * \param _feature pointer to the Feature.
          */
-        void preProcess() override;
+        void modifyLandmark(LandmarkBasePtr _landmark,
+                            FeatureBasePtr _feature);
 
         /** Post-process
          *
@@ -157,23 +141,16 @@ class ProcessorTrackerFeatureLandmarkExternal : public ProcessorTrackerFeature
                                  const VectorComposite& _pose_sen) const;
 };
 
-inline ProcessorTrackerFeatureLandmarkExternal::ProcessorTrackerFeatureLandmarkExternal(ParamsProcessorTrackerFeatureLandmarkExternalPtr _params_tfle) :
-        ProcessorTrackerFeature("ProcessorTrackerFeatureLandmarkExternal", "PO", 0, _params_tfle),
+inline ProcessorLandmarkExternal::ProcessorLandmarkExternal(ParamsProcessorLandmarkExternalPtr _params_tfle) :
+        ProcessorTracker("ProcessorLandmarkExternal", "PO", 0, _params_tfle),
         params_tfle_(_params_tfle)
 {
     //
 }
 
-inline ProcessorTrackerFeatureLandmarkExternal::~ProcessorTrackerFeatureLandmarkExternal()
+inline ProcessorLandmarkExternal::~ProcessorLandmarkExternal()
 {
     //
 }
 
-inline bool ProcessorTrackerFeatureLandmarkExternal::correctFeatureDrift(const FeatureBasePtr _origin_feature,
-                                                                         const FeatureBasePtr _last_feature,
-                                                                         FeatureBasePtr _incoming_feature)
-{
-    return true;
-}
-
 } // namespace wolf
\ No newline at end of file

src/processor/processor_tracker.cpp

@@ -158,8 +158,8 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
             incoming_ptr_->link(frame);
 
             // Process info
-            // TrackerLandmark: If we have been given a map, all landmarks in the map are known. Process them.
-            if (not getProblem()->getMap()->getLandmarkList().empty())
+            // If we have known landmarks or features. Process them.
+            if (not getProblem()->getMap()->getLandmarkList().empty() or not known_features_last_.empty())
                 processKnown();
             
             // Both Trackers:  We have a last_ Capture with not enough features, so populate it.

test/CMakeLists.txt

@@ -197,15 +197,15 @@ wolf_add_gtest(gtest_map_yaml gtest_map_yaml.cpp)
 # Parameter prior test
 wolf_add_gtest(gtest_param_prior gtest_param_prior.cpp)
 
-# ProcessorTrackerFeatureLandmarkExternal class test
-wolf_add_gtest(gtest_processor_tracker_feature_landmark_external gtest_processor_tracker_feature_landmark_external.cpp)
-
 # ProcessorFixedWingModel class test
 wolf_add_gtest(gtest_processor_fixed_wing_model gtest_processor_fixed_wing_model.cpp)
 
 # ProcessorDiffDrive class test
 wolf_add_gtest(gtest_processor_diff_drive gtest_processor_diff_drive.cpp)
 
+# ProcessorLandmarkExternal class test
+wolf_add_gtest(gtest_processor_landmark_external gtest_processor_landmark_external.cpp)
+
 # ProcessorLoopClosure class test
 wolf_add_gtest(gtest_processor_loop_closure gtest_processor_loop_closure.cpp)
 

test/gtest_processor_tracker_feature_landmark_external.cpp → test/gtest_processor_landmark_external.cpp

@@ -33,7 +33,7 @@
 #include "core/state_block/state_quaternion.h"
 #include "core/ceres_wrapper/solver_ceres.h"
 
-#include "core/processor/processor_tracker_feature_landmark_external.h"
+#include "core/processor/processor_landmark_external.h"
 
 // STL
 #include <iterator>
@@ -42,7 +42,7 @@
 using namespace wolf;
 using namespace Eigen;
 
-class ProcessorTrackerFeatureLandmarkExternalTest : public testing::Test
+class ProcessorLandmarkExternalTest : public testing::Test
 {
     protected:
         int dim;
@@ -53,7 +53,7 @@ class ProcessorTrackerFeatureLandmarkExternalTest : public testing::Test
         ProblemPtr problem;
         SolverManagerPtr solver;
         SensorBasePtr sensor, sensor_odom;
-        ProcessorTrackerFeatureLandmarkExternalPtr processor;
+        ProcessorLandmarkExternalPtr processor;
         ProcessorMotionPtr processor_motion;
         std::vector<LandmarkBasePtr> landmarks;
 
@@ -81,7 +81,7 @@ class ProcessorTrackerFeatureLandmarkExternalTest : public testing::Test
         void assertVectorComposite(const VectorComposite& v1, const VectorComposite& v2) const;
 };
 
-void ProcessorTrackerFeatureLandmarkExternalTest::initProblem(int _dim, 
+void ProcessorLandmarkExternalTest::initProblem(int _dim, 
                                                               bool _orientation,
                                                               double _quality_th, 
                                                               double _dist_th, 
@@ -125,9 +125,8 @@ void ProcessorTrackerFeatureLandmarkExternalTest::initProblem(int _dim,
     }
 
     // Processors
-    ParamsProcessorTrackerFeatureLandmarkExternalPtr params = std::make_shared<ParamsProcessorTrackerFeatureLandmarkExternal>();
+    ParamsProcessorLandmarkExternalPtr params = std::make_shared<ParamsProcessorLandmarkExternal>();
     params->time_tolerance = dt/2;
-    params->min_features_for_keyframe = 0;
     params->max_new_features = -1;
     params->voting_active = true;
     params->apply_loss_function = false;
@@ -136,7 +135,7 @@ void ProcessorTrackerFeatureLandmarkExternalTest::initProblem(int _dim,
     params->filter_dist_th = _dist_th;
     params->filter_track_length_th = _track_length_th;
     params->time_span = _time_span;
-    processor = ProcessorBase::emplace<ProcessorTrackerFeatureLandmarkExternal>(sensor, params);
+    processor = ProcessorBase::emplace<ProcessorLandmarkExternal>(sensor, params);
 
     if (dim == 2)
     {
@@ -190,7 +189,7 @@ void ProcessorTrackerFeatureLandmarkExternalTest::initProblem(int _dim,
     state_sensor = sensor->getState("PO");
 }
 
-void ProcessorTrackerFeatureLandmarkExternalTest::randomStep()
+void ProcessorLandmarkExternalTest::randomStep()
 {
     // compute delta
     VectorXd delta;
@@ -215,7 +214,7 @@ void ProcessorTrackerFeatureLandmarkExternalTest::randomStep()
     state_robot = processor_motion->getState("PO");
 }
 
-CaptureLandmarksExternalPtr ProcessorTrackerFeatureLandmarkExternalTest::computeCaptureLandmarks() const
+CaptureLandmarksExternalPtr ProcessorLandmarkExternalTest::computeCaptureLandmarks() const
 {
     // Detections
     auto cap = std::make_shared<CaptureLandmarksExternal>(t, sensor);
@@ -253,7 +252,7 @@ CaptureLandmarksExternalPtr ProcessorTrackerFeatureLandmarkExternalTest::compute
     return cap;
 }
 
-void ProcessorTrackerFeatureLandmarkExternalTest::testConfiguration(int dim, 
+void ProcessorLandmarkExternalTest::testConfiguration(int dim, 
                                                                     bool orientation, 
                                                                     double quality_th, 
                                                                     double dist_th, 
@@ -363,7 +362,7 @@ void ProcessorTrackerFeatureLandmarkExternalTest::testConfiguration(int dim,
     }
 }
 
-void ProcessorTrackerFeatureLandmarkExternalTest::assertVectorComposite(const VectorComposite& v1, const VectorComposite& v2) const
+void ProcessorLandmarkExternalTest::assertVectorComposite(const VectorComposite& v1, const VectorComposite& v2) const
 {
     if (v1.includesStructure("PO") and v2.includesStructure("PO"))
     {
@@ -385,7 +384,7 @@ void ProcessorTrackerFeatureLandmarkExternalTest::assertVectorComposite(const Ve
 }
 
 // / TESTS //////////////////////////////////////////
-TEST_F(ProcessorTrackerFeatureLandmarkExternalTest, P_2d_existing_lmks)
+TEST_F(ProcessorLandmarkExternalTest, P_2d_existing_lmks)
 {
     testConfiguration(2,      //int dim
                       false,  //bool orientation
@@ -396,7 +395,7 @@ TEST_F(ProcessorTrackerFeatureLandmarkExternalTest, P_2d_existing_lmks)
                       true);  //bool add_landmarks
 }
 
-TEST_F(ProcessorTrackerFeatureLandmarkExternalTest, P_2d_no_lmks)
+TEST_F(ProcessorLandmarkExternalTest, P_2d_no_lmks)
 {
     testConfiguration(2,      //int dim
                       false,  //bool orientation
@@ -407,7 +406,7 @@ TEST_F(ProcessorTrackerFeatureLandmarkExternalTest, P_2d_no_lmks)
                       false); //bool add_landmarks
 }
 
-TEST_F(ProcessorTrackerFeatureLandmarkExternalTest, P_2d_no_lmks_quality)
+TEST_F(ProcessorLandmarkExternalTest, P_2d_no_lmks_quality)
 {
     testConfiguration(2,      //int dim
                       false,  //bool orientation
@@ -418,7 +417,7 @@ TEST_F(ProcessorTrackerFeatureLandmarkExternalTest, P_2d_no_lmks_quality)
                       false); //bool add_landmarks
 }
 
-TEST_F(ProcessorTrackerFeatureLandmarkExternalTest, PO_2d_existing_lmks)
+TEST_F(ProcessorLandmarkExternalTest, PO_2d_existing_lmks)
 {
     testConfiguration(2,      //int dim
                       true,   //bool orientation
@@ -429,7 +428,7 @@ TEST_F(ProcessorTrackerFeatureLandmarkExternalTest, PO_2d_existing_lmks)
                       true);  //bool add_landmarks
 }
 
-TEST_F(ProcessorTrackerFeatureLandmarkExternalTest, PO_2d_no_lmks)
+TEST_F(ProcessorLandmarkExternalTest, PO_2d_no_lmks)
 {
     testConfiguration(2,      //int dim
                       true,   //bool orientation
@@ -440,7 +439,7 @@ TEST_F(ProcessorTrackerFeatureLandmarkExternalTest, PO_2d_no_lmks)
                       false); //bool add_landmarks
 }
 
-TEST_F(ProcessorTrackerFeatureLandmarkExternalTest, PO_2d_no_lmks_quality)
+TEST_F(ProcessorLandmarkExternalTest, PO_2d_no_lmks_quality)
 {
     testConfiguration(2,      //int dim
                       true,   //bool orientation
@@ -451,7 +450,7 @@ TEST_F(ProcessorTrackerFeatureLandmarkExternalTest, PO_2d_no_lmks_quality)
                       false); //bool add_landmarks
 }
 
-TEST_F(ProcessorTrackerFeatureLandmarkExternalTest, P_3d_existing_lmks)
+TEST_F(ProcessorLandmarkExternalTest, P_3d_existing_lmks)
 {
     testConfiguration(3,      //int dim
                       false,  //bool orientation
@@ -462,7 +461,7 @@ TEST_F(ProcessorTrackerFeatureLandmarkExternalTest, P_3d_existing_lmks)
                       true);  //bool add_landmarks
 }
 
-TEST_F(ProcessorTrackerFeatureLandmarkExternalTest, P_3d_no_lmks)
+TEST_F(ProcessorLandmarkExternalTest, P_3d_no_lmks)
 {
     testConfiguration(3,      //int dim
                       false,  //bool orientation
@@ -473,7 +472,7 @@ TEST_F(ProcessorTrackerFeatureLandmarkExternalTest, P_3d_no_lmks)
                       false); //bool add_landmarks
 }
 
-TEST_F(ProcessorTrackerFeatureLandmarkExternalTest, P_3d_no_lmks_quality)
+TEST_F(ProcessorLandmarkExternalTest, P_3d_no_lmks_quality)
 {
     testConfiguration(3,      //int dim
                       false,  //bool orientation
@@ -484,7 +483,7 @@ TEST_F(ProcessorTrackerFeatureLandmarkExternalTest, P_3d_no_lmks_quality)
                       false); //bool add_landmarks
 }
 
-TEST_F(ProcessorTrackerFeatureLandmarkExternalTest, PO_3d_existing_lmks)
+TEST_F(ProcessorLandmarkExternalTest, PO_3d_existing_lmks)
 {
     testConfiguration(3,      //int dim
                       true,   //bool orientation
@@ -495,7 +494,7 @@ TEST_F(ProcessorTrackerFeatureLandmarkExternalTest, PO_3d_existing_lmks)
                       true);  //bool add_landmarks
 }
 
-TEST_F(ProcessorTrackerFeatureLandmarkExternalTest, PO_3d_no_lmks)
+TEST_F(ProcessorLandmarkExternalTest, PO_3d_no_lmks)
 {
     testConfiguration(3,      //int dim
                       true,   //bool orientation
@@ -506,7 +505,7 @@ TEST_F(ProcessorTrackerFeatureLandmarkExternalTest, PO_3d_no_lmks)
                       false); //bool add_landmarks
 }
 
-TEST_F(ProcessorTrackerFeatureLandmarkExternalTest, PO_3d_no_lmks_quality)
+TEST_F(ProcessorLandmarkExternalTest, PO_3d_no_lmks_quality)
 {
     testConfiguration(3,      //int dim
                       true,   //bool orientation