Refactoring in processor object: removing useless things, reordering, rewriting voteForKeyframe

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()});

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

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());