developing processorLandmarkExternal

CMakeLists.txt

@@ -152,6 +152,7 @@ SET(HDRS_FACTOR
 SET(HDRS_FEATURE
   include/${PROJECT_NAME}/feature/feature_base.h
   include/${PROJECT_NAME}/feature/feature_diff_drive.h
+  include/${PROJECT_NAME}/feature/feature_landmark_external.h
   include/${PROJECT_NAME}/feature/feature_match.h
   include/${PROJECT_NAME}/feature/feature_motion.h
   include/${PROJECT_NAME}/feature/feature_odom_2d.h
@@ -273,6 +274,7 @@ SET(SRCS_FACTOR
 SET(SRCS_FEATURE
   src/feature/feature_base.cpp
   src/feature/feature_diff_drive.cpp
+  src/feature/feature_landmark_external.cpp
   src/feature/feature_motion.cpp
   src/feature/feature_odom_2d.cpp
   src/feature/feature_pose.cpp

include/core/feature/feature_landmark_external.h

+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program.  If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+#pragma once
+
+// Wolf includes
+#include "core/feature/feature_base.h"
+
+// std includes
+
+namespace wolf
+{
+
+WOLF_PTR_TYPEDEFS(FeatureLandmarkExternal);
+
+// class FeatureLandmarkExternal
+class FeatureLandmarkExternal : public FeatureBase
+{
+  protected:
+    int external_id_;    ///< The id of the landmark assigned by an external classifier
+    int external_type_;  ///< The type of the landmark assigned by an external classifier
+
+  public:
+    /** \brief Constructor from capture pointer and measure
+     *
+     * \param _measurement the measurement
+     * \param _meas_covariance the noise of the measurement
+     * \param _external_id the id assigned by an external tracker (-1 for not tracked)
+     * \param _external_type the type index of the landmark detection (-1 for not classified)
+     *
+     */
+    FeatureLandmarkExternal(const Eigen::VectorXd& _measurement,
+                            const Eigen::MatrixXd& _meas_covariance,
+                            const int&             _external_id   = -1,
+                            const int&             _external_type = -1);
+
+    ~FeatureLandmarkExternal() override;
+
+    int  getExternalType() const;
+    void setExternalType(const int& _external_type);
+    int  getExternalId() const;
+    void setExternalId(const int& _external_id);
+};
+
+}  // namespace wolf

include/core/processor/processor_landmark_external.h

@@ -33,121 +33,131 @@ WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorLandmarkExternal);
 
 struct ParamsProcessorLandmarkExternal : public ParamsProcessorTracker
 {
-    bool use_orientation;                   ///< use orientation measure or not when emplacing factors
-    double match_dist_th;                   ///< for considering tracked detection: distance threshold to previous detection
-    unsigned int new_features_for_keyframe; ///< for keyframe voting: amount of new features with respect to origin (sufficient condition if more than min_features_for_keyframe)
-    double filter_quality_th;               ///< min quality to consider the detection
-    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 (sufficient condition if more than min_features_for_keyframe)
+    bool         use_orientation;  ///< use orientation measure or not when emplacing factors
+    double       match_dist_th;    ///< for considering tracked detection: distance threshold to previous detection
+    unsigned int new_features_for_keyframe;  ///< for keyframe voting: amount of new features with respect to origin
+                                             ///< (sufficient condition if more than min_features_for_keyframe)
+    double       filter_quality_th;          ///< min quality to consider the detection
+    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 (sufficient condition if more than
+                             ///< min_features_for_keyframe)
+    bool check_dist_when_ids_match;  ///< check the match_dist_th also in detections with matching external ID
+    bool close_loops_when_ids_match; ///< emplace loop closure factors if external ID matches
 
     ParamsProcessorLandmarkExternal() = default;
-    ParamsProcessorLandmarkExternal(std::string _unique_name,
-                                       const wolf::ParamsServer & _server):
-        ParamsProcessorTracker(_unique_name, _server)
+    ParamsProcessorLandmarkExternal(std::string _unique_name, const wolf::ParamsServer& _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");
-        filter_track_length_th    = _server.getParam<unsigned int>(prefix + _unique_name + "/filter/track_length_th");
-        match_dist_th             = _server.getParam<double>      (prefix + _unique_name + "/match_dist_th");
-        time_span                 = _server.getParam<double>      (prefix + _unique_name + "/keyframe_vote/time_span");
-        new_features_for_keyframe = _server.getParam<unsigned int>(prefix + _unique_name + "/keyframe_vote/new_features_for_keyframe");
+        use_orientation        = _server.getParam<bool>(prefix + _unique_name + "/use_orientation");
+        filter_quality_th      = _server.getParam<double>(prefix + _unique_name + "/filter/quality_th");
+        filter_track_length_th = _server.getParam<unsigned int>(prefix + _unique_name + "/filter/track_length_th");
+        match_dist_th          = _server.getParam<double>(prefix + _unique_name + "/match_dist_th");
+        time_span              = _server.getParam<double>(prefix + _unique_name + "/keyframe_vote/time_span");
+        new_features_for_keyframe =
+            _server.getParam<unsigned int>(prefix + _unique_name + "/keyframe_vote/new_features_for_keyframe");
+        check_dist_when_ids_match = _server.getParam<bool>(prefix + _unique_name + "/check_dist_when_ids_match");
+        close_loops_when_ids_match = _server.getParam<bool>(prefix + _unique_name + "/close_loops_when_ids_match");
     }
 };
 
 WOLF_PTR_TYPEDEFS(ProcessorLandmarkExternal);
 
-//Class
+// Class
 class ProcessorLandmarkExternal : public ProcessorTracker
 {
-    public:
-        ProcessorLandmarkExternal(ParamsProcessorLandmarkExternalPtr _params_tracker_feature);
-        ~ProcessorLandmarkExternal() override;
-
-        // Factory method for high level API
-        WOLF_PROCESSOR_CREATE(ProcessorLandmarkExternal, ParamsProcessorLandmarkExternal);
-
-        void configure(SensorBasePtr _sensor) override { };
-
-    protected:
-
-        ParamsProcessorLandmarkExternalPtr params_tfle_;
-        TrackMatrix track_matrix_;
-        std::set<SizeStd> lmks_ids_origin_;
-
-        /** Pre-process incoming Capture
-         *
-         * This is called by process() just after assigning incoming_ptr_ to a valid Capture.
-         *
-         * Extract the detections and fill unmatched_detections_incoming_ (FeaturePtrList)
-         */
-        void preProcess() override;
-
-
-        /** \brief Process known Features
-         * \return The number of successful matches.
-         *
-         * This function tracks features from last to incoming and starts new tracks for new (not tracked) features in incoming
-         */
-        unsigned int processKnown() override;
-
-        /**\brief Process new Features
-         *
-         */
-        unsigned int processNew(const int& _max_features) override { return 0;};
-
-        /** \brief Vote for KeyFrame generation
-         *
-         * If a KeyFrame criterion is validated, this function returns true,
-         * meaning that it wants to create a KeyFrame at the \b last Capture.
-         *
-         * WARNING! This function only votes! It does not create KeyFrames!
-         */
-        bool voteForKeyFrame() const override;
-        
-        /** \brief Emplaces a new factor for each known feature in Capture \b last
-         */
-        void establishFactors() override;
-        
-        /** \brief Emplaces a new factor
-         * \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
-         */
-        LandmarkBasePtr emplaceLandmark(FeatureBasePtr _feature_ptr);
-
-        /** \brief Modifies (if needed) a landmark
-         * \param _landmark pointer to the landmark
-         * \param _feature pointer to the Feature.
-         */
-        void modifyLandmark(LandmarkBasePtr _landmark,
-                            FeatureBasePtr _feature);
-
-        /** Post-process
-         *
-         * This is called by process() after finishing the processing algorithm.
-         *
-         * It does nothing for now
-         */
-        void postProcess() override {};
-
-        void advanceDerived() override;
-        void resetDerived() override;
-
-        double detectionDistance(FeatureBasePtr _ftr1,
-                                 FeatureBasePtr _ftr2,
-                                 const VectorComposite& _pose1,
-                                 const VectorComposite& _pose2,
-                                 const VectorComposite& _pose_sen) const;
+  public:
+    ProcessorLandmarkExternal(ParamsProcessorLandmarkExternalPtr _params_tracker_feature);
+    ~ProcessorLandmarkExternal() override;
+
+    // Factory method for high level API
+    WOLF_PROCESSOR_CREATE(ProcessorLandmarkExternal, ParamsProcessorLandmarkExternal);
+
+    void configure(SensorBasePtr _sensor) override{};
+
+  protected:
+    ParamsProcessorLandmarkExternalPtr params_tfle_;
+    TrackMatrix                        track_matrix_;
+    std::set<SizeStd>                  lmks_ids_origin_;
+    std::map<int, SizeStd>             external_id_to_landmark_id_; 
+
+    /** Pre-process incoming Capture
+     *
+     * This is called by process() just after assigning incoming_ptr_ to a valid Capture.
+     *
+     * Extract the detections and fill unmatched_detections_incoming_ (FeaturePtrList)
+     */
+    void preProcess() override;
+
+    /** \brief Process known Features
+     * \return The number of successful matches.
+     *
+     * This function tracks features from last to incoming and starts new tracks for new (not tracked) features in
+     * incoming
+     */
+    unsigned int processKnown() override;
+
+    /**\brief Process new Features
+     *
+     */
+    unsigned int processNew(const int& _max_features) override
+    {
+        return 0;
+    };
+
+    /** \brief Vote for KeyFrame generation
+     *
+     * If a KeyFrame criterion is validated, this function returns true,
+     * meaning that it wants to create a KeyFrame at the \b last Capture.
+     *
+     * WARNING! This function only votes! It does not create KeyFrames!
+     */
+    bool voteForKeyFrame() const override;
+
+    /** \brief Emplaces a new factor for each known feature in Capture \b last
+     */
+    void establishFactors() override;
+
+    /** \brief Emplaces a new factor
+     * \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
+     */
+    LandmarkBasePtr emplaceLandmark(FeatureBasePtr _feature_ptr);
+
+    /** \brief Modifies (if needed) a landmark
+     * \param _landmark pointer to the landmark
+     * \param _feature pointer to the Feature.
+     */
+    void modifyLandmark(LandmarkBasePtr _landmark, FeatureBasePtr _feature);
+
+    /** Post-process
+     *
+     * This is called by process() after finishing the processing algorithm.
+     *
+     * It does nothing for now
+     */
+    void postProcess() override{};
+
+    void advanceDerived() override;
+    void resetDerived() override;
+
+    double detectionDistance(FeatureBasePtr         _ftr1,
+                             FeatureBasePtr         _ftr2,
+                             const VectorComposite& _pose1,
+                             const VectorComposite& _pose2,
+                             const VectorComposite& _pose_sen) const;
+
+    SizeStd getAvailableLandmarkId() const;
 };
 
-inline ProcessorLandmarkExternal::ProcessorLandmarkExternal(ParamsProcessorLandmarkExternalPtr _params_tfle) :
-        ProcessorTracker("ProcessorLandmarkExternal", "PO", 0, _params_tfle),
-        params_tfle_(_params_tfle),
-        lmks_ids_origin_()
+inline ProcessorLandmarkExternal::ProcessorLandmarkExternal(ParamsProcessorLandmarkExternalPtr _params_tfle)
+    : ProcessorTracker("ProcessorLandmarkExternal", "PO", 0, _params_tfle),
+      params_tfle_(_params_tfle),
+      lmks_ids_origin_()
 {
     //
 }
@@ -157,4 +167,4 @@ inline ProcessorLandmarkExternal::~ProcessorLandmarkExternal()
     //
 }
 
-} // namespace wolf
+}  // namespace wolf

src/feature/feature_landmark_external.cpp

+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program.  If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+#include "core/feature/feature_landmark_external.h"
+
+namespace wolf
+{
+
+FeatureLandmarkExternal::FeatureLandmarkExternal(const Eigen::VectorXd& _measurement,
+                                                 const Eigen::MatrixXd& _meas_covariance,
+                                                 const int&             _external_id,
+                                                 const int&             _external_type)
+    : FeatureBase("FeatureLandmarkExternal", _measurement, _meas_covariance),
+      external_id_(_external_id),
+      external_type_(_external_type)
+{
+}
+
+FeatureLandmarkExternal::~FeatureLandmarkExternal()
+{
+    //
+}
+
+int FeatureLandmarkExternal::getExternalType() const
+{
+    return external_type_;
+}
+
+void FeatureLandmarkExternal::setExternalType(const int& _external_type)
+{
+    external_type_ = _external_type;
+}
+
+int FeatureLandmarkExternal::getExternalId() const
+{
+    return external_id_;
+}
+
+void FeatureLandmarkExternal::setExternalId(const int& _external_id)
+{
+    external_id_ = _external_id;
+}
+
+}  // namespace wolf