odom 3d has new interpolate()

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