Merge branch 'calibration' into 'master'

Calibration

See merge request !123

src/imu_tools.h

+/*
+ * imu_tools.h
+ *
+ *  Created on: Jul 29, 2017
+ *      Author: jsola
+ */
+
+#ifndef IMU_TOOLS_H_
+#define IMU_TOOLS_H_
+
+
+#include "wolf.h"
+#include "rotations.h"
+
+namespace wolf 
+{
+namespace imu {
+using namespace Eigen;
+
+template<typename T = wolf::Scalar>
+inline Matrix<T, 10, 1> identity()
+{
+    Matrix<T, 10, 1> ret;
+    ret.setZero();
+    ret(6) = 1.0;
+    return ret;
+}
+
+template<typename D1, typename D2>
+inline void inverse(const MatrixBase<D1>& d,
+                    typename D1::Scalar dt,
+                    MatrixBase<D2>& id)
+{
+    MatrixSizeCheck<10, 1>::check(d);
+    MatrixSizeCheck<10, 1>::check(id);
+
+    Map<const Matrix<typename D1::Scalar, 3, 1> >   dp    ( &d( 0 ) );
+    Map<const Quaternion<typename D1::Scalar> >     dq    ( &d( 3 ) );
+    Map<const Matrix<typename D1::Scalar, 3, 1> >   dv    ( &d( 7 ) );
+    Map<Matrix<typename D2::Scalar, 3, 1> >         idp  ( &id( 0 ));
+    Map<Quaternion<typename D2::Scalar> >           idq  ( &id( 3 ));
+    Map<Matrix<typename D2::Scalar, 3, 1> >         idv  ( &id( 7 ));
+
+    idp = - ( dq.conjugate() * (dp - dv * dt) );
+    idv = - ( dq.conjugate() * dv );
+    idq =     dq.conjugate();
+}
+
+template<typename D>
+inline Matrix<typename D::Scalar, 10, 1> inverse(const MatrixBase<D>& d,
+                                                 typename D::Scalar dt)
+{
+    Matrix<typename D::Scalar, 10, 1> id;
+
+    inverse(d, dt, id);
+
+    return id;
+}
+
+
+template<typename D1, typename D2, typename D3>
+inline void compose(const MatrixBase<D1>& d1,
+                    const MatrixBase<D2>& d2,
+                    Scalar dt,
+                    MatrixBase<D3>& sum)
+{
+    MatrixSizeCheck<10, 1>::check(d1);
+    MatrixSizeCheck<10, 1>::check(d2);
+    MatrixSizeCheck<10, 1>::check(sum);
+
+    Map<const Matrix<typename D1::Scalar, 3, 1> >   dp1    ( &d1( 0 ) );
+    Map<const Quaternion<typename D1::Scalar> >     dq1    ( &d1( 3 ) );
+    Map<const Matrix<typename D1::Scalar, 3, 1> >   dv1    ( &d1( 7 ) );
+    Map<const Matrix<typename D2::Scalar, 3, 1> >   dp2    ( &d2( 0 ) );
+    Map<const Quaternion<typename D2::Scalar> >     dq2    ( &d2( 3 ) );
+    Map<const Matrix<typename D2::Scalar, 3, 1> >   dv2    ( &d2( 7 ) );
+    Map<Matrix<typename D3::Scalar, 3, 1> >         sum_p  ( &sum( 0 ));
+    Map<Quaternion<typename D3::Scalar> >           sum_q  ( &sum( 3 ));
+    Map<Matrix<typename D3::Scalar, 3, 1> >         sum_v  ( &sum( 7 ));
+
+    sum_p = dp1 + dv1*dt + dq1*dp2;
+    sum_v = dv1 +          dq1*dv2;
+    sum_q =                dq1*dq2; // dq here to avoid possible aliasing between d1 and sum
+}
+
+template<typename D1, typename D2>
+inline void a() {}
+
+template<typename D1, typename D2>
+inline Matrix<typename D1::Scalar, 10, 1> compose(const MatrixBase<D1>& d1,
+                                                  const MatrixBase<D2>& d2,
+                                                  Scalar dt)
+{
+    Matrix<typename D1::Scalar, 10, 1>  ret;
+    compose(d1, d2, dt, ret);
+    return ret;
+}
+
+template<typename D1, typename D2, typename D3, typename D4, typename D5>
+inline void compose(const MatrixBase<D1>& d1,
+                    const MatrixBase<D2>& d2,
+                    Scalar dt,
+                    MatrixBase<D3>& sum,
+                    MatrixBase<D4>& J_sum_d1,
+                    MatrixBase<D5>& J_sum_d2)
+{
+    MatrixSizeCheck<10, 1>::check(d1);
+    MatrixSizeCheck<10, 1>::check(d2);
+    MatrixSizeCheck<10, 1>::check(sum);
+    MatrixSizeCheck< 9, 1>::check(J_sum_d1);
+    MatrixSizeCheck< 9, 1>::check(J_sum_d2);
+
+    // Maps over provided data
+    Map<const Matrix<typename D1::Scalar, 3, 1> >   dp1    ( &d1( 0 ) );
+    Map<const Quaternion<typename D1::Scalar> >     dq1    ( &d1( 3 ) );
+    Map<const Matrix<typename D1::Scalar, 3, 1> >   dv1    ( &d1( 7 ) );
+    Map<const Matrix<typename D2::Scalar, 3, 1> >   dp2    ( &d2( 0 ) );
+    Map<const Quaternion<typename D2::Scalar> >     dq2    ( &d2( 3 ) );
+    Map<const Matrix<typename D2::Scalar, 3, 1> >   dv2    ( &d2( 7 ) );
+    Map<Matrix<typename D3::Scalar, 3, 1> >         sum_p  ( &sum( 0 ));
+    Map<Quaternion<typename D3::Scalar> >           sum_q  ( &sum( 3 ));
+    Map<Matrix<typename D3::Scalar, 3, 1> >         sum_v  ( &sum( 7 ));
+
+    // Some useful temporaries
+    Matrix<typename D1::Scalar, 3, 3> dR1 = dq1.matrix(); // First  Delta, DR
+    Matrix<typename D2::Scalar, 3, 3> dR2 = dq2.matrix(); // Second delta, dR
+
+    // Jac wrt first delta
+    J_sum_d1.setIdentity();                                     // dDp'/dDp = dDv'/dDv = I
+    J_sum_d1.block(0,3,3,3).noalias() = - dR1 * skew(dp2) ;     // dDp'/dDo
+    J_sum_d1.block(0,6,3,3) = Matrix3s::Identity() * dt;        // dDp'/dDv = I*dt
+    J_sum_d1.block(3,3,3,3) = dR2.transpose();                  // dDo'/dDo
+    J_sum_d1.block(6,3,3,3).noalias() = - dR1 * skew(dv2) ;     // dDv'/dDo
+
+    // Jac wrt second delta
+    J_sum_d2.setIdentity();                                     //
+    J_sum_d2.block(0,0,3,3) = dR1;                              // dDp'/ddp
+    J_sum_d2.block(6,6,3,3) = dR1;                              // dDv'/ddv
+    // J_sum_d2.block(3,3,3,3) = Matrix3s::Identity();          // dDo'/ddo = I
+
+    // compose deltas -- done here to avoid aliasing when calling with `d1` and `sum` pointing to the same variable
+    compose(d1, d2, dt, sum);
+}
+
+template<typename D1, typename D2, typename D3>
+inline void between(const MatrixBase<D1>& d1,
+                    const MatrixBase<D2>& d2,
+                    Scalar dt,
+                    MatrixBase<D3>& d2_minus_d1)
+{
+    MatrixSizeCheck<10, 1>::check(d1);
+    MatrixSizeCheck<10, 1>::check(d2);
+    MatrixSizeCheck<10, 1>::check(d2_minus_d1);
+
+    Map<const Matrix<typename D1::Scalar, 3, 1> >   dp1    ( &d1(0) );
+    Map<const Quaternion<typename D1::Scalar> >     dq1    ( &d1(3) );
+    Map<const Matrix<typename D1::Scalar, 3, 1> >   dv1    ( &d1(7) );
+    Map<const Matrix<typename D2::Scalar, 3, 1> >   dp2    ( &d2(0) );
+    Map<const Quaternion<typename D2::Scalar> >     dq2    ( &d2(3) );
+    Map<const Matrix<typename D2::Scalar, 3, 1> >   dv2    ( &d2(7) );
+    Map<Matrix<typename D3::Scalar, 3, 1> >         diff_p (&d2_minus_d1(0));
+    Map<Quaternion<typename D3::Scalar> >           diff_q (&d2_minus_d1(3));
+    Map<Matrix<typename D3::Scalar, 3, 1> >         diff_v (&d2_minus_d1(7));
+
+    diff_p = dq1.conjugate() * ( dp2 - dp1 - dv1*dt );
+    diff_q = dq1.conjugate() *   dq2;
+    diff_v = dq1.conjugate() * ( dv2 - dv1 );
+}
+
+template<typename D1, typename D2>
+inline Matrix<typename D1::Scalar, 10, 1> between(const MatrixBase<D1>& d1,
+                                                  const MatrixBase<D2>& d2,
+                                                  Scalar dt)
+{
+    Matrix<typename D1::Scalar, 10, 1> d_bet;
+    between(d1, d2, dt, d_bet);
+    return d_bet;
+}
+
+template<typename Derived>
+Matrix<typename Derived::Scalar, 9, 1> lift(const MatrixBase<Derived>& delta_in)
+{
+    MatrixSizeCheck<10, 1>::check(delta_in);
+
+    Matrix<typename Derived::Scalar, 9, 1> ret;
+
+    Map<const Matrix<typename Derived::Scalar, 3, 1> >   dp_in  ( &delta_in(0) );
+    Map<const Quaternion<typename Derived::Scalar> >     dq_in  ( &delta_in(3) );
+    Map<const Matrix<typename Derived::Scalar, 3, 1> >   dv_in  ( &delta_in(7) );
+    Map<Matrix<typename Derived::Scalar, 3, 1> >         dp_ret     ( & ret(0) );
+    Map<Matrix<typename Derived::Scalar, 3, 1> >         do_ret     ( & ret(3) );
+    Map<Matrix<typename Derived::Scalar, 3, 1> >         dv_ret     ( & ret(6) );
+
+    dp_ret = dp_in;
+    do_ret = log_q(dq_in);
+    dv_ret = dv_in;
+
+    return ret;
+}
+
+template<typename Derived>
+Matrix<typename Derived::Scalar, 10, 1> retract(const MatrixBase<Derived>& d_in)
+{
+    MatrixSizeCheck<9, 1>::check(d_in);
+
+    Matrix<typename Derived::Scalar, 10, 1> ret;
+
+    Map<const Matrix<typename Derived::Scalar, 3, 1> >   dp_in  ( &d_in(0) );
+    Map<const Matrix<typename Derived::Scalar, 3, 1> >   do_in  ( &d_in(3) );
+    Map<const Matrix<typename Derived::Scalar, 3, 1> >   dv_in  ( &d_in(6) );
+    Map<Matrix<typename Derived::Scalar, 3, 1> >         dp     ( & ret(0) );
+    Map<Quaternion<typename Derived::Scalar> >           dq     ( & ret(3) );
+    Map<Matrix<typename Derived::Scalar, 3, 1> >         dv     ( & ret(7) );
+
+    dp = dp_in;
+    dq = exp_q(do_in);
+    dv = dv_in;
+
+    return ret;
+}
+
+template<typename D1, typename D2, typename D3>
+inline void diff(const MatrixBase<D1>& d1,
+                    const MatrixBase<D2>& d2,
+                    MatrixBase<D3>& err)
+{
+    err = lift( between(d1, d2, 0.0) );
+}
+
+template<typename D1, typename D2>
+inline Matrix<typename D1::Scalar, 9, 1> diff(const MatrixBase<D1>& d1,
+                    const MatrixBase<D2>& d2)
+{
+
+    return lift( between(d1, d2, 0.0) );
+}
+
+
+} // namespace imu
+} // namespace wolf
+
+#endif /* IMU_TOOLS_H_ */

src/processor_imu.cpp

@@ -108,6 +108,125 @@ 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 (_motion_second.ts_ <= _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;
+}
+
 } // namespace wolf
 
 

src/processor_imu.h

@@ -406,102 +406,6 @@ inline Eigen::VectorXs ProcessorIMU::deltaZero() const
     return (Eigen::VectorXs(10) << 0,0,0,  0,0,0,1,  0,0,0 ).finished(); // p, q, v
 }
 
-inline Motion ProcessorIMU::interpolate(const Motion& _motion_ref,
-                                        Motion& _motion_second,
-                                        TimeStamp& _ts)
-{
-    // 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.delta_ = deltaZero();
-        motion_int.delta_cov_.setZero();
-        return motion_int;
-    }
-    if (_motion_second.ts_ <= _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");
-
-    // Interpolate between motion_ref and motion, as in:
-    //
-    // motion_ref ------ ts_ ------ motion
-    //                 return
-    //
-    // and return the value at the given time_stamp ts_.
-    //
-    // The 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 quaternion receives a slerp interpolation
-    //    q_ret = q_ref.slerp( (ts - t_ref) / dt , q);
-    //
-    // See extensive documentation in ProcessorMotion::interpolate().
-
-    // reference
-    TimeStamp           t_ref       = _motion_ref.ts_;
-
-    // final
-    TimeStamp           t_sec       = _motion_second.ts_;
-    Map<VectorXs>       dp_sec(_motion_second.delta_.data(), 3);
-    Map<Quaternions>    dq_sec(_motion_second.delta_.data() + 3);
-    Map<VectorXs>       dv_sec(_motion_second.delta_.data() + 7, 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);
-    Map<VectorXs>       dv_int(motion_int.delta_.data() + 7, 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 - t_ref) / (t_sec - t_ref); // interpolation factor (0 to 1)
-    motion_int.ts_  = _ts;
-    dp_int          = tau * dp_sec;
-    dq_int          = Quaternions::Identity().slerp(tau, dq_sec);
-    dv_int          = tau * dv_sec;
-    deltaPlusDelta(_motion_ref.delta_integr_, motion_int.delta_, (t_sec - t_ref), motion_int.delta_integr_, J_ref, J_int);
-
-    // interpolate covariances
-    motion_int.delta_cov_           = tau * _motion_second.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 )
-    dp_sec          = dq_int.conjugate() * ((1 - tau) * dp_sec);
-    dq_sec          = dq_int.conjugate() * dq_sec;
-    dv_sec          = dq_int.conjugate() * ((1 - tau) * dv_sec);
-    _motion_second.delta_cov_ = (1 - tau) * _motion_second.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;
-}
-
-
 inline void ProcessorIMU::resetDerived()
 {
     // Cast a pointer to origin IMU frame

src/processor_motion.cpp

@@ -318,7 +318,7 @@ void ProcessorMotion::integrateOneStep()
                           + jacobian_delta_        * delta_cov_                                 * jacobian_delta_.transpose();
 
     // push all into buffer
-    getBuffer().get().push_back(Motion(incoming_ptr_->getTimeStamp(),
+    getBuffer().get().emplace_back(incoming_ptr_->getTimeStamp(),
                                    incoming_ptr_->getData(),
                                    incoming_ptr_->getDataCovariance(),
                                    delta_,
@@ -327,7 +327,7 @@ void ProcessorMotion::integrateOneStep()
                                    delta_integrated_cov_,
                                    jacobian_delta_,
                                    jacobian_delta_preint_,
-                                   jacobian_calib_));
+                                   jacobian_calib_);
 }
 
 void ProcessorMotion::reintegrateBuffer(CaptureMotionPtr _capture_ptr)
@@ -345,6 +345,7 @@ void ProcessorMotion::reintegrateBuffer(CaptureMotionPtr _capture_ptr)
         const Scalar dt = motion_it->ts_ - prev_motion_it->ts_;
 
         // re-convert data to delta with the new calibration parameters
+        // FIXME: Get calibration params from Capture or capture->Sensor
         VectorXs calib = getCalibration();
 
         data2delta(motion_it->data_, motion_it->data_cov_, dt, motion_it->delta_, motion_it->delta_cov_, calib, jacobian_delta_calib_);

src/sensor_base.cpp

@@ -6,14 +6,20 @@ namespace wolf {
 
 unsigned int SensorBase::sensor_id_count_ = 0;
 
-SensorBase::SensorBase(const std::string& _type, StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, StateBlockPtr _intr_ptr,
-                       const unsigned int _noise_size, const bool _extr_dyn) :
+SensorBase::SensorBase(const std::string& _type,
+                       StateBlockPtr _p_ptr,
+                       StateBlockPtr _o_ptr,
+                       StateBlockPtr _intr_ptr,
+                       const unsigned int _noise_size,
+                       const bool _extr_dyn,
+                       const bool _intr_dyn) :
         NodeBase("SENSOR", _type),
         hardware_ptr_(),
         state_block_vec_(3), // allow for 3 state blocks by default. Resize in derived constructors if needed.
         is_removing_(false),
         sensor_id_(++sensor_id_count_), // simple ID factory
         extrinsic_dynamic_(_extr_dyn),
+        intrinsic_dynamic_(_intr_dyn),
         noise_std_(_noise_size),
         noise_cov_(_noise_size, _noise_size)
 {
@@ -24,14 +30,20 @@ SensorBase::SensorBase(const std::string& _type, StateBlockPtr _p_ptr, StateBloc
     //
 }
 
-SensorBase::SensorBase(const std::string& _type, StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, StateBlockPtr _intr_ptr,
-                       const Eigen::VectorXs & _noise_std, const bool _extr_dyn) :
+SensorBase::SensorBase(const std::string& _type,
+                       StateBlockPtr _p_ptr,
+                       StateBlockPtr _o_ptr,
+                       StateBlockPtr _intr_ptr,
+                       const Eigen::VectorXs & _noise_std,
+                       const bool _extr_dyn,
+                       const bool _intr_dyn) :
         NodeBase("SENSOR", _type),
         hardware_ptr_(),
         state_block_vec_(6), // allow for 3 state blocks by default. Resize in derived constructors if needed.
         is_removing_(false),
         sensor_id_(++sensor_id_count_), // simple ID factory
         extrinsic_dynamic_(_extr_dyn),
+        intrinsic_dynamic_(_intr_dyn),
         noise_std_(_noise_std),
         noise_cov_(_noise_std.size(), _noise_std.size())
 {

src/sensor_base.h

@@ -41,6 +41,7 @@ class SensorBase : public NodeBase, public std::enable_shared_from_this<SensorBa
         unsigned int sensor_id_;   // sensor ID
 
         bool extrinsic_dynamic_;// extrinsic parameters vary with time? If so, they will be taken from the Capture nodes. TODO: Not Yet Implemented.
+        bool intrinsic_dynamic_;// intrinsic parameters vary with time? If so, they will be taken from the Capture nodes. TODO: Not Yet Implemented.
 
         Eigen::VectorXs noise_std_; // std of sensor noise
         Eigen::MatrixXs noise_cov_; // cov matrix of noise
@@ -57,7 +58,13 @@ class SensorBase : public NodeBase, public std::enable_shared_from_this<SensorBa
          * \param _extr_dyn Flag indicating if extrinsics are dynamic (moving) or static (not moving)
          *
          **/
-        SensorBase(const std::string& _type, StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, StateBlockPtr _intr_ptr, const unsigned int _noise_size, const bool _extr_dyn = false);
+        SensorBase(const std::string& _type,
+                   StateBlockPtr _p_ptr,
+                   StateBlockPtr _o_ptr,
+                   StateBlockPtr _intr_ptr,
+                   const unsigned int _noise_size,
+                   const bool _extr_dyn = false,
+                   const bool _intr_dyn = false);
 
         /** \brief Constructor with noise std vector
          *
@@ -70,7 +77,14 @@ class SensorBase : public NodeBase, public std::enable_shared_from_this<SensorBa
          * \param _extr_dyn Flag indicating if extrinsics are dynamic (moving) or static (not moving)
          *
          **/
-        SensorBase(const std::string& _type, StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, StateBlockPtr _intr_ptr, const Eigen::VectorXs & _noise_std, const bool _extr_dyn = false);
+        SensorBase(const std::string& _type,
+                   StateBlockPtr _p_ptr,
+                   StateBlockPtr _o_ptr,
+                   StateBlockPtr _intr_ptr,
+                   const Eigen::VectorXs & _noise_std,
+                   const bool _extr_dyn = false,
+                   const bool _intr_dyn = false);
+
         virtual ~SensorBase();
         void remove();
 

src/test/CMakeLists.txt

@@ -56,9 +56,9 @@ target_link_libraries(gtest_feature_base ${PROJECT_NAME})
 wolf_add_gtest(gtest_frame_base gtest_frame_base.cpp)
 target_link_libraries(gtest_frame_base ${PROJECT_NAME})
 
-# FrameIMU class test
-wolf_add_gtest(gtest_frame_imu gtest_frame_imu.cpp)
-target_link_libraries(gtest_frame_imu ${PROJECT_NAME})
+# IMU tools test
+wolf_add_gtest(gtest_imu_tools gtest_imu_tools.cpp)
+# target_link_libraries(gtest_imu_tools ${PROJECT_NAME}) // WOLF library not needed
 
 # LocalParametrizationXxx classes test
 wolf_add_gtest(gtest_local_param gtest_local_param.cpp)
@@ -106,6 +106,10 @@ target_link_libraries(gtest_constraint_odom_3D ${PROJECT_NAME})
 wolf_add_gtest(gtest_feature_imu gtest_feature_imu.cpp)
 target_link_libraries(gtest_feature_imu ${PROJECT_NAME})
 
+# FrameIMU class test
+wolf_add_gtest(gtest_frame_imu gtest_frame_imu.cpp)
+target_link_libraries(gtest_frame_imu ${PROJECT_NAME})
+
 # IMU Bias + estimation tests
 #wolf_add_gtest(gtest_imuBias gtest_imuBias.cpp)
 #target_link_libraries(gtest_imuBias ${PROJECT_NAME})

src/test/gtest_imu_tools.cpp

+/*
+ * gtest_imu_tools.cpp
+ *
+ *  Created on: Jul 29, 2017
+ *      Author: jsola
+ */
+
+#include "imu_tools.h"
+
+#include "utils_gtest.h"
+
+
+using namespace Eigen;
+using namespace wolf;
+
+TEST(IMU_tools, identity)
+{
+    VectorXs id_true;
+    id_true.setZero(10);
+    id_true(6) = 1.0;
+
+    VectorXs id = imu::identity<Scalar>();
+    ASSERT_MATRIX_APPROX(id, id_true, 1e-10);
+}
+
+TEST(IMU_tools, inverse)
+{
+    VectorXs d(10), id(10), iid(10), iiid(10), I(10);
+    Vector4s qv;
+    Scalar dt = 0.1;
+
+    qv = (Vector4s() << 3, 4, 5, 6).finished().normalized();
+    d << 0, 1, 2, qv, 7, 8, 9;
+
+    id   = imu::inverse(d, dt);
+
+    imu::compose(id, d, dt, I);
+    ASSERT_MATRIX_APPROX(I, imu::identity(), 1e-10);
+    imu::compose(d, id, -dt, I); // Observe -dt is reversed !!
+    ASSERT_MATRIX_APPROX(I, imu::identity(), 1e-10);
+
+    imu::inverse(id, -dt, iid); // Observe -dt is reversed !!
+    ASSERT_MATRIX_APPROX( d,  iid, 1e-10);
+    iiid = imu::inverse(iid, dt);
+    ASSERT_MATRIX_APPROX(id, iiid, 1e-10);
+}
+
+TEST(IMU_tools, compose_between)
+{
+    VectorXs dx1(10), dx2(10), dx3(10);
+    Matrix<Scalar, 10, 1> d1, d2, d3;
+    Vector4s qv;
+    Scalar dt = 0.1;
+
+    qv = (Vector4s() << 3, 4, 5, 6).finished().normalized();
+    dx1 << 0, 1, 2, qv, 7, 8, 9;
+    d1 = dx1;
+    qv = (Vector4s() << 6, 5, 4, 3).finished().normalized();
+    dx2 << 9, 8, 7, qv, 2, 1, 0;
+    d2 = dx2;
+
+    // combinations of templates for sum()
+    imu::compose(dx1, dx2, dt, dx3);
+    imu::compose(d1, d2, dt, d3);
+    ASSERT_MATRIX_APPROX(d3, dx3, 1e-10);
+
+    imu::compose(d1, dx2, dt, dx3);
+    d3 = imu::compose(dx1, d2, dt);
+    ASSERT_MATRIX_APPROX(d3, dx3, 1e-10);
+
+    // minus(d1, d3) should recover delta_2
+    VectorXs diffx(10);
+    Matrix<Scalar,10,1> diff;
+    imu::between(d1, d3, dt, diff);
+    ASSERT_MATRIX_APPROX(diff, d2, 1e-10);
+
+    // minus(d3, d1, -dt) should recover inverse(d2, dt)
+    diff = imu::between(d3, d1, -dt);
+    ASSERT_MATRIX_APPROX(diff, imu::inverse(d2, dt), 1e-10);
+}
+
+TEST(IMU_tools, lift_retract)
+{
+    VectorXs d_min(9); d_min << 0, 1, 2, .3, .4, .5, 6, 7, 8; // use angles in the ball theta < pi
+
+    // transform to delta
+    VectorXs delta = imu::retract(d_min);
+
+    // expected delta
+    Vector3s dp = d_min.head(3);
+    Quaternions dq = v2q(d_min.segment(3,3));
+    Vector3s dv = d_min.tail(3);
+    VectorXs delta_true(10); delta_true << dp, dq.coeffs(), dv;
+    ASSERT_MATRIX_APPROX(delta, delta_true, 1e-10);
+
+    // transform to a new tangent -- should be the original tangent
+    VectorXs d_from_delta = imu::lift(delta);
+    ASSERT_MATRIX_APPROX(d_from_delta, d_min, 1e-10);
+
+    // transform to a new delta -- should be the previous delta
+    VectorXs delta_from_d = imu::retract(d_from_delta);
+    ASSERT_MATRIX_APPROX(delta_from_d, delta, 1e-10);
+}
+
+TEST(IMU_tools, diff)
+{
+    VectorXs d1(10), d2(10);
+    Vector4s qv = (Vector4s() << 3, 4, 5, 6).finished().normalized();
+    d1 << 0, 1, 2, qv, 7, 8, 9;
+    d2 = d1;
+
+    VectorXs err(9);
+    imu::diff(d1, d2, err);
+    ASSERT_MATRIX_APPROX(err, VectorXs::Zero(9), 1e-10);
+    ASSERT_MATRIX_APPROX(imu::diff(d1, d2), VectorXs::Zero(9), 1e-10);
+}
+
+int main(int argc, char **argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}
+

src/test/gtest_processor_imu.cpp

@@ -206,6 +206,54 @@ 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,  0,0,0,  0,0,0; // Try some non-zero biases
+
+    problem->getProcessorMotionPtr()->setOrigin(x0, t);
+
+    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->getProcessorMotionPtr()->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->getProcessorMotionPtr()->getMotion();
+
+    cap_imu_ptr->setTimeStamp(0.15);
+    sensor_ptr->process(cap_imu_ptr);
+    Motion mot_final = problem->getProcessorMotionPtr()->getMotion();
+    mot_final.delta_ = mot_final.delta_integr_;
+    Motion mot_sec = mot_final;
+
+    problem->getProcessorMotionPtr()->getBuffer().print(0,1,1,0);
+
+class P : wolf::ProcessorIMU
+{
+    public:
+        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)
 {
     t.set(0); // clock in 0,1 ms ticks