Merge branch '5-remove-interpolate' into 'devel'

Resolve "Remove interpolate()"

Closes #5

See merge request !10

include/IMU/processor/processor_IMU.h

@@ -61,9 +61,6 @@ class ProcessorIMU : public ProcessorMotion{
                                 const Scalar _dt,
                                 Eigen::VectorXs& _x_plus_delta ) override;
         virtual Eigen::VectorXs deltaZero() const override;
-        virtual Motion interpolate(const Motion& _motion_ref,
-                                   Motion& _motion,
-                                   TimeStamp& _ts) override;
         virtual bool voteForKeyFrame() override;
         virtual CaptureMotionPtr emplaceCapture(const FrameBasePtr& _frame_own,
                                                 const SensorBasePtr& _sensor,

src/processor/processor_IMU.cpp

@@ -57,131 +57,6 @@ bool ProcessorIMU::voteForKeyFrame()
     return false;
 }
 
-Motion ProcessorIMU::interpolate(const Motion& _motion_ref, Motion& _motion_second, TimeStamp& _ts)
-{
-    /* Note: See extensive documentation in ProcessorMotion::interpolate().
-     *
-     * Interpolate between motion_ref and motion, as in:
-     *
-     *    motion_ref ------ ts_ ------ motion_sec
-     *                    return
-     *
-     * and return the motion at the given time_stamp ts_.
-     *
-     * DATA:
-     * Data receives no change
-     *
-     * DELTA:
-     * The delta's position and velocity receive linear interpolation:
-     *    p_ret = (ts - t_ref) / dt * (p - p_ref)
-     *    v_ret = (ts - t_ref) / dt * (v - v_ref)
-     *
-     * The delta's quaternion receives a slerp interpolation
-     *    q_ret = q_ref.slerp( (ts - t_ref) / dt , q);
-     *
-     * DELTA_INTEGR:
-     * The interpolated delta integral is just the reference integral plus the interpolated delta
-     *
-     * The second integral does not change
-     *
-     * Covariances receive linear interpolation
-     *    Q_ret = (ts - t_ref) / dt * Q_sec
-     */
-    /*
-    // resolve out-of-bounds time stamp as if the time stamp was exactly on the bounds
-    if (_ts <= _motion_ref.ts_)    // behave as if _ts == _motion_ref.ts_
-    {
-        // return null motion. Second stays the same.
-        Motion motion_int     ( _motion_ref );
-        motion_int.data_      = _motion_second.data_;
-        motion_int.data_cov_  = _motion_second.data_cov_;
-        motion_int.delta_     = deltaZero();
-        motion_int.delta_cov_ . setZero();
-        return motion_int;
-    }
-    if (_ts >= _motion_second.ts_)    // behave as if _ts == _motion_second.ts_
-    {
-        // return original second motion. Second motion becomes null motion
-        Motion motion_int         ( _motion_second );
-        _motion_second.delta_     = deltaZero();
-        _motion_second.delta_cov_ . setZero();
-        return motion_int;
-    }
-    assert(_motion_ref.ts_ <= _ts && "Interpolation time stamp out of bounds");
-    assert(_ts <= _motion_second.ts_ && "Interpolation time stamp out of bounds");
-
-    assert(_motion_ref.delta_.size() == delta_size_ && "Wrong delta size");
-    assert(_motion_ref.delta_cov_.cols() == delta_cov_size_ && "Wrong delta cov size");
-    assert(_motion_ref.delta_cov_.rows() == delta_cov_size_ && "Wrong delta cov size");
-    assert(_motion_ref.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(_motion_second.delta_.size() == delta_size_ && "Wrong delta size");
-    assert(_motion_second.delta_cov_.cols() == delta_cov_size_ && "Wrong delta cov size");
-    assert(_motion_second.delta_cov_.rows() == delta_cov_size_ && "Wrong delta cov size");
-    assert(_motion_second.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");
-
-    // reference
-    TimeStamp t_ref = _motion_ref.ts_;
-
-    // second
-    TimeStamp t_sec = _motion_second.ts_;
-    Map<VectorXs>    motion_sec_dp  (_motion_second.delta_.data() + 0, 3);
-    Map<Quaternions> motion_sec_dq  (_motion_second.delta_.data() + 3   );
-    Map<VectorXs>    motion_sec_dv  (_motion_second.delta_.data() + 7, 3);
-
-    // interpolated
-    Motion motion_int = motionZero(_ts);
-
-    // Jacobians for covariance propagation
-    MatrixXs J_ref(delta_cov_size_, delta_cov_size_);
-    MatrixXs J_int(delta_cov_size_, delta_cov_size_);
-
-    // interpolation factor
-    Scalar dt1    = _ts - t_ref;
-    Scalar dt2    = t_sec - _ts;
-    Scalar tau    = dt1 / (t_sec - t_ref); // interpolation factor (0 to 1)
-    Scalar tau_sq = tau * tau;
-
-    // copy data
-    motion_int.data_      = _motion_second.data_;
-    motion_int.data_cov_  = _motion_second.data_cov_;
-
-    // interpolate delta
-    motion_int.ts_        = _ts;
-    Map<VectorXs>    motion_int_dp  (motion_int.delta_.data() + 0, 3);
-    Map<Quaternions> motion_int_dq  (motion_int.delta_.data() + 3   );
-    Map<VectorXs>    motion_int_dv  (motion_int.delta_.data() + 7, 3);
-    motion_int_dp         = tau_sq * motion_sec_dp; // FIXME: delta_p not correctly interpolated
-    motion_int_dv         = tau    * motion_sec_dv;
-    motion_int_dq         = Quaternions::Identity().slerp(tau, motion_sec_dq);
-    motion_int.delta_cov_ = tau    * _motion_second.delta_cov_;
-
-    // integrate
-    deltaPlusDelta(_motion_ref.delta_integr_, motion_int.delta_, dt1, motion_int.delta_integr_, J_ref, J_int);
-    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 )
-    motion_sec_dp = motion_int_dq.conjugate() * (motion_sec_dp - motion_int_dp - motion_int_dv * dt2);
-    motion_sec_dv = motion_int_dq.conjugate() * (motion_sec_dv - motion_int_dv);
-    motion_sec_dq = motion_int_dq.conjugate() *  motion_sec_dq;
-    _motion_second.delta_cov_ *= (1 - tau); // 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;
-    */
-
-    //    return _motion_ref;
-
-    return ProcessorMotion::interpolate(_motion_ref, _motion_second, _ts);
-
-}
 
 CaptureMotionPtr ProcessorIMU::emplaceCapture(const FrameBasePtr& _frame_own,
                                               const SensorBasePtr& _sensor,

test/gtest_processor_IMU.cpp

@@ -201,54 +201,6 @@ TEST(ProcessorIMU, voteForKeyFrame)
 
 }
 
-//replace TEST by TEST_F if SetUp() needed
-TEST_F(ProcessorIMUt, interpolate)
-{
-    using namespace wolf;
-
-    t.set(0);
-    x0 << 0,0,0,  0,0,0,1,  0,0,0;
-    MatrixXs P0(9,9); P0.setIdentity();
-    problem->setPrior(x0, P0, t, 0.01);
-
-    data << 2, 0, 0, 0, 0, 0; // only acc_x
-    cap_imu_ptr->setData(data);
-
-    // make one step to depart from origin
-    cap_imu_ptr->setTimeStamp(0.05);
-    sensor_ptr->process(cap_imu_ptr);
-    Motion mot_ref = problem->getProcessorMotion()->getMotion();
-
-    // make two steps, then pretend it's just one
-    cap_imu_ptr->setTimeStamp(0.10);
-    sensor_ptr->process(cap_imu_ptr);
-    Motion mot_int_gt = problem->getProcessorMotion()->getMotion();
-
-    cap_imu_ptr->setTimeStamp(0.15);
-    sensor_ptr->process(cap_imu_ptr);
-    Motion mot_final = problem->getProcessorMotion()->getMotion();
-    mot_final.delta_ = mot_final.delta_integr_;
-    Motion mot_sec = mot_final;
-
-//    problem->getProcessorMotion()->getBuffer().print(0,1,1,0);
-
-class P : public wolf::ProcessorIMU
-{
-    public:
-        P() : ProcessorIMU(std::make_shared<ProcessorParamsIMU>()) {}
-        Motion interp(const Motion& ref, Motion& sec, TimeStamp ts)
-        {
-            return ProcessorIMU::interpolate(ref, sec, ts);
-        }
-} imu;
-
-Motion mot_int = imu.interp(mot_ref, mot_sec, TimeStamp(0.10));
-
-ASSERT_MATRIX_APPROX(mot_int.data_,  mot_int_gt.data_, 1e-6);
-//ASSERT_MATRIX_APPROX(mot_int.delta_, mot_int_gt.delta_, 1e-6); // FIXME: delta_p not correctly interpolated
-ASSERT_MATRIX_APPROX(mot_final.delta_integr_,  mot_sec.delta_integr_, 1e-6);
-
-}
 
 TEST_F(ProcessorIMUt, acc_x)
 {