Refactor voteForKeyframe

src/processor/processor_tracker_landmark_object.cpp

@@ -161,25 +161,43 @@ unsigned int ProcessorTrackerLandmarkObject::detectNewFeatures(const int& _max_n
 
 bool ProcessorTrackerLandmarkObject::voteForKeyFrame() const
 {   
+
     WOLF_INFO("voteForKeyFrame")
-    // if no detections in last capture, no case where it is usefull to create a KF from last
-    if (detections_last_.empty())
-        return false;
-    auto origin = getOrigin();
-    auto incoming = getIncoming();
 
+    // A few variables to examine the state of the system
+    // Feature detection wise
+    bool too_few_detections_last = detections_last_.size() < min_features_for_keyframe_;
+    bool too_few_detections_incoming = detections_incoming_.size() < min_features_for_keyframe_;
+
+    // Feature-Landmark matching wise
+    // TODO: 
+    // Ideally we would need to use the size of the output of findLandmarks: _features_out
+    // -> number of matched features after processKnown
+    // Stored in matches_landmark_from_incoming_ (ProcessorTrackerLandmark)
+
+
+    // Time wise
     double dt_incoming_origin = getIncoming()->getTimeStamp().get() - getOrigin()->getTimeStamp().get();
-    WOLF_INFO(origin->id(), " -> ", incoming->id(), " dt : ", dt_incoming_origin)
     bool enough_time_vote = dt_incoming_origin > min_time_vote_; 
-    bool too_long_since_last_KF = dt_incoming_origin > max_time_vote_;
+    bool too_long_since_origin_KF = dt_incoming_origin > max_time_vote_;
+    WOLF_INFO(getOrigin()->id(), " -> ", getIncoming()->id(), " dt : ", dt_incoming_origin)
 
-    // the elapsed time since last KF is too long 
-    if (too_long_since_last_KF){
-        return true;
+
+    // if not enough detections in LAST capture for some reason, useless to create a KF from LAST
+    if (too_few_detections_last)
+    {
+        WOLF_DEBUG("Not enough features in last to vote: ", detections_last_.size(), " < ", min_features_for_keyframe_)
+        return false;
     }
 
-    // no detection in incoming capture and a minimum time since last KF has past
-    if ((detections_incoming_.size() < min_features_for_keyframe_) and enough_time_vote){
+    // voting needs to be done, after checking there is enough features in LAST to vote
+    // 2 cases in which we want to vote for a Keyframe:
+    // - number of detections in INCOMING are too few and enough time has passed since ORIGIN
+    if (enough_time_vote && too_few_detections_incoming){
+        return true;
+    }
+    // - the time elapsed since ORIGIN is too long
+    if (too_long_since_origin_KF){
         return true;
     }
 
@@ -196,10 +214,25 @@ unsigned int ProcessorTrackerLandmarkObject::findLandmarks(const LandmarkBasePtr
                                                              FeatureBasePtrList& _features_out,
                                                              LandmarkMatchMap& _feature_landmark_correspondences)
 {
+    // world to rob
+    Vector3d pos_rob = getLast()->getFrame()->getP()->getState();
+    Quaterniond quat_rob (getLast()->getFrame()->getO()->getState().data());
+    Eigen::Isometry3d w_M_r = Eigen::Translation<double,3>(pos_rob.head(3)) * quat_rob;
+
+    // rob to sensor
+    Vector3d pos_sen = getSensor()->getP()->getState();
+    Quaterniond quat_sen (getSensor()->getO()->getState().data());
+    Eigen::Isometry3d r_M_c = Eigen::Translation<double,3>(pos_sen.head(3)) * quat_sen;
+
     for (auto feat : detections_incoming_)
     {
         std::string object_type = std::static_pointer_cast<FeatureObject>(feat)->getObjectType();
 
+        // camera to feat
+        Vector3d pos_obj = feat->getMeasurement().head(3);
+        Quaterniond quat_obj (feat->getMeasurement().tail(4).data());
+        Eigen::Isometry3d c_M_f = Eigen::Translation<double,3>(pos_obj) * quat_obj;
+
         if (feat->getCapture() != nullptr){
             break;
         }
@@ -210,22 +243,6 @@ unsigned int ProcessorTrackerLandmarkObject::findLandmarks(const LandmarkBasePtr
 
             if(lmk_obj != nullptr and lmk_obj->getObjectType() == object_type)
             {
-                
-                // world to rob
-                Vector3d pos = getLast()->getFrame()->getP()->getState();
-                Quaterniond quat (getLast()->getFrame()->getO()->getState().data());
-                Eigen::Isometry3d w_M_r = Eigen::Translation<double,3>(pos.head(3)) * quat;
-
-                // rob to sensor
-                pos = getSensor()->getP()->getState();
-                quat.coeffs() = getSensor()->getO()->getState();
-                Eigen::Isometry3d r_M_c = Eigen::Translation<double,3>(pos.head(3)) * quat;
-
-                // camera to feat
-                pos = feat->getMeasurement().head(3);
-                quat.coeffs() = feat->getMeasurement().tail(4);
-                Eigen::Isometry3d c_M_f = Eigen::Translation<double,3>(pos) * quat;
-
                 // world to feat
                 Eigen::Isometry3d w_M_f = w_M_r * r_M_c * c_M_f;
                 Quaterniond quat_feat;
@@ -243,7 +260,7 @@ unsigned int ProcessorTrackerLandmarkObject::findLandmarks(const LandmarkBasePtr
 
                 if (e_rot < e_rot_thr_ && e_pos < e_pos_thr_){
                     _features_out.push_back(feat);
-                    double score(1.0);
+                    double score(1.0);  // not used
                     LandmarkMatchPtr matched_landmark = std::make_shared<LandmarkMatch>(lmk, score);
                     _feature_landmark_correspondences.emplace (feat, matched_landmark);
                     feat->link(_capture); // since all features are created in preProcess() are unlinked