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_capture_holder.cpp b/src/processor_capture_holder.cpp
index cc34d1d2f42a271f57d818fbfb0ade5cc8088109..398f829aa397888878ed7a0ca4fee73080740315 100644
--- a/src/processor_capture_holder.cpp
+++ b/src/processor_capture_holder.cpp
@@ -90,7 +90,7 @@ CaptureBasePtr ProcessorCaptureHolder::findCaptureContainingTimeStamp(const Time
// {
// // go to the previous motion capture
// if (capture_ptr == last_ptr_)
-// capture_ptr = std::static_pointer_cast<CaptureMotion>(origin_ptr_);
+// capture_ptr = origin_ptr_;
// else if (capture_ptr->getOriginFramePtr() == nullptr)
// return nullptr;
// else
diff --git a/src/processor_motion.cpp b/src/processor_motion.cpp
index cd0a2615486677178ad32b03c3e1152901a034a7..b9d6d3218072c8cf8aa23af26c54b5bd560c2480 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;
//
}
@@ -308,7 +307,7 @@ CaptureMotionPtr ProcessorMotion::findCaptureContainingTimeStamp(const TimeStamp
{
// go to the previous motion capture
if (capture_ptr == last_ptr_)
- capture_ptr = std::static_pointer_cast<CaptureMotion>(origin_ptr_);
+ capture_ptr = origin_ptr_;
else if (capture_ptr->getOriginFramePtr() == nullptr)
return nullptr;
else
diff --git a/src/processor_motion.h b/src/processor_motion.h
index 93d1af04a43ee77e02661331d1beb8031e2ed49a..2c0c776ae17fb4c4fefaf6426027e843f5ca4591 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:
@@ -407,7 +400,7 @@ class ProcessorMotion : public ProcessorBase
CaptureMotionPtr getCaptureMotionContainingTimeStamp(const TimeStamp& _ts);
public:
- virtual CaptureBasePtr getOriginPtr();
+ virtual CaptureMotionPtr getOriginPtr();
virtual CaptureMotionPtr getLastPtr();
virtual CaptureMotionPtr getIncomingPtr();
@@ -419,7 +412,7 @@ class ProcessorMotion : public ProcessorBase
Size delta_size_; ///< the size of the deltas
Size delta_cov_size_; ///< the size of the delta covariances matrix
Size calib_size_; ///< size of the extra parameters (TBD in derived classes)
- CaptureBasePtr origin_ptr_;
+ CaptureMotionPtr origin_ptr_;
CaptureMotionPtr last_ptr_;
CaptureMotionPtr incoming_ptr_;
@@ -541,7 +534,7 @@ inline Motion ProcessorMotion::motionZero(const TimeStamp& _ts)
);
}
-inline CaptureBasePtr ProcessorMotion::getOriginPtr()
+inline CaptureMotionPtr ProcessorMotion::getOriginPtr()
{
return origin_ptr_;
}
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_),
diff --git a/src/sensors/sensor_diff_drive.cpp b/src/sensors/sensor_diff_drive.cpp
index 52e4177c7c6896c7da6b421194ffbc8278eb8e00..0931b700ce658b486823fa930e3f1c454f87be5e 100644
--- a/src/sensors/sensor_diff_drive.cpp
+++ b/src/sensors/sensor_diff_drive.cpp
@@ -103,24 +103,22 @@ SensorBasePtr SensorDiffDrive::create(const std::string& _unique_name,
// problem->installSensor() return a SensorBasePtr.
//bool SensorDiffDrive::addCapture(CaptureBasePtr _capture_ptr)
//{
-// std::shared_ptr<CaptureMotion> capture_ptr = std::static_pointer_cast<CaptureMotion>(_capture_ptr);
-
// if (intrinsics_.data_is_position_)
// {
-// Eigen::Vector2s data = capture_ptr->getData();
+// Eigen::Vector2s data = _capture_ptr->getData();
// // dt is set to one as we are dealing with wheel position
// data = pose_inc_(data, intrinsics_.left_radius_, intrinsics_.right_radius_,
// intrinsics_.separation_, 1);
-// capture_ptr->setData(data);
+// _capture_ptr->setData(data);
// Eigen::Matrix2s data_cov;
// data_cov << 0.00001, 0, 0, 0.00001; // Todo
// computeDataCov(data, data_cov);
-// capture_ptr->setDataCovariance(data_cov);
+// _capture_ptr->setDataCovariance(data_cov);
// }
// /// @todo tofix
diff --git a/src/test/gtest_constraint_IMU.cpp b/src/test/gtest_constraint_IMU.cpp
index 3accb792b20b23f5f7d5e6b31fbf0a7dfd7fd662..4c4c85369af796d32bcb6d9f0d207957a7006905 100644
--- a/src/test/gtest_constraint_IMU.cpp
+++ b/src/test/gtest_constraint_IMU.cpp
@@ -979,9 +979,9 @@ class ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRot : public testing::Test
sensor_odo->process(capture_odo);
-// WOLF_TRACE("Jac calib: ", std::static_pointer_cast<CaptureMotion>(processor_imu->getOriginPtr())->getJacobianCalib().row(0));
+// WOLF_TRACE("Jac calib: ", processor_imu->getOriginPtr()->getJacobianCalib().row(0));
// WOLF_TRACE("orig calib: ", processor_imu->getOriginPtr()->getCalibration().transpose());
-// WOLF_TRACE("orig calib preint: ", std::static_pointer_cast<CaptureMotion>(processor_imu->getOriginPtr())->getCalibrationPreint().transpose());
+// WOLF_TRACE("orig calib preint: ", processor_imu->getOriginPtr()->getCalibrationPreint().transpose());
//prepare next odometry measurement
quat_odo = Eigen::Quaternions::Identity(); //set to identity to have next odom relative to this last KF
diff --git a/src/test/gtest_processor_IMU.cpp b/src/test/gtest_processor_IMU.cpp
index f8aac4c30823043472aa73614012bd55cedf4287..7d8e3e549fa7a6e7d105184a6799f24608c0c10b 100644
--- a/src/test/gtest_processor_IMU.cpp
+++ b/src/test/gtest_processor_IMU.cpp
@@ -437,7 +437,7 @@ TEST_F(ProcessorIMUt, gyro_x_biasedAbx)
// init things
problem->setPrior(x0, P0, t, 0.01);
- std::static_pointer_cast<wolf::CaptureMotion>(problem->getProcessorMotionPtr()->getOriginPtr())->setCalibration(bias);
+ problem->getProcessorMotionPtr()->getOriginPtr()->setCalibration(bias);
problem->getProcessorMotionPtr()->getLastPtr()->setCalibrationPreint(bias);
// WOLF_DEBUG("calib: ", cap_imu_ptr->getCalibration().transpose());
@@ -493,7 +493,7 @@ TEST_F(ProcessorIMUt, gyro_xy_biasedAbxy)
Vector6s bias; bias << abx,aby,0, 0,0,0;
Vector3s acc_bias = bias.head(3);
- std::static_pointer_cast<wolf::CaptureMotion>(problem->getProcessorMotionPtr()->getOriginPtr())->setCalibration(bias);
+ problem->getProcessorMotionPtr()->getOriginPtr()->setCalibration(bias);
problem->getProcessorMotionPtr()->getLastPtr()->setCalibrationPreint(bias);
wolf::Scalar rate_of_turn = 5 * M_PI/180.0;