From e7bda1143658c5c2dd25d2f7d3c868d9a7a76909 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joan=20Vallv=C3=A9=20Navarro?= <jvallve@iri.upc.edu>
Date: Fri, 23 Sep 2022 13:59:51 +0200
Subject: [PATCH] wip
---
CMakeLists.txt | 8 +-
.../core/capture/capture_landmarks_external.h | 59 +++++++
...cessor_tracker_feature_landmark_external.h | 164 ++++++++++++++++++
src/capture/capture_landmarks_external.cpp | 50 ++++++
...ssor_tracker_feature_landmark_external.cpp | 144 +++++++++++++++
5 files changed, 423 insertions(+), 2 deletions(-)
create mode 100644 include/core/capture/capture_landmarks_external.h
create mode 100644 include/core/processor/processor_tracker_feature_landmark_external.h
create mode 100644 src/capture/capture_landmarks_external.cpp
create mode 100644 src/processor/processor_tracker_feature_landmark_external.cpp
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 685499123..4d5f01210 100644
--- a/CMakeLists.txt
+++ b/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
)
diff --git a/include/core/capture/capture_landmarks_external.h b/include/core/capture/capture_landmarks_external.h
new file mode 100644
index 000000000..ceaad953c
--- /dev/null
+++ b/include/core/capture/capture_landmarks_external.h
@@ -0,0 +1,59 @@
+//--------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
diff --git a/include/core/processor/processor_tracker_feature_landmark_external.h b/include/core/processor/processor_tracker_feature_landmark_external.h
new file mode 100644
index 000000000..9e3173a37
--- /dev/null
+++ b/include/core/processor/processor_tracker_feature_landmark_external.h
@@ -0,0 +1,164 @@
+//--------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
diff --git a/src/capture/capture_landmarks_external.cpp b/src/capture/capture_landmarks_external.cpp
new file mode 100644
index 000000000..32d3dca5e
--- /dev/null
+++ b/src/capture/capture_landmarks_external.cpp
@@ -0,0 +1,50 @@
+//--------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
diff --git a/src/processor/processor_tracker_feature_landmark_external.cpp b/src/processor/processor_tracker_feature_landmark_external.cpp
new file mode 100644
index 000000000..94d236810
--- /dev/null
+++ b/src/processor/processor_tracker_feature_landmark_external.cpp
@@ -0,0 +1,144 @@
+//--------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
--
GitLab