diff --git a/include/core/processor/processor_landmark_external.h b/include/core/processor/processor_landmark_external.h
new file mode 100644
index 0000000000000000000000000000000000000000..6e4ba1f4167de4c4189dea87621b02c474aae5bf
--- /dev/null
+++ b/include/core/processor/processor_landmark_external.h
@@ -0,0 +1,156 @@
+//--------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.h"
+#include "core/processor/track_matrix.h"
+
+namespace wolf
+{
+
+WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorLandmarkExternal);
+
+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
+ double filter_dist_th; ///< for considering tracked detection: distance threshold to previous 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
+
+ ParamsProcessorLandmarkExternal() = default;
+ 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_dist_th = _server.getParam<double> (prefix + _unique_name + "/filter_dist_th");
+ filter_track_length_th = _server.getParam<unsigned int>(prefix + _unique_name + "/filter_track_length_th");
+ time_span = _server.getParam<double> (prefix + _unique_name + "/keyframe_vote/time_span");
+ }
+};
+
+WOLF_PTR_TYPEDEFS(ProcessorLandmarkExternal);
+
+//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_;
+
+ /** 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;
+};
+
+inline ProcessorLandmarkExternal::ProcessorLandmarkExternal(ParamsProcessorLandmarkExternalPtr _params_tfle) :
+ ProcessorTracker("ProcessorLandmarkExternal", "PO", 0, _params_tfle),
+ params_tfle_(_params_tfle)
+{
+ //
+}
+
+inline ProcessorLandmarkExternal::~ProcessorLandmarkExternal()
+{
+ //
+}
+
+} // 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
deleted file mode 100644
index 3e4d21321d5ef4ec5308f3f06a338de9422bc685..0000000000000000000000000000000000000000
--- a/include/core/processor/processor_tracker_feature_landmark_external.h
+++ /dev/null
@@ -1,179 +0,0 @@
-//--------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
-{
- bool use_orientation; ///< use orientation measure or not when emplacing factors
- double filter_quality_th; ///< min quality to consider the detection
- double filter_dist_th; ///< for considering tracked detection: distance threshold to previous 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
-
- ParamsProcessorTrackerFeatureLandmarkExternal() = default;
- ParamsProcessorTrackerFeatureLandmarkExternal(std::string _unique_name,
- const wolf::ParamsServer & _server):
- ParamsProcessorTrackerFeature(_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_dist_th = _server.getParam<double> (prefix + _unique_name + "/filter_dist_th");
- filter_track_length_th = _server.getParam<unsigned int>(prefix + _unique_name + "/filter_track_length_th");
- time_span = _server.getParam<double> (prefix + _unique_name + "/keyframe_vote/time_span");
- }
-};
-
-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_;
- //std::set<FeatureBasePtr> unmatched_detections_incoming_, unmatched_detections_last_;
- std::list<FeatureBasePtr> unmatched_detections_incoming_;
-
- /** \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;
-
- double detectionDistance(FeatureBasePtr _ftr1,
- FeatureBasePtr _ftr2,
- const VectorComposite& _pose1,
- const VectorComposite& _pose2,
- const VectorComposite& _pose_sen) const;
-};
-
-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/processor/processor_landmark_external.cpp b/src/processor/processor_landmark_external.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..63adbb9265f01d1e13a8520524025c8059ee7e25
--- /dev/null
+++ b/src/processor/processor_landmark_external.cpp
@@ -0,0 +1,645 @@
+//--------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/processor/processor_landmark_external.h"
+#include "core/capture/capture_landmarks_external.h"
+#include "core/feature/feature_base.h"
+#include "core/factor/factor_relative_pose_2d_with_extrinsics.h"
+#include "core/factor/factor_relative_position_2d_with_extrinsics.h"
+#include "core/factor/factor_relative_pose_3d_with_extrinsics.h"
+#include "core/factor/factor_relative_position_3d_with_extrinsics.h"
+#include "core/landmark/landmark_base.h"
+#include "core/state_block/state_block_derived.h"
+#include "core/state_block/state_quaternion.h"
+#include "core/state_block/state_angle.h"
+#include "core/math/SE2.h"
+#include "core/math/SE3.h"
+
+using namespace Eigen;
+
+namespace wolf
+{
+
+void ProcessorLandmarkExternal::preProcess()
+{
+ assert(new_features_incoming_.empty());
+
+ auto dim = getProblem()->getDim();
+ auto cap_landmarks = std::dynamic_pointer_cast<CaptureLandmarksExternal>(incoming_ptr_);
+ if (not cap_landmarks)
+ throw std::runtime_error("ProcessorLandmarkExternal::preProcess incoming_ptr_ should be of type 'CaptureLandmarksExternal'");
+
+ // Extract features from capture detections
+ auto landmark_detections = cap_landmarks->getDetections();
+ std::set<int> ids;
+ for (auto detection : landmark_detections)
+ {
+ WOLF_WARN_COND(ids.count(detection.id), "ProcessorLandmarkExternal::preProcess: detection with repeated id, discarding...");
+
+ // Filter by quality
+ if (detection.quality < params_tfle_->filter_quality_th or ids.count(detection.id))
+ continue;
+
+ // measure and covariance
+ VectorXd meas;
+ MatrixXd cov;
+ if (not params_tfle_->use_orientation)
+ {
+ assert(detection.measure.size() >= dim);
+ assert(detection.covariance.rows() >= dim and detection.covariance.rows() == detection.covariance.cols());
+
+ meas = detection.measure.head(dim);
+ cov = detection.covariance.topLeftCorner(dim,dim);
+ }
+ else
+ {
+ assert(detection.measure.size() == (dim == 2 ? 3 : 7));
+ assert(detection.covariance.rows() == (dim == 2 ? 3 : 6) and detection.covariance.rows() == detection.covariance.cols());
+
+ meas = detection.measure;
+ cov = detection.covariance;
+ }
+
+ // emplace feature
+ FeatureBasePtr ftr = FeatureBase::emplace<FeatureBase>(cap_landmarks,
+ "FeatureLandmarkExternal",
+ meas,
+ cov);
+ ftr->setLandmarkId(detection.id);
+
+ if (detection.id != -1 and detection.id != 0)
+ ids.insert(detection.id);
+
+ //unmatched_detections_incoming_.push_back(ftr);
+
+ // add new feature
+ new_features_incoming_.push_back(ftr);
+ }
+ WOLF_INFO("ProcessorLandmarkExternal::preprocess: found ", new_features_incoming_.size(), " new features");
+}
+
+unsigned int ProcessorLandmarkExternal::processKnown()
+{
+ // clear list of known features
+ known_features_incoming_.clear();
+
+ // Track features from last_ptr_ to incoming_ptr_
+ if (last_ptr_)
+ {
+ WOLF_INFO("Searching ", known_features_last_.size(), " tracked features...");
+ auto pose_sen = getSensor()->getState("PO");
+ auto pose_in = getProblem()->getState(last_ptr_->getTimeStamp(), "PO");
+ auto pose_out = getProblem()->getState(incoming_ptr_->getTimeStamp(), "PO");
+
+ for (auto feat_last : known_features_last_)
+ {
+ auto feat_candidate_it = new_features_incoming_.begin();
+ while (feat_candidate_it != new_features_incoming_.end())
+ {
+ if ((*feat_candidate_it)->landmarkId() == feat_last->landmarkId() and
+ detectionDistance(feat_last, (*feat_candidate_it), pose_in, pose_out, pose_sen) < params_tfle_->filter_dist_th)
+ {
+ // grow track
+ track_matrix_.add(feat_last, *feat_candidate_it);
+
+ // feature is known
+ known_features_incoming_.push_back((*feat_candidate_it));
+
+ // remove from unmatched
+ feat_candidate_it = new_features_incoming_.erase(feat_candidate_it);
+ break;
+ }
+ else
+ feat_candidate_it++;
+ }
+ }
+ WOLF_INFO("Tracked ", known_features_incoming_.size(), " features.");
+ }
+
+ // Add new features (not tracked) as known features
+ WOLF_INFO_COND(not new_features_incoming_.empty(), "Adding new features ", new_features_incoming_.size());
+ while (not new_features_incoming_.empty())
+ {
+ auto ftr = new_features_incoming_.front();
+
+ // new track
+ track_matrix_.newTrack(ftr);
+
+ // feature is now known
+ known_features_incoming_.push_back(ftr);
+
+ // remove from unmatched
+ new_features_incoming_.pop_front();
+ }
+
+ // check all features have been emplaced
+ assert(std::all_of(known_features_incoming_.begin(), known_features_incoming_.end(), [](FeatureBasePtr f){return f->getCapture() != nullptr;}) &&
+ "any not linked feature returned by trackFeatures()");
+
+ return known_features_incoming_.size();
+}
+
+double ProcessorLandmarkExternal::detectionDistance(FeatureBasePtr _ftr1,
+ FeatureBasePtr _ftr2,
+ const VectorComposite& _pose1,
+ const VectorComposite& _pose2,
+ const VectorComposite& _pose_sen) const
+{
+ // Any not available info of poses, assume identity
+ if (not _pose1.includesStructure("PO") or not _pose2.includesStructure("PO") or not _pose_sen.includesStructure("PO"))
+ {
+ if (getProblem()->getDim() == 2)
+ return (_ftr1->getMeasurement().head<2>() - _ftr2->getMeasurement().head<2>()).norm();
+ else
+ return (_ftr1->getMeasurement().head<3>() - _ftr2->getMeasurement().head<3>()).norm();
+ }
+ else
+ {
+ if (getProblem()->getDim() == 2)
+ {
+ auto pose_s1 = SE2::compose(_pose1, _pose_sen);
+ auto pose_s2 = SE2::compose(_pose2, _pose_sen);
+ auto p1 = pose_s1.at('P') + Rotation2Dd(pose_s1.at('O')(0)) * _ftr1->getMeasurement().head<2>();
+ auto p2 = pose_s2.at('P') + Rotation2Dd(pose_s2.at('O')(0)) * _ftr2->getMeasurement().head<2>();
+ return (p1-p2).norm();
+ }
+ else
+ {
+ auto pose_s1 = SE3::compose(_pose1, _pose_sen);
+ auto pose_s2 = SE3::compose(_pose2, _pose_sen);
+ auto p1 = pose_s1.at('P') + Quaterniond(Vector4d(pose_s1.at('O'))) * _ftr1->getMeasurement().head<3>();
+ auto p2 = pose_s2.at('P') + Quaterniond(Vector4d(pose_s2.at('O'))) * _ftr2->getMeasurement().head<3>();
+ return (p1-p2).norm();
+ }
+ }
+}
+
+bool ProcessorLandmarkExternal::voteForKeyFrame() const
+{
+ auto track_ids_last = track_matrix_.trackIds(last_ptr_);
+
+ WOLF_INFO("Nbr. of active feature tracks: " , track_ids_last.size() );
+
+ // no tracks longer than filter_track_length_th
+ auto n_tracks = 0;
+ for (auto track_id : track_ids_last)
+ {
+ if (track_matrix_.trackSize(track_id) >= params_tfle_->filter_track_length_th)
+ n_tracks++;
+ }
+ WOLF_INFO("Nbr. of active feature tracks (longer than ", params_tfle_->filter_track_length_th, "): " , n_tracks );
+ if (n_tracks == 0)
+ return false;
+
+ auto matches_origin_last = track_matrix_.matches(origin_ptr_, last_ptr_);
+ bool vote = false;// = n_tracks < params_tracker_feature_->min_features_for_keyframe;
+
+ if (origin_ptr_)
+ {
+ WOLF_INFO("Time span since last frame: " ,
+ last_ptr_->getTimeStamp() - origin_ptr_->getTimeStamp() ,
+ " (should be greater than ",
+ params_tfle_->time_span,
+ ")");
+ vote = vote or last_ptr_->getTimeStamp() - origin_ptr_->getTimeStamp() > params_tfle_->time_span;
+ }
+
+ WOLF_INFO( (vote ? "Vote ": "Do not vote ") , "for KF" );
+
+ return vote;
+}
+
+void ProcessorLandmarkExternal::establishFactors()
+{
+ if (origin_ptr_ == last_ptr_)
+ return;
+
+
+ // will emplace a factor (and landmark if needed) for each known feature in last with long tracks (params_tfle_->filter_track_length_th)
+ FactorBasePtrList fac_list;
+ auto track_ids_last = track_matrix_.trackIds(last_ptr_);
+
+ for (auto track_id : track_ids_last)
+ {
+ // check track length
+ if (track_matrix_.trackSize(track_id) < params_tfle_->filter_track_length_th)
+ continue;
+
+ auto feature = track_matrix_.feature(track_id, last_ptr_);
+
+ // get landmark
+ LandmarkBasePtr lmk = getProblem()->getMap()->getLandmark(feature->landmarkId());
+
+ // emplace landmark
+ if (not lmk)
+ lmk = emplaceLandmark(feature);
+
+ // modify landmark
+ modifyLandmark(lmk, feature);
+
+ // emplace factor
+ auto fac = emplaceFactor(feature, lmk);
+
+ if (fac)
+ fac_list.push_back(fac);
+ }
+
+ WOLF_INFO("ProcessorLandmarkExternal::establishFactors: emplaced ", fac_list.size(), " factors!");
+}
+
+FactorBasePtr ProcessorLandmarkExternal::emplaceFactor(FeatureBasePtr _feature, LandmarkBasePtr _landmark)
+{
+ assert(_feature);
+ assert(_landmark);
+
+ // 2D - Relative Position
+ if (getProblem()->getDim() == 2 and (_feature->getMeasurement().size() == 2 or not params_tfle_->use_orientation))
+ {
+ return FactorBase::emplace<FactorRelativePosition2dWithExtrinsics>(_feature,
+ _feature,
+ _landmark,
+ shared_from_this(),
+ params_tfle_->apply_loss_function);
+ }
+ // 2D - Relative Pose
+ else if (getProblem()->getDim() == 2 and _feature->getMeasurement().size() == 3 and params_tfle_->use_orientation)
+ {
+ return FactorBase::emplace<FactorRelativePose2dWithExtrinsics>(_feature,
+ _feature,
+ _landmark,
+ shared_from_this(),
+ params_tfle_->apply_loss_function);
+ }
+ // 3D - Relative Position
+ else if (getProblem()->getDim() == 3 and (_feature->getMeasurement().size() == 3 or not params_tfle_->use_orientation))
+ {
+ return FactorBase::emplace<FactorRelativePosition3dWithExtrinsics>(_feature,
+ _feature,
+ _landmark,
+ shared_from_this(),
+ params_tfle_->apply_loss_function);
+ }
+ // 3D - Relative Pose
+ else if (getProblem()->getDim() == 3 and _feature->getMeasurement().size() == 7 and params_tfle_->use_orientation)
+ {
+ return FactorBase::emplace<FactorRelativePose3dWithExtrinsics>(_feature,
+ _feature,
+ _landmark,
+ shared_from_this(),
+ params_tfle_->apply_loss_function);
+ }
+ else
+ throw std::runtime_error("ProcessorLandmarkExternal::emplaceFactor unknown case");
+
+ // not reachable
+ return nullptr;
+
+}
+
+LandmarkBasePtr ProcessorLandmarkExternal::emplaceLandmark(FeatureBasePtr _feature)
+{
+ assert(_feature);
+ assert(_feature->getCapture());
+ assert(_feature->getCapture()->getFrame());
+ assert(_feature->getCapture()->getSensorP());
+ assert(_feature->getCapture()->getSensorO());
+ assert(getProblem());
+ assert(getProblem()->getMap());
+
+ if (getProblem()->getMap()->getLandmark(_feature->landmarkId()) != nullptr)
+ throw std::runtime_error("ProcessorLandmarkExternal::emplaceLandmark: attempting to create an existing landmark");
+
+ LandmarkBasePtr lmk;
+
+ // 2D - Relative Position
+ if (getProblem()->getDim() == 2 and (_feature->getMeasurement().size() == 2 or not params_tfle_->use_orientation))
+ {
+ Vector2d frm_p = _feature->getCapture()->getFrame()->getP()->getState();
+ Vector2d sen_p = _feature->getCapture()->getSensorP()->getState();
+ double frm_o = _feature->getCapture()->getFrame()->getO()->getState()(0);
+ double sen_o = _feature->getCapture()->getSensorO()->getState()(0);
+
+ Vector2d lmk_p = frm_p + Rotation2Dd(frm_o) * (sen_p + Rotation2Dd(sen_o) * _feature->getMeasurement());
+
+ lmk = LandmarkBase::emplace<LandmarkBase>(getProblem()->getMap(),
+ "LandmarkRelativePosition2d",
+ std::make_shared<StatePoint2d>(lmk_p),
+ nullptr);
+ lmk->setId(_feature->landmarkId());
+ }
+ // 2D - Relative Pose
+ else if (getProblem()->getDim() == 2 and _feature->getMeasurement().size() == 3 and params_tfle_->use_orientation)
+ {
+ Vector2d frm_p = _feature->getCapture()->getFrame()->getP()->getState();
+ Vector2d sen_p = _feature->getCapture()->getSensorP()->getState();
+ double frm_o = _feature->getCapture()->getFrame()->getO()->getState()(0);
+ double sen_o = _feature->getCapture()->getSensorO()->getState()(0);
+
+ Vector2d lmk_p = frm_p + Rotation2Dd(frm_o) * (sen_p + Rotation2Dd(sen_o) * _feature->getMeasurement().head<2>());
+ double lmk_o = frm_o + sen_o + _feature->getMeasurement()(2);
+
+ lmk = LandmarkBase::emplace<LandmarkBase>(getProblem()->getMap(),
+ "LandmarkRelativePose2d",
+ std::make_shared<StatePoint2d>(lmk_p),
+ std::make_shared<StateAngle>(lmk_o));
+ lmk->setId(_feature->landmarkId());
+ }
+ // 3D - Relative Position
+ else if (getProblem()->getDim() == 3 and (_feature->getMeasurement().size() == 3 or not params_tfle_->use_orientation))
+ {
+ Vector3d frm_p = _feature->getCapture()->getFrame()->getP()->getState();
+ Vector3d sen_p = _feature->getCapture()->getSensorP()->getState();
+ Quaterniond frm_o = Quaterniond(Vector4d(_feature->getCapture()->getFrame()->getO()->getState()));
+ Quaterniond sen_o = Quaterniond(Vector4d(_feature->getCapture()->getSensorO()->getState()));
+
+ Vector3d lmk_p = frm_p + frm_o * (sen_p + sen_o * _feature->getMeasurement());
+
+ lmk = LandmarkBase::emplace<LandmarkBase>(getProblem()->getMap(),
+ "LandmarkRelativePosition3d",
+ std::make_shared<StatePoint3d>(lmk_p),
+ nullptr);
+ lmk->setId(_feature->landmarkId());
+ }
+ // 3D - Relative Pose
+ else if (getProblem()->getDim() == 3 and _feature->getMeasurement().size() == 7 and params_tfle_->use_orientation)
+ {
+ Vector3d frm_p = _feature->getCapture()->getFrame()->getP()->getState();
+ Vector3d sen_p = _feature->getCapture()->getSensorP()->getState();
+ Quaterniond frm_o = Quaterniond(Vector4d(_feature->getCapture()->getFrame()->getO()->getState()));
+ Quaterniond sen_o = Quaterniond(Vector4d(_feature->getCapture()->getSensorO()->getState()));
+
+ Vector3d lmk_p = frm_p + frm_o * (sen_p + sen_o * _feature->getMeasurement().head<3>());
+ Quaterniond lmk_o = frm_o * sen_o * Quaterniond(_feature->getMeasurement().tail<4>());
+
+ lmk = LandmarkBase::emplace<LandmarkBase>(getProblem()->getMap(),
+ "LandmarkRelativePose3d",
+ std::make_shared<StatePoint3d>(lmk_p),
+ std::make_shared<StateQuaternion>(lmk_o));
+ lmk->setId(_feature->landmarkId());
+ }
+ else
+ throw std::runtime_error("ProcessorLandmarkExternal::emplaceLandmark unknown case");
+
+ return lmk;
+}
+
+void ProcessorLandmarkExternal::modifyLandmark(LandmarkBasePtr _landmark,
+ FeatureBasePtr _feature)
+{
+ assert(_feature);
+ assert(_feature->getCapture());
+ assert(_feature->getCapture()->getFrame());
+ assert(_feature->getCapture()->getSensorP());
+ assert(_feature->getCapture()->getSensorO());
+ assert(getProblem());
+
+ if (not _landmark)
+ throw std::runtime_error("ProcessorLandmarkExternal::modifyLandmark: null landmark");
+
+ // 2D - Relative Position
+ if (getProblem()->getDim() == 2 and (_feature->getMeasurement().size() == 2 or not params_tfle_->use_orientation))
+ {
+ // nothing to do (we assume P already in landmark)
+ return;
+ }
+ // 2D - Relative Pose
+ else if (getProblem()->getDim() == 2 and _feature->getMeasurement().size() == 3 and params_tfle_->use_orientation)
+ {
+ // no orientation, add it
+ if (not _landmark->getO())
+ {
+ double frm_o = _feature->getCapture()->getFrame()->getO()->getState()(0);
+ double sen_o = _feature->getCapture()->getSensorO()->getState()(0);
+
+ double lmk_o = frm_o + sen_o + _feature->getMeasurement()(2);
+
+ _landmark->addStateBlock('O', std::make_shared<StateAngle>(lmk_o), getProblem());
+ }
+ }
+ // 3D - Relative Position
+ else if (getProblem()->getDim() == 3 and (_feature->getMeasurement().size() == 3 or not params_tfle_->use_orientation))
+ {
+ // nothing to do (we assume P already in landmark)
+ return;
+ }
+ // 3D - Relative Pose
+ else if (getProblem()->getDim() == 3 and _feature->getMeasurement().size() == 7 and params_tfle_->use_orientation)
+ {
+ // no orientation, add it
+ if (not _landmark->getO())
+ {
+ Quaterniond frm_o = Quaterniond(Vector4d(_feature->getCapture()->getFrame()->getO()->getState()));
+ Quaterniond sen_o = Quaterniond(Vector4d(_feature->getCapture()->getSensorO()->getState()));
+
+ Quaterniond lmk_o = frm_o * sen_o * Quaterniond(_feature->getMeasurement().tail<4>());
+
+ _landmark->addStateBlock('O', std::make_shared<StateQuaternion>(lmk_o), getProblem());
+ }
+ }
+ else
+ throw std::runtime_error("ProcessorLandmarkExternal::modifyLandmark unknown case");
+}
+
+// FactorBasePtr ProcessorLandmarkExternal::emplaceFactor(FeatureBasePtr _feature,
+// FeatureBasePtr _feature_other_ptr)
+// {
+// assert(_feature);
+// assert(_feature_other_ptr);
+// assert(_feature->getCapture());
+// assert(_feature->getCapture()->getFrame());
+
+// assert(getProblem());
+// assert(getProblem()->getMap());
+
+// // Check track length
+// if (params_tfle_->filter_track_length_th > 1)
+// {
+// auto snapshot = track_matrix_.snapshot(_feature->getCapture());
+// const auto& it = std::find_if(snapshot.begin(), snapshot.end(),
+// [_feature](const std::pair<SizeStd, FeatureBasePtr>& pair)
+// {
+// return pair.second == _feature;
+// });
+// assert(it != snapshot.end());
+// if (track_matrix_.track(it->first).size() < params_tfle_->filter_track_length_th)
+// return nullptr;
+// }
+
+// // Get landmark
+// LandmarkBasePtr lmk = getProblem()->getMap()->getLandmark(_feature->landmarkId());
+
+// // 2D - Relative Position
+// if (getProblem()->getDim() == 2 and (_feature->getMeasurement().size() == 2 or not params_tfle_->use_orientation))
+// {
+// // no landmark, create it
+// if (not lmk)
+// {
+// Vector2d frm_p = _feature->getCapture()->getFrame()->getP()->getState();
+// Vector2d sen_p = _feature->getCapture()->getSensorP()->getState();
+// double frm_o = _feature->getCapture()->getFrame()->getO()->getState()(0);
+// double sen_o = _feature->getCapture()->getSensorO()->getState()(0);
+
+// Vector2d lmk_p = frm_p + Rotation2Dd(frm_o) * (sen_p + Rotation2Dd(sen_o) * _feature->getMeasurement());
+
+// lmk = LandmarkBase::emplace<LandmarkBase>(getProblem()->getMap(),
+// "LandmarkRelativePosition2d",
+// std::make_shared<StatePoint2d>(lmk_p),
+// nullptr);
+// lmk->setId(_feature->landmarkId());
+// }
+
+// // emplace factor
+// return FactorBase::emplace<FactorRelativePosition2dWithExtrinsics>(_feature,
+// _feature,
+// lmk,
+// shared_from_this(),
+// params_tfle_->apply_loss_function);
+// }
+// // 2D - Relative Pose
+// else if (getProblem()->getDim() == 2 and _feature->getMeasurement().size() == 3 and params_tfle_->use_orientation)
+// {
+// // no landmark, create it
+// if (not lmk)
+// {
+// Vector2d frm_p = _feature->getCapture()->getFrame()->getP()->getState();
+// Vector2d sen_p = _feature->getCapture()->getSensorP()->getState();
+// double frm_o = _feature->getCapture()->getFrame()->getO()->getState()(0);
+// double sen_o = _feature->getCapture()->getSensorO()->getState()(0);
+
+// Vector2d lmk_p = frm_p + Rotation2Dd(frm_o) * (sen_p + Rotation2Dd(sen_o) * _feature->getMeasurement().head<2>());
+// double lmk_o = frm_o + sen_o + _feature->getMeasurement()(2);
+
+// lmk = LandmarkBase::emplace<LandmarkBase>(getProblem()->getMap(),
+// "LandmarkRelativePose2d",
+// std::make_shared<StatePoint2d>(lmk_p),
+// std::make_shared<StateAngle>(lmk_o));
+// lmk->setId(_feature->landmarkId());
+// }
+// // no orientation, add it
+// else if (not lmk->getO())
+// {
+// double frm_o = _feature->getCapture()->getFrame()->getO()->getState()(0);
+// double sen_o = _feature->getCapture()->getSensorO()->getState()(0);
+
+// double lmk_o = frm_o + sen_o + _feature->getMeasurement()(2);
+
+// lmk->addStateBlock('O', std::make_shared<StateAngle>(lmk_o), getProblem());
+// }
+
+// // emplace factor
+// return FactorBase::emplace<FactorRelativePose2dWithExtrinsics>(_feature,
+// _feature,
+// lmk,
+// shared_from_this(),
+// params_tfle_->apply_loss_function);
+// }
+// // 3D - Relative Position
+// else if (getProblem()->getDim() == 3 and (_feature->getMeasurement().size() == 3 or not params_tfle_->use_orientation))
+// {
+// // no landmark, create it
+// if (not lmk)
+// {
+// Vector3d frm_p = _feature->getCapture()->getFrame()->getP()->getState();
+// Vector3d sen_p = _feature->getCapture()->getSensorP()->getState();
+// Quaterniond frm_o = Quaterniond(Vector4d(_feature->getCapture()->getFrame()->getO()->getState()));
+// Quaterniond sen_o = Quaterniond(Vector4d(_feature->getCapture()->getSensorO()->getState()));
+
+// Vector3d lmk_p = frm_p + frm_o * (sen_p + sen_o * _feature->getMeasurement());
+
+// lmk = LandmarkBase::emplace<LandmarkBase>(getProblem()->getMap(),
+// "LandmarkRelativePosition3d",
+// std::make_shared<StatePoint3d>(lmk_p),
+// nullptr);
+// lmk->setId(_feature->landmarkId());
+// }
+
+// // emplace factor
+// return FactorBase::emplace<FactorRelativePosition3dWithExtrinsics>(_feature,
+// _feature,
+// lmk,
+// shared_from_this(),
+// params_tfle_->apply_loss_function);
+// }
+// // 3D - Relative Pose
+// else if (getProblem()->getDim() == 3 and _feature->getMeasurement().size() == 7 and params_tfle_->use_orientation)
+// {
+// // no landmark, create it
+// if (not lmk)
+// {
+// Vector3d frm_p = _feature->getCapture()->getFrame()->getP()->getState();
+// Vector3d sen_p = _feature->getCapture()->getSensorP()->getState();
+// Quaterniond frm_o = Quaterniond(Vector4d(_feature->getCapture()->getFrame()->getO()->getState()));
+// Quaterniond sen_o = Quaterniond(Vector4d(_feature->getCapture()->getSensorO()->getState()));
+
+// Vector3d lmk_p = frm_p + frm_o * (sen_p + sen_o * _feature->getMeasurement().head<3>());
+// Quaterniond lmk_o = frm_o * sen_o * Quaterniond(_feature->getMeasurement().tail<4>());
+
+// lmk = LandmarkBase::emplace<LandmarkBase>(getProblem()->getMap(),
+// "LandmarkRelativePose3d",
+// std::make_shared<StatePoint3d>(lmk_p),
+// std::make_shared<StateQuaternion>(lmk_o));
+// lmk->setId(_feature->landmarkId());
+// }
+// // no orientation, add it
+// else if (not lmk->getO())
+// {
+// Quaterniond frm_o = Quaterniond(Vector4d(_feature->getCapture()->getFrame()->getO()->getState()));
+// Quaterniond sen_o = Quaterniond(Vector4d(_feature->getCapture()->getSensorO()->getState()));
+
+// Quaterniond lmk_o = frm_o * sen_o * Quaterniond(_feature->getMeasurement().tail<4>());
+
+// lmk->addStateBlock('O', std::make_shared<StateQuaternion>(lmk_o), getProblem());
+// }
+
+// // emplace factor
+// return FactorBase::emplace<FactorRelativePose3dWithExtrinsics>(_feature,
+// _feature,
+// lmk,
+// shared_from_this(),
+// params_tfle_->apply_loss_function);
+// }
+// else
+// throw std::runtime_error("ProcessorLandmarkExternal::emplaceFactor unknown case");
+
+// // not reachable
+// return nullptr;
+// }
+
+void ProcessorLandmarkExternal::advanceDerived()
+{
+ // Reset here the list of correspondences.
+ known_features_last_ = std::move(known_features_incoming_);
+}
+void ProcessorLandmarkExternal::resetDerived()
+{
+ // Reset here the list of correspondences.
+ known_features_last_ = std::move(known_features_incoming_);
+}
+
+} // namespace wolf
+
+// Register in the FactoryProcessor
+#include "core/processor/factory_processor.h"
+namespace wolf {
+WOLF_REGISTER_PROCESSOR(ProcessorLandmarkExternal)
+WOLF_REGISTER_PROCESSOR_AUTO(ProcessorLandmarkExternal)
+} // namespace wolf
diff --git a/src/processor/processor_tracker.cpp b/src/processor/processor_tracker.cpp
index bba0f441766ec0d24a40566e40a64cd79dfef2f9..9dc1fa5ab42f7dc88504b63977eb90611bc7bde2 100644
--- a/src/processor/processor_tracker.cpp
+++ b/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.
diff --git a/src/processor/processor_tracker_feature_landmark_external.cpp b/src/processor/processor_tracker_feature_landmark_external.cpp
deleted file mode 100644
index 0238219e4c0f0e3de4e03659440d113f635ecf63..0000000000000000000000000000000000000000
--- a/src/processor/processor_tracker_feature_landmark_external.cpp
+++ /dev/null
@@ -1,431 +0,0 @@
-//--------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/processor/processor_tracker_feature_landmark_external.h"
-#include "core/capture/capture_landmarks_external.h"
-#include "core/feature/feature_base.h"
-#include "core/factor/factor_relative_pose_2d_with_extrinsics.h"
-#include "core/factor/factor_relative_position_2d_with_extrinsics.h"
-#include "core/factor/factor_relative_pose_3d_with_extrinsics.h"
-#include "core/factor/factor_relative_position_3d_with_extrinsics.h"
-#include "core/landmark/landmark_base.h"
-#include "core/state_block/state_block_derived.h"
-#include "core/state_block/state_quaternion.h"
-#include "core/state_block/state_angle.h"
-#include "core/math/SE2.h"
-#include "core/math/SE3.h"
-
-using namespace Eigen;
-
-namespace wolf
-{
-
-void ProcessorTrackerFeatureLandmarkExternal::preProcess()
-{
- assert(unmatched_detections_incoming_.empty());
-
- auto dim = getProblem()->getDim();
- 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();
- std::set<int> ids;
- for (auto detection : landmark_detections)
- {
- WOLF_WARN_COND(ids.count(detection.id), "ProcessorTrackerFeatureLandmarkExternal::preProcess: detection with repeated id, discarding...");
-
- if (detection.quality < params_tfle_->filter_quality_th
- or ids.count(detection.id))
- continue;
-
- Eigen::VectorXd meas;
- Eigen::MatrixXd cov;
- if (not params_tfle_->use_orientation)
- {
- assert(detection.measure.size() >= dim);
- assert(detection.covariance.rows() >= dim and detection.covariance.rows() == detection.covariance.cols());
-
- meas = detection.measure.head(dim);
- cov = detection.covariance.topLeftCorner(dim,dim);
- }
- else
- {
- assert(detection.measure.size() == (dim == 2 ? 3 : 7));
- assert(detection.covariance.rows() == (dim == 2 ? 3 : 6) and detection.covariance.rows() == detection.covariance.cols());
-
- meas = detection.measure;
- cov = detection.covariance;
- }
-
- FeatureBasePtr ftr = FeatureBase::emplace<FeatureBase>(cap_landmarks,
- "FeatureLandmarkExternal",
- meas,
- cov);
- ftr->setLandmarkId(detection.id);
-
- if (detection.id != -1 and detection.id != 0)
- ids.insert(detection.id);
-
- unmatched_detections_incoming_.push_back(ftr);
- }
-}
-
-unsigned int ProcessorTrackerFeatureLandmarkExternal::trackFeatures(const FeatureBasePtrList& _features_in,
- const CaptureBasePtr& _capture,
- FeatureBasePtrList& _features_out,
- FeatureMatchMap& _feature_correspondences)
-{
- if (_capture != incoming_ptr_)
- throw std::runtime_error("ProcessorTrackerFeatureLandmarkExternal::trackFeatures should be called with incoming capture");
- if (not _features_in.empty() and _features_in.front()->getCapture() != last_ptr_)
- throw std::runtime_error("ProcessorTrackerFeatureLandmarkExternal::trackFeatures should be called with features in not from last");
-
- auto pose_sen = getSensor()->getState("PO");
- auto pose_in = getProblem()->getState(last_ptr_->getTimeStamp(), "PO");
- auto pose_out = getProblem()->getState(incoming_ptr_->getTimeStamp(), "PO");
-
- // Track features given by ProcessorTrackerFeature
- WOLF_INFO("Searching ", _features_in.size(), " tracked features...");
- for (auto feat_in : _features_in)
- {
- // TODO: change for to while, unmatched_detections_incoming_ before std::set now std::list
- for (auto feat_candidate : unmatched_detections_incoming_)
- {
- if (feat_candidate->landmarkId() == feat_in->landmarkId() and
- detectionDistance(feat_in, feat_candidate, pose_in, pose_out, pose_sen) < params_tfle_->filter_dist_th)
- {
- // add match
- _features_out.push_back(feat_candidate);
- _feature_correspondences[_features_out.back()] = std::make_shared<FeatureMatch>(FeatureMatch({feat_in,0}));
-
- // remove from unmatched
- unmatched_detections_incoming_.erase(feat_candidate);
- break;
- }
- }
- }
- WOLF_INFO("Found ", _features_out.size(), " so far.");
-
- // // Track features in last not matched yet
- // WOLF_INFO("Searching ", unmatched_detections_last_.size(), " untracked features...");
- // auto feat_last_it = unmatched_detections_last_.begin();
- // while (feat_last_it != unmatched_detections_last_.end())
- // {
- // auto feat_in = *feat_last_it;
- // bool found = false;
- // for (auto feat_candidate : unmatched_detections_incoming_)
- // {
- // if (feat_candidate->landmarkId() == feat_in->landmarkId() and
- // detectionDistance(feat_in, feat_candidate, pose_in, pose_out, pose_sen) < params_tfle_->filter_dist_th)
- // {
- // // add track
- // track_matrix_.newTrack(feat_in);
-
- // // add match
- // _features_out.push_back(feat_candidate);
- // _feature_correspondences[_features_out.back()] = std::make_shared<FeatureMatch>(FeatureMatch({feat_in,0}));
-
- // // remove from unmatched
- // unmatched_detections_incoming_.erase(feat_candidate);
- // found = true;
- // break;
- // }
- // }
- // if (found)
- // {
- // //feat_in->link(last_ptr_);
- // feat_last_it = unmatched_detections_last_.erase(feat_last_it);
- // }
- // else
- // feat_last_it++;
- // }
- // WOLF_INFO("Found ", _features_out.size(), " in total!");
-
- return _features_out.size();
-}
-
-double ProcessorTrackerFeatureLandmarkExternal::detectionDistance(FeatureBasePtr _ftr1,
- FeatureBasePtr _ftr2,
- const VectorComposite& _pose1,
- const VectorComposite& _pose2,
- const VectorComposite& _pose_sen) const
-{
- // Any not available info of poses, assume identity
- if (not _pose1.includesStructure("PO") or not _pose2.includesStructure("PO") or not _pose_sen.includesStructure("PO"))
- {
- if (getProblem()->getDim() == 2)
- return (_ftr1->getMeasurement().head<2>() - _ftr2->getMeasurement().head<2>()).norm();
- else
- return (_ftr1->getMeasurement().head<3>() - _ftr2->getMeasurement().head<3>()).norm();
- }
- else
- {
- if (getProblem()->getDim() == 2)
- {
- auto pose_s1 = SE2::compose(_pose1, _pose_sen);
- auto pose_s2 = SE2::compose(_pose2, _pose_sen);
- auto p1 = pose_s1.at('P') + Eigen::Rotation2Dd(pose_s1.at('O')(0)) * _ftr1->getMeasurement().head<2>();
- auto p2 = pose_s2.at('P') + Eigen::Rotation2Dd(pose_s2.at('O')(0)) * _ftr2->getMeasurement().head<2>();
- return (p1-p2).norm();
- }
- else
- {
- auto pose_s1 = SE3::compose(_pose1, _pose_sen);
- auto pose_s2 = SE3::compose(_pose2, _pose_sen);
- auto p1 = pose_s1.at('P') + Eigen::Quaterniond(Eigen::Vector4d(pose_s1.at('O'))) * _ftr1->getMeasurement().head<3>();
- auto p2 = pose_s2.at('P') + Eigen::Quaterniond(Eigen::Vector4d(pose_s2.at('O'))) * _ftr2->getMeasurement().head<3>();
- return (p1-p2).norm();
- }
- }
-}
-
-bool ProcessorTrackerFeatureLandmarkExternal::voteForKeyFrame() const
-{
- auto matches_origin_last = track_matrix_.matches(origin_ptr_, last_ptr_);
-
- WOLF_INFO("Nbr. of active feature tracks: " , matches_origin_last.size() );
-
- // no tracks longer than filter_track_length_th
- auto n_tracks = 0;
- for (auto match : matches_origin_last)
- {
- if (track_matrix_.track(match.first).size() >= params_tfle_->filter_track_length_th)
- n_tracks++;
- }
- WOLF_INFO("Nbr. of active feature tracks (longer than ", params_tfle_->filter_track_length_th, "): " , n_tracks );
- if (n_tracks == 0)
- return false;
-
- bool vote = false;// = n_tracks < params_tracker_feature_->min_features_for_keyframe;
-
- if (origin_ptr_)
- {
- WOLF_INFO("Time span since last frame: " ,
- last_ptr_->getTimeStamp() - origin_ptr_->getTimeStamp() ,
- " (should be greater than ",
- params_tfle_->time_span,
- ")");
- vote = vote or last_ptr_->getTimeStamp() - origin_ptr_->getTimeStamp() > params_tfle_->time_span;
- }
-
- WOLF_INFO( (vote ? "Vote ": "Do not vote ") , "for KF" );
-
- return vote;
-}
-
-unsigned int ProcessorTrackerFeatureLandmarkExternal::detectNewFeatures(const int& _max_new_features,
- const CaptureBasePtr& _capture,
- FeatureBasePtrList& _features_out)
-{
- // NOT NECESSARY SINCE ALL FEATURES ARE TRACKED IN trackFeatures(...)
- return 0;
-}
-
-FactorBasePtr ProcessorTrackerFeatureLandmarkExternal::emplaceFactor(FeatureBasePtr _feature_ptr,
- FeatureBasePtr _feature_other_ptr)
-{
- assert(_feature_ptr);
- assert(_feature_other_ptr);
- assert(_feature_ptr->getCapture());
- assert(_feature_ptr->getCapture()->getFrame());
-
- assert(getProblem());
- assert(getProblem()->getMap());
-
- // Check track length
- if (params_tfle_->filter_track_length_th > 1)
- {
- auto snapshot = track_matrix_.snapshot(_feature_ptr->getCapture());
- const auto& it = std::find_if(snapshot.begin(), snapshot.end(),
- [_feature_ptr](const std::pair<SizeStd, FeatureBasePtr>& pair)
- {
- return pair.second == _feature_ptr;
- });
- assert(it != snapshot.end());
- if (track_matrix_.track(it->first).size() < params_tfle_->filter_track_length_th)
- return nullptr;
- }
-
- // Get landmark
- LandmarkBasePtr lmk = getProblem()->getMap()->getLandmark(_feature_ptr->landmarkId());
-
- // 2D - Relative Position
- if (getProblem()->getDim() == 2 and (_feature_ptr->getMeasurement().size() == 2 or not params_tfle_->use_orientation))
- {
- // no landmark, create it
- if (not lmk)
- {
- Vector2d frm_p = _feature_ptr->getCapture()->getFrame()->getP()->getState();
- Vector2d sen_p = _feature_ptr->getCapture()->getSensorP()->getState();
- double frm_o = _feature_ptr->getCapture()->getFrame()->getO()->getState()(0);
- double sen_o = _feature_ptr->getCapture()->getSensorO()->getState()(0);
-
- Vector2d lmk_p = frm_p + Rotation2Dd(frm_o) * (sen_p + Rotation2Dd(sen_o) * _feature_ptr->getMeasurement());
-
- lmk = LandmarkBase::emplace<LandmarkBase>(getProblem()->getMap(),
- "LandmarkRelativePosition2d",
- std::make_shared<StatePoint2d>(lmk_p),
- nullptr);
- lmk->setId(_feature_ptr->landmarkId());
- }
-
- // emplace factor
- return FactorBase::emplace<FactorRelativePosition2dWithExtrinsics>(_feature_ptr,
- _feature_ptr,
- lmk,
- shared_from_this(),
- params_tfle_->apply_loss_function);
- }
- // 2D - Relative Pose
- else if (getProblem()->getDim() == 2 and _feature_ptr->getMeasurement().size() == 3 and params_tfle_->use_orientation)
- {
- // no landmark, create it
- if (not lmk)
- {
- Vector2d frm_p = _feature_ptr->getCapture()->getFrame()->getP()->getState();
- Vector2d sen_p = _feature_ptr->getCapture()->getSensorP()->getState();
- double frm_o = _feature_ptr->getCapture()->getFrame()->getO()->getState()(0);
- double sen_o = _feature_ptr->getCapture()->getSensorO()->getState()(0);
-
- Vector2d lmk_p = frm_p + Rotation2Dd(frm_o) * (sen_p + Rotation2Dd(sen_o) * _feature_ptr->getMeasurement().head<2>());
- double lmk_o = frm_o + sen_o + _feature_ptr->getMeasurement()(2);
-
- lmk = LandmarkBase::emplace<LandmarkBase>(getProblem()->getMap(),
- "LandmarkRelativePose2d",
- std::make_shared<StatePoint2d>(lmk_p),
- std::make_shared<StateAngle>(lmk_o));
- lmk->setId(_feature_ptr->landmarkId());
- }
- // no orientation, add it
- else if (not lmk->getO())
- {
- double frm_o = _feature_ptr->getCapture()->getFrame()->getO()->getState()(0);
- double sen_o = _feature_ptr->getCapture()->getSensorO()->getState()(0);
-
- double lmk_o = frm_o + sen_o + _feature_ptr->getMeasurement()(2);
-
- lmk->addStateBlock('O', std::make_shared<StateAngle>(lmk_o), getProblem());
- }
-
- // emplace factor
- return FactorBase::emplace<FactorRelativePose2dWithExtrinsics>(_feature_ptr,
- _feature_ptr,
- lmk,
- shared_from_this(),
- params_tfle_->apply_loss_function);
- }
- // 3D - Relative Position
- else if (getProblem()->getDim() == 3 and (_feature_ptr->getMeasurement().size() == 3 or not params_tfle_->use_orientation))
- {
- // no landmark, create it
- if (not lmk)
- {
- Vector3d frm_p = _feature_ptr->getCapture()->getFrame()->getP()->getState();
- Vector3d sen_p = _feature_ptr->getCapture()->getSensorP()->getState();
- Quaterniond frm_o = Quaterniond(Vector4d(_feature_ptr->getCapture()->getFrame()->getO()->getState()));
- Quaterniond sen_o = Quaterniond(Vector4d(_feature_ptr->getCapture()->getSensorO()->getState()));
-
- Vector3d lmk_p = frm_p + frm_o * (sen_p + sen_o * _feature_ptr->getMeasurement());
-
- lmk = LandmarkBase::emplace<LandmarkBase>(getProblem()->getMap(),
- "LandmarkRelativePosition3d",
- std::make_shared<StatePoint3d>(lmk_p),
- nullptr);
- lmk->setId(_feature_ptr->landmarkId());
- }
-
- // emplace factor
- return FactorBase::emplace<FactorRelativePosition3dWithExtrinsics>(_feature_ptr,
- _feature_ptr,
- lmk,
- shared_from_this(),
- params_tfle_->apply_loss_function);
- }
- // 3D - Relative Pose
- else if (getProblem()->getDim() == 3 and _feature_ptr->getMeasurement().size() == 7 and params_tfle_->use_orientation)
- {
- // no landmark, create it
- if (not lmk)
- {
- Vector3d frm_p = _feature_ptr->getCapture()->getFrame()->getP()->getState();
- Vector3d sen_p = _feature_ptr->getCapture()->getSensorP()->getState();
- Quaterniond frm_o = Quaterniond(Vector4d(_feature_ptr->getCapture()->getFrame()->getO()->getState()));
- Quaterniond sen_o = Quaterniond(Vector4d(_feature_ptr->getCapture()->getSensorO()->getState()));
-
- Vector3d lmk_p = frm_p + frm_o * (sen_p + sen_o * _feature_ptr->getMeasurement().head<3>());
- Quaterniond lmk_o = frm_o * sen_o * Quaterniond(_feature_ptr->getMeasurement().tail<4>());
-
- lmk = LandmarkBase::emplace<LandmarkBase>(getProblem()->getMap(),
- "LandmarkRelativePose3d",
- std::make_shared<StatePoint3d>(lmk_p),
- std::make_shared<StateQuaternion>(lmk_o));
- lmk->setId(_feature_ptr->landmarkId());
- }
- // no orientation, add it
- else if (not lmk->getO())
- {
- Quaterniond frm_o = Quaterniond(Vector4d(_feature_ptr->getCapture()->getFrame()->getO()->getState()));
- Quaterniond sen_o = Quaterniond(Vector4d(_feature_ptr->getCapture()->getSensorO()->getState()));
-
- Quaterniond lmk_o = frm_o * sen_o * Quaterniond(_feature_ptr->getMeasurement().tail<4>());
-
- lmk->addStateBlock('O', std::make_shared<StateQuaternion>(lmk_o), getProblem());
- }
-
- // emplace factor
- return FactorBase::emplace<FactorRelativePose3dWithExtrinsics>(_feature_ptr,
- _feature_ptr,
- lmk,
- shared_from_this(),
- params_tfle_->apply_loss_function);
- }
- else
- throw std::runtime_error("ProcessorTrackerFeatureLandmarkExternal::emplaceFactor unknown case");
-
- // not reachable
- return nullptr;
-}
-
-void ProcessorTrackerFeatureLandmarkExternal::advanceDerived()
-{
- ProcessorTrackerFeature::advanceDerived();
-
- //unmatched_detections_last_ = std::move(unmatched_detections_incoming_);
- known_features_last_.splice(known_features_last_.end(), unmatched_detections_incoming_);
-}
-void ProcessorTrackerFeatureLandmarkExternal::resetDerived()
-{
- ProcessorTrackerFeature::resetDerived();
-
- //unmatched_detections_last_ = std::move(unmatched_detections_incoming_);
- known_features_last_.splice(known_features_last_.end(), unmatched_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
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index 261438924ee686d0799f738278b5f1186630a48e..299594805cb46a551145613baf30481f4e7aed22 100644
--- a/test/CMakeLists.txt
+++ b/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)
diff --git a/test/gtest_processor_tracker_feature_landmark_external.cpp b/test/gtest_processor_landmark_external.cpp
similarity index 91%
rename from test/gtest_processor_tracker_feature_landmark_external.cpp
rename to test/gtest_processor_landmark_external.cpp
index fa1979aa5fc588e00ce5a55dfee8433dd1f3a4a1..d061cd04c091ab88fd323de80a1e59e76eb0138c 100644
--- a/test/gtest_processor_tracker_feature_landmark_external.cpp
+++ b/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