wip

CMakeLists.txt

@@ -108,12 +108,13 @@ configure_file(${CMAKE_CURRENT_SOURCE_DIR}/internal/config.h.in "${WOLF_CONFIG_D
 # ============ HEADERS ============ 
 SET(HDRS_CAPTURE
   include/${PROJECT_NAME}/capture/capture_base.h
+  include/${PROJECT_NAME}/capture/capture_diff_drive.h
+  include/${PROJECT_NAME}/capture/capture_landmarks_external.h
   include/${PROJECT_NAME}/capture/capture_motion.h
   include/${PROJECT_NAME}/capture/capture_odom_2d.h
   include/${PROJECT_NAME}/capture/capture_odom_3d.h
   include/${PROJECT_NAME}/capture/capture_pose.h
   include/${PROJECT_NAME}/capture/capture_void.h
-  include/${PROJECT_NAME}/capture/capture_diff_drive.h
   )
 SET(HDRS_COMMON
   include/${PROJECT_NAME}/common/factory.h
@@ -185,6 +186,7 @@ SET(HDRS_PROCESSOR
   include/${PROJECT_NAME}/processor/processor_pose.h
   include/${PROJECT_NAME}/processor/processor_tracker.h
   include/${PROJECT_NAME}/processor/processor_tracker_feature.h
+  include/${PROJECT_NAME}/processor/processor_tracker_feature_landmark_external.h
   include/${PROJECT_NAME}/processor/processor_tracker_landmark.h
   include/${PROJECT_NAME}/processor/track_matrix.h
   )
@@ -244,12 +246,13 @@ SET(HDRS_YAML
 # ============ SOURCES ============ 
 SET(SRCS_CAPTURE
   src/capture/capture_base.cpp
+  src/capture/capture_diff_drive.cpp
+  src/capture/capture_landmarks_external.cpp
   src/capture/capture_motion.cpp
   src/capture/capture_odom_2d.cpp
   src/capture/capture_odom_3d.cpp
   src/capture/capture_pose.cpp
   src/capture/capture_void.cpp
-  src/capture/capture_diff_drive.cpp
   )
 SET(SRCS_COMMON
   src/common/node_base.cpp
@@ -294,6 +297,7 @@ SET(SRCS_PROCESSOR
   src/processor/processor_pose.cpp
   src/processor/processor_tracker.cpp
   src/processor/processor_tracker_feature.cpp
+  src/processor/processor_tracker_feature_landmark_external.cpp
   src/processor/processor_tracker_landmark.cpp
   src/processor/track_matrix.cpp
   )

include/core/capture/capture_landmarks_external.h

+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 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/capture/capture_base.h"
+
+namespace wolf {
+
+struct LandmarkDetection
+{
+    Eigen::VectorXd measure;     // either pose or position
+    Eigen::MatrixXd covariance;  // covariance of the measure
+    int id;                      // id of landmark
+}
+
+WOLF_PTR_TYPEDEFS(CaptureLandmarksExternal);
+
+//class CaptureLandmarksExternal
+class CaptureLandmarksExternal : public CaptureBase
+{
+    protected:
+        std::list<LandmarkDetection> detections_;
+
+    public:
+
+        CaptureLandmarksExternal(const TimeStamp& _ts, 
+                                 SensorBasePtr _sensor_ptr, 
+                                 const std::vector<Eigen::VectorXd>& _detections, 
+                                 const std::vector<Eigen::MatrixXd>& _covs, 
+                                 const std::vector<int>& _ids);
+
+        ~CaptureLandmarksExternal() override;
+
+        std::list<LandmarkDetection> getDetections() const {return detections_;};
+
+        void addDetection(const Eigen::VectorXd& _detection, const Eigen::MatrixXd& _cov, const int& _id);
+};
+
+} //namespace wolf
\ No newline at end of file

include/core/processor/processor_tracker_feature_landmark_external.h

+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 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
+
+#include "core/common/wolf.h"
+#include "core/processor/processor_tracker_feature.h"
+
+namespace wolf
+{
+
+WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorTrackerFeatureLandmarkExternal);
+
+struct ParamsProcessorTrackerFeatureLandmarkExternal : public ParamsProcessorTrackerFeature
+{
+    unsigned int min_track_length; ///< length of the track necessary to consider the detection
+
+    ParamsProcessorTrackerFeatureLandmarkExternal() = default;
+    ParamsProcessorTrackerFeatureLandmarkExternal(std::string _unique_name,
+                                       const wolf::ParamsServer & _server):
+        ParamsProcessorTrackerFeature(_unique_name, _server)
+    {
+        min_track_length = _server.getParam<unsigned int>(prefix + _unique_name + "/min_track_length");
+    }
+};
+
+WOLF_PTR_TYPEDEFS(ProcessorTrackerFeatureLandmarkExternal);
+
+//Class
+class ProcessorTrackerFeatureLandmarkExternal : public ProcessorTrackerFeature
+{
+    public:
+        ProcessorTrackerFeatureLandmarkExternal(ParamsProcessorTrackerFeatureLandmarkExternalPtr _params_tracker_feature);
+        ~ProcessorTrackerFeatureLandmarkExternal() override;
+
+        // Factory method for high level API
+        WOLF_PROCESSOR_CREATE(ProcessorTrackerFeatureLandmarkExternal, ParamsProcessorTrackerFeatureLandmarkExternal);
+
+        void configure(SensorBasePtr _sensor) override { };
+
+    protected:
+
+        ParamsProcessorTrackerFeatureLandmarkExternalPtr params_tfle_;
+        FeatureBasePtrList detections_incoming_, detections_last_;
+
+        /** \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
+         *
+         * 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.
+         *
+         * \return the number of features tracked
+         */
+        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
+         */
+        bool correctFeatureDrift(const FeatureBasePtr _origin_feature,
+                                 const FeatureBasePtr _last_feature,
+                                 FeatureBasePtr _incoming_feature) override;
+
+        /** \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 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_.
+         */
+        unsigned int detectNewFeatures(const int& _max_new_features,
+                                       const CaptureBasePtr& _capture,
+                                       FeatureBasePtrList& _features_out) 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.
+         */
+        FactorBasePtr emplaceFactor(FeatureBasePtr _feature_ptr,
+                                    FeatureBasePtr _feature_other_ptr) override;
+
+        /** 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)
+         */
+        void preProcess() override;
+
+        /** 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;
+};
+
+inline ProcessorTrackerFeatureLandmarkExternal::ProcessorTrackerFeatureLandmarkExternal(ParamsProcessorTrackerFeatureLandmarkExternalPtr _params_tfle) :
+        ProcessorTrackerFeature("ProcessorTrackerFeatureLandmarkExternal", "PO", 0, _params_tfle),
+        params_tfle_(_params_tfle)
+{
+    //
+}
+
+inline ProcessorTrackerFeatureLandmarkExternal::~ProcessorTrackerFeatureLandmarkExternal()
+{
+    //
+}
+
+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/capture/capture_landmarks_external.cpp

+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 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/capture/capture_landmarks_external.h"
+
+namespace wolf{
+
+CaptureLandmarksExternal::CaptureLandmarksExternal(const TimeStamp& _ts, 
+                                                   SensorBasePtr _sensor_ptr, 
+                                                   const std::vector<Eigen::VectorXd>& _detections, 
+                                                   const std::vector<Eigen::MatrixXd>& _covs, 
+                                                   const std::vector<int>& _ids) :
+	CaptureBase("CaptureLandmarksExternal", _ts, _sensor_ptr)
+{
+    if (_detections.size() != _covs.size() or _detections.size() != _ids.size())
+        throw std::runtime_error("CaptureLandmarksExternal constructor: '_detections', '_covs' and '_ids' should have the same size.")
+    
+    for  (auto i = 0; i < _detections.size(); i++)
+        addDetection(_detections.at(i), _covs.at(i), _ids.at(i));
+}
+
+CaptureLandmarksExternal::~CaptureLandmarksExternal()
+{
+	//
+}
+
+void CaptureLandmarksExternal::addDetection(const Eigen::VectorXd& _detection, const Eigen::MatrixXd& _cov, const int& _id)
+{
+    detections_.push_back(LandmarkDetection{_detection, _cov, _id});
+}
+
+} // namespace wolf

src/processor/processor_tracker_feature_landmark_external.cpp

+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 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 "processor_tracker_feature_landmark_external.h"
+#include "core/capture/capture_landmarks_external.h"
+#include "core/feature/feature_base.h"
+
+namespace wolf
+{
+
+void ProcessorTrackerFeatureLandmarkExternal::preProcess()
+{
+    assert(detections_incoming_.empty())
+
+    auto cap_landmarks = std::dynamic_pointer_cast<CaptureLandmarksExternal>(incoming_ptr_);
+    if (not cap_landmarks)
+        throw std::runtime_error("ProcessorTrackerFeatureLandmarkExternal::preProcess incoming_ptr_ should be of type 'CaptureLandmarksExternal'");
+
+    auto landmark_detections = cap_landmarks->getDetections();
+    for (auto detection : landmark_detections)
+    {
+        FeatureBasePtr ftr = FeatureBase::emplace<FeatureBase>(cap_landmarks,
+                                                               "FeatureLandmarkExternal",
+                                                               detection.measure,
+                                                               detection.covariance);
+        ftr.setLandmarkId(detection.id);
+
+        detections_incoming_.push_back(ftr);
+    }
+}
+
+unsigned int ProcessorTrackerFeatureLandmarkExternal::trackFeatures(const FeatureBasePtrList& _features_in,
+                                                                    const CaptureBasePtr& _capture,
+                                                                    FeatureBasePtrList& _features_out,
+                                                                    FeatureMatchMap& _feature_correspondences)
+{
+    WOLF_INFO("tracking " , _features_in.size() , " features...");
+
+    if (_capture != last_ptr and _capture != incoming_ptr)
+        throw std::runtime_error("ProcessorTrackerFeatureLandmarkExternal::trackFeatures unknown capture");
+
+    FeatureBasePtrList& landmark_detections = (_capture == last_ptr_ ? detections_last_ : detections_incoming_);
+
+    for (auto feat_in : _features_in)
+    {
+        for (auto feat_candidate : landmark_detections)
+        {
+            if (feat_candidate->getLandmarkId() == feat_in->getLandmarkId()) // TODO
+            {
+                _features_out.push_back(feat_candidate);
+                _feature_correspondences[_features_out.back()] = std::make_shared<FeatureMatch>(FeatureMatch({feat_in,0}));
+
+                WOLF_INFO("track: " , feat_in->trackId() , " last: " , feat_in->id() , " inc: " , feat_candidate->id() , " !" );
+            }
+        }
+    }
+
+    return _features_out.size();
+}
+
+bool ProcessorTrackerFeatureLandmarkExternal::voteForKeyFrame() const
+{
+    WOLF_INFO("Nbr. of active feature tracks: " , incoming_ptr_->getFeatureList().size() );
+
+    bool vote = incoming_ptr_->getFeatureList().size() < params_tracker_feature_->min_features_for_keyframe;
+
+    WOLF_INFO( (vote ? "Vote ": "Do not vote ") , "for KF" );
+
+    return incoming_ptr_->getFeatureList().size() < params_tracker_feature_->min_features_for_keyframe;
+}
+
+unsigned int ProcessorTrackerFeatureLandmarkExternal::detectNewFeatures(const int& _max_new_features,
+                                                                        const CaptureBasePtr& _capture,
+                                                                        FeatureBasePtrList& _features_out)
+{
+    if (_capture != last_ptr and _capture != incoming_ptr)
+        throw std::runtime_error("ProcessorTrackerFeatureLandmarkExternal::detectNewFeatures unknown capture");
+
+    auto cap_landmarks = std::dynamic_pointer_cast<CaptureLandmarksExternal>(_capture);
+    if (not cap_landmarks)
+        throw std::runtime_error("ProcessorTrackerFeatureLandmarkExternal capture should be of type 'CaptureLandmarksExternal'");
+
+    // detecting new features
+    FeatureBasePtrList& landmark_detections = (_capture == last_ptr_ ? detections_last_ : detections_incoming_);
+    while (not landmark_detections.empty())
+    {
+        _features_out.push_back(landmark_detections.front());
+        landmark_detections.pop_front();
+        
+        if (_max_new_features != -1 and _features_out.size() >= _max_new_features)
+            break;
+    }
+
+    WOLF_INFO(_features_out.size() , " new features detected!");
+
+    return _features_out.size();
+}
+
+FactorBasePtr ProcessorTrackerFeatureLandmarkExternal::emplaceFactor(FeatureBasePtr _feature_ptr,
+                                                                     FeatureBasePtr _feature_other_ptr)
+{
+    // TODO
+}
+
+void ProcessorTrackerFeatureLandmarkExternal::advanceDerived()
+{
+    ProcessorTrackerFeature::advanceDerived();
+
+    detections_last_ = std::move(detections_incoming_);
+}
+void ProcessorTrackerFeatureLandmarkExternal::resetDerived()
+{
+    ProcessorTrackerFeature::resetDerived();
+
+    detections_last_ = std::move(detections_incoming_);
+}
+
+} // namespace wolf
+
+// Register in the FactoryProcessor
+#include "core/processor/factory_processor.h"
+namespace wolf {
+WOLF_REGISTER_PROCESSOR(ProcessorTrackerFeatureLandmarkExternal)
+WOLF_REGISTER_PROCESSOR_AUTO(ProcessorTrackerFeatureLandmarkExternal)
+} // namespace wolf