devel->main

This reverts commit 190ea5b1.

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