wip

include/core/math/SE2.h

@@ -320,6 +320,13 @@ inline void compose(const VectorComposite& _x1,
     _c['O'] = Matrix1d( pi2pi(a1 + a2) ) ;
 }
 
+inline VectorComposite compose(const VectorComposite& x1, const VectorComposite& x2)
+{
+    VectorComposite c("PO", {2,1});
+    compose(x1.at('P'), x1.at('O'), x2.at('P'), x2.at('O'), c['P'], c['O']);
+    return c;
+}
+
 inline void compose(const VectorComposite& _x1,
              const VectorComposite& _x2,
              VectorComposite& _c,

include/core/processor/processor_tracker_feature_landmark_external.h

@@ -32,16 +32,18 @@ WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorTrackerFeatureLandmarkExternal);
 
 struct ParamsProcessorTrackerFeatureLandmarkExternal : public ParamsProcessorTrackerFeature
 {
-    unsigned int filter_track_length_th;  ///< length of the track necessary to consider the detection
     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
 
     ParamsProcessorTrackerFeatureLandmarkExternal() = default;
     ParamsProcessorTrackerFeatureLandmarkExternal(std::string _unique_name,
                                        const wolf::ParamsServer & _server):
         ParamsProcessorTrackerFeature(_unique_name, _server)
     {
-        filter_track_length_th  = _server.getParam<unsigned int>(prefix + _unique_name + "/filter_track_length_th");
         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");
     }
 };
 
@@ -142,6 +144,12 @@ class ProcessorTrackerFeatureLandmarkExternal : public ProcessorTrackerFeature
 
         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) :

include/core/processor/processor_tracker_landmark.h

@@ -88,7 +88,7 @@ WOLF_PTR_TYPEDEFS(ProcessorTrackerLandmark);
  *     - createLandmark() : creates a Landmark using a new Feature                  <=== IMPLEMENT
  *     - findLandmarks() : find the new Landmarks again in \b incoming              <=== IMPLEMENT
  *   - establishFactors() : which calls the pure virtual:
- *     - createFactor() : create a Feature-Landmark factor of the correct derived type <=== IMPLEMENT
+ *     - emplaceFactor() : create a Feature-Landmark factor of the correct derived type <=== IMPLEMENT
  *
  * Should you need extra functionality for your derived types, you can overload these two methods,
  *

src/processor/processor_tracker_feature_landmark_external.cpp

@@ -31,6 +31,8 @@
 #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;
 
@@ -46,14 +48,24 @@ void ProcessorTrackerFeatureLandmarkExternal::preProcess()
         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;
+
         FeatureBasePtr ftr = FeatureBase::emplace<FeatureBase>(cap_landmarks,
                                                                "FeatureLandmarkExternal",
                                                                detection.measure,
                                                                detection.covariance);
         ftr->setLandmarkId(detection.id);
 
+        if (detection.id != -1 and detection.id != 0)
+            ids.insert(detection.id);
+
         detections_incoming_.push_back(ftr);
     }
 }
@@ -65,16 +77,25 @@ unsigned int ProcessorTrackerFeatureLandmarkExternal::trackFeatures(const Featur
 {
     WOLF_INFO("tracking " , _features_in.size() , " features...");
 
+    if (_features_in.empty())
+        return 0;
+
     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_);
 
+    assert(not _features_in.front() and not _features_in.front()->getCapture());
+    auto pose_sen = getSensor()->getState("PO");
+    auto pose_in  = getProblem()->getState(_features_in.front()->getCapture()->getTimeStamp(), "PO");
+    auto pose_out = getProblem()->getState(_capture->getTimeStamp(), "PO");
+
     for (auto feat_in : _features_in)
     {
         for (auto feat_candidate : landmark_detections)
         {
-            if (feat_candidate->landmarkId() == feat_in->landmarkId())
+            if (feat_candidate->landmarkId() == feat_in->landmarkId() and 
+                detectionDistance(feat_in, feat_candidate, pose_in, pose_out, pose_sen) < params_tfle_->filter_dist_th)
             {
                 _features_out.push_back(feat_candidate);
                 _feature_correspondences[_features_out.back()] = std::make_shared<FeatureMatch>(FeatureMatch({feat_in,0}));
@@ -87,6 +108,41 @@ unsigned int ProcessorTrackerFeatureLandmarkExternal::trackFeatures(const Featur
     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
 {
     WOLF_INFO("Nbr. of active feature tracks: " , incoming_ptr_->getFeatureList().size() );
@@ -131,6 +187,20 @@ FactorBasePtr ProcessorTrackerFeatureLandmarkExternal::emplaceFactor(FeatureBase
     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());
     
@@ -155,11 +225,11 @@ FactorBasePtr ProcessorTrackerFeatureLandmarkExternal::emplaceFactor(FeatureBase
         }
 
         // emplace factor
-        return FactorBase::emplace<FactorRelativePose2dWithExtrinsics>(_feature_ptr, 
-                                                                       _feature_ptr,
-                                                                       lmk,
-                                                                       shared_from_this(),
-                                                                       params_tfle_->apply_loss_function);
+        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)