diff --git a/src/processor/processor_motion.cpp b/src/processor/processor_motion.cpp
index 9f997e1f51e71d2405cf5c0c36abc459149f29ee..3c80ad1050683825586155488563993cc21023ca 100644
--- a/src/processor/processor_motion.cpp
+++ b/src/processor/processor_motion.cpp
@@ -48,19 +48,19 @@ void ProcessorMotion::splitBuffer(const wolf::CaptureMotionPtr& _capture_source,
// 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);
- }
+// // 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->setOriginFrame(_keyframe_target);
@@ -524,7 +524,7 @@ Motion ProcessorMotion::interpolate(const Motion& _ref1, const Motion& _ref2, co
interpolated.jacobian_delta_integr_,
interpolated.jacobian_delta_);
- _second.ts_ = _ref2.ts_;
+ _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_,
@@ -534,12 +534,13 @@ Motion ProcessorMotion::interpolate(const Motion& _ref1, const Motion& _ref2, co
_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_);
+
+// deltaPlusDelta(_second.delta_integr_,
+// _second.delta_,
+// _second.ts_.get() - _ref1.ts_.get(),
+// _second.delta_integr_,
+// _second.jacobian_delta_integr_,
+// _second.jacobian_delta_);
return interpolated;
diff --git a/test/gtest_processor_motion.cpp b/test/gtest_processor_motion.cpp
index 2c02ccf5764fc2555d0b922efdcdb0e14c8d29a1..e508069f55ffeeffcc7ab28f00af7341f2dd9c9b 100644
--- a/test/gtest_processor_motion.cpp
+++ b/test/gtest_processor_motion.cpp
@@ -146,54 +146,54 @@ 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);
+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_ , motions[3].data_ , 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_ , motions[3].data_ , 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_ , VectorXs::Zero(2) , 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);
-//}
+ ASSERT_MATRIX_APPROX(second.delta_integr_ , motions[3].delta_integr_ , 1e-8);
+}
int main(int argc, char **argv)
{