Merge branch 'feature/proc_motion' into 'devel'

Feature/proc motion

See merge request mobile_robotics/wolf!248

src/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)

src/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,20 +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);
@@ -107,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())
             {
@@ -154,18 +165,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);
@@ -176,12 +176,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;
         }
 

src/processor_motion.h

@@ -207,8 +207,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
          *
@@ -476,7 +479,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
-
 };
 
 }
@@ -485,11 +487,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

src/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

src/processor_odom_2D.h

@@ -60,6 +60,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);
 
         virtual CaptureMotionPtr createCapture(const TimeStamp& _ts,
                                                const SensorBasePtr& _sensor,

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

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,

src/processors/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:

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)

src/test/gtest_processor_motion.cpp

@@ -54,6 +54,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(){}
 
 };
@@ -135,6 +146,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);