Fix motion buffer first motion zero

src/processor/processor_motion.cpp

@@ -29,7 +29,7 @@ ProcessorMotion::ProcessorMotion(const std::string& _type,
         calib_preint_(_calib_size),
         jacobian_delta_preint_(delta_cov_size_, delta_cov_size_),
         jacobian_delta_(delta_cov_size_, delta_cov_size_),
-        jacobian_calib_(delta_size_, calib_size_)
+        jacobian_calib_(delta_cov_size_, calib_size_)
 {
     //
 }
@@ -48,21 +48,9 @@ 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);
-//    }
     // start with empty motion
-    _capture_source->getBuffer().get().push_front(motionZero(_keyframe_target->getTimeStamp()));
+    TimeStamp t_zero = _capture_target->getBuffer().get().back().ts_; // last tick of previous buffer is zero tick of current buffer
+    _capture_source->getBuffer().get().push_front(motionZero(t_zero));
 
     // Update the existing capture
     _capture_source->setOriginFrame(_keyframe_target);
@@ -409,13 +397,20 @@ void ProcessorMotion::integrateOneStep()
 
 void ProcessorMotion::reintegrateBuffer(CaptureMotionPtr _capture_ptr)
 {
-//    // start with empty motion
-//    _capture_ptr->getBuffer().get().push_front(motionZero(_capture_ptr->getOriginFrame()->getTimeStamp()));
-//
-    VectorXs calib = _capture_ptr->getCalibrationPreint();
+    VectorXs calib              = _capture_ptr->getCalibrationPreint();
+
+    // some temporaries for integration
+    delta_integrated_    =deltaZero();
+    delta_integrated_cov_.setZero();
+    jacobian_calib_      .setZero();
+
+    // check that first motion in buffer is zero!
+    assert(_capture_ptr->getBuffer().get().front().delta_integr_     == delta_integrated_     && "Buffer's first motion is not zero!");
+    assert(_capture_ptr->getBuffer().get().front().delta_integr_cov_ == delta_integrated_cov_ && "Buffer's first motion is not zero!");
+    assert(_capture_ptr->getBuffer().get().front().jacobian_calib_   == jacobian_calib_       && "Buffer's first motion is not zero!");
 
     // Iterate all the buffer
-    auto motion_it = _capture_ptr->getBuffer().get().begin();
+    auto motion_it      = _capture_ptr->getBuffer().get().begin();
     auto prev_motion_it = motion_it;
     motion_it++;
     while (motion_it != _capture_ptr->getBuffer().get().end())
@@ -433,7 +428,7 @@ void ProcessorMotion::reintegrateBuffer(CaptureMotionPtr _capture_ptr)
                             jacobian_delta_calib_);
 
         // integrate delta into delta_integr, and rewrite the buffer
-        deltaPlusDelta(prev_motion_it->delta_integr_,
+        deltaPlusDelta(delta_integrated_,
                        motion_it->delta_,
                        dt,
                        motion_it->delta_integr_,
@@ -442,11 +437,17 @@ void ProcessorMotion::reintegrateBuffer(CaptureMotionPtr _capture_ptr)
 
         // integrate Jacobian wrt calib
         if ( hasCalibration() )
-            motion_it->jacobian_calib_ = motion_it->jacobian_delta_integr_ * prev_motion_it->jacobian_calib_ + motion_it->jacobian_delta_ * jacobian_delta_calib_;
+            motion_it->jacobian_calib_ = motion_it->jacobian_delta_integr_ * jacobian_calib_
+                                       + motion_it->jacobian_delta_ * jacobian_delta_calib_;
 
         // Integrate covariance
-        motion_it->delta_integr_cov_ = motion_it->jacobian_delta_integr_ * prev_motion_it->delta_integr_cov_ * motion_it->jacobian_delta_integr_.transpose()
-                                     + motion_it->jacobian_delta_        * motion_it->delta_cov_             * motion_it->jacobian_delta_.transpose();
+        motion_it->delta_integr_cov_ = motion_it->jacobian_delta_integr_ * delta_integrated_cov_ * motion_it->jacobian_delta_integr_.transpose()
+                                     + motion_it->jacobian_delta_        * motion_it->delta_cov_ * motion_it->jacobian_delta_.transpose();
+
+        // update temporaries
+        delta_integrated_       = motion_it->delta_integr_;
+        delta_integrated_cov_   = motion_it->delta_integr_cov_;
+        jacobian_calib_         = motion_it->jacobian_calib_;
 
         // advance in buffer
         motion_it++;
@@ -593,7 +594,7 @@ CaptureMotionPtr ProcessorMotion::findCaptureContainingTimeStamp(const TimeStamp
     // Note: since the buffer goes from a KF in the past until the next KF, we need to:
     //  1. See that the KF contains a CaptureMotion
     //  2. See that the TS is smaller than the KF's TS
-    //  3. See that the TS is bigger than the KF's first Motion in the CaptureMotion's buffer
+    //  3. See that the TS is bigger than the first Motion in the CaptureMotion's buffer
     FrameBasePtr     frame          = nullptr;
     CaptureBasePtr   capture        = nullptr;
     CaptureMotionPtr capture_motion = nullptr;

test/gtest_odom_2D.cpp

@@ -476,15 +476,16 @@ TEST(Odom2D, KF_callback)
     for (int n=1; n<=N; n++)
     {
         t += dt;
-        //        WOLF_DEBUG("   estimated(", t, ") = ", problem->getState(t).transpose());
-        //        WOLF_DEBUG("ground truth(", t, ") = ", integrated_pose_vector[n].transpose());
-        EXPECT_POSE2D_APPROX(problem->getState(t), integrated_pose_vector[n], 1e-6);
+        WOLF_DEBUG("   estimated(", t, ") = ", problem->getState(t).transpose());
+        WOLF_DEBUG("ground truth(", t, ") = ", integrated_pose_vector[n].transpose());
+        ASSERT_POSE2D_APPROX(problem->getState(t), integrated_pose_vector[n], 1e-6);
     }
 }
 
 int main(int argc, char **argv)
 {
   testing::InitGoogleTest(&argc, argv);
+  testing::GTEST_FLAG(filter) = "Odom2D.KF_callback";
   return RUN_ALL_TESTS();
 }
 

test/gtest_processor_motion.cpp

@@ -18,20 +18,43 @@ using namespace Eigen;
 using namespace wolf;
 using std::static_pointer_cast;
 
-class ProcessorMotion_test : public ProcessorOdom2D, public testing::Test{
+class ProcessorOdom2DPublic : public ProcessorOdom2D
+{
     public:
-        Scalar              dt;
-        ProblemPtr          problem;
-        SensorOdom2DPtr     sensor;
-        ProcessorOdom2DPtr  processor;
-        CaptureMotionPtr    capture;
-        Vector2s            data;
-        Matrix2s            data_cov;
-
-        ProcessorMotion_test() :
-            ProcessorOdom2D(std::make_shared<ProcessorParamsOdom2D>()),
-            dt(0)
-        { }
+        ProcessorOdom2DPublic(ProcessorParamsOdom2DPtr params) : ProcessorOdom2D(params)
+        {
+            //
+        }
+        virtual ~ProcessorOdom2DPublic(){}
+
+        void splitBuffer(const wolf::CaptureMotionPtr& capture_source,
+                         TimeStamp ts_split,
+                         const FrameBasePtr& keyframe_target,
+                         const wolf::CaptureMotionPtr& capture_target)
+        {
+            ProcessorOdom2D::splitBuffer(capture_source,
+                                         ts_split,
+                                         keyframe_target,
+                                         capture_target);
+        }
+};
+
+WOLF_PTR_TYPEDEFS(ProcessorOdom2DPublic);
+
+class ProcessorMotion_test : public testing::Test{
+    public:
+        Scalar                      dt;
+        ProblemPtr                  problem;
+        SensorOdom2DPtr             sensor;
+        ProcessorOdom2DPublicPtr    processor;
+        CaptureMotionPtr            capture;
+        Vector2s                    data;
+        Matrix2s                    data_cov;
+
+//        ProcessorMotion_test() :
+//            ProcessorOdom2D(std::make_shared<ProcessorParamsOdom2D>()),
+//            dt(0)
+//        { }
 
         virtual void SetUp()
         {
@@ -39,6 +62,7 @@ class ProcessorMotion_test : public ProcessorOdom2D, public testing::Test{
 
             dt                      = 1.0;
             problem = Problem::create("PO", 2);
+            sensor = static_pointer_cast<SensorOdom2D>(problem->installSensor("ODOM 2D", "odom", Vector3s(0,0,0), wolf_root + "/test/yaml/sensor_odom_2D.yaml"));
             ProcessorParamsOdom2DPtr params(std::make_shared<ProcessorParamsOdom2D>());
             params->time_tolerance  = 0.25;
             params->dist_traveled   = 100;
@@ -46,8 +70,7 @@ class ProcessorMotion_test : public ProcessorOdom2D, public testing::Test{
             params->max_time_span   = 2.5*dt; // force KF at every third process()
             params->cov_det         = 100;
             params->unmeasured_perturbation_std = 0.001;
-            sensor = static_pointer_cast<SensorOdom2D>(problem->installSensor("ODOM 2D", "odom", Vector3s(0,0,0), wolf_root + "/test/yaml/sensor_odom_2D.yaml"));
-            processor = static_pointer_cast<ProcessorOdom2D>(problem->installProcessor("ODOM 2D", "odom", sensor, params));
+            processor = ProcessorBase::emplace<ProcessorOdom2DPublic>(sensor, params);
             capture = std::make_shared<CaptureMotion>("ODOM 2D", 0.0, sensor, data, data_cov, 3, 3, nullptr);
 
             Vector3s x0; x0 << 0, 0, 0;
@@ -55,6 +78,7 @@ 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);
@@ -108,6 +132,47 @@ TEST_F(ProcessorMotion_test, IntegrateCircle)
     ASSERT_MATRIX_APPROX(problem->getCurrentState(), (Vector3s()<<0,0,0).finished(), 1e-8);
 }
 
+TEST_F(ProcessorMotion_test, splitBuffer)
+{
+    data << 1, 2*M_PI/10; // advance in circle
+    data_cov.setIdentity();
+    TimeStamp t(0.0);
+
+    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);
+        WOLF_DEBUG("t: ", t, "  x: ", problem->getCurrentState().transpose());
+    }
+
+    SensorBasePtr    S = processor->getSensor();
+
+    TimeStamp        t_target = 8.5;
+    FrameBasePtr     F_target = problem->emplaceFrame(KEY, t_target);
+    CaptureBasePtr   C_last   = processor->getLast();
+    CaptureMotionPtr C_source = std::dynamic_pointer_cast<CaptureMotion>(C_last);
+    FrameBasePtr     F_origin = C_source->getOriginFrame();
+    CaptureMotionPtr C_target = CaptureBase::emplace<CaptureMotion>(F_target,
+                                                                    "ODOM 2D",
+                                                                    t_target,
+                                                                    sensor,
+                                                                    data,
+                                                                    3,
+                                                                    3,
+                                                                    F_origin);
+
+    processor->splitBuffer(C_source,
+                           t_target,
+                           F_target,
+                           C_target);
+
+    C_target->getBuffer().print(1,1,1,0);
+    C_source->getBuffer().print(1,1,1,0);
+}
+
 //TEST_F(ProcessorMotion_test, Interpolate)
 //{
 //    data << 1, 2*M_PI/10; // advance in turn