diff --git a/include/core/processor/processor_motion.h b/include/core/processor/processor_motion.h
index 81df227434c641cfa120855a242a98b918805e50..2eb19c69cd77ebb4b249dad0cda83d9a388c450e 100644
--- a/include/core/processor/processor_motion.h
+++ b/include/core/processor/processor_motion.h
@@ -402,39 +402,6 @@ class ProcessorMotion : public ProcessorBase
*/
virtual Eigen::VectorXs deltaZero() const = 0;
-// /** \brief Interpolate motion to an intermediate time-stamp
-// *
-// * @param _ref The motion reference
-// * @param _second The second motion. It is modified by the function (see documentation below).
-// * @param _ts The intermediate time stamp: it must be bounded by `_ref.ts_ <= _ts <= _second.ts_`.
-// * @return The interpolated motion (see documentation below).
-// *
-// * This function interpolates a motion between two existing motions.
-// *
-// * In this base implementation, we just provide the closest motion provided (ref or second),
-// * so really no interpolation takes place and just the current data and delta are updated.
-// *
-// * Should you require finer interpolation, you must overload this method in your derived class.
-// */
-// virtual Motion interpolate(const Motion& _ref, Motion& _second, TimeStamp& _ts);
-
-// /** \brief Interpolate motion to an intermediate time-stamp
-// *
-// * @param _ref1 The first motion reference
-// * @param _ref2 The second motion reference.
-// * @param _ts The intermediate time stamp: it must be bounded by `_ref.ts_ <= _ts <= _second.ts_`.
-// * @param _second The second part motion after interpolation, so that return (+) second = ref2.
-// * @return The interpolated motion (see documentation below).
-// *
-// * This function interpolates a motion between two existing motions.
-// *
-// * In this base implementation, we just provide the closest motion provided (ref1 or ref2),
-// * the second motion being the complementary part,
-// * so really no interpolation takes place and just the current data and delta are updated.
-// *
-// * Should you require finer interpolation, you must overload this method in your derived class.
-// */
-// virtual Motion interpolate(const Motion& _ref1, const Motion& _ref2, const TimeStamp& _ts, Motion& _second);
/** \brief emplace a CaptureMotion
* \param _ts time stamp
diff --git a/include/core/processor/processor_odom_2D.h b/include/core/processor/processor_odom_2D.h
index 33833bbee6ac5240295c50fabcde3746fba43845..8fca11b3e5913e683a9cfde766ad1e072ef5c32e 100644
--- a/include/core/processor/processor_odom_2D.h
+++ b/include/core/processor/processor_odom_2D.h
@@ -72,10 +72,6 @@ class ProcessorOdom2D : public ProcessorMotion
const Scalar _Dt,
Eigen::VectorXs& _x_plus_delta) override;
virtual Eigen::VectorXs deltaZero() const override;
-// virtual Motion interpolate(const Motion& _ref1,
-// const Motion& _ref2,
-// const TimeStamp& _ts,
-// Motion& _second) override;
virtual CaptureMotionPtr emplaceCapture(const FrameBasePtr& _frame_own,
const SensorBasePtr& _sensor,
diff --git a/include/core/processor/processor_odom_3D.h b/include/core/processor/processor_odom_3D.h
index 36125c4ea6b2ed64c3fffaf2baf6f4b4c832ab32..bd1e3cf013135ac90bfcb9dec749b9b64a63ce07 100644
--- a/include/core/processor/processor_odom_3D.h
+++ b/include/core/processor/processor_odom_3D.h
@@ -91,13 +91,6 @@ class ProcessorOdom3D : public ProcessorMotion
const Scalar _Dt,
Eigen::VectorXs& _x_plus_delta) override;
Eigen::VectorXs deltaZero() const override;
-// Motion interpolate(const Motion& _motion_ref,
-// Motion& _motion,
-// TimeStamp& _ts) override;
-// virtual Motion interpolate(const Motion& _ref1,
-// const Motion& _ref2,
-// const TimeStamp& _ts,
-// Motion& _second) override;
bool voteForKeyFrame() override;
virtual CaptureMotionPtr emplaceCapture(const FrameBasePtr& _frame_own,
const SensorBasePtr& _sensor,
diff --git a/src/processor/processor_motion.cpp b/src/processor/processor_motion.cpp
index 5d2c9032e962c2268f0adae9cb314537477779de..633a98022487b5f54cfc93d27e2ac3018ecaea6d 100644
--- a/src/processor/processor_motion.cpp
+++ b/src/processor/processor_motion.cpp
@@ -455,139 +455,6 @@ void ProcessorMotion::reintegrateBuffer(CaptureMotionPtr _capture_ptr)
}
}
-//Motion ProcessorMotion::interpolate(const Motion& _ref, Motion& _second, TimeStamp& _ts)
-//{
-// // Check time bounds
-// assert(_ref.ts_ <= _second.ts_ && "Interpolation bounds not causal.");
-// assert(_ts >= _ref.ts_ && "Interpolation time is before the _ref motion.");
-// assert(_ts <= _second.ts_ && "Interpolation time is after the _second motion.");
-//
-// // Fraction of the time interval
-// Scalar tau = (_ts - _ref.ts_) / (_second.ts_ - _ref.ts_);
-//
-// if (tau < 0.5)
-// {
-// // _ts is closest to _ref
-// Motion interpolated ( _ref );
-// interpolated.ts_ = _ts;
-// interpolated.data_ . setZero();
-// interpolated.data_cov_ . setZero();
-// interpolated.delta_ = deltaZero();
-// interpolated.delta_cov_ . setZero();
-// interpolated.jacobian_delta_integr_ . setIdentity();
-// interpolated.jacobian_delta_ . setZero();
-//
-// return interpolated;
-// }
-// else
-// {
-// // _ts is closest to _second
-// Motion interpolated ( _second );
-// interpolated.ts_ = _ts;
-// _second.data_ . setZero();
-// _second.data_cov_ . setZero();
-// _second.delta_ = deltaZero();
-// _second.delta_cov_ . setZero();
-// _second.jacobian_delta_integr_ . setIdentity();
-// _second.jacobian_delta_ . setZero();
-//
-// return interpolated;
-// }
-//
-//}
-//
-//Motion ProcessorMotion::interpolate(const Motion& _ref1, const Motion& _ref2, const TimeStamp& _ts, Motion& _second)
-//{
-// // Check time bounds
-// assert(_ref1.ts_ <= _ref2.ts_ && "Interpolation bounds not causal.");
-// assert(_ts >= _ref1.ts_ && "Interpolation time is before the _ref1 motion.");
-// assert(_ts <= _ref2.ts_ && "Interpolation time is after the _ref2 motion.");
-//
-// // Fraction of the time interval
-// Scalar tau = (_ts - _ref1.ts_) / (_ref2.ts_ - _ref1.ts_);
-//
-//
-//
-//
-// Motion interpolated(_ref1);
-// interpolated.ts_ = _ts;
-// interpolated.data_ = (1-tau)*_ref1.data_ + tau*_ref2.data_;
-// interpolated.data_cov_ = (1-tau)*_ref1.data_cov_ + tau*_ref2.data_cov_; // bof
-// computeCurrentDelta(interpolated.data_,
-// interpolated.data_cov_,
-// calib_preint_,
-// _ts.get() - _ref1.ts_.get(),
-// interpolated.delta_,
-// interpolated.delta_cov_,
-// interpolated.jacobian_calib_);
-// deltaPlusDelta(_ref1.delta_integr_,
-// interpolated.delta_,
-// _ts.get() - _ref1.ts_.get(),
-// interpolated.delta_integr_,
-// interpolated.jacobian_delta_integr_,
-// interpolated.jacobian_delta_);
-//
-// _second = _ref2;
-// _second.data_ = tau*_ref1.data_ + (1-tau)*_ref2.data_;
-// _second.data_cov_ = tau*_ref1.data_cov_ + (1-tau)*_ref2.data_cov_; // bof
-// computeCurrentDelta(_second.data_,
-// _second.data_cov_,
-// calib_preint_,
-// _ref2.ts_.get() - _ts.get(),
-// _second.delta_,
-// _second.delta_cov_,
-// _second.jacobian_calib_);
-//
-//// deltaPlusDelta(_second.delta_integr_,
-//// _second.delta_,
-//// _second.ts_.get() - _ref1.ts_.get(),
-//// _second.delta_integr_,
-//// _second.jacobian_delta_integr_,
-//// _second.jacobian_delta_);
-//
-// return interpolated;
-//
-//
-//
-//
-// // if (tau < 0.5)
-// // {
-// // // _ts is closest to _ref1
-// // Motion interpolated ( _ref1 );
-// // // interpolated.ts_ = _ref1.ts_;
-// // // interpolated.data_ = _ref1.data_;
-// // // interpolated.data_cov_ = _ref1.data_cov_;
-// // interpolated.delta_ = deltaZero();
-// // interpolated.delta_cov_ . setZero();
-// // // interpolated.delta_integr_ = _ref1.delta_integr_;
-// // // interpolated.delta_integr_cov_ = _ref1.delta_integr_cov_;
-// // interpolated.jacobian_delta_integr_ . setIdentity();
-// // interpolated.jacobian_delta_ . setZero();
-//
-// // _second = _ref2;
-//
-// // return interpolated;
-// // }
-// // else
-// // {
-// // // _ts is closest to _ref2
-// // Motion interpolated ( _ref2 );
-//
-// // _second = _ref2;
-// // // _second.data_ = _ref2.data_;
-// // // _second.data_cov_ = _ref2.data_cov_;
-// // _second.delta_ = deltaZero();
-// // _second.delta_cov_ . setZero();
-// // // _second.delta_integr_ = _ref2.delta_integr_;
-// // // _second.delta_integr_cov_ = _ref2.delta_integr_cov_;
-// // _second.jacobian_delta_integr_ . setIdentity();
-// // _second.jacobian_delta_ . setZero();
-//
-// // return interpolated;
-// // }
-//
-//}
-
CaptureMotionPtr ProcessorMotion::findCaptureContainingTimeStamp(const TimeStamp& _ts) const
{
// We need to search in previous keyframes for the capture containing a motion buffer with the queried time stamp
diff --git a/src/processor/processor_odom_2D.cpp b/src/processor/processor_odom_2D.cpp
index 6c2a3f4192ac41cf0543b1d1a9b952351e78c572..dda3af4ddaa964537b95d85eac1a8cccf1fb7167 100644
--- a/src/processor/processor_odom_2D.cpp
+++ b/src/processor/processor_odom_2D.cpp
@@ -92,11 +92,6 @@ void ProcessorOdom2D::statePlusDelta(const Eigen::VectorXs& _x, const Eigen::Vec
_x_plus_delta(2) = pi2pi(_x(2) + _delta(2));
}
-//Motion ProcessorOdom2D::interpolate(const Motion& _ref1, const Motion& _ref2, const TimeStamp& _ts, Motion& _second)
-//{
-// return ProcessorMotion::interpolate(_ref1, _ref2, _ts, _second);
-//}
-
bool ProcessorOdom2D::voteForKeyFrame()
{
// Distance criterion
diff --git a/src/processor/processor_odom_3D.cpp b/src/processor/processor_odom_3D.cpp
index 7a481e995c244180e586a52a08a84cea2fd767d5..6c20d3e167f66449ccab2f1e041febaacce4ce5e 100644
--- a/src/processor/processor_odom_3D.cpp
+++ b/src/processor/processor_odom_3D.cpp
@@ -147,191 +147,6 @@ void ProcessorOdom3D::statePlusDelta(const Eigen::VectorXs& _x, const Eigen::Vec
q_out_ = q1_ * q2_;
}
-//Motion ProcessorOdom3D::interpolate(const Motion& _motion_ref,
-// Motion& _motion_second,
-// TimeStamp& _ts)
-//{
-//
-//// WOLF_TRACE("motion ref ts: ", _motion_ref.ts_.get());
-//// WOLF_TRACE("interp ts : ", _ts.get());
-//// WOLF_TRACE("motion ts : ", _motion_second.ts_.get());
-////
-//// WOLF_TRACE("ref delta size: ", _motion_ref.delta_.size(), " , cov size: ", _motion_ref.delta_cov_.size());
-//// WOLF_TRACE("ref Delta size: ", _motion_ref.delta_integr_.size(), " , cov size: ", _motion_ref.delta_integr_cov_.size());
-//// WOLF_TRACE("sec delta size: ", _motion_second.delta_.size(), " , cov size: ", _motion_second.delta_cov_.size());
-//// WOLF_TRACE("sec Delta size: ", _motion_second.delta_integr_.size(), " , cov size: ", _motion_second.delta_integr_cov_.size());
-//
-// // 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.delta_ = deltaZero();
-// motion_int.delta_cov_.setZero();
-// return motion_int;
-// }
-// if (_motion_second.ts_ <= _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");
-//
-// using namespace Eigen;
-// // Interpolate between motion_ref and motion, as in:
-// //
-// // motion_ref ------ ts_ ------ motion
-// // return
-// //
-// // and return the value at the given time_stamp ts_.
-// //
-// // The position receives linear interpolation:
-// // p_ret = (ts - t_ref) / dt * (p - p_ref)
-// //
-// // the quaternion receives a slerp interpolation
-// // q_ret = q_ref.slerp( (ts - t_ref) / dt , q);
-// //
-// // See extensive documentation in ProcessorMotion::interpolate().
-//
-// // reference
-// TimeStamp t_ref = _motion_ref.ts_;
-//
-// // final
-// TimeStamp t_sec = _motion_second.ts_;
-// Map<VectorXs> dp_sec(_motion_second.delta_.data(), 3);
-// Map<Quaternions> dq_sec(_motion_second.delta_.data() + 3);
-//
-// // interpolated
-// Motion motion_int = motionZero(_ts);
-// Map<VectorXs> dp_int(motion_int.delta_.data(), 3);
-// Map<Quaternions> dq_int(motion_int.delta_.data() + 3);
-//
-// // Jacobians for covariance propagation
-// MatrixXs J_ref(delta_cov_size_, delta_cov_size_);
-// MatrixXs J_int(delta_cov_size_, delta_cov_size_);
-//
-// // interpolate delta
-// Scalar tau = (_ts - t_ref) / (t_sec - t_ref); // interpolation factor (0 to 1)
-// motion_int.ts_ = _ts;
-// dp_int = tau * dp_sec;
-// dq_int = Quaternions::Identity().slerp(tau, dq_sec);
-// deltaPlusDelta(_motion_ref.delta_integr_, motion_int.delta_, (t_sec - t_ref), motion_int.delta_integr_, J_ref, J_int);
-//
-// // interpolate covariances
-// motion_int.delta_cov_ = tau * _motion_second.delta_cov_;
-//// 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 )
-// dp_sec = dq_int.conjugate() * ((1 - tau) * dp_sec);
-// dq_sec = dq_int.conjugate() * dq_sec;
-// _motion_second.delta_cov_ = (1 - tau) * _motion_second.delta_cov_; // 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;
-//}
-
-//Motion ProcessorOdom3D::interpolate(const Motion& _ref1,
-// const Motion& _ref2,
-// const TimeStamp& _ts,
-// Motion& _second)
-//{
-// // resolve out-of-bounds time stamp as if the time stamp was exactly on the bounds
-// if (_ts <= _ref1.ts_ || _ref2.ts_ <= _ts)
-// return ProcessorMotion::interpolate(_ref1, _ref2, _ts, _second);
-//
-// assert(_ref1.ts_ <= _ts && "Interpolation time stamp out of bounds");
-// assert(_ts <= _ref2.ts_ && "Interpolation time stamp out of bounds");
-//
-// assert(_ref1.delta_.size() == delta_size_ && "Wrong delta size");
-// assert(_ref1.delta_cov_.cols() == delta_cov_size_ && "Wrong delta cov size");
-// assert(_ref1.delta_cov_.rows() == delta_cov_size_ && "Wrong delta cov size");
-// assert(_ref1.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(_ref2.delta_.size() == delta_size_ && "Wrong delta size");
-// assert(_ref2.delta_cov_.cols() == delta_cov_size_ && "Wrong delta cov size");
-// assert(_ref2.delta_cov_.rows() == delta_cov_size_ && "Wrong delta cov size");
-// assert(_ref2.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");
-//
-//
-// using namespace Eigen;
-// // Interpolate between ref1 and ref2, as in:
-// //
-// // ref1 ------ ts ------ ref2
-// // return second
-// //
-// // and return the value at the given time_stamp ts_, and the second motion.
-// //
-// // The position receives linear interpolation:
-// // p_ret = (ts - t_ref) / dt * (p - p_ref)
-// //
-// // the quaternion receives a slerp interpolation
-// // q_ret = q_ref.slerp( (ts - t_ref) / dt , q);
-// //
-// // See extensive documentation in ProcessorMotion::interpolate().
-//
-// // reference
-// TimeStamp t1 = _ref1.ts_;
-//
-// // final
-// TimeStamp t2 = _ref2.ts_;
-// Map<const VectorXs> dp2(_ref2.delta_.data(), 3);
-// Map<const Quaternions> dq2(_ref2.delta_.data() + 3);
-//
-// // interpolated
-// Motion motion_int = motionZero(_ts);
-// Map<VectorXs> dp_int(motion_int.delta_.data(), 3);
-// Map<Quaternions> dq_int(motion_int.delta_.data() + 3);
-//
-// // Jacobians for covariance propagation
-// MatrixXs J_ref(delta_cov_size_, delta_cov_size_);
-// MatrixXs J_int(delta_cov_size_, delta_cov_size_);
-//
-// // interpolate delta
-// Scalar tau = (_ts - t1) / (t2 - t1); // interpolation factor (0 to 1)
-// motion_int.ts_ = _ts;
-// dp_int = tau * dp2;
-// dq_int = Quaternions::Identity().slerp(tau, dq2);
-// deltaPlusDelta(_ref1.delta_integr_, motion_int.delta_, (t2 - t1), motion_int.delta_integr_, J_ref, J_int);
-//
-// // interpolate covariances
-// motion_int.delta_cov_ = tau * _ref2.delta_cov_;
-// // 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 )
-// _second = _ref2;
-// Map<VectorXs> dp_sec(_second.delta_.data(), 3);
-// Map<Quaternions> dq_sec(_second.delta_.data() + 3);
-// dp_sec = dq_int.conjugate() * ((1 - tau) * dp2);
-// dq_sec = dq_int.conjugate() * dq2;
-// _second.delta_cov_ = (1 - tau) * _ref2.delta_cov_; // 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;
-//}
-
bool ProcessorOdom3D::voteForKeyFrame()
diff --git a/test/gtest_processor_motion.cpp b/test/gtest_processor_motion.cpp
index e92eaedbdba94dd659400ee27f566fce493767db..93bf18911e2daadcabdd5ad7df66a9161fdb679c 100644
--- a/test/gtest_processor_motion.cpp
+++ b/test/gtest_processor_motion.cpp
@@ -78,18 +78,6 @@ class ProcessorMotion_test : public testing::Test{
problem->setPrior(x0, P0, 0.0, 0.01);
}
-
-// Motion interpolate(const Motion& _ref, Motion& _second, TimeStamp& _ts)
-// {
-// return ProcessorMotion::interpolate(_ref, _second, _ts);
-// }
-//
-// Motion interpolate(const Motion& _ref1, const Motion& _ref2, const TimeStamp& _ts, Motion& _second)
-// {
-// return ProcessorMotion::interpolate(_ref1, _ref2, _ts, _second);
-// }
-
-
virtual void TearDown(){}
};
@@ -173,92 +161,6 @@ TEST_F(ProcessorMotion_test, splitBuffer)
C_source->getBuffer().print(1,1,1,0);
}
-//TEST_F(ProcessorMotion_test, Interpolate)
-//{
-// data << 1, 2*M_PI/10; // advance in turn
-// data_cov.setIdentity();
-// TimeStamp t(0.0);
-// std::vector<Motion> motions;
-// motions.push_back(motionZero(t));
-//
-// for (int i = 0; i<10; i++) // one full turn exactly
-// {
-// t += dt;
-// capture->setTimeStamp(t);
-// capture->setData(data);
-// capture->setDataCovariance(data_cov);
-// processor->captureCallback(capture);
-// motions.push_back(processor->getMotion(t));
-// WOLF_DEBUG("t: ", t, " x: ", problem->getCurrentState().transpose());
-// }
-//
-// TimeStamp tt = 2.2;
-// Motion ref = motions[2];
-// Motion second = motions[3];
-// Motion interp = interpolate(ref, second, tt);
-//
-// ASSERT_NEAR( interp.ts_.get() , 2.2 , 1e-8);
-// ASSERT_MATRIX_APPROX(interp.data_ , VectorXs::Zero(2) , 1e-8);
-// ASSERT_MATRIX_APPROX(interp.delta_ , VectorXs::Zero(3) , 1e-8);
-// ASSERT_MATRIX_APPROX(interp.delta_integr_ , motions[2].delta_integr_ , 1e-8);
-//
-// tt = 2.6;
-// interp = interpolate(ref, second, tt);
-//
-// ASSERT_NEAR( interp.ts_.get() , 2.6 , 1e-8);
-// ASSERT_MATRIX_APPROX(interp.data_ , motions[3].data_ , 1e-8);
-// ASSERT_MATRIX_APPROX(interp.delta_ , motions[3].delta_ , 1e-8);
-// ASSERT_MATRIX_APPROX(interp.delta_integr_ , motions[3].delta_integr_ , 1e-8);
-//}
-//
-//TEST_F(ProcessorMotion_test, Interpolate_alternative)
-//{
-// data << 1, 2*M_PI/10; // advance in turn
-// data_cov.setIdentity();
-// TimeStamp t(0.0);
-// std::vector<Motion> motions;
-// motions.push_back(motionZero(t));
-//
-// for (int i = 0; i<10; i++) // one full turn exactly
-// {
-// t += dt;
-// capture->setTimeStamp(t);
-// capture->setData(data);
-// capture->setDataCovariance(data_cov);
-// capture->process();
-// motions.push_back(processor->getMotion(t));
-// WOLF_DEBUG("t: ", t, " x: ", problem->getCurrentState().transpose());
-// }
-//
-// TimeStamp tt = 2.2;
-// Motion ref1 = motions[2];
-// Motion ref2 = motions[3];
-// Motion second(0.0, 2, 3, 3, 0);
-// Motion interp = interpolate(ref1, ref2, tt, second);
-//
-// ASSERT_NEAR( interp.ts_.get() , 2.2 , 1e-8);
-// ASSERT_MATRIX_APPROX(interp.data_ , data , 1e-8);
-//// ASSERT_MATRIX_APPROX(interp.delta_ , VectorXs::Zero(3) , 1e-8);
-//// ASSERT_MATRIX_APPROX(interp.delta_integr_ , motions[2].delta_integr_ , 1e-8);
-//
-// ASSERT_NEAR( second.ts_.get() , 3.0 , 1e-8);
-// ASSERT_MATRIX_APPROX(second.data_ , data , 1e-8);
-//// ASSERT_MATRIX_APPROX(second.delta_ , motions[3].delta_ , 1e-8);
-// ASSERT_MATRIX_APPROX(second.delta_integr_ , motions[3].delta_integr_ , 1e-8);
-//
-// tt = 2.6;
-// interp = interpolate(ref1, ref2, tt, second);
-//
-// ASSERT_NEAR( interp.ts_.get() , 2.6 , 1e-8);
-// ASSERT_MATRIX_APPROX(interp.data_ , data , 1e-8);
-//// ASSERT_MATRIX_APPROX(interp.delta_ , motions[3].delta_ , 1e-8);
-//// ASSERT_MATRIX_APPROX(interp.delta_integr_ , motions[3].delta_integr_ , 1e-8);
-//
-// ASSERT_NEAR( second.ts_.get() , 3.0 , 1e-8);
-// ASSERT_MATRIX_APPROX(second.data_ , data , 1e-8);
-//// ASSERT_MATRIX_APPROX(second.delta_ , VectorXs::Zero(3) , 1e-8);
-// ASSERT_MATRIX_APPROX(second.delta_integr_ , motions[3].delta_integr_ , 1e-8);
-//}
int main(int argc, char **argv)
{
diff --git a/test/gtest_processor_odom_3D.cpp b/test/gtest_processor_odom_3D.cpp
index df554557cc80013231a7e88cc31c22cfe403cccf..421efc97afa7227e15013bd4a97d8f6c8b409759 100644
--- a/test/gtest_processor_odom_3D.cpp
+++ b/test/gtest_processor_odom_3D.cpp
@@ -151,425 +151,6 @@ TEST(ProcessorOdom3D, deltaPlusDelta_Jac)
}
-//TEST(ProcessorOdom3D, Interpolate0) // basic test
-//{
-// /* Conditions:
-// * ref d = id
-// * ref D = id
-// * fin d = pos
-// * fin D = id
-// */
-//
-// ProcessorOdom3D prc(std::make_shared<ProcessorParamsOdom3D>());
-//
-// Motion ref(0.0,6,7,6,0), final(0.0,6,7,6,0), interpolated(0.0,6,7,6,0);
-//
-// // set ref
-// ref.ts_ = 1;
-// ref.delta_ << 0,0,0, 0,0,0,1;
-// ref.delta_integr_ << 0,0,0, 0,0,0,1;
-//
-// WOLF_DEBUG("ref delta= ", ref.delta_.transpose());
-// WOLF_DEBUG("ref Delta= ", ref.delta_integr_.transpose());
-//
-// // set final
-// final.ts_ = 5;
-// final.delta_ << 1,0,0, 0,0,0,1;
-// final.delta_integr_ << 0,0,0, 0,0,0,1;
-// prc.deltaPlusDelta(ref.delta_integr_, final.delta_, (final.ts_ - ref.ts_), final.delta_integr_);
-//
-// WOLF_DEBUG("final delta= ", final.delta_.transpose());
-// WOLF_DEBUG("final Delta= ", final.delta_integr_.transpose());
-//
-// // interpolate!
-// Motion second = final;
-// TimeStamp t; t = 2;
-// // +--+--------+---> time(s)
-// // 1 2 3 4 5 // 2 = 25% into interpolated, 75% into second
-// interpolated = prc.interpolate(ref, second, t);
-//
-// WOLF_DEBUG("interpolated delta= ", interpolated.delta_.transpose());
-// WOLF_DEBUG("interpolated Delta= ", interpolated.delta_integr_.transpose());
-//
-// // delta
-// ASSERT_MATRIX_APPROX(interpolated.delta_.head<3>() , 0.25 * final.delta_.head<3>(), Constants::EPS);
-// ASSERT_MATRIX_APPROX(second.delta_.head<3>() , 0.75 * final.delta_.head<3>(), Constants::EPS);
-//
-//}
-//
-//TEST(ProcessorOdom3D, Interpolate1) // delta algebra test
-//{
-// ProcessorOdom3D prc(std::make_shared<ProcessorParamsOdom3D>());
-//
-// /*
-// * We create several poses: origin, ref, int, second, final, as follows:
-// *
-// * +---+---+---+---->
-// * o r i s,f
-// *
-// * We compute all deltas between them: d_or, d_ri, d_is, d_rf
-// * We create the motions R, F
-// * We interpolate, and get I, S
-// */
-//
-// // absolute poses: origin, ref, interp, second=final
-// Vector7s x_o, x_r, x_i, x_s, x_f;
-// Map<Vector3s> p_o(x_o.data(), 3);
-// Map<Quaternions> q_o(x_o.data() +3);
-// Map<Vector3s> p_r(x_r.data(), 3);
-// Map<Quaternions> q_r(x_r.data() +3);
-// Map<Vector3s> p_i(x_i.data(), 3);
-// Map<Quaternions> q_i(x_i.data() +3);
-// Map<Vector3s> p_s(x_s.data(), 3);
-// Map<Quaternions> q_s(x_s.data() +3);
-// Map<Vector3s> p_f(x_f.data(), 3);
-// Map<Quaternions> q_f(x_f.data() +3);
-//
-// // deltas -- referred to previous delta
-// // o-r r-i i-s s-f
-// Vector7s dx_or, dx_ri, dx_is, dx_rf;
-// Map<Vector3s> dp_or(dx_or.data(), 3);
-// Map<Quaternions> dq_or(dx_or.data() +3);
-// Map<Vector3s> dp_ri(dx_ri.data(), 3);
-// Map<Quaternions> dq_ri(dx_ri.data() +3);
-// Map<Vector3s> dp_is(dx_is.data(), 3);
-// Map<Quaternions> dq_is(dx_is.data() +3);
-// Map<Vector3s> dp_rf(dx_rf.data(), 3);
-// Map<Quaternions> dq_rf(dx_rf.data() +3);
-// Map<Vector7s> dx_rs(dx_rf.data(), 7); // this ensures dx_rs = dx_rf
-//
-// // Deltas -- always referred to origin
-// // o-r o-i o-s o-f
-// Vector7s Dx_or, Dx_oi, Dx_os, Dx_of;
-// Map<Vector3s> Dp_or(Dx_or.data(), 3);
-// Map<Quaternions> Dq_or(Dx_or.data() +3);
-// Map<Vector3s> Dp_oi(Dx_oi.data(), 3);
-// Map<Quaternions> Dq_oi(Dx_oi.data() +3);
-// Map<Vector3s> Dp_os(Dx_os.data(), 3);
-// Map<Quaternions> Dq_os(Dx_os.data() +3);
-// Map<Vector3s> Dp_of(Dx_of.data(), 3);
-// Map<Quaternions> Dq_of(Dx_of.data() +3);
-//
-// // time stamps and intervals
-// TimeStamp t_o(0), t_r(1), t_i(2.3), t_f(5); // t_i=2: 25% of motion; t_i=2.3: a general interpolation point
-// Scalar dt_ri = t_i - t_r;
-// Scalar dt_rf = t_f - t_r;
-//
-// WOLF_DEBUG("t_o: ", t_o.get(), "; t_r: ", t_r.get(), "; t_i: ", t_i.get(), "; t_f: ", t_f.get());
-// WOLF_DEBUG("dt_ri: ", dt_ri, "; dt_rf ", dt_rf)
-//
-// // Constant velocity model
-// Vector3s v;
-// Vector3s w;
-//
-// // Motion structures
-// Motion R(0.0,6,7,6,0), I(0.0,6,7,6,0), S(0.0,6,7,6,0), F(0.0,6,7,6,0);
-//
-// /////////// start experiment ///////////////
-//
-// // set origin and ref states
-// x_o << 1,2,3, 4,5,6,7; q_o.normalize();
-// x_r << 7,6,5, 4,3,2,1; q_r.normalize();
-//
-// // set constant velocity params
-// v << 3,2,1; // linear velocity
-// w << .1,.2,.3; // angular velocity
-//
-// // compute other poses from model
-// p_i = p_r + v * dt_ri;
-// q_i = q_r * v2q (w * dt_ri);
-// p_f = p_r + v * dt_rf;
-// q_f = q_r * v2q (w * dt_rf);
-// x_s = x_f;
-//
-// WOLF_DEBUG("o = ", x_o.transpose());
-// WOLF_DEBUG("r = ", x_r.transpose());
-// WOLF_DEBUG("i = ", x_i.transpose());
-// WOLF_DEBUG("s = ", x_s.transpose());
-// WOLF_DEBUG("f = ", x_f.transpose());
-//
-// // deltas -- referred to previous delta
-// dp_or = q_o.conjugate() * (p_r - p_o);
-// dq_or = q_o.conjugate() * q_r;
-// dp_ri = q_r.conjugate() * (p_i - p_r);
-// dq_ri = q_r.conjugate() * q_i;
-// dp_is = q_i.conjugate() * (p_s - p_i);
-// dq_is = q_i.conjugate() * q_s;
-// dp_rf = q_r.conjugate() * (p_f - p_r);
-// dq_rf = q_r.conjugate() * q_f;
-//
-// // Deltas -- always referred to origin
-// Dp_or = q_o.conjugate() * (p_r - p_o);
-// Dq_or = q_o.conjugate() * q_r;
-// Dp_oi = q_o.conjugate() * (p_i - p_o);
-// Dq_oi = q_o.conjugate() * q_i;
-// Dp_os = q_o.conjugate() * (p_s - p_o);
-// Dq_os = q_o.conjugate() * q_s;
-// Dp_of = q_o.conjugate() * (p_f - p_o);
-// Dq_of = q_o.conjugate() * q_f;
-//
-// // set ref
-// R.ts_ = t_r;
-// R.delta_ = dx_or; // origin to ref
-// R.delta_integr_ = Dx_or; // origin to ref
-//
-// WOLF_DEBUG("* R.d = ", R.delta_.transpose());
-// WOLF_DEBUG(" or = ", dx_or.transpose());
-// ASSERT_MATRIX_APPROX(R.delta_ , dx_or, Constants::EPS);
-//
-// WOLF_DEBUG(" R.D = ", R.delta_integr_.transpose());
-// WOLF_DEBUG(" or = ", Dx_or.transpose());
-// ASSERT_MATRIX_APPROX(R.delta_integr_ , Dx_or, Constants::EPS);
-//
-// // set final
-// F.ts_ = t_f;
-// F.delta_ = dx_rf; // ref to final
-// F.delta_integr_ = Dx_of; // origin to final
-//
-// WOLF_DEBUG("* F.d = ", F.delta_.transpose());
-// WOLF_DEBUG(" rf = ", dx_rf.transpose());
-// ASSERT_MATRIX_APPROX(F.delta_ , dx_rf, Constants::EPS);
-//
-// WOLF_DEBUG(" F.D = ", F.delta_integr_.transpose());
-// WOLF_DEBUG(" of = ", Dx_of.transpose());
-// ASSERT_MATRIX_APPROX(F.delta_integr_ , Dx_of, Constants::EPS);
-//
-// S = F; // avoid overwriting final
-// WOLF_DEBUG("* S.d = ", S.delta_.transpose());
-// WOLF_DEBUG(" rs = ", dx_rs.transpose());
-// ASSERT_MATRIX_APPROX(S.delta_ , dx_rs, Constants::EPS);
-//
-// WOLF_DEBUG(" S.D = ", S.delta_integr_.transpose());
-// WOLF_DEBUG(" os = ", Dx_os.transpose());
-// ASSERT_MATRIX_APPROX(S.delta_integr_ , Dx_os, Constants::EPS);
-//
-// // interpolate!
-// WOLF_DEBUG("*** INTERPOLATE *** I has been computed; S has changed.");
-// I = prc.interpolate(R, S, t_i);
-//
-// WOLF_DEBUG("* I.d = ", I.delta_.transpose());
-// WOLF_DEBUG(" ri = ", dx_ri.transpose());
-// ASSERT_MATRIX_APPROX(I.delta_ , dx_ri, Constants::EPS);
-//
-// WOLF_DEBUG(" I.D = ", I.delta_integr_.transpose());
-// WOLF_DEBUG(" oi = ", Dx_oi.transpose());
-// ASSERT_MATRIX_APPROX(I.delta_integr_ , Dx_oi, Constants::EPS);
-//
-// WOLF_DEBUG("* S.d = ", S.delta_.transpose());
-// WOLF_DEBUG(" is = ", dx_is.transpose());
-// ASSERT_MATRIX_APPROX(S.delta_ , dx_is, Constants::EPS);
-//
-// WOLF_DEBUG(" S.D = ", S.delta_integr_.transpose());
-// WOLF_DEBUG(" os = ", Dx_os.transpose());
-// ASSERT_MATRIX_APPROX(S.delta_integr_ , Dx_os, Constants::EPS);
-//
-//}
-//
-//TEST(ProcessorOdom3D, Interpolate2) // timestamp out of bounds test
-//{
-// ProcessorOdom3D prc(std::make_shared<ProcessorParamsOdom3D>());
-//
-// /*
-// * We create several poses: origin, ref, int, second, final, as follows:
-// *
-// * +---+---+---+---->
-// * o r i s,f
-// *
-// * We compute all deltas between them: d_or, d_ri, d_is, d_rf
-// * We create the motions R, F
-// * We interpolate, and get I, S
-// */
-//
-// // deltas -- referred to previous delta
-// // o-r r-i i-s s-f
-// VectorXs dx_or(7), dx_ri(7), dx_is(7), dx_rf(7);
-// Map<VectorXs> dx_rs(dx_rf.data(), 7); // this ensures dx_rs = dx_rf
-//
-// // Deltas -- always referred to origin
-// // o-r o-i o-s o-f
-// VectorXs Dx_or(7), Dx_oi(7), Dx_os(7), Dx_of(7);
-//
-// // time stamps and intervals
-// TimeStamp t_o(0), t_r(1), t_i, t_f(5); // we'll set t_i later
-//
-// WOLF_DEBUG("t_o: ", t_o.get(), "; t_r: ", t_r.get(), "; t_f: ", t_f.get());
-//
-// // Motion structures
-// Motion R(0.0,6,7,6,0), I(0.0,6,7,6,0), S(0.0,6,7,6,0), F(0.0,6,7,6,0);
-//
-// /////////// start experiment ///////////////
-//
-// // set final and ref deltas
-// dx_or << 1,2,3, 4,5,6,7; dx_or.tail<4>().normalize();
-// dx_rf << 7,6,5, 4,3,2,1; dx_rf.tail<4>().normalize();
-// Dx_or = dx_or;
-// prc.deltaPlusDelta(Dx_or, dx_rf, t_f - t_r, Dx_of);
-// Dx_os = Dx_of;
-//
-// // set ref
-// R.ts_ = t_r;
-// R.delta_ = dx_or; // origin to ref
-// R.delta_integr_ = Dx_or; // origin to ref
-//
-// WOLF_DEBUG("* R.d = ", R.delta_.transpose());
-// WOLF_DEBUG(" or = ", dx_or.transpose());
-// ASSERT_MATRIX_APPROX(R.delta_ , dx_or, Constants::EPS);
-//
-// WOLF_DEBUG(" R.D = ", R.delta_integr_.transpose());
-// WOLF_DEBUG(" or = ", Dx_or.transpose());
-// ASSERT_MATRIX_APPROX(R.delta_integr_ , Dx_or, Constants::EPS);
-//
-// // set final
-// F.ts_ = t_f;
-// F.delta_ = dx_rf; // ref to final
-// F.delta_integr_ = Dx_of; // origin to final
-//
-// WOLF_DEBUG("* F.d = ", F.delta_.transpose());
-// WOLF_DEBUG(" rf = ", dx_rf.transpose());
-// ASSERT_MATRIX_APPROX(F.delta_ , dx_rf, Constants::EPS);
-//
-// WOLF_DEBUG(" F.D = ", F.delta_integr_.transpose());
-// WOLF_DEBUG(" of = ", Dx_of.transpose());
-// ASSERT_MATRIX_APPROX(F.delta_integr_ , Dx_of, Constants::EPS);
-//
-// S = F; // avoid overwriting final
-// WOLF_DEBUG("* S.d = ", S.delta_.transpose());
-// WOLF_DEBUG(" rs = ", dx_rs.transpose());
-// ASSERT_MATRIX_APPROX(S.delta_ , dx_rs, Constants::EPS);
-//
-// WOLF_DEBUG(" S.D = ", S.delta_integr_.transpose());
-// WOLF_DEBUG(" os = ", Dx_os.transpose());
-// ASSERT_MATRIX_APPROX(S.delta_integr_ , Dx_os, Constants::EPS);
-//
-// // interpolate!
-// t_i = 0.5; /// before ref!
-// WOLF_DEBUG("*** INTERPOLATE *** I has been computed; S has changed.");
-// I = prc.interpolate(R, S, t_i);
-//
-// WOLF_DEBUG("* I.d = ", I.delta_.transpose());
-// WOLF_DEBUG(" ri = ", prc.deltaZero().transpose());
-// ASSERT_MATRIX_APPROX(I.delta_ , prc.deltaZero(), Constants::EPS);
-//
-// WOLF_DEBUG(" I.D = ", I.delta_integr_.transpose());
-// WOLF_DEBUG(" oi = ", Dx_or.transpose());
-// ASSERT_MATRIX_APPROX(I.delta_integr_ , Dx_or, Constants::EPS);
-//
-// WOLF_DEBUG("* S.d = ", S.delta_.transpose());
-// WOLF_DEBUG(" is = ", dx_rf.transpose());
-// ASSERT_MATRIX_APPROX(S.delta_ , dx_rf, Constants::EPS);
-//
-// WOLF_DEBUG(" S.D = ", S.delta_integr_.transpose());
-// WOLF_DEBUG(" os = ", Dx_of.transpose());
-// ASSERT_MATRIX_APPROX(S.delta_integr_ , Dx_of, Constants::EPS);
-//
-// // interpolate!
-// t_i = 5.5; /// after ref!
-// S = F;
-// WOLF_DEBUG("*** INTERPOLATE *** I has been computed; S has changed.");
-// I = prc.interpolate(R, S, t_i);
-//
-// WOLF_DEBUG("* I.d = ", I.delta_.transpose());
-// WOLF_DEBUG(" ri = ", dx_rf.transpose());
-// ASSERT_MATRIX_APPROX(I.delta_ , dx_rf, Constants::EPS);
-//
-// WOLF_DEBUG(" I.D = ", I.delta_integr_.transpose());
-// WOLF_DEBUG(" oi = ", Dx_of.transpose());
-// ASSERT_MATRIX_APPROX(I.delta_integr_ , Dx_of, Constants::EPS);
-//
-// WOLF_DEBUG("* S.d = ", S.delta_.transpose());
-// WOLF_DEBUG(" is = ", prc.deltaZero().transpose());
-// ASSERT_MATRIX_APPROX(S.delta_ , prc.deltaZero(), Constants::EPS);
-//
-// WOLF_DEBUG(" S.D = ", S.delta_integr_.transpose());
-// WOLF_DEBUG(" os = ", Dx_of.transpose());
-// ASSERT_MATRIX_APPROX(S.delta_integr_ , Dx_of, Constants::EPS);
-//
-//}
-//
-//
-//TEST(ProcessorOdom3D, Interpolate3) // timestamp out of bounds test
-//{
-// ProcessorOdom3D prc(std::make_shared<ProcessorParamsOdom3D>());
-//
-// /*
-// * We create several poses: origin, ref, int, second, final, as follows:
-// *
-// * +---+---+---+---->
-// * o r i s,f
-// *
-// * We compute all deltas between them: d_or, d_ri, d_is, d_rf
-// * We create the motions R, F
-// * We interpolate, and get I, S
-// */
-//
-// // deltas -- referred to previous delta
-// // o-r r-i i-s s-f
-// VectorXs dx_or(7), dx_ri(7), dx_is(7), dx_if(7), dx_rf(7);
-// Map<VectorXs> dx_rs(dx_rf.data(), 7); // this ensures dx_rs = dx_rf
-//
-// // Deltas -- always referred to origin
-// // o-r o-i o-s o-f
-// VectorXs Dx_or(7), Dx_oi(7), Dx_os(7), Dx_of(7);
-//
-// // time stamps and intervals
-// TimeStamp t_o(0), t_r(1), t_i, t_f(5); // we'll set t_i later
-//
-// WOLF_DEBUG("t_o: ", t_o.get(), "; t_r: ", t_r.get(), "; t_f: ", t_f.get());
-//
-// // Motion structures
-// Motion R(0.0,6,7,6,0), I(0.0,6,7,6,0), S(0.0,6,7,6,0), F(0.0,6,7,6,0);
-//
-//
-// /////////// start experiment ///////////////
-//
-// // set ref and final deltas
-// dx_or << 1,2,3, 4,5,6,7; dx_or.tail<4>().normalize();
-// dx_rf << 7,6,5, 4,3,2,1; dx_rf.tail<4>().normalize();
-// Dx_or = dx_or;
-// prc.deltaPlusDelta(Dx_or, dx_rf, t_f - t_r, Dx_of);
-// Dx_os = Dx_of;
-//
-// // set ref
-// R.ts_ = t_r;
-// R.delta_ = dx_or; // origin to ref
-// R.delta_integr_ = Dx_or; // origin to ref
-//
-// WOLF_DEBUG("* R.d = ", R.delta_.transpose());
-// WOLF_DEBUG(" or = ", dx_or.transpose());
-// ASSERT_MATRIX_APPROX(R.delta_ , dx_or, Constants::EPS);
-//
-// WOLF_DEBUG(" R.D = ", R.delta_integr_.transpose());
-// WOLF_DEBUG(" or = ", Dx_or.transpose());
-// ASSERT_MATRIX_APPROX(R.delta_integr_ , Dx_or, Constants::EPS);
-//
-// // set final
-// F.ts_ = t_f;
-// F.delta_ = dx_rf; // ref to final
-// F.delta_integr_ = Dx_of; // origin to final
-//
-// WOLF_DEBUG("* F.d = ", F.delta_.transpose());
-// WOLF_DEBUG(" rf = ", dx_rf.transpose());
-// ASSERT_MATRIX_APPROX(F.delta_ , dx_rf, Constants::EPS);
-//
-// WOLF_DEBUG(" F.D = ", F.delta_integr_.transpose());
-// WOLF_DEBUG(" of = ", Dx_of.transpose());
-// ASSERT_MATRIX_APPROX(F.delta_integr_ , Dx_of, Constants::EPS);
-//
-// // interpolate!
-// t_i = 2; /// 25% between refs!
-// WOLF_DEBUG("*** INTERPOLATE *** I and S have been computed.");
-// I = prc.interpolate(R, F, t_i, S);
-//
-//
-// prc.deltaPlusDelta(R.delta_integr_, I.delta_, t_i-t_r, Dx_oi);
-// ASSERT_MATRIX_APPROX(I.delta_integr_ , Dx_oi, Constants::EPS);
-//
-// prc.deltaPlusDelta(I.delta_, S.delta_, t_f-t_i, dx_rf);
-// ASSERT_MATRIX_APPROX(F.delta_ , dx_rf, Constants::EPS);
-//
-//}
-
-
int main(int argc, char **argv)