Skip to content
Snippets Groups Projects
Commit 0477ffd0 authored by Joan Vallvé Navarro's avatar Joan Vallvé Navarro
Browse files

processor landmark external vote for kf more complete

parent 14836e08
No related branches found
No related tags found
1 merge request!466devel->main
Hide whitespace changes
Inline Side-by-side
include/core/processor/processor_landmark_external.h
+ 12
− 9
View file @ 0477ffd0
......@@ -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_()
{
//
}
......
src/processor/processor_landmark_external.cpp
+ 34
− 182
View file @ 0477ffd0
......@@ -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.
......
test/gtest_processor_landmark_external.cpp
+ 9
− 7
View file @ 0477ffd0
......@@ -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);
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment