Fix some things in interpolate()

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;
 

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)
 {