From 8d0354a2855513fe6bda6beb6c06501826af9016 Mon Sep 17 00:00:00 2001 From: Joan Sola <jsola@iri.upc.edu> Date: Thu, 8 Mar 2018 12:20:52 +0100 Subject: [PATCH] Manage time_tolerance in processor base and tracker ... ... not yet in processor motion. --- src/processor_base.h | 5 +---- src/processor_frame_nearest_neighbor_filter.cpp | 2 +- src/processor_loopclosure_base.cpp | 4 ++-- src/processor_loopclosure_base.h | 2 +- src/processor_tracker.cpp | 2 +- src/processor_tracker.h | 2 +- src/processor_tracker_feature.cpp | 4 ++-- src/processor_tracker_feature.h | 2 +- src/processor_tracker_feature_dummy.h | 6 +++--- src/processor_tracker_landmark.cpp | 4 ++-- src/processor_tracker_landmark.h | 2 +- src/processors/processor_diff_drive.cpp | 7 +------ src/processors/processor_diff_drive.h | 7 +++---- src/test/gtest_problem.cpp | 6 +++--- src/test/gtest_processor_base.cpp | 3 +-- 15 files changed, 24 insertions(+), 34 deletions(-) diff --git a/src/processor_base.h b/src/processor_base.h index f9b33533b..0d10721f9 100644 --- a/src/processor_base.h +++ b/src/processor_base.h @@ -106,9 +106,6 @@ class KFPackBuffer */ struct ProcessorParamsBase { - ProcessorParamsBase() {time_tolerance = 0;}; - virtual ~ProcessorParamsBase() = default; - std::string type; std::string name; Scalar time_tolerance; @@ -129,7 +126,7 @@ class ProcessorBase : public NodeBase, public std::enable_shared_from_this<Proce static unsigned int processor_id_count_; public: - ProcessorBase(const std::string& _type, const Scalar& _time_tolerance = 0); + ProcessorBase(const std::string& _type, const Scalar& _time_tolerance); virtual ~ProcessorBase(); void remove(); diff --git a/src/processor_frame_nearest_neighbor_filter.cpp b/src/processor_frame_nearest_neighbor_filter.cpp index a9c120565..8d23b6d4e 100644 --- a/src/processor_frame_nearest_neighbor_filter.cpp +++ b/src/processor_frame_nearest_neighbor_filter.cpp @@ -4,7 +4,7 @@ namespace wolf { ProcessorFrameNearestNeighborFilter::ProcessorFrameNearestNeighborFilter(const Params& _params): - ProcessorLoopClosureBase("FRAME NEAREST NEIGHBOR FILTER"), + ProcessorLoopClosureBase("FRAME NEAREST NEIGHBOR FILTER", _params.time_tolerance), params_(_params) { // area of ellipse based on the Chi-Square Probabilities diff --git a/src/processor_loopclosure_base.cpp b/src/processor_loopclosure_base.cpp index f1a2a3c64..3369b5bc5 100644 --- a/src/processor_loopclosure_base.cpp +++ b/src/processor_loopclosure_base.cpp @@ -10,8 +10,8 @@ namespace wolf { -ProcessorLoopClosureBase::ProcessorLoopClosureBase(const std::string& _type): - ProcessorBase(_type) +ProcessorLoopClosureBase::ProcessorLoopClosureBase(const std::string& _type, const Scalar _time_tolerance): + ProcessorBase(_type, _time_tolerance) { // } diff --git a/src/processor_loopclosure_base.h b/src/processor_loopclosure_base.h index 8c5f14938..8de42fa59 100644 --- a/src/processor_loopclosure_base.h +++ b/src/processor_loopclosure_base.h @@ -45,7 +45,7 @@ protected: public: - ProcessorLoopClosureBase(const std::string& _type); + ProcessorLoopClosureBase(const std::string& _type, const Scalar _time_tolerance); virtual ~ProcessorLoopClosureBase() = default; diff --git a/src/processor_tracker.cpp b/src/processor_tracker.cpp index 1268c2deb..45718021a 100644 --- a/src/processor_tracker.cpp +++ b/src/processor_tracker.cpp @@ -14,7 +14,7 @@ namespace wolf { -ProcessorTracker::ProcessorTracker(const std::string& _type, const unsigned int _max_new_features, const Scalar& _time_tolerance) : +ProcessorTracker::ProcessorTracker(const std::string& _type, const Scalar& _time_tolerance, const unsigned int _max_new_features) : ProcessorBase(_type, _time_tolerance), processing_step_(FIRST_TIME_WITHOUT_PACK), origin_ptr_(nullptr), diff --git a/src/processor_tracker.h b/src/processor_tracker.h index d4c63b71f..d8b39bf39 100644 --- a/src/processor_tracker.h +++ b/src/processor_tracker.h @@ -87,7 +87,7 @@ class ProcessorTracker : public ProcessorBase FeatureBaseList new_features_incoming_; ///< list of the new features of \b last successfully tracked in \b incoming public: - ProcessorTracker(const std::string& _type, const unsigned int _max_new_features = 0, const Scalar& _time_tolerance = 0.1); + ProcessorTracker(const std::string& _type, const Scalar& _time_tolerance, const unsigned int _max_new_features); virtual ~ProcessorTracker(); /** \brief Full processing of an incoming Capture. diff --git a/src/processor_tracker_feature.cpp b/src/processor_tracker_feature.cpp index e7da3b557..9c1efa233 100644 --- a/src/processor_tracker_feature.cpp +++ b/src/processor_tracker_feature.cpp @@ -10,8 +10,8 @@ namespace wolf { -ProcessorTrackerFeature::ProcessorTrackerFeature(const std::string& _type, const unsigned int _max_new_features) : - ProcessorTracker(_type, _max_new_features) +ProcessorTrackerFeature::ProcessorTrackerFeature(const std::string& _type, const Scalar _time_tolerance, const unsigned int _max_new_features) : + ProcessorTracker(_type, _time_tolerance, _max_new_features) { } diff --git a/src/processor_tracker_feature.h b/src/processor_tracker_feature.h index 302d5f416..f196ddfa7 100644 --- a/src/processor_tracker_feature.h +++ b/src/processor_tracker_feature.h @@ -76,7 +76,7 @@ class ProcessorTrackerFeature : public ProcessorTracker /** \brief Constructor with type */ - ProcessorTrackerFeature(const std::string& _type, const unsigned int _max_new_features = 0); + ProcessorTrackerFeature(const std::string& _type, const Scalar _time_tolerance, const unsigned int _max_new_features); virtual ~ProcessorTrackerFeature(); protected: diff --git a/src/processor_tracker_feature_dummy.h b/src/processor_tracker_feature_dummy.h index 6fa449695..f6a917cf1 100644 --- a/src/processor_tracker_feature_dummy.h +++ b/src/processor_tracker_feature_dummy.h @@ -22,7 +22,7 @@ class ProcessorTrackerFeatureDummy : public ProcessorTrackerFeature { public: - ProcessorTrackerFeatureDummy(const unsigned int _max_new_features, const unsigned int _min_feat_for_keyframe); + ProcessorTrackerFeatureDummy(const Scalar _time_tolerance, const unsigned int _max_new_features, const unsigned int _min_feat_for_keyframe); virtual ~ProcessorTrackerFeatureDummy(); protected: @@ -73,8 +73,8 @@ class ProcessorTrackerFeatureDummy : public ProcessorTrackerFeature }; -inline ProcessorTrackerFeatureDummy::ProcessorTrackerFeatureDummy(const unsigned int _max_new_features, const unsigned int _min_feat_for_keyframe) : - ProcessorTrackerFeature("TRACKER FEATURE DUMMY", _max_new_features), +inline ProcessorTrackerFeatureDummy::ProcessorTrackerFeatureDummy(const Scalar _time_tolerance, const unsigned int _max_new_features, const unsigned int _min_feat_for_keyframe) : + ProcessorTrackerFeature("TRACKER FEATURE DUMMY", _time_tolerance, _max_new_features), n_feature_(0), min_feat_for_keyframe_(_min_feat_for_keyframe) { // diff --git a/src/processor_tracker_landmark.cpp b/src/processor_tracker_landmark.cpp index 9c0603659..9fd9bd499 100644 --- a/src/processor_tracker_landmark.cpp +++ b/src/processor_tracker_landmark.cpp @@ -14,8 +14,8 @@ namespace wolf { -ProcessorTrackerLandmark::ProcessorTrackerLandmark(const std::string& _type, const unsigned int& _max_new_features, const Scalar& _time_tolerance) : - ProcessorTracker(_type, _max_new_features, _time_tolerance) +ProcessorTrackerLandmark::ProcessorTrackerLandmark(const std::string& _type, const Scalar& _time_tolerance, const unsigned int& _max_new_features) : + ProcessorTracker(_type, _time_tolerance, _max_new_features) { } diff --git a/src/processor_tracker_landmark.h b/src/processor_tracker_landmark.h index 44db01813..8f6c5c3e6 100644 --- a/src/processor_tracker_landmark.h +++ b/src/processor_tracker_landmark.h @@ -72,7 +72,7 @@ WOLF_PTR_TYPEDEFS(ProcessorTrackerLandmark); class ProcessorTrackerLandmark : public ProcessorTracker { public: - ProcessorTrackerLandmark(const std::string& _type, const unsigned int& _max_new_features = 0, const Scalar& _time_tolerance = 0.1); + ProcessorTrackerLandmark(const std::string& _type, const Scalar& _time_tolerance, const unsigned int& _max_new_features); virtual ~ProcessorTrackerLandmark(); protected: diff --git a/src/processors/processor_diff_drive.cpp b/src/processors/processor_diff_drive.cpp index 43eb7fed0..535cd5c17 100644 --- a/src/processors/processor_diff_drive.cpp +++ b/src/processors/processor_diff_drive.cpp @@ -242,16 +242,11 @@ FeatureBasePtr ProcessorDiffDrive::createFeature(CaptureMotionPtr _capture_motio return key_feature_ptr; } -//bool ProcessorDiffDrive::keyFrameCallback(FrameBasePtr _keyframe_ptr, const Scalar& _time_tol) -//{ -// return ProcessorMotion::keyFrameCallback(_keyframe_ptr, _time_tol); -//} - ProcessorBasePtr ProcessorDiffDrive::create(const std::string& _unique_name, const ProcessorParamsBasePtr _params, const SensorBasePtr _sensor_ptr) { - const auto params = std::dynamic_pointer_cast<ProcessorParamsDiffDrive>(_params); + const auto params = std::static_pointer_cast<ProcessorParamsDiffDrive>(_params); if (params == nullptr) { diff --git a/src/processors/processor_diff_drive.h b/src/processors/processor_diff_drive.h index 030cb9516..6daba6031 100644 --- a/src/processors/processor_diff_drive.h +++ b/src/processors/processor_diff_drive.h @@ -17,7 +17,8 @@ WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsDiffDrive); struct ProcessorParamsDiffDrive : public ProcessorParamsBase { - ProcessorParamsDiffDrive(const Scalar _dist_travel_th, + ProcessorParamsDiffDrive(const Scalar _time_tolerance, + const Scalar _dist_travel_th, const Scalar _theta_traveled_th, const Scalar _cov_det_th, const Scalar _unmeasured_perturbation_std = 0.0001) : @@ -26,7 +27,7 @@ struct ProcessorParamsDiffDrive : public ProcessorParamsBase cov_det_th_(_cov_det_th), unmeasured_perturbation_std_(_unmeasured_perturbation_std) { - // + time_tolerance = _time_tolerance; } Scalar dist_traveled_th_; @@ -110,8 +111,6 @@ protected: virtual FeatureBasePtr createFeature(CaptureMotionPtr _capture_motion) override; -// bool keyFrameCallback(FrameBasePtr _keyframe_ptr, const Scalar& _time_tol) override; - public: /// @brief Factory method diff --git a/src/test/gtest_problem.cpp b/src/test/gtest_problem.cpp index 0bbcf9d18..98459c49d 100644 --- a/src/test/gtest_problem.cpp +++ b/src/test/gtest_problem.cpp @@ -79,7 +79,7 @@ TEST(Problem, Installers) SensorBasePtr S = P->installSensor ("ODOM 3D", "odometer", xs, wolf_root + "/src/examples/sensor_odom_3D.yaml"); // install processor tracker (dummy installation under an Odometry sensor -- it's OK for this test) - ProcessorBasePtr pt = std::make_shared<ProcessorTrackerFeatureDummy>(ProcessorTrackerFeatureDummy(5, 10)); + ProcessorBasePtr pt = std::make_shared<ProcessorTrackerFeatureDummy>(ProcessorTrackerFeatureDummy(0.1, 5, 10)); S->addProcessor(pt); @@ -218,7 +218,7 @@ TEST(Problem, StateBlocks) ASSERT_EQ(P->getStateBlockList().size(), 2 + 3); ASSERT_EQ(P->getStateBlockNotificationList().size(), 2 + 3); - ProcessorBasePtr pt = std::make_shared<ProcessorTrackerFeatureDummy>(ProcessorTrackerFeatureDummy(5, 10)); + ProcessorBasePtr pt = std::make_shared<ProcessorTrackerFeatureDummy>(ProcessorTrackerFeatureDummy(0.1, 5, 10)); St->addProcessor(pt); ProcessorBasePtr pm = P->installProcessor("ODOM 3D", "odom integrator", "odometer", wolf_root + "/src/examples/processor_odom_3D.yaml"); @@ -247,7 +247,7 @@ TEST(Problem, Covariances) SensorBasePtr Sm = P->installSensor ("ODOM 3D", "odometer",xs, wolf_root + "/src/examples/sensor_odom_3D.yaml"); SensorBasePtr St = P->installSensor ("CAMERA", "camera", xs, wolf_root + "/src/examples/camera_params_ueye_sim.yaml"); - ProcessorBasePtr pt = std::make_shared<ProcessorTrackerFeatureDummy>(ProcessorTrackerFeatureDummy(5, 10)); + ProcessorBasePtr pt = std::make_shared<ProcessorTrackerFeatureDummy>(ProcessorTrackerFeatureDummy(0.1, 5, 10)); St->addProcessor(pt); ProcessorBasePtr pm = P->installProcessor("ODOM 3D", "odom integrator", "odometer", wolf_root + "/src/examples/processor_odom_3D.yaml"); diff --git a/src/test/gtest_processor_base.cpp b/src/test/gtest_processor_base.cpp index ce31eeed3..fd5648149 100644 --- a/src/test/gtest_processor_base.cpp +++ b/src/test/gtest_processor_base.cpp @@ -42,8 +42,7 @@ TEST(ProcessorBase, KeyFrameCallback) SensorBasePtr sens_trk = make_shared<SensorBase>("FEATURE", std::make_shared<StateBlock>(Eigen::VectorXs::Zero(2)), std::make_shared<StateBlock>(Eigen::VectorXs::Zero(1)), std::make_shared<StateBlock>(Eigen::VectorXs::Zero(2)), 2); - shared_ptr<ProcessorTrackerFeatureDummy> proc_trk = make_shared<ProcessorTrackerFeatureDummy>(5, 5); - proc_trk->setTimeTolerance(dt/2); + shared_ptr<ProcessorTrackerFeatureDummy> proc_trk = make_shared<ProcessorTrackerFeatureDummy>(dt/2, 5, 5); problem->addSensor(sens_trk); sens_trk->addProcessor(proc_trk); -- GitLab