diff --git a/demos/demo_toy_pbe.cpp b/demos/demo_toy_pbe.cpp index 57b9b7bc7b8bca93282251e26c8163ea3b24d0e7..c6d8feace5148000a869e1820a57b7cc8c0df5bd 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 5ab552427e9e72099b2c95c5ef7c2c16ff99af00..f4255f974b83a5c290c3559a40f20a330254a1e7 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 a11c6d6f6f12298644cafd95c3436fa9650931f9..04545dc68fa15dd952bf9040f1f70815599a65f3 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());