From a1db8ed5a0b7bec0f5f10bba97e23a0d139dee42 Mon Sep 17 00:00:00 2001
From: Mederic Fourmy <mederic.fourmy@gmail.com>
Date: Wed, 18 May 2022 12:21:44 +0200
Subject: [PATCH] Refactoring in processor object: removing useless things,
 reordering, rewriting voteForKeyframe

---
 demos/demo_toy_pbe.cpp                              |  2 +-
 demos/yaml/processor_tracker_landmark_object.yaml   |  2 --
 src/processor/processor_tracker_landmark_object.cpp | 11 +++++++++--
 3 files changed, 10 insertions(+), 5 deletions(-)

diff --git a/demos/demo_toy_pbe.cpp b/demos/demo_toy_pbe.cpp
index 57b9b7b..c6d8fea 100644
--- a/demos/demo_toy_pbe.cpp
+++ b/demos/demo_toy_pbe.cpp
@@ -80,7 +80,7 @@ int main() {
 
     ParamsSensorPosePtr params_sp = std::make_shared<ParamsSensorPose>();
     auto sen = problem->installSensor("SensorPose", "sensor_pose", (Vector7d()<<0,0,0,0,0,0,1).finished(), params_sp);
-    auto prc = problem->installProcessor("ProcessorTrackerLandmarkObject", "objects_wrapper", "sensor_pose", wolf_root + "/demos/processor_tracker_landmark_object.yaml");
+    auto prc = problem->installProcessor("ProcessorTrackerLandmarkObject", "objects_wrapper", "sensor_pose", wolf_root + "/demos/yaml/processor_tracker_landmark_object.yaml");
     auto prc_obj = std::static_pointer_cast<ProcessorTrackerLandmarkObject>(prc);
 
     VectorComposite x0("PO", {Vector3d(0,0,0), Quaterniond::Identity().coeffs()});
diff --git a/demos/yaml/processor_tracker_landmark_object.yaml b/demos/yaml/processor_tracker_landmark_object.yaml
index 5ab5524..f4255f9 100644
--- a/demos/yaml/processor_tracker_landmark_object.yaml
+++ b/demos/yaml/processor_tracker_landmark_object.yaml
@@ -25,7 +25,5 @@ lmk_score_thr: 0.0
 e_pos_thr: 0.1
 e_rot_thr: 0.3
 fps: 30
-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
diff --git a/src/processor/processor_tracker_landmark_object.cpp b/src/processor/processor_tracker_landmark_object.cpp
index a11c6d6..04545dc 100644
--- a/src/processor/processor_tracker_landmark_object.cpp
+++ b/src/processor/processor_tracker_landmark_object.cpp
@@ -162,7 +162,7 @@ unsigned int ProcessorTrackerLandmarkObject::detectNewFeatures(const int& _max_n
 bool ProcessorTrackerLandmarkObject::voteForKeyFrame() const
 {   
 
-    WOLF_INFO("voteForKeyFrame")
+    WOLF_INFO("voteForKeyFrame!")
 
     // A few variables to examine the state of the system
     // Feature detection wise
@@ -186,7 +186,7 @@ bool ProcessorTrackerLandmarkObject::voteForKeyFrame() const
     // 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_)
+        WOLF_INFO("Not enough features in last to vote: ", detections_last_.size(), " < ", min_features_for_keyframe_)
         return false;
     }
 
@@ -194,10 +194,12 @@ bool ProcessorTrackerLandmarkObject::voteForKeyFrame() const
     // 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){
+        WOLF_INFO("too_few_detections_incoming")
         return true;
     }
     // - the time elapsed since ORIGIN is too long
     if (too_long_since_origin_KF){
+        WOLF_INFO("too_long_since_origin_KF")
         return true;
     }
 
@@ -214,6 +216,11 @@ unsigned int ProcessorTrackerLandmarkObject::findLandmarks(const LandmarkBasePtr
                                                              FeatureBasePtrList& _features_out,
                                                              LandmarkMatchMap& _feature_landmark_correspondences)
 {
+    // This is the right thing to test but SEGFAULTS!
+    if (!last_ptr_){
+        return 0;
+    }
+
     // world to rob
     Vector3d pos_rob = getLast()->getFrame()->getP()->getState();
     Quaterniond quat_rob (getLast()->getFrame()->getO()->getState().data());
-- 
GitLab