diff --git a/src/processor_tracker.cpp b/src/processor_tracker.cpp index e1bf664c72e64aa5de39e0175f42be963d55be74..411f24537f72e1d51e5a0c3349037e7b8d035b40 100644 --- a/src/processor_tracker.cpp +++ b/src/processor_tracker.cpp @@ -150,6 +150,13 @@ void ProcessorTracker::process(CaptureBasePtr const _incoming_ptr) { processKnown(); + // eventually add more features + if (last_ptr_->getFeatureList().size() < params_tracker_->min_features_for_keyframe) + { + WOLF_TRACE("Adding more features..."); + processNew(params_tracker_->max_new_features); + } + if (voteForKeyFrame() && permittedKeyFrame()) { // We create a KF @@ -164,9 +171,9 @@ void ProcessorTracker::process(CaptureBasePtr const _incoming_ptr) // process processNew(params_tracker_->max_new_features); - // Set key - last_ptr_->getFramePtr()->setKey(); - + // // Set key + // last_ptr_->getFramePtr()->setKey(); + // // Set state to the keyframe last_ptr_->getFramePtr()->setState(getProblem()->getState(last_ptr_->getTimeStamp())); @@ -187,10 +194,6 @@ void ProcessorTracker::process(CaptureBasePtr const _incoming_ptr) { // We do not create a KF - // eventually add more features - if (last_ptr_->getFeatureList().size() < params_tracker_->min_features_for_keyframe) - processNew(params_tracker_->max_new_features); - // Advance this last_ptr_->getFramePtr()->addCapture(incoming_ptr_); // Add incoming Capture to the tracker's last Frame last_ptr_->remove(); @@ -207,6 +210,7 @@ void ProcessorTracker::process(CaptureBasePtr const _incoming_ptr) break; } + number_of_tracks_ = last_ptr_->getFeatureList().size(); postProcess(); } diff --git a/src/processor_tracker.h b/src/processor_tracker.h index 0005a10d668ada812ccc1602301a5d982afa6f8f..957ae5a6776bdeffba152279d4aa58918019f099 100644 --- a/src/processor_tracker.h +++ b/src/processor_tracker.h @@ -89,6 +89,8 @@ class ProcessorTracker : public ProcessorBase FeatureBaseList new_features_last_; ///< List of new features in \b last for landmark initialization and new key-frame creation. FeatureBaseList new_features_incoming_; ///< list of the new features of \b last successfully tracked in \b incoming + size_t number_of_tracks_; + public: ProcessorTracker(const std::string& _type, ProcessorParamsTrackerPtr _params_tracker); @@ -194,6 +196,16 @@ class ProcessorTracker : public ProcessorBase FeatureBaseList& getNewFeaturesListLast(); + const size_t& previousNumberOfTracks() const + { + return number_of_tracks_; + } + + size_t& previousNumberOfTracks() + { + return number_of_tracks_; + } + protected: void computeProcessingStep(); diff --git a/src/processors/processor_tracker_feature_trifocal.cpp b/src/processors/processor_tracker_feature_trifocal.cpp index 13f60794f529e806753370a1e590a9f79b354a4d..f4ec0faf6eed9ad125a54f1901de0ab82beecbb0 100644 --- a/src/processors/processor_tracker_feature_trifocal.cpp +++ b/src/processors/processor_tracker_feature_trifocal.cpp @@ -266,41 +266,24 @@ bool ProcessorTrackerFeatureTrifocal::correctFeatureDrift(const FeatureBasePtr _ bool ProcessorTrackerFeatureTrifocal::voteForKeyFrame() { - // List of conditions - -// // A. crossing voting threshold with ascending number of features -// bool vote_up = true; -// // 1. vote if we did not have enough features before -// vote_up = vote_up && (last_ptr_->getFeatureList().size() < params_tracker_feature_trifocal_->min_features_for_keyframe); -// // 2. vote if we have enough features now -// vote_up = vote_up && (incoming_ptr_->getFeatureList().size() >= params_tracker_feature_trifocal_->min_features_for_keyframe); -// -// // B. crossing voting threshold with descending number of features -// bool vote_down = true; -// // 1. vote if we had enough features before -// vote_down = vote_down && (last_ptr_->getFeatureList().size() >= params_tracker_feature_trifocal_->min_features_for_keyframe); -// // 2. vote if we have not enough features now -// vote_down = vote_down && (incoming_ptr_->getFeatureList().size() < params_tracker_feature_trifocal_->min_features_for_keyframe); -// - -// FIX: This LIST are ALWAYS empty. -// WOLF_TRACE("getNewFeaturesListLast().size() ", getNewFeaturesListLast().size()); -// WOLF_TRACE("getNewFeaturesListIncoming().size() ", getNewFeaturesListIncoming().size()); - - // A. crossing voting threshold with ascending number of features bool vote_up = true; // 1. vote if we did not have enough features before - vote_up = vote_up && (getNewFeaturesListLast().size() < params_tracker_feature_trifocal_->min_features_for_keyframe); + vote_up = vote_up && (previousNumberOfTracks() < params_tracker_feature_trifocal_->min_features_for_keyframe); // 2. vote if we have enough features now - vote_up = vote_up && (getNewFeaturesListIncoming().size() >= params_tracker_feature_trifocal_->min_features_for_keyframe); + vote_up = vote_up && (incoming_ptr_->getFeatureList().size() >= params_tracker_feature_trifocal_->min_features_for_keyframe); // B. crossing voting threshold with descending number of features bool vote_down = true; // 1. vote if we had enough features before - vote_down = vote_down && (getNewFeaturesListLast().size() >= params_tracker_feature_trifocal_->min_features_for_keyframe); + vote_down = vote_down && (last_ptr_->getFeatureList().size() >= params_tracker_feature_trifocal_->min_features_for_keyframe); // 2. vote if we have not enough features now - vote_down = vote_down && (getNewFeaturesListIncoming().size() < params_tracker_feature_trifocal_->min_features_for_keyframe); + vote_down = vote_down && (incoming_ptr_->getFeatureList().size() < params_tracker_feature_trifocal_->min_features_for_keyframe); + + if (vote_up) + WOLF_TRACE("VOTE UP"); + if (vote_down) + WOLF_TRACE("VOTE DOWN"); return vote_up || vote_down; }