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

wip to be finished...

parent dfc98d19
No related branches found
No related tags found
1 merge request!466devel->main
......@@ -68,7 +68,8 @@ class ProcessorTrackerFeatureLandmarkExternal : public ProcessorTrackerFeature
protected:
ParamsProcessorTrackerFeatureLandmarkExternalPtr params_tfle_;
std::set<FeatureBasePtr> unmatched_detections_incoming_, unmatched_detections_last_;
//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
......
......@@ -64,7 +64,7 @@ void ProcessorTrackerFeatureLandmarkExternal::preProcess()
{
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);
}
......@@ -86,7 +86,7 @@ void ProcessorTrackerFeatureLandmarkExternal::preProcess()
if (detection.id != -1 and detection.id != 0)
ids.insert(detection.id);
unmatched_detections_incoming_.insert(ftr);
unmatched_detections_incoming_.push_back(ftr);
}
}
......@@ -105,8 +105,10 @@ unsigned int ProcessorTrackerFeatureLandmarkExternal::trackFeatures(const Featur
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
......@@ -122,36 +124,42 @@ unsigned int ProcessorTrackerFeatureLandmarkExternal::trackFeatures(const Featur
}
}
}
// Track features in last not matched yet
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_last_it = unmatched_detections_last_.erase(feat_last_it);
else
feat_last_it++;
}
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();
}
......@@ -195,6 +203,8 @@ 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)
......@@ -202,7 +212,7 @@ bool ProcessorTrackerFeatureLandmarkExternal::voteForKeyFrame() const
if (track_matrix_.track(match.first).size() >= params_tfle_->filter_track_length_th)
n_tracks++;
}
WOLF_INFO("Nbr. of active feature tracks: " , 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;
......@@ -400,13 +410,15 @@ void ProcessorTrackerFeatureLandmarkExternal::advanceDerived()
{
ProcessorTrackerFeature::advanceDerived();
unmatched_detections_last_ = std::move(unmatched_detections_incoming_);
//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_);
//unmatched_detections_last_ = std::move(unmatched_detections_incoming_);
known_features_last_.splice(known_features_last_.end(), unmatched_detections_incoming_);
}
} // namespace wolf
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment