diff --git a/include/IMU/processor/processor_IMU.h b/include/IMU/processor/processor_IMU.h
index 42099045146461b1ca9145dd5e3b450b75916f96..59f87aa68961348e0033797483e07ab6709a5a84 100644
--- a/include/IMU/processor/processor_IMU.h
+++ b/include/IMU/processor/processor_IMU.h
@@ -61,9 +61,6 @@ class ProcessorIMU : public ProcessorMotion{
const Scalar _dt,
Eigen::VectorXs& _x_plus_delta ) override;
virtual Eigen::VectorXs deltaZero() const override;
- virtual Motion interpolate(const Motion& _motion_ref,
- Motion& _motion,
- TimeStamp& _ts) override;
virtual bool voteForKeyFrame() override;
virtual CaptureMotionPtr emplaceCapture(const FrameBasePtr& _frame_own,
const SensorBasePtr& _sensor,
diff --git a/src/processor/processor_IMU.cpp b/src/processor/processor_IMU.cpp
index fb2803d725dd1c57328cd81f26759fabb68bfba9..2d83320e714ff9c8d37768cb5946941de4807776 100644
--- a/src/processor/processor_IMU.cpp
+++ b/src/processor/processor_IMU.cpp
@@ -57,131 +57,6 @@ bool ProcessorIMU::voteForKeyFrame()
return false;
}
-Motion ProcessorIMU::interpolate(const Motion& _motion_ref, Motion& _motion_second, TimeStamp& _ts)
-{
- /* Note: See extensive documentation in ProcessorMotion::interpolate().
- *
- * Interpolate between motion_ref and motion, as in:
- *
- * motion_ref ------ ts_ ------ motion_sec
- * return
- *
- * and return the motion at the given time_stamp ts_.
- *
- * DATA:
- * Data receives no change
- *
- * DELTA:
- * The delta's position and velocity receive linear interpolation:
- * p_ret = (ts - t_ref) / dt * (p - p_ref)
- * v_ret = (ts - t_ref) / dt * (v - v_ref)
- *
- * The delta's quaternion receives a slerp interpolation
- * q_ret = q_ref.slerp( (ts - t_ref) / dt , q);
- *
- * DELTA_INTEGR:
- * The interpolated delta integral is just the reference integral plus the interpolated delta
- *
- * The second integral does not change
- *
- * Covariances receive linear interpolation
- * Q_ret = (ts - t_ref) / dt * Q_sec
- */
- /*
- // resolve out-of-bounds time stamp as if the time stamp was exactly on the bounds
- if (_ts <= _motion_ref.ts_) // behave as if _ts == _motion_ref.ts_
- {
- // return null motion. Second stays the same.
- Motion motion_int ( _motion_ref );
- motion_int.data_ = _motion_second.data_;
- motion_int.data_cov_ = _motion_second.data_cov_;
- motion_int.delta_ = deltaZero();
- motion_int.delta_cov_ . setZero();
- return motion_int;
- }
- if (_ts >= _motion_second.ts_) // behave as if _ts == _motion_second.ts_
- {
- // return original second motion. Second motion becomes null motion
- Motion motion_int ( _motion_second );
- _motion_second.delta_ = deltaZero();
- _motion_second.delta_cov_ . setZero();
- return motion_int;
- }
- assert(_motion_ref.ts_ <= _ts && "Interpolation time stamp out of bounds");
- assert(_ts <= _motion_second.ts_ && "Interpolation time stamp out of bounds");
-
- assert(_motion_ref.delta_.size() == delta_size_ && "Wrong delta size");
- assert(_motion_ref.delta_cov_.cols() == delta_cov_size_ && "Wrong delta cov size");
- assert(_motion_ref.delta_cov_.rows() == delta_cov_size_ && "Wrong delta cov size");
- assert(_motion_ref.delta_integr_.size() == delta_size_ && "Wrong delta size");
- assert(_motion_ref.delta_integr_cov_.cols() == delta_cov_size_ && "Wrong delta cov size");
- assert(_motion_ref.delta_integr_cov_.rows() == delta_cov_size_ && "Wrong delta cov size");
-
- assert(_motion_second.delta_.size() == delta_size_ && "Wrong delta size");
- assert(_motion_second.delta_cov_.cols() == delta_cov_size_ && "Wrong delta cov size");
- assert(_motion_second.delta_cov_.rows() == delta_cov_size_ && "Wrong delta cov size");
- assert(_motion_second.delta_integr_.size() == delta_size_ && "Wrong delta size");
- assert(_motion_second.delta_integr_cov_.cols() == delta_cov_size_ && "Wrong delta cov size");
- assert(_motion_second.delta_integr_cov_.rows() == delta_cov_size_ && "Wrong delta cov size");
-
- // reference
- TimeStamp t_ref = _motion_ref.ts_;
-
- // second
- TimeStamp t_sec = _motion_second.ts_;
- Map<VectorXs> motion_sec_dp (_motion_second.delta_.data() + 0, 3);
- Map<Quaternions> motion_sec_dq (_motion_second.delta_.data() + 3 );
- Map<VectorXs> motion_sec_dv (_motion_second.delta_.data() + 7, 3);
-
- // interpolated
- Motion motion_int = motionZero(_ts);
-
- // Jacobians for covariance propagation
- MatrixXs J_ref(delta_cov_size_, delta_cov_size_);
- MatrixXs J_int(delta_cov_size_, delta_cov_size_);
-
- // interpolation factor
- Scalar dt1 = _ts - t_ref;
- Scalar dt2 = t_sec - _ts;
- Scalar tau = dt1 / (t_sec - t_ref); // interpolation factor (0 to 1)
- Scalar tau_sq = tau * tau;
-
- // copy data
- motion_int.data_ = _motion_second.data_;
- motion_int.data_cov_ = _motion_second.data_cov_;
-
- // interpolate delta
- motion_int.ts_ = _ts;
- Map<VectorXs> motion_int_dp (motion_int.delta_.data() + 0, 3);
- Map<Quaternions> motion_int_dq (motion_int.delta_.data() + 3 );
- Map<VectorXs> motion_int_dv (motion_int.delta_.data() + 7, 3);
- motion_int_dp = tau_sq * motion_sec_dp; // FIXME: delta_p not correctly interpolated
- motion_int_dv = tau * motion_sec_dv;
- motion_int_dq = Quaternions::Identity().slerp(tau, motion_sec_dq);
- motion_int.delta_cov_ = tau * _motion_second.delta_cov_;
-
- // integrate
- deltaPlusDelta(_motion_ref.delta_integr_, motion_int.delta_, dt1, motion_int.delta_integr_, J_ref, J_int);
- motion_int.delta_integr_cov_ = J_ref * _motion_ref.delta_integr_cov_ * J_ref.transpose()
- + J_int * _motion_second.delta_cov_ * J_int.transpose();
-
- // update second delta ( in place update )
- motion_sec_dp = motion_int_dq.conjugate() * (motion_sec_dp - motion_int_dp - motion_int_dv * dt2);
- motion_sec_dv = motion_int_dq.conjugate() * (motion_sec_dv - motion_int_dv);
- motion_sec_dq = motion_int_dq.conjugate() * motion_sec_dq;
- _motion_second.delta_cov_ *= (1 - tau); // easy interpolation // TODO check for correctness
- //Dp = Dp; // trivial, just leave the code commented
- //Dq = Dq; // trivial, just leave the code commented
- //_motion.delta_integr_cov_ = _motion.delta_integr_cov_; // trivial, just leave the code commented
-
- return motion_int;
- */
-
- // return _motion_ref;
-
- return ProcessorMotion::interpolate(_motion_ref, _motion_second, _ts);
-
-}
CaptureMotionPtr ProcessorIMU::emplaceCapture(const FrameBasePtr& _frame_own,
const SensorBasePtr& _sensor,
diff --git a/test/gtest_processor_IMU.cpp b/test/gtest_processor_IMU.cpp
index 9abf094e61b6a9d31819c27c100fe2b179214ab5..9c51e9c2b327f4af0b86210525ab01f27eee4005 100644
--- a/test/gtest_processor_IMU.cpp
+++ b/test/gtest_processor_IMU.cpp
@@ -201,54 +201,6 @@ TEST(ProcessorIMU, voteForKeyFrame)
}
-//replace TEST by TEST_F if SetUp() needed
-TEST_F(ProcessorIMUt, interpolate)
-{
- using namespace wolf;
-
- t.set(0);
- x0 << 0,0,0, 0,0,0,1, 0,0,0;
- MatrixXs P0(9,9); P0.setIdentity();
- problem->setPrior(x0, P0, t, 0.01);
-
- data << 2, 0, 0, 0, 0, 0; // only acc_x
- cap_imu_ptr->setData(data);
-
- // make one step to depart from origin
- cap_imu_ptr->setTimeStamp(0.05);
- sensor_ptr->process(cap_imu_ptr);
- Motion mot_ref = problem->getProcessorMotion()->getMotion();
-
- // make two steps, then pretend it's just one
- cap_imu_ptr->setTimeStamp(0.10);
- sensor_ptr->process(cap_imu_ptr);
- Motion mot_int_gt = problem->getProcessorMotion()->getMotion();
-
- cap_imu_ptr->setTimeStamp(0.15);
- sensor_ptr->process(cap_imu_ptr);
- Motion mot_final = problem->getProcessorMotion()->getMotion();
- mot_final.delta_ = mot_final.delta_integr_;
- Motion mot_sec = mot_final;
-
-// problem->getProcessorMotion()->getBuffer().print(0,1,1,0);
-
-class P : public wolf::ProcessorIMU
-{
- public:
- P() : ProcessorIMU(std::make_shared<ProcessorParamsIMU>()) {}
- Motion interp(const Motion& ref, Motion& sec, TimeStamp ts)
- {
- return ProcessorIMU::interpolate(ref, sec, ts);
- }
-} imu;
-
-Motion mot_int = imu.interp(mot_ref, mot_sec, TimeStamp(0.10));
-
-ASSERT_MATRIX_APPROX(mot_int.data_, mot_int_gt.data_, 1e-6);
-//ASSERT_MATRIX_APPROX(mot_int.delta_, mot_int_gt.delta_, 1e-6); // FIXME: delta_p not correctly interpolated
-ASSERT_MATRIX_APPROX(mot_final.delta_integr_, mot_sec.delta_integr_, 1e-6);
-
-}
TEST_F(ProcessorIMUt, acc_x)
{