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