diff --git a/src/examples/test_kf_callback.cpp b/src/examples/test_kf_callback.cpp
index 5101341c0a3ac7ea06a6245361dfac35ed1cde26..75d0c293951a09c467096297062d60ba1b0ce9f0 100644
--- a/src/examples/test_kf_callback.cpp
+++ b/src/examples/test_kf_callback.cpp
@@ -25,7 +25,7 @@ int main()
prc_odo->setTimeTolerance(0.1);
SensorBasePtr sen_ftr = problem->installSensor ("ODOM 2D", "other odometer", (Vector3s()<<0,0,0).finished(),"");
- shared_ptr<ProcessorTrackerFeatureDummy> prc_ftr = make_shared<ProcessorTrackerFeatureDummy>(7, 4);
+ shared_ptr<ProcessorTrackerFeatureDummy> prc_ftr = make_shared<ProcessorTrackerFeatureDummy>(0.5, 7, 4);
prc_ftr->setName("tracker");
sen_ftr->addProcessor(prc_ftr);
prc_ftr->setTimeTolerance(0.1);
diff --git a/src/examples/test_processor_tracker_feature.cpp b/src/examples/test_processor_tracker_feature.cpp
index 15655bce2e5f2553974f5278127117169dcd65dd..fcf29c545f2cdd837f5190f0c123e8bde25835d7 100644
--- a/src/examples/test_processor_tracker_feature.cpp
+++ b/src/examples/test_processor_tracker_feature.cpp
@@ -31,7 +31,7 @@ int main()
std::make_shared<StateBlock>(Eigen::VectorXs::Zero(1)),
std::make_shared<StateBlock>(Eigen::VectorXs::Zero(2)), 2);
- shared_ptr<ProcessorTrackerFeatureDummy> processor_ptr_ = make_shared<ProcessorTrackerFeatureDummy>(7, 4);
+ shared_ptr<ProcessorTrackerFeatureDummy> processor_ptr_ = make_shared<ProcessorTrackerFeatureDummy>(.25, 7, 4);
wolf_problem_ptr_->addSensor(sensor_ptr_);
sensor_ptr_->addProcessor(processor_ptr_);
diff --git a/src/examples/test_processor_tracker_landmark.cpp b/src/examples/test_processor_tracker_landmark.cpp
index bc5955a56a0cc50f0daffc85b455a9bfa9179898..6f828298d8147e1bba28f373558d8eda96c04276 100644
--- a/src/examples/test_processor_tracker_landmark.cpp
+++ b/src/examples/test_processor_tracker_landmark.cpp
@@ -66,7 +66,7 @@ int main()
std::make_shared<StateBlock>(Eigen::VectorXs::Zero(1)),
std::make_shared<StateBlock>(Eigen::VectorXs::Zero(2)), 2);
- std::shared_ptr<ProcessorTrackerLandmarkDummy> processor_ptr_ = std::make_shared< ProcessorTrackerLandmarkDummy>(5);
+ std::shared_ptr<ProcessorTrackerLandmarkDummy> processor_ptr_ = std::make_shared< ProcessorTrackerLandmarkDummy>(.25, 5);
wolf_problem_ptr_->addSensor(sensor_ptr_);
sensor_ptr_->addProcessor(processor_ptr_);
diff --git a/src/processor_base.h b/src/processor_base.h
index f9b33533bfa3dd85a0025ed481320c0cda493227..0d10721f910fd04a119c6864ee50f285ea75d258 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 a9c1205653bd6f19c55c85c49058fdb75be22b9d..8d23b6d4e7270db98b84778abdcf610815901d74 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_image_feature.cpp b/src/processor_image_feature.cpp
index c728686d73a4d9ce85ef5c61cf6dfd381522a1b6..56f331f86618154df9b36eb21163694f1c556aa5 100644
--- a/src/processor_image_feature.cpp
+++ b/src/processor_image_feature.cpp
@@ -13,7 +13,7 @@ namespace wolf
#if defined (HAVE_OPENCV3)
ProcessorImageFeature::ProcessorImageFeature(ProcessorParamsImage _params) :
- ProcessorTrackerFeature("IMAGE", _params.algorithm.max_new_features),
+ ProcessorTrackerFeature("IMAGE", _params.time_tolerance, _params.algorithm.max_new_features),
params_(_params),
active_search_grid_()
{
@@ -81,7 +81,7 @@ ProcessorImageFeature::ProcessorImageFeature(ProcessorParamsImage _params) :
#else
ProcessorImageFeature::ProcessorImageFeature(ProcessorParamsImage _params) :
- ProcessorTrackerFeature("IMAGE", _params.algorithm.max_new_features),
+ ProcessorTrackerFeature("IMAGE", _params.time_tolerance, _params.algorithm.max_new_features),
matcher_ptr_(nullptr),
detector_descriptor_ptr_(nullptr),
params_(_params),
diff --git a/src/processor_loopclosure_base.cpp b/src/processor_loopclosure_base.cpp
index f1a2a3c649ab3295ca5400834015a1dd6dc3f3ef..3369b5bc53b0b4a24e35813b122139f0ea1e975b 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 8c5f1493865816820ace7e78b4e556a7ccd8bde2..8de42fa59b6beea14f0e7209f5de8c15b6c9fe49 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 1268c2debc5c2a7320e1479009657820f728ddcb..45718021a33625a45d08d116b2042dce179ee189 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 d4c63b71f56091e9ec770fefaf0d99f7a7e32b4b..d8b39bf39422d59b3b73f46339d08685b4c08f5d 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 e7da3b55749c25a00a0027772f6483d659d8a9f7..9c1efa23330838108b68d07e4d0f0b7f95ebcab5 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 302d5f41699c9345913f316f3cef0e5c0a4d7844..f196ddfa7a1570e3c14663e5e02f6bec5aa690ba 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 6fa449695f09a56fa93932dddf4d359e29000369..f6a917cf12cd5ed09bc62afdc88a7b207df9ffb0 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 9c06036595520a7c30fb65fcb1b0a91f26310ba3..9fd9bd499d3e60395ed331eff03570cda8ca12b2 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 44db01813ba1002ded7dc1309f10a3bb964164c8..8f6c5c3e63fa14e8e9b1321fc97f481b89a74620 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/processor_tracker_landmark_dummy.cpp b/src/processor_tracker_landmark_dummy.cpp
index 71051458bfdd9ef5b5f4013984e20de93c4370b8..02d5f9893fdd4df0ba791e649448f703861a71b6 100644
--- a/src/processor_tracker_landmark_dummy.cpp
+++ b/src/processor_tracker_landmark_dummy.cpp
@@ -12,8 +12,10 @@
namespace wolf
{
-ProcessorTrackerLandmarkDummy::ProcessorTrackerLandmarkDummy(const unsigned int& _max_new_features) :
- ProcessorTrackerLandmark("TRACKER LANDMARK DUMMY", _max_new_features, 0.1), n_feature_(0), landmark_idx_non_visible_(0)
+ProcessorTrackerLandmarkDummy::ProcessorTrackerLandmarkDummy(const Scalar _time_tolerance, const unsigned int& _max_new_features) :
+ ProcessorTrackerLandmark("TRACKER LANDMARK DUMMY", _time_tolerance, _max_new_features),
+ n_feature_(0),
+ landmark_idx_non_visible_(0)
{
//
diff --git a/src/processor_tracker_landmark_dummy.h b/src/processor_tracker_landmark_dummy.h
index 4c8a2b8c8d09477a9ea27ba2020ac8151e9f28e0..835fa27ce8a323ea71583166673ee903b2fa1973 100644
--- a/src/processor_tracker_landmark_dummy.h
+++ b/src/processor_tracker_landmark_dummy.h
@@ -18,7 +18,7 @@ WOLF_PTR_TYPEDEFS(ProcessorTrackerLandmarkDummy);
class ProcessorTrackerLandmarkDummy : public ProcessorTrackerLandmark
{
public:
- ProcessorTrackerLandmarkDummy(const unsigned int& _max_new_features);
+ ProcessorTrackerLandmarkDummy(const Scalar _time_tolerance, const unsigned int& _max_new_features);
virtual ~ProcessorTrackerLandmarkDummy();
protected:
diff --git a/src/processors/processor_diff_drive.cpp b/src/processors/processor_diff_drive.cpp
index 43eb7fed018937d7eae6228e92b5bc1a4558fc6a..535cd5c178b0f184d23cfcc95d3689683a8464cb 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 030cb9516036bfbae7fd22c2a79073c3a3400146..6daba603107608f25cbd79db659ca5de30b850ea 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 0bbcf9d1872735c5bf6b41aa29405b255f7dd7eb..98459c49d85397f64ea619316e867be11440d45f 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 ce31eeed39d927a660b33e249c76981c5c0bef50..fd5648149f8f576372b2a2cd31e31de97665f3c6 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);