diff --git a/include/core/processor/processor_landmark_external.h b/include/core/processor/processor_landmark_external.h
index 6e4ba1f4167de4c4189dea87621b02c474aae5bf..5c574a6e9a3a0a8c94254329dfaaa766be5f8db4 100644
--- a/include/core/processor/processor_landmark_external.h
+++ b/include/core/processor/processor_landmark_external.h
@@ -33,11 +33,12 @@ 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
+ bool use_orientation; ///< use orientation measure or not when emplacing factors
+ double match_dist_th; ///< for considering tracked detection: distance threshold to previous detection
+ unsigned int new_features_for_keyframe; ///< for keyframe voting: amount of new features with respect to origin (sufficient condition if more than min_features_for_keyframe)
+ double filter_quality_th; ///< min quality to consider the 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 (sufficient condition if more than min_features_for_keyframe)
ParamsProcessorLandmarkExternal() = default;
ParamsProcessorLandmarkExternal(std::string _unique_name,
@@ -45,9 +46,9 @@ struct ParamsProcessorLandmarkExternal : public ParamsProcessorTracker
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");
+ filter_quality_th = _server.getParam<double> (prefix + _unique_name + "/filter/quality_th");
+ filter_track_length_th = _server.getParam<unsigned int>(prefix + _unique_name + "/filter/track_length_th");
+ match_dist_th = _server.getParam<double> (prefix + _unique_name + "/match_dist_th");
time_span = _server.getParam<double> (prefix + _unique_name + "/keyframe_vote/time_span");
}
};
@@ -70,6 +71,7 @@ class ProcessorLandmarkExternal : public ProcessorTracker
ParamsProcessorLandmarkExternalPtr params_tfle_;
TrackMatrix track_matrix_;
+ std::set<SizeStd> lmks_ids_origin_;
/** Pre-process incoming Capture
*
@@ -143,7 +145,8 @@ class ProcessorLandmarkExternal : public ProcessorTracker
inline ProcessorLandmarkExternal::ProcessorLandmarkExternal(ParamsProcessorLandmarkExternalPtr _params_tfle) :
ProcessorTracker("ProcessorLandmarkExternal", "PO", 0, _params_tfle),
- params_tfle_(_params_tfle)
+ params_tfle_(_params_tfle),
+ lmks_ids_origin_()
{
//
}
diff --git a/src/processor/processor_landmark_external.cpp b/src/processor/processor_landmark_external.cpp
index 63adbb9265f01d1e13a8520524025c8059ee7e25..2ac3ff66ae6d3ed22787b7b8166cf9c4c5b8965a 100644
--- a/src/processor/processor_landmark_external.cpp
+++ b/src/processor/processor_landmark_external.cpp
@@ -116,7 +116,7 @@ unsigned int ProcessorLandmarkExternal::processKnown()
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)
+ detectionDistance(feat_last, (*feat_candidate_it), pose_in, pose_out, pose_sen) < params_tfle_->match_dist_th)
{
// grow track
track_matrix_.add(feat_last, *feat_candidate_it);
@@ -197,33 +197,46 @@ bool ProcessorLandmarkExternal::voteForKeyFrame() const
{
auto track_ids_last = track_matrix_.trackIds(last_ptr_);
- WOLF_INFO("Nbr. of active feature tracks: " , track_ids_last.size() );
+ WOLF_INFO("Active feature tracks: " , track_ids_last.size() );
// no tracks longer than filter_track_length_th
auto n_tracks = 0;
+ auto n_new_tracks = 0;
for (auto track_id : track_ids_last)
{
if (track_matrix_.trackSize(track_id) >= params_tfle_->filter_track_length_th)
+ {
n_tracks++;
+ if (not lmks_ids_origin_.count(track_matrix_.feature(track_id, last_ptr_)->landmarkId()))
+ n_new_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_)
+
+ // Necessary condition: active valid tracks
+ bool vote_min_features = n_tracks >= params_tfle_->min_features_for_keyframe;
+ WOLF_INFO("vote_min_features: ", vote_min_features,
+ " - Active feature tracks longer than ", params_tfle_->filter_track_length_th, ": ", n_tracks,
+ " (should be equal or bigger than ", params_tfle_->min_features_for_keyframe, ")");
+
+ bool vote_new_features(true), vote_time_span(true);
+ if (vote_min_features and 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;
+ // Sufficient condition: new valid tracks
+ vote_new_features = n_new_tracks >= params_tfle_->new_features_for_keyframe;
+ WOLF_INFO("vote_new_features: " , vote_new_features,
+ " - n_new_tracks = ", n_new_tracks,
+ " (should be equal or bigger than ", params_tfle_->new_features_for_keyframe, ")");
+
+ // Sufficient condition: time span
+ vote_time_span = last_ptr_->getTimeStamp() - origin_ptr_->getTimeStamp() > params_tfle_->time_span;
+ WOLF_INFO("vote_time_span: " , vote_time_span,
+ " - time_span = ", last_ptr_->getTimeStamp() - origin_ptr_->getTimeStamp(),
+ " (should be bigger than ", params_tfle_->time_span, ")");
}
- WOLF_INFO( (vote ? "Vote ": "Do not vote ") , "for KF" );
+ bool vote = vote_min_features and (vote_new_features or vote_time_span);
+
+ WOLF_INFO( (vote ? "Vote ": "Do NOT vote ") , "for KF" );
return vote;
}
@@ -233,6 +246,8 @@ void ProcessorLandmarkExternal::establishFactors()
if (origin_ptr_ == last_ptr_)
return;
+ // reset n_tracks_origin_
+ lmks_ids_origin_.clear();
// 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;
@@ -259,6 +274,8 @@ void ProcessorLandmarkExternal::establishFactors()
// emplace factor
auto fac = emplaceFactor(feature, lmk);
+ lmks_ids_origin_.insert(lmk->id());
+
if (fac)
fac_list.push_back(fac);
}
@@ -459,171 +476,6 @@ void ProcessorLandmarkExternal::modifyLandmark(LandmarkBasePtr _landmark,
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.
diff --git a/test/gtest_processor_landmark_external.cpp b/test/gtest_processor_landmark_external.cpp
index d061cd04c091ab88fd323de80a1e59e76eb0138c..201e8f61a26fe70aac63d99365668ea3cc2968c0 100644
--- a/test/gtest_processor_landmark_external.cpp
+++ b/test/gtest_processor_landmark_external.cpp
@@ -82,12 +82,12 @@ class ProcessorLandmarkExternalTest : public testing::Test
};
void ProcessorLandmarkExternalTest::initProblem(int _dim,
- bool _orientation,
- double _quality_th,
- double _dist_th,
- unsigned int _track_length_th,
- double _time_span,
- bool _add_landmarks)
+ bool _orientation,
+ double _quality_th,
+ double _dist_th,
+ unsigned int _track_length_th,
+ double _time_span,
+ bool _add_landmarks)
{
dim = _dim;
orientation = _orientation;
@@ -132,7 +132,9 @@ void ProcessorLandmarkExternalTest::initProblem(int _dim,
params->apply_loss_function = false;
params->use_orientation = orientation;
params->filter_quality_th = _quality_th;
- params->filter_dist_th = _dist_th;
+ params->match_dist_th = _dist_th;
+ params->min_features_for_keyframe = 1;
+ params->new_features_for_keyframe = 1000;
params->filter_track_length_th = _track_length_th;
params->time_span = _time_span;
processor = ProcessorBase::emplace<ProcessorLandmarkExternal>(sensor, params);