diff --git a/src/examples/test_processor_odom_3D.cpp b/src/examples/test_processor_odom_3D.cpp
index e01e30d75289eed205bbdefc2195f78c8c042e9f..6cb5e458dd92f40abfb37d26aa601c0242c8f510 100644
--- a/src/examples/test_processor_odom_3D.cpp
+++ b/src/examples/test_processor_odom_3D.cpp
@@ -53,15 +53,19 @@ int main (int argc, char** argv)
CeresManager ceres_manager(problem, ceres_options);
SensorBasePtr sen = problem->installSensor("ODOM 3D", "odom", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/src/examples/sensor_odom_3D.yaml");
- problem->installProcessor("ODOM 3D", "odometry integrator", "odom");
- problem->getProcessorMotionPtr()->setOrigin((Vector7s()<<0,0,0,0,0,0,1).finished(), TimeStamp(0));
+ ProcessorParamsOdom3DPtr proc_params = std::make_shared<ProcessorParamsOdom3D>();
+ problem->installProcessor("ODOM 3D", "odometry integrator", sen, proc_params);
+ // Time and prior
+ Scalar dt = 0.1;
+
+ problem->setPrior((Vector7s()<<0,0,0,0,0,0,1).finished(), Matrix6s::Identity() * 1e-8, TimeStamp(0), dt/2);
+
+ // Motion data
Scalar dx = .1;
Scalar dyaw = 2*M_PI/5;
Vector6s data((Vector6s() << dx*cos(dyaw/2),dx*sin(dyaw/2),0,0,0,dyaw).finished()); // will integrate this data repeatedly
- Scalar dt = 0.1;
-
CaptureMotionPtr cap_odo = std::make_shared<CaptureMotion>(TimeStamp(0), sen, data, 7, 6, nullptr);
cout << "t: " << std::setprecision(2) << 0 << " \t x = ( " << problem->getCurrentState().transpose() << ")" << endl;
diff --git a/src/processor_GPS.cpp b/src/processor_GPS.cpp
index d66ba6e9b109060504987dfeb03cd30f77205102..74f20e48cc56598179f6d0c8af170fa7c97400a4 100644
--- a/src/processor_GPS.cpp
+++ b/src/processor_GPS.cpp
@@ -11,8 +11,8 @@
namespace wolf
{
-ProcessorGPS::ProcessorGPS() :
- ProcessorBase("GPS"),
+ProcessorGPS::ProcessorGPS(Scalar time_tolerance_) :
+ ProcessorBase("GPS", time_tolerance_),
capture_gps_ptr_(nullptr)
{
gps_covariance_ = 10;
diff --git a/src/processor_GPS.h b/src/processor_GPS.h
index d89ff86b4e5c6310cdf21daa5551e8dfe0b49507..7a72dbadedd9290300758aef7dedc17ccefbce98 100644
--- a/src/processor_GPS.h
+++ b/src/processor_GPS.h
@@ -25,7 +25,7 @@ class ProcessorGPS : public ProcessorBase
public:
ProcessorGPS();
- virtual ~ProcessorGPS();
+ virtual ~ProcessorGPS(Scalar time_tolerance_);
virtual void init(CaptureBasePtr _capture_ptr);
virtual void process(CaptureBasePtr _capture_ptr);
virtual bool voteForKeyFrame();
diff --git a/src/processor_IMU.cpp b/src/processor_IMU.cpp
index 06b1522286268331335fdc8bbdedcf3eda87e4d8..bb0ae86bffcb26e3b9d913bb57e65c23b6539231 100644
--- a/src/processor_IMU.cpp
+++ b/src/processor_IMU.cpp
@@ -4,7 +4,7 @@
namespace wolf {
ProcessorIMU::ProcessorIMU(const ProcessorParamsIMU& _params) :
- ProcessorMotion("IMU", 10, 10, 9, 6, 6, _params.time_tolerance),
+ ProcessorMotion("IMU", _params.time_tolerance, 10, 10, 9, 6, 6),
max_time_span_ (_params.max_time_span ),
max_buff_length_(_params.max_buff_length ),
dist_traveled_ (_params.dist_traveled ),
diff --git a/src/processor_motion.cpp b/src/processor_motion.cpp
index a8af1d1a594bd710f4f94c6b4e581bb7947a5057..3fa2234ad2722e14db221471cb6dd9a2565ab3c2 100644
--- a/src/processor_motion.cpp
+++ b/src/processor_motion.cpp
@@ -3,12 +3,12 @@ namespace wolf
{
ProcessorMotion::ProcessorMotion(const std::string& _type,
+ Scalar _time_tolerance,
Size _state_size,
Size _delta_size,
Size _delta_cov_size,
Size _data_size,
- Size _calib_size,
- Scalar _time_tolerance) :
+ Size _calib_size) :
ProcessorBase(_type, _time_tolerance),
processing_step_(RUNNING_WITHOUT_PACK),
x_size_(_state_size),
diff --git a/src/processor_motion.h b/src/processor_motion.h
index ea1421aa9e0ad7b6be65fb8d6df5f2b6663dc9a9..11e299f3089bcccc40ecd3793a6c16a6f7eaaced 100644
--- a/src/processor_motion.h
+++ b/src/processor_motion.h
@@ -113,12 +113,12 @@ class ProcessorMotion : public ProcessorBase
// This is the main public interface
public:
ProcessorMotion(const std::string& _type,
+ Scalar _time_tolerance,
Size _state_size,
Size _delta_size,
Size _delta_cov_size,
Size _data_size,
- Size _calib_size = 0,
- Scalar _time_tolerance = 0.1);
+ Size _calib_size = 0);
virtual ~ProcessorMotion();
// Instructions to the processor:
diff --git a/src/processor_odom_2D.cpp b/src/processor_odom_2D.cpp
index 38c37f5c15c42cb38fe978e835b949d910c7aaaf..5ebc1bffafb2937cfb3e017e57df9af70274e4e5 100644
--- a/src/processor_odom_2D.cpp
+++ b/src/processor_odom_2D.cpp
@@ -3,7 +3,7 @@ namespace wolf
{
ProcessorOdom2D::ProcessorOdom2D(const ProcessorParamsOdom2D& _params) :
- ProcessorMotion("ODOM 2D", 3, 3, 3, 2, 0, _params.time_tolerance),
+ ProcessorMotion("ODOM 2D", _params.time_tolerance, 3, 3, 3, 2, 0),
dist_traveled_th_(_params.dist_traveled_th_),
theta_traveled_th_(_params.theta_traveled_th_),
cov_det_th_(_params.cov_det_th_),
diff --git a/src/processor_odom_3D.cpp b/src/processor_odom_3D.cpp
index b5e619163571bdf11ee888d8631ad5ef62826b18..8165797715f213d27459eff3ad86944beaff495e 100644
--- a/src/processor_odom_3D.cpp
+++ b/src/processor_odom_3D.cpp
@@ -3,7 +3,7 @@ namespace wolf
{
ProcessorOdom3D::ProcessorOdom3D(const ProcessorParamsOdom3D& _params, SensorOdom3DPtr _sensor_ptr) :
- ProcessorMotion("ODOM 3D", 7, 7, 6, 6, 0, _params.time_tolerance ),
+ ProcessorMotion("ODOM 3D", _params.time_tolerance, 7, 7, 6, 6, 0 ),
max_time_span_ ( _params.max_time_span ),
max_buff_length_( _params.max_buff_length ),
dist_traveled_ ( _params.dist_traveled ),