diff --git a/src/processor_IMU.cpp b/src/processor_IMU.cpp
index 2e6340d05f75b96315f317bb7f17d29c80cb7121..c31c0533e4a779820d3aebe2fa09f2c7813f8072 100644
--- a/src/processor_IMU.cpp
+++ b/src/processor_IMU.cpp
@@ -4,7 +4,7 @@
namespace wolf {
ProcessorIMU::ProcessorIMU(ProcessorIMUParamsPtr _params) :
- ProcessorMotion("IMU", 10, 10, 9, 6, 0.01, 6),
+ ProcessorMotion("IMU", 10, 10, 9, 6, 6, 0.01),
max_time_span_ (_params ? _params ->max_time_span : 1.0 ),
max_buff_length_(_params ? _params ->max_buff_length : 10000 ),
dist_traveled_ (_params ? _params ->dist_traveled : 1.0 ),
diff --git a/src/processor_base.h b/src/processor_base.h
index c8b6f7381f9f96b5278908566eb0cb88eb8046c6..87ba0f2f73c77b4daf1f2eb0a643b15bcdf8754e 100644
--- a/src/processor_base.h
+++ b/src/processor_base.h
@@ -20,6 +20,8 @@ namespace wolf {
/** \brief Key frame class pack
*
* To store a key_frame with an associated time tolerance.
+ *
+ * Used in keyframe callbacks as the minimal pack of information needed by the processor receiving the callback.
*/
class KFPack
{
diff --git a/src/processor_motion.cpp b/src/processor_motion.cpp
index cd0a2615486677178ad32b03c3e1152901a034a7..63cf1e0747917255b71ad285d4584b5f34a2b77e 100644
--- a/src/processor_motion.cpp
+++ b/src/processor_motion.cpp
@@ -7,8 +7,8 @@ ProcessorMotion::ProcessorMotion(const std::string& _type,
Size _delta_size,
Size _delta_cov_size,
Size _data_size,
- Scalar _time_tolerance,
- Size _calib_size) :
+ Size _calib_size,
+ Scalar _time_tolerance) :
ProcessorBase(_type, _time_tolerance),
processing_step_(RUNNING_WITHOUT_PACK),
x_size_(_state_size),
@@ -30,7 +30,6 @@ ProcessorMotion::ProcessorMotion(const std::string& _type,
jacobian_delta_(delta_cov_size_, delta_cov_size_),
jacobian_calib_(delta_size_, calib_size_)
{
- status_ = IDLE;
//
}
diff --git a/src/processor_motion.h b/src/processor_motion.h
index 93d1af04a43ee77e02661331d1beb8031e2ed49a..bc12469728c66b6d9e57d0b4ed789cde7b04a923 100644
--- a/src/processor_motion.h
+++ b/src/processor_motion.h
@@ -110,13 +110,6 @@ class ProcessorMotion : public ProcessorBase
protected:
ProcessingStep processing_step_; ///< State machine controlling the processing step
- private:
- enum
- {
- IDLE,
- RUNNING
- } status_;
-
// This is the main public interface
public:
ProcessorMotion(const std::string& _type,
@@ -124,8 +117,8 @@ class ProcessorMotion : public ProcessorBase
Size _delta_size,
Size _delta_cov_size,
Size _data_size,
- Scalar _time_tolerance = 0.1,
- Size _calib_size = 0);
+ Size _calib_size = 0,
+ Scalar _time_tolerance = 0.1);
virtual ~ProcessorMotion();
// Instructions to the processor:
diff --git a/src/processor_odom_2D.cpp b/src/processor_odom_2D.cpp
index a5cc0a1a35e2704280dceb4431d48a03bda59a61..c95bda8bcf458252e69fec35144a49e105855620 100644
--- a/src/processor_odom_2D.cpp
+++ b/src/processor_odom_2D.cpp
@@ -6,8 +6,9 @@ ProcessorOdom2D::ProcessorOdom2D(const Scalar& _dist_traveled_th,
const Scalar& _theta_traveled_th,
const Scalar& _cov_det_th,
const Scalar& _elapsed_time_th,
- const Scalar& _unmeasured_perturbation_std) :
- ProcessorMotion("ODOM 2D", 3, 3, 3, 2),
+ const Scalar& _unmeasured_perturbation_std,
+ const Scalar& _time_tolerance) :
+ ProcessorMotion("ODOM 2D", 3, 3, 3, 2, 0, _time_tolerance),
dist_traveled_th_(_dist_traveled_th),
theta_traveled_th_(_theta_traveled_th),
cov_det_th_(_cov_det_th),
diff --git a/src/processor_odom_2D.h b/src/processor_odom_2D.h
index c9b10b1400e7e5cf9a5603ce2f319b7bfd7a7172..251533a254b5b1a3b08c1335fed48a8f2d37e4f3 100644
--- a/src/processor_odom_2D.h
+++ b/src/processor_odom_2D.h
@@ -35,7 +35,8 @@ class ProcessorOdom2D : public ProcessorMotion
const Scalar& _theta_traveled_th = 0.17,
const Scalar& _cov_det_th = 1.0,
const Scalar& _elapsed_time_th = 1.0,
- const Scalar& _unmeasured_perturbation_std = 0.001);
+ const Scalar& _unmeasured_perturbation_std = 0.001,
+ const Scalar& _time_tolerance = 0.01);
virtual ~ProcessorOdom2D();
virtual bool voteForKeyFrame() override;
diff --git a/src/processor_odom_3D.cpp b/src/processor_odom_3D.cpp
index 70b31a4430236a4c07b4b5430585bebd3c66c40c..d0eb4dc5f353af9cf84bf03bfd68c737bffd4237 100644
--- a/src/processor_odom_3D.cpp
+++ b/src/processor_odom_3D.cpp
@@ -3,7 +3,7 @@ namespace wolf
{
ProcessorOdom3D::ProcessorOdom3D(ProcessorOdom3DParamsPtr _params, SensorOdom3DPtr _sensor_ptr) :
- ProcessorMotion("ODOM 3D", 7, 7, 6, 6),
+ ProcessorMotion("ODOM 3D", 7, 7, 6, 6, 0, (_params ? _params->time_tolerance : 0.01) ),
max_time_span_ (_params ? _params ->max_time_span : 1.0 ),
max_buff_length_(_params ? _params ->max_buff_length : 10 ),
dist_traveled_ (_params ? _params ->dist_traveled : 1.0 ),
diff --git a/src/processors/processor_diff_drive.cpp b/src/processors/processor_diff_drive.cpp
index 535cd5c178b0f184d23cfcc95d3689683a8464cb..8031040ce0b184d9c300143897925141140e5e3a 100644
--- a/src/processors/processor_diff_drive.cpp
+++ b/src/processors/processor_diff_drive.cpp
@@ -13,7 +13,7 @@ namespace wolf
{
ProcessorDiffDrive::ProcessorDiffDrive(const ProcessorParamsDiffDrive ¶ms) :
- ProcessorMotion("DIFF DRIVE", 3, 3, 3, 2, 0.15, 3),
+ ProcessorMotion("DIFF DRIVE", 3, 3, 3, 2, 3, 0.15),
unmeasured_perturbation_cov_(Matrix3s::Identity()*
params.unmeasured_perturbation_std_*
params.unmeasured_perturbation_std_),