diff --git a/include/core/processor/processor_tracker_feature_landmark_external.h b/include/core/processor/processor_tracker_feature_landmark_external.h index ae2f71789bee991f93a8c75b7529d9b273e53cc8..3e4d21321d5ef4ec5308f3f06a338de9422bc685 100644 --- a/include/core/processor/processor_tracker_feature_landmark_external.h +++ b/include/core/processor/processor_tracker_feature_landmark_external.h @@ -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 diff --git a/src/processor/processor_tracker_feature_landmark_external.cpp b/src/processor/processor_tracker_feature_landmark_external.cpp index 4f4e90e5c78fddec16cddd5c6f77585df3689a1e..0238219e4c0f0e3de4e03659440d113f635ecf63 100644 --- a/src/processor/processor_tracker_feature_landmark_external.cpp +++ b/src/processor/processor_tracker_feature_landmark_external.cpp @@ -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