diff --git a/src/processor_odom_3D.cpp b/src/processor_odom_3D.cpp
index 49da6be8103ab17a3a50ed75dd87492db3dedb40..131ba62eda3a6d152fab51edd17373c3127563ca 100644
--- a/src/processor_odom_3D.cpp
+++ b/src/processor_odom_3D.cpp
@@ -156,7 +156,9 @@ 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)
+Motion ProcessorOdom3D::interpolate(const Motion& _motion_ref,
+ Motion& _motion_second,
+ TimeStamp& _ts)
{
// WOLF_TRACE("motion ref ts: ", _motion_ref.ts_.get());
@@ -256,6 +258,91 @@ Motion ProcessorOdom3D::interpolate(const Motion& _motion_ref, Motion& _motion_s
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;
+}
+
+
ProcessorBasePtr ProcessorOdom3D::create(const std::string& _unique_name, const ProcessorParamsBasePtr _params, const SensorBasePtr _sen_ptr)
{
// cast inputs to the correct type
diff --git a/src/processor_odom_3D.h b/src/processor_odom_3D.h
index 9a9828d4b2dcc8574de9b22ba8d55503e7a3875a..ed4ff950f73c7f61a5be4d9b3801d42ac725a42a 100644
--- a/src/processor_odom_3D.h
+++ b/src/processor_odom_3D.h
@@ -83,6 +83,10 @@ class ProcessorOdom3D : public ProcessorMotion
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 createCapture(const TimeStamp& _ts,
const SensorBasePtr& _sensor,
diff --git a/src/test/gtest_odom_3D.cpp b/src/test/gtest_odom_3D.cpp
index bc36ac8457f36b9747ff16e46e89bdc5e8e6f8cc..b737ab5b83944a9d54a72a21017b73ac53c20630 100644
--- a/src/test/gtest_odom_3D.cpp
+++ b/src/test/gtest_odom_3D.cpp
@@ -496,6 +496,89 @@ TEST(ProcessorOdom3D, Interpolate2) // timestamp out of bounds test
}
+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)