diff --git a/include/base/capture/capture_motion.h b/include/base/capture/capture_motion.h
index b867d2ec16332223ee87fbdfb2468150333abc43..a93b627aa0120820128a9127f88a457b42152544 100644
--- a/include/base/capture/capture_motion.h
+++ b/include/base/capture/capture_motion.h
@@ -104,7 +104,7 @@ class CaptureMotion : public CaptureBase
Eigen::VectorXs data_; ///< Motion data in form of vector mandatory
Eigen::MatrixXs data_cov_; ///< Motion data covariance
MotionBuffer buffer_; ///< Buffer of motions between this Capture and the next one.
- FrameBasePtr origin_frame_ptr_; ///< Pointer to the origin frame of the motion
+ FrameBaseWPtr origin_frame_ptr_; ///< Pointer to the origin frame of the motion
};
inline const Eigen::VectorXs& CaptureMotion::getData() const
@@ -158,7 +158,7 @@ inline Eigen::VectorXs CaptureMotion::correctDelta(const VectorXs& _delta, const
inline FrameBasePtr CaptureMotion::getOriginFramePtr()
{
- return origin_frame_ptr_;
+ return origin_frame_ptr_.lock();
}
inline void CaptureMotion::setOriginFramePtr(FrameBasePtr _frame_ptr)
diff --git a/include/base/processor/processor_motion.h b/include/base/processor/processor_motion.h
index 0ce05f202e9eb68290871cbcd2319b8c12528a99..df43f9f841e51c7d8d0cae06354b04c0752b159d 100644
--- a/include/base/processor/processor_motion.h
+++ b/include/base/processor/processor_motion.h
@@ -205,8 +205,11 @@ class ProcessorMotion : public ProcessorBase
Scalar updateDt();
void integrateOneStep();
- void splitBuffer(const TimeStamp& _t_split, MotionBuffer& _oldest_part);
void reintegrateBuffer(CaptureMotionPtr _capture_ptr);
+ void splitBuffer(const wolf::CaptureMotionPtr& capture_source,
+ TimeStamp ts_split,
+ const FrameBasePtr& keyframe_target,
+ const wolf::CaptureMotionPtr& capture_target);
/** Pre-process incoming Capture
*
@@ -469,7 +472,6 @@ class ProcessorMotion : public ProcessorBase
Eigen::MatrixXs jacobian_delta_; ///< jacobian of delta composition w.r.t current delta
Eigen::MatrixXs jacobian_calib_; ///< jacobian of delta preintegration wrt calibration params
Eigen::MatrixXs jacobian_delta_calib_; ///< jacobian of delta wrt calib params
-
};
}
@@ -478,11 +480,6 @@ class ProcessorMotion : public ProcessorBase
namespace wolf{
-inline void ProcessorMotion::splitBuffer(const TimeStamp& _t_split, MotionBuffer& _oldest_part)
-{
- last_ptr_->getBuffer().split(_t_split, _oldest_part);
-}
-
inline void ProcessorMotion::resetDerived()
{
// Blank function, to be implemented in derived classes
diff --git a/include/base/processor/processor_odom_2D.h b/include/base/processor/processor_odom_2D.h
index b7ea895dff86ca53d06ec5eb38e9e0473de7711c..83e2100111973f0ae8f7544325ba31032a452cab 100644
--- a/include/base/processor/processor_odom_2D.h
+++ b/include/base/processor/processor_odom_2D.h
@@ -59,6 +59,10 @@ class ProcessorOdom2D : public ProcessorMotion
virtual Motion interpolate(const Motion& _ref,
Motion& _second,
TimeStamp& _ts) override;
+ virtual Motion interpolate(const Motion& _ref1,
+ const Motion& _ref2,
+ const TimeStamp& _ts,
+ Motion& _second) override;
virtual CaptureMotionPtr createCapture(const TimeStamp& _ts,
const SensorBasePtr& _sensor,
diff --git a/include/base/processor/processor_odom_3D.h b/include/base/processor/processor_odom_3D.h
index 77766efc3cef5cb5db3451c85e8f536e5d27c5ac..491ae23c7593d2b9c13ad52ed2b90f046a91274a 100644
--- a/include/base/processor/processor_odom_3D.h
+++ b/include/base/processor/processor_odom_3D.h
@@ -81,6 +81,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/processor/processor_diff_drive.cpp b/src/processor/processor_diff_drive.cpp
index 423e429dbc8190597db82847098558a4de68be58..c1b8bbf5680318090e46320d9e413765e01c9b5e 100644
--- a/src/processor/processor_diff_drive.cpp
+++ b/src/processor/processor_diff_drive.cpp
@@ -44,15 +44,22 @@ void ProcessorDiffDrive::computeCurrentDelta(const Eigen::VectorXs& _data,
case DiffDriveModel::Two_Factor_Model:
std::tie(vel, J_vel_data, J_vel_calib) =
wheelPositionIncrementToVelocity<DiffDriveModel::Two_Factor_Model>(
- _data, _data_cov,
- intrinsics.left_radius_, intrinsics.right_radius_, intrinsics.separation_,
- _calib, _dt);
+ _data,
+ _data_cov,
+ intrinsics.left_radius_,
+ intrinsics.right_radius_,
+ intrinsics.separation_,
+ _calib,
+ _dt);
break;
case DiffDriveModel::Three_Factor_Model:
std::tie(vel, J_vel_data, J_vel_calib) =
wheelPositionIncrementToVelocity<DiffDriveModel::Three_Factor_Model>(
- _data, _data_cov,
- intrinsics.left_radius_, intrinsics.right_radius_, intrinsics.separation_,
+ _data,
+ _data_cov,
+ intrinsics.left_radius_,
+ intrinsics.right_radius_,
+ intrinsics.separation_,
_calib, _dt);
break;
case DiffDriveModel::Five_Factor_Model:
diff --git a/src/processor/processor_motion.cpp b/src/processor/processor_motion.cpp
index 0fe59c59aa026654b2ed5a3d8b924b3d5cfdee48..10a1759fddc497a31614080ca07ee265ed881a1b 100644
--- a/src/processor/processor_motion.cpp
+++ b/src/processor/processor_motion.cpp
@@ -39,6 +39,36 @@ ProcessorMotion::~ProcessorMotion()
// std::cout << "destructed -p-Mot" << id() << std::endl;
}
+void ProcessorMotion::splitBuffer(const wolf::CaptureMotionPtr& _capture_source,
+ TimeStamp _ts_split,
+ const FrameBasePtr& _keyframe_target,
+ const wolf::CaptureMotionPtr& _capture_target)
+{
+ // split the buffer
+ // and give the part of the buffer before the new keyframe to the capture for the KF callback
+ _capture_source->getBuffer().split(_ts_split, _capture_target->getBuffer());
+
+ // interpolate individual delta which has been cut by the split timestamp
+ if (!_capture_source->getBuffer().get().empty()
+ && _capture_target->getBuffer().get().back().ts_ != _ts_split)
+ {
+ // interpolate Motion at the new time stamp
+ Motion motion_interpolated = interpolate(_capture_target->getBuffer().get().back(), // last Motion of old buffer
+ _capture_source->getBuffer().get().front(), // first motion of new buffer
+ _ts_split,
+ _capture_source->getBuffer().get().front());
+
+ // add to old buffer
+ _capture_target->getBuffer().get().push_back(motion_interpolated);
+ }
+
+ // Update the existing capture
+ _capture_source->setOriginFramePtr(_keyframe_target);
+
+ // re-integrate existing buffer -- note: the result of re-integration is stored in the same buffer!
+ reintegrateBuffer(_capture_source);
+}
+
void ProcessorMotion::process(CaptureBasePtr _incoming_ptr)
{
if (_incoming_ptr == nullptr)
@@ -86,18 +116,7 @@ void ProcessorMotion::process(CaptureBasePtr _incoming_ptr)
// split the buffer
// and give the part of the buffer before the new keyframe to the capture for the KF callback
- existing_capture->getBuffer().split(ts_from_callback, capture_for_keyframe_callback->getBuffer());
-
- // interpolate individual delta
- if (!existing_capture->getBuffer().get().empty() && capture_for_keyframe_callback->getBuffer().get().back().ts_ != ts_from_callback)
- {
- // interpolate Motion at the new time stamp
- Motion motion_interpolated = interpolate(capture_for_keyframe_callback->getBuffer().get().back(), // last Motion of old buffer
- existing_capture->getBuffer().get().front(), // first motion of new buffer
- ts_from_callback);
- // add to old buffer
- capture_for_keyframe_callback->getBuffer().get().push_back(motion_interpolated);
- }
+ splitBuffer(existing_capture, ts_from_callback, keyframe_from_callback, capture_for_keyframe_callback);
// create motion feature and add it to the capture
auto new_feature = emplaceFeature(capture_for_keyframe_callback);
@@ -105,12 +124,6 @@ void ProcessorMotion::process(CaptureBasePtr _incoming_ptr)
// create motion constraint and add it to the feature, and constrain to the other capture (origin)
emplaceConstraint(new_feature, keyframe_origin->getCaptureOf(getSensorPtr()) );
- // Update the existing capture
- existing_capture->setOriginFramePtr(keyframe_from_callback);
-
- // re-integrate existing buffer -- note: the result of re-integration is stored in the same buffer!
- reintegrateBuffer(existing_capture);
-
// modify existing feature and constraint (if they exist in the existing capture)
if (!existing_capture->getFeatureList().empty())
{
@@ -151,18 +164,7 @@ void ProcessorMotion::process(CaptureBasePtr _incoming_ptr)
// split the buffer
// and give the part of the buffer before the new keyframe to the capture for the KF callback
- last_ptr_->getBuffer().split(ts_from_callback, capture_for_keyframe_callback->getBuffer());
-
- // interpolate individual delta
- if (!last_ptr_->getBuffer().get().empty() && capture_for_keyframe_callback->getBuffer().get().back().ts_ != ts_from_callback)
- {
- // interpolate Motion at the new time stamp
- Motion motion_interpolated = interpolate(capture_for_keyframe_callback->getBuffer().get().back(), // last Motion of old buffer
- last_ptr_->getBuffer().get().front(), // first motion of new buffer
- ts_from_callback);
- // add to old buffer
- capture_for_keyframe_callback->getBuffer().get().push_back(motion_interpolated);
- }
+ splitBuffer(last_ptr_, ts_from_callback, keyframe_from_callback, capture_for_keyframe_callback);
// create motion feature and add it to the capture
auto feature_for_keyframe_callback = emplaceFeature(capture_for_keyframe_callback);
@@ -173,12 +175,6 @@ void ProcessorMotion::process(CaptureBasePtr _incoming_ptr)
// reset processor origin
origin_ptr_ = capture_for_keyframe_callback;
- // Update the existing capture
- last_ptr_->setOriginFramePtr(keyframe_from_callback);
-
- // re-integrate existing buffer -- note: the result of re-integration is stored in the same buffer!
- reintegrateBuffer(last_ptr_);
-
break;
}
diff --git a/src/processor/processor_odom_2D.cpp b/src/processor/processor_odom_2D.cpp
index 2c5bfe456d0d31e1be0460faf0c8df78b2922556..791103d47ad9594ff8afa0374309a0d08fb3c890 100644
--- a/src/processor/processor_odom_2D.cpp
+++ b/src/processor/processor_odom_2D.cpp
@@ -114,6 +114,11 @@ Motion ProcessorOdom2D::interpolate(const Motion& _ref, Motion& _second, TimeSta
}
+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 4f6d75911f97be52b232e06104a7302eb6c5b246..716d6831ad3a8175041e5273d7cba8b6b4e2be6f 100644
--- a/src/processor/processor_odom_3D.cpp
+++ b/src/processor/processor_odom_3D.cpp
@@ -155,7 +155,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());
@@ -254,6 +256,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/test/gtest_odom_3D.cpp b/test/gtest_odom_3D.cpp
index 864f19c1443cd64064c9fa474961c454ea617e9c..ab4f82bb186b949e5dd0d71b5e9e812972a6708e 100644
--- a/test/gtest_odom_3D.cpp
+++ b/test/gtest_odom_3D.cpp
@@ -486,6 +486,92 @@ 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)
{
testing::InitGoogleTest(&argc, argv);
diff --git a/test/gtest_processor_motion.cpp b/test/gtest_processor_motion.cpp
index 3e379c9a5dd72300a5c2cfa614d2d5b5933f9542..205c79f9d60a78a4b006e204b6537adea720274c 100644
--- a/test/gtest_processor_motion.cpp
+++ b/test/gtest_processor_motion.cpp
@@ -53,6 +53,17 @@ class ProcessorMotion_test : public ProcessorOdom2D, 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(){}
};
@@ -133,6 +144,55 @@ TEST_F(ProcessorMotion_test, Interpolate)
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);
+ processor->process(capture);
+ 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_ , 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);
+
+ ASSERT_NEAR( second.ts_.get() , 3.0 , 1e-8);
+ ASSERT_MATRIX_APPROX(second.data_ , motions[3].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_ , 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);
+
+ ASSERT_NEAR( second.ts_.get() , 3.0 , 1e-8);
+ ASSERT_MATRIX_APPROX(second.data_ , VectorXs::Zero(2) , 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)
{
testing::InitGoogleTest(&argc, argv);