diff --git a/include/core/math/SE2.h b/include/core/math/SE2.h
index 95631a8a6fb82ac22595b64d3e1d5b4fe7849cb4..c3ca6dd59bad3d7a110724eaebb5f4a9a470ab42 100644
--- a/include/core/math/SE2.h
+++ b/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,
diff --git a/include/core/processor/processor_tracker_feature_landmark_external.h b/include/core/processor/processor_tracker_feature_landmark_external.h
index 578181788acdc0b74cf938d8e1b2e4dd0ce27d55..8444bbb381872cb3c9e54b2e90c591dca9bbb530 100644
--- a/include/core/processor/processor_tracker_feature_landmark_external.h
+++ b/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) :
diff --git a/include/core/processor/processor_tracker_landmark.h b/include/core/processor/processor_tracker_landmark.h
index bbccccf950aa02f499d215a21f5488a724330435..ab109ffdbf4302cfeb517be5e184ae3499d77fa2 100644
--- a/include/core/processor/processor_tracker_landmark.h
+++ b/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,
*
diff --git a/src/processor/processor_tracker_feature_landmark_external.cpp b/src/processor/processor_tracker_feature_landmark_external.cpp
index e2dd598eab84c8036035c626a73d10c49a2b5f63..be4ff3be06b1c9a59a767b6aab7ce8c1511ec6b9 100644
--- a/src/processor/processor_tracker_feature_landmark_external.cpp
+++ b/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)