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_),