Fix too many keyframe creation

demos/processor_tracker_landmark_object.yaml

@@ -5,11 +5,11 @@ time_tolerance: 0.1222
 vote:
     voting_active:              true
     min_time_vote:              0 # s
-    max_time_vote:              0.9999 # s
+    max_time_vote:              1.001 # s
     min_features_for_keyframe:  1
     nb_vote_for_every_first:    0
 
-reestimate_last_frame: true   # for a better prior on the new keyframe: use only if no motion processor
+reestimate_last_frame: false   # for a better prior on the new keyframe: use only if no motion processor
 add_3d_cstr: false             # add 3D constraints between the KF so that they do not jump when using apriltag only
 max_new_features: -1
 apply_loss_function: true
\ No newline at end of file

src/processor/processor_tracker_landmark_object.cpp

@@ -102,8 +102,8 @@ LandmarkBasePtr ProcessorTrackerLandmarkObject::emplaceLandmark(FeatureBasePtr _
 }
 
 unsigned int ProcessorTrackerLandmarkObject::detectNewFeatures(const int& _max_new_features,
-                                                                 const CaptureBasePtr& _capture,
-                                                                 FeatureBasePtrList& _features_out)
+                                                               const CaptureBasePtr& _capture,
+                                                               FeatureBasePtrList& _features_out)
 {
     // list of landmarks in the map
     auto lmk_lst = getProblem()->getMap()->getLandmarkList();
@@ -148,15 +148,22 @@ bool ProcessorTrackerLandmarkObject::voteForKeyFrame() const
     if (detections_last_.empty())
         return false;
 
+    auto origin = getOrigin();
+    auto incoming = getIncoming();
+    std::cout << "voteForKeyFrame" << std::endl;
+    std::cout << origin->id() << " " << getOrigin()->getTimeStamp().get() << std::endl;
+    std::cout << incoming->id() << " " <<getIncoming()->getTimeStamp().get() << std::endl;
+
+
     double dt_incoming_origin = getIncoming()->getTimeStamp().get() - getOrigin()->getTimeStamp().get();
-    bool more_than_min_time_vote = dt_incoming_origin > min_time_vote_; 
+    bool enough_time_vote = dt_incoming_origin > min_time_vote_; 
     bool too_long_since_last_KF = dt_incoming_origin > max_time_vote_;
     // the elapsed time since last KF is too long 
     if (too_long_since_last_KF){
         return true;
     }
     // no detection in incoming capture and a minimum time since last KF has past
-    if ((detections_incoming_.size() > min_features_for_keyframe_) and more_than_min_time_vote)
+    if ((detections_incoming_.size() < min_features_for_keyframe_) and enough_time_vote)
         return true;
 
     // Vote for every image processed at the beginning if possible