diff --git a/include/core/processor/processor_motion.h b/include/core/processor/processor_motion.h
index cd1d7972d274dc6bafafa3227af7b27c11f76ebd..81df227434c641cfa120855a242a98b918805e50 100644
--- a/include/core/processor/processor_motion.h
+++ b/include/core/processor/processor_motion.h
@@ -402,39 +402,39 @@ class ProcessorMotion : public ProcessorBase
          */
         virtual Eigen::VectorXs deltaZero() const = 0;
 
-        /** \brief Interpolate motion to an intermediate time-stamp
-         *
-         * @param _ref    The motion reference
-         * @param _second The second motion. It is modified by the function (see documentation below).
-         * @param _ts     The intermediate time stamp: it must be bounded by  `_ref.ts_ <= _ts <= _second.ts_`.
-         * @return        The interpolated motion (see documentation below).
-         *
-         * This function interpolates a motion between two existing motions.
-         *
-         * In this base implementation, we just provide the closest motion provided (ref or second),
-         * so really no interpolation takes place and just the current data and delta are updated.
-         *
-         * Should you require finer interpolation, you must overload this method in your derived class.
-         */
-        virtual Motion interpolate(const Motion& _ref, Motion& _second, TimeStamp& _ts);
-
-        /** \brief Interpolate motion to an intermediate time-stamp
-         *
-         * @param _ref1   The first motion reference
-         * @param _ref2   The second motion reference.
-         * @param _ts     The intermediate time stamp: it must be bounded by  `_ref.ts_ <= _ts <= _second.ts_`.
-         * @param _second The second part motion after interpolation, so that return (+) second = ref2.
-         * @return        The interpolated motion (see documentation below).
-         *
-         * This function interpolates a motion between two existing motions.
-         *
-         * In this base implementation, we just provide the closest motion provided (ref1 or ref2),
-         * the second motion being the complementary part,
-         * so really no interpolation takes place and just the current data and delta are updated.
-         *
-         * Should you require finer interpolation, you must overload this method in your derived class.
-         */
-        virtual Motion interpolate(const Motion& _ref1, const Motion& _ref2, const TimeStamp& _ts, Motion& _second);
+//        /** \brief Interpolate motion to an intermediate time-stamp
+//         *
+//         * @param _ref    The motion reference
+//         * @param _second The second motion. It is modified by the function (see documentation below).
+//         * @param _ts     The intermediate time stamp: it must be bounded by  `_ref.ts_ <= _ts <= _second.ts_`.
+//         * @return        The interpolated motion (see documentation below).
+//         *
+//         * This function interpolates a motion between two existing motions.
+//         *
+//         * In this base implementation, we just provide the closest motion provided (ref or second),
+//         * so really no interpolation takes place and just the current data and delta are updated.
+//         *
+//         * Should you require finer interpolation, you must overload this method in your derived class.
+//         */
+//        virtual Motion interpolate(const Motion& _ref, Motion& _second, TimeStamp& _ts);
+
+//        /** \brief Interpolate motion to an intermediate time-stamp
+//         *
+//         * @param _ref1   The first motion reference
+//         * @param _ref2   The second motion reference.
+//         * @param _ts     The intermediate time stamp: it must be bounded by  `_ref.ts_ <= _ts <= _second.ts_`.
+//         * @param _second The second part motion after interpolation, so that return (+) second = ref2.
+//         * @return        The interpolated motion (see documentation below).
+//         *
+//         * This function interpolates a motion between two existing motions.
+//         *
+//         * In this base implementation, we just provide the closest motion provided (ref1 or ref2),
+//         * the second motion being the complementary part,
+//         * so really no interpolation takes place and just the current data and delta are updated.
+//         *
+//         * Should you require finer interpolation, you must overload this method in your derived class.
+//         */
+//        virtual Motion interpolate(const Motion& _ref1, const Motion& _ref2, const TimeStamp& _ts, Motion& _second);
 
         /** \brief emplace a CaptureMotion
          * \param _ts time stamp
diff --git a/include/core/processor/processor_odom_2D.h b/include/core/processor/processor_odom_2D.h
index a55b2e1c0b8610ccc17da17878b9936d64cfc407..33833bbee6ac5240295c50fabcde3746fba43845 100644
--- a/include/core/processor/processor_odom_2D.h
+++ b/include/core/processor/processor_odom_2D.h
@@ -72,10 +72,10 @@ class ProcessorOdom2D : public ProcessorMotion
                                     const Scalar _Dt,
                                     Eigen::VectorXs& _x_plus_delta) override;
         virtual Eigen::VectorXs deltaZero() const override;
-        virtual Motion interpolate(const Motion& _ref1,
-                                   const Motion& _ref2,
-                                   const TimeStamp& _ts,
-                                   Motion& _second) override;
+//        virtual Motion interpolate(const Motion& _ref1,
+//                                   const Motion& _ref2,
+//                                   const TimeStamp& _ts,
+//                                   Motion& _second) override;
 
         virtual CaptureMotionPtr emplaceCapture(const FrameBasePtr& _frame_own,
                                                 const SensorBasePtr& _sensor,
diff --git a/include/core/processor/processor_odom_3D.h b/include/core/processor/processor_odom_3D.h
index 47334e580d8c9f1b50e8e99e05f4f76e5585180f..36125c4ea6b2ed64c3fffaf2baf6f4b4c832ab32 100644
--- a/include/core/processor/processor_odom_3D.h
+++ b/include/core/processor/processor_odom_3D.h
@@ -91,13 +91,13 @@ class ProcessorOdom3D : public ProcessorMotion
                         const Scalar _Dt,
                         Eigen::VectorXs& _x_plus_delta) override;
         Eigen::VectorXs deltaZero() const override;
-        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;
+//        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 emplaceCapture(const FrameBasePtr& _frame_own,
                                                 const SensorBasePtr& _sensor,
diff --git a/src/processor/processor_motion.cpp b/src/processor/processor_motion.cpp
index 3c80ad1050683825586155488563993cc21023ca..06d9d37f2b51e47472acd44bd6641105d30e438e 100644
--- a/src/processor/processor_motion.cpp
+++ b/src/processor/processor_motion.cpp
@@ -452,138 +452,138 @@ void ProcessorMotion::reintegrateBuffer(CaptureMotionPtr _capture_ptr)
     }
 }
 
-Motion ProcessorMotion::interpolate(const Motion& _ref, Motion& _second, TimeStamp& _ts)
-{
-    // Check time bounds
-    assert(_ref.ts_ <= _second.ts_ && "Interpolation bounds not causal.");
-    assert(_ts >= _ref.ts_    && "Interpolation time is before the _ref    motion.");
-    assert(_ts <= _second.ts_ && "Interpolation time is after  the _second motion.");
-
-    // Fraction of the time interval
-    Scalar tau    = (_ts - _ref.ts_) / (_second.ts_ - _ref.ts_);
-
-    if (tau < 0.5)
-    {
-        // _ts is closest to _ref
-        Motion interpolated                 ( _ref );
-        interpolated.ts_                    = _ts;
-        interpolated.data_                  . setZero();
-        interpolated.data_cov_              . setZero();
-        interpolated.delta_                 = deltaZero();
-        interpolated.delta_cov_             . setZero();
-        interpolated.jacobian_delta_integr_ . setIdentity();
-        interpolated.jacobian_delta_        . setZero();
-
-        return interpolated;
-    }
-    else
-    {
-        // _ts is closest to _second
-        Motion interpolated                 ( _second );
-        interpolated.ts_                    = _ts;
-        _second.data_                       . setZero();
-        _second.data_cov_                   . setZero();
-        _second.delta_                      = deltaZero();
-        _second.delta_cov_                  . setZero();
-        _second.jacobian_delta_integr_      . setIdentity();
-        _second.jacobian_delta_             . setZero();
-
-        return interpolated;
-    }
-
-}
-
-Motion ProcessorMotion::interpolate(const Motion& _ref1, const Motion& _ref2, const TimeStamp& _ts, Motion& _second)
-{
-    // Check time bounds
-    assert(_ref1.ts_ <= _ref2.ts_ && "Interpolation bounds not causal.");
-    assert(_ts >= _ref1.ts_       && "Interpolation time is before the _ref1 motion.");
-    assert(_ts <= _ref2.ts_       && "Interpolation time is after  the _ref2 motion.");
-
-    // Fraction of the time interval
-    Scalar tau    = (_ts - _ref1.ts_) / (_ref2.ts_ - _ref1.ts_);
-
-
-
-
-    Motion interpolated(_ref1);  
-    interpolated.ts_        = _ts;
-    interpolated.data_      = (1-tau)*_ref1.data_ + tau*_ref2.data_;
-    interpolated.data_cov_  = (1-tau)*_ref1.data_cov_ + tau*_ref2.data_cov_;  // bof
-    computeCurrentDelta(interpolated.data_,
-                        interpolated.data_cov_,
-                        calib_preint_,
-                        _ts.get() - _ref1.ts_.get(),
-                        interpolated.delta_,
-                        interpolated.delta_cov_,
-                        interpolated.jacobian_calib_);
-    deltaPlusDelta(_ref1.delta_integr_,
-                   interpolated.delta_,
-                   _ts.get() - _ref1.ts_.get(),
-                   interpolated.delta_integr_,
-                   interpolated.jacobian_delta_integr_,
-                   interpolated.jacobian_delta_);
-
-    _second           = _ref2;
-    _second.data_     = tau*_ref1.data_ + (1-tau)*_ref2.data_;
-    _second.data_cov_ = tau*_ref1.data_cov_ + (1-tau)*_ref2.data_cov_;  // bof
-    computeCurrentDelta(_second.data_,
-                        _second.data_cov_,
-                        calib_preint_,
-                        _ref2.ts_.get() - _ts.get(),
-                        _second.delta_,
-                        _second.delta_cov_,
-                        _second.jacobian_calib_);
-
-//    deltaPlusDelta(_second.delta_integr_,
-//                   _second.delta_,
-//                   _second.ts_.get() - _ref1.ts_.get(),
-//                   _second.delta_integr_,
-//                   _second.jacobian_delta_integr_,
-//                   _second.jacobian_delta_);
-
-    return interpolated;
-
-
-
-
-    // if (tau < 0.5)
-    // {
-    //     // _ts is closest to _ref1
-    //     Motion interpolated                 ( _ref1 );
-    //     // interpolated.ts_                    = _ref1.ts_;
-    //     // interpolated.data_                  = _ref1.data_;
-    //     // interpolated.data_cov_              = _ref1.data_cov_;
-    //     interpolated.delta_                 = deltaZero();
-    //     interpolated.delta_cov_             . setZero();
-    //     // interpolated.delta_integr_          = _ref1.delta_integr_;
-    //     // interpolated.delta_integr_cov_      = _ref1.delta_integr_cov_;
-    //     interpolated.jacobian_delta_integr_ . setIdentity();
-    //     interpolated.jacobian_delta_        . setZero();
-
-    //     _second = _ref2;
-
-    //     return interpolated;
-    // }
-    // else
-    // {
-    //     // _ts is closest to _ref2
-    //     Motion interpolated                 ( _ref2 );
-
-    //     _second                             = _ref2;
-    //     // _second.data_                       = _ref2.data_;
-    //     // _second.data_cov_                   = _ref2.data_cov_;
-    //     _second.delta_                      = deltaZero();
-    //     _second.delta_cov_                  . setZero();
-    //     // _second.delta_integr_               = _ref2.delta_integr_;
-    //     // _second.delta_integr_cov_           = _ref2.delta_integr_cov_;
-    //     _second.jacobian_delta_integr_      . setIdentity();
-    //     _second.jacobian_delta_             . setZero();
-
-    //     return interpolated;
-    // }
-
-}
+//Motion ProcessorMotion::interpolate(const Motion& _ref, Motion& _second, TimeStamp& _ts)
+//{
+//    // Check time bounds
+//    assert(_ref.ts_ <= _second.ts_ && "Interpolation bounds not causal.");
+//    assert(_ts >= _ref.ts_    && "Interpolation time is before the _ref    motion.");
+//    assert(_ts <= _second.ts_ && "Interpolation time is after  the _second motion.");
+//
+//    // Fraction of the time interval
+//    Scalar tau    = (_ts - _ref.ts_) / (_second.ts_ - _ref.ts_);
+//
+//    if (tau < 0.5)
+//    {
+//        // _ts is closest to _ref
+//        Motion interpolated                 ( _ref );
+//        interpolated.ts_                    = _ts;
+//        interpolated.data_                  . setZero();
+//        interpolated.data_cov_              . setZero();
+//        interpolated.delta_                 = deltaZero();
+//        interpolated.delta_cov_             . setZero();
+//        interpolated.jacobian_delta_integr_ . setIdentity();
+//        interpolated.jacobian_delta_        . setZero();
+//
+//        return interpolated;
+//    }
+//    else
+//    {
+//        // _ts is closest to _second
+//        Motion interpolated                 ( _second );
+//        interpolated.ts_                    = _ts;
+//        _second.data_                       . setZero();
+//        _second.data_cov_                   . setZero();
+//        _second.delta_                      = deltaZero();
+//        _second.delta_cov_                  . setZero();
+//        _second.jacobian_delta_integr_      . setIdentity();
+//        _second.jacobian_delta_             . setZero();
+//
+//        return interpolated;
+//    }
+//
+//}
+//
+//Motion ProcessorMotion::interpolate(const Motion& _ref1, const Motion& _ref2, const TimeStamp& _ts, Motion& _second)
+//{
+//    // Check time bounds
+//    assert(_ref1.ts_ <= _ref2.ts_ && "Interpolation bounds not causal.");
+//    assert(_ts >= _ref1.ts_       && "Interpolation time is before the _ref1 motion.");
+//    assert(_ts <= _ref2.ts_       && "Interpolation time is after  the _ref2 motion.");
+//
+//    // Fraction of the time interval
+//    Scalar tau    = (_ts - _ref1.ts_) / (_ref2.ts_ - _ref1.ts_);
+//
+//
+//
+//
+//    Motion interpolated(_ref1);
+//    interpolated.ts_        = _ts;
+//    interpolated.data_      = (1-tau)*_ref1.data_ + tau*_ref2.data_;
+//    interpolated.data_cov_  = (1-tau)*_ref1.data_cov_ + tau*_ref2.data_cov_;  // bof
+//    computeCurrentDelta(interpolated.data_,
+//                        interpolated.data_cov_,
+//                        calib_preint_,
+//                        _ts.get() - _ref1.ts_.get(),
+//                        interpolated.delta_,
+//                        interpolated.delta_cov_,
+//                        interpolated.jacobian_calib_);
+//    deltaPlusDelta(_ref1.delta_integr_,
+//                   interpolated.delta_,
+//                   _ts.get() - _ref1.ts_.get(),
+//                   interpolated.delta_integr_,
+//                   interpolated.jacobian_delta_integr_,
+//                   interpolated.jacobian_delta_);
+//
+//    _second           = _ref2;
+//    _second.data_     = tau*_ref1.data_ + (1-tau)*_ref2.data_;
+//    _second.data_cov_ = tau*_ref1.data_cov_ + (1-tau)*_ref2.data_cov_;  // bof
+//    computeCurrentDelta(_second.data_,
+//                        _second.data_cov_,
+//                        calib_preint_,
+//                        _ref2.ts_.get() - _ts.get(),
+//                        _second.delta_,
+//                        _second.delta_cov_,
+//                        _second.jacobian_calib_);
+//
+////    deltaPlusDelta(_second.delta_integr_,
+////                   _second.delta_,
+////                   _second.ts_.get() - _ref1.ts_.get(),
+////                   _second.delta_integr_,
+////                   _second.jacobian_delta_integr_,
+////                   _second.jacobian_delta_);
+//
+//    return interpolated;
+//
+//
+//
+//
+//    // if (tau < 0.5)
+//    // {
+//    //     // _ts is closest to _ref1
+//    //     Motion interpolated                 ( _ref1 );
+//    //     // interpolated.ts_                    = _ref1.ts_;
+//    //     // interpolated.data_                  = _ref1.data_;
+//    //     // interpolated.data_cov_              = _ref1.data_cov_;
+//    //     interpolated.delta_                 = deltaZero();
+//    //     interpolated.delta_cov_             . setZero();
+//    //     // interpolated.delta_integr_          = _ref1.delta_integr_;
+//    //     // interpolated.delta_integr_cov_      = _ref1.delta_integr_cov_;
+//    //     interpolated.jacobian_delta_integr_ . setIdentity();
+//    //     interpolated.jacobian_delta_        . setZero();
+//
+//    //     _second = _ref2;
+//
+//    //     return interpolated;
+//    // }
+//    // else
+//    // {
+//    //     // _ts is closest to _ref2
+//    //     Motion interpolated                 ( _ref2 );
+//
+//    //     _second                             = _ref2;
+//    //     // _second.data_                       = _ref2.data_;
+//    //     // _second.data_cov_                   = _ref2.data_cov_;
+//    //     _second.delta_                      = deltaZero();
+//    //     _second.delta_cov_                  . setZero();
+//    //     // _second.delta_integr_               = _ref2.delta_integr_;
+//    //     // _second.delta_integr_cov_           = _ref2.delta_integr_cov_;
+//    //     _second.jacobian_delta_integr_      . setIdentity();
+//    //     _second.jacobian_delta_             . setZero();
+//
+//    //     return interpolated;
+//    // }
+//
+//}
 
 CaptureMotionPtr ProcessorMotion::findCaptureContainingTimeStamp(const TimeStamp& _ts) const
 {
diff --git a/src/processor/processor_odom_2D.cpp b/src/processor/processor_odom_2D.cpp
index 96241bd069a64221ecce0ce86b5481f7e675ae1f..6c2a3f4192ac41cf0543b1d1a9b952351e78c572 100644
--- a/src/processor/processor_odom_2D.cpp
+++ b/src/processor/processor_odom_2D.cpp
@@ -92,10 +92,10 @@ void ProcessorOdom2D::statePlusDelta(const Eigen::VectorXs& _x, const Eigen::Vec
     _x_plus_delta(2) = pi2pi(_x(2) + _delta(2));
 }
 
-Motion ProcessorOdom2D::interpolate(const Motion& _ref1, const Motion& _ref2, const TimeStamp& _ts, Motion& _second)
-{
-    return ProcessorMotion::interpolate(_ref1, _ref2, _ts, _second);
-}
+//Motion ProcessorOdom2D::interpolate(const Motion& _ref1, const Motion& _ref2, const TimeStamp& _ts, Motion& _second)
+//{
+//    return ProcessorMotion::interpolate(_ref1, _ref2, _ts, _second);
+//}
 
 bool ProcessorOdom2D::voteForKeyFrame()
 {
diff --git a/src/processor/processor_odom_3D.cpp b/src/processor/processor_odom_3D.cpp
index 204ecd9d54d4dbd4f359e51b93553cf95baa7436..7a481e995c244180e586a52a08a84cea2fd767d5 100644
--- a/src/processor/processor_odom_3D.cpp
+++ b/src/processor/processor_odom_3D.cpp
@@ -147,190 +147,190 @@ 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)
-{
-
-//    WOLF_TRACE("motion ref ts: ", _motion_ref.ts_.get());
-//    WOLF_TRACE("interp ts    : ", _ts.get());
-//    WOLF_TRACE("motion ts    : ", _motion_second.ts_.get());
+//Motion ProcessorOdom3D::interpolate(const Motion& _motion_ref,
+//                                    Motion& _motion_second,
+//                                    TimeStamp& _ts)
+//{
 //
-//    WOLF_TRACE("ref delta size: ", _motion_ref.delta_.size(), " , cov size: ", _motion_ref.delta_cov_.size());
-//    WOLF_TRACE("ref Delta size: ", _motion_ref.delta_integr_.size(), " , cov size: ", _motion_ref.delta_integr_cov_.size());
-//    WOLF_TRACE("sec delta size: ", _motion_second.delta_.size(), " , cov size: ", _motion_second.delta_cov_.size());
-//    WOLF_TRACE("sec Delta size: ", _motion_second.delta_integr_.size(), " , cov size: ", _motion_second.delta_integr_cov_.size());
-
-    // 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");
-
-    using namespace Eigen;
-    // 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 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           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);
-
-    // 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 - 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);
-    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;
-    _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;
-}
-
-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;
-}
+////    WOLF_TRACE("motion ref ts: ", _motion_ref.ts_.get());
+////    WOLF_TRACE("interp ts    : ", _ts.get());
+////    WOLF_TRACE("motion ts    : ", _motion_second.ts_.get());
+////
+////    WOLF_TRACE("ref delta size: ", _motion_ref.delta_.size(), " , cov size: ", _motion_ref.delta_cov_.size());
+////    WOLF_TRACE("ref Delta size: ", _motion_ref.delta_integr_.size(), " , cov size: ", _motion_ref.delta_integr_cov_.size());
+////    WOLF_TRACE("sec delta size: ", _motion_second.delta_.size(), " , cov size: ", _motion_second.delta_cov_.size());
+////    WOLF_TRACE("sec Delta size: ", _motion_second.delta_integr_.size(), " , cov size: ", _motion_second.delta_integr_cov_.size());
+//
+//    // 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");
+//
+//    using namespace Eigen;
+//    // 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 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           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);
+//
+//    // 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 - 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);
+//    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;
+//    _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;
+//}
+
+//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;
+//}
 
 
 
diff --git a/test/gtest_processor_motion.cpp b/test/gtest_processor_motion.cpp
index e508069f55ffeeffcc7ab28f00af7341f2dd9c9b..7e27fb1758188d4b9038d94c733788f94b92c3a6 100644
--- a/test/gtest_processor_motion.cpp
+++ b/test/gtest_processor_motion.cpp
@@ -55,15 +55,15 @@ 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);
-        }
+//        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(){}
@@ -108,92 +108,92 @@ TEST_F(ProcessorMotion_test, IntegrateCircle)
     ASSERT_MATRIX_APPROX(problem->getCurrentState(), (Vector3s()<<0,0,0).finished(), 1e-8);
 }
 
-TEST_F(ProcessorMotion_test, Interpolate)
-{
-    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->captureCallback(capture);
-        motions.push_back(processor->getMotion(t));
-        WOLF_DEBUG("t: ", t, "  x: ", problem->getCurrentState().transpose());
-    }
-
-    TimeStamp tt    = 2.2;
-    Motion ref      = motions[2];
-    Motion second   = motions[3];
-    Motion interp   = interpolate(ref, second, tt);
-
-    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);
-
-    tt      = 2.6;
-    interp  = interpolate(ref, second, tt);
-
-    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);
-}
-
-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);
-        capture->process();
-        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_           , data                      , 1e-8);
+//TEST_F(ProcessorMotion_test, Interpolate)
+//{
+//    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->captureCallback(capture);
+//        motions.push_back(processor->getMotion(t));
+//        WOLF_DEBUG("t: ", t, "  x: ", problem->getCurrentState().transpose());
+//    }
+//
+//    TimeStamp tt    = 2.2;
+//    Motion ref      = motions[2];
+//    Motion second   = motions[3];
+//    Motion interp   = interpolate(ref, second, tt);
+//
+//    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_           , 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_           , data                      , 1e-8);
+//
+//    tt      = 2.6;
+//    interp  = interpolate(ref, second, tt);
+//
+//    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_           , data                      , 1e-8);
-//    ASSERT_MATRIX_APPROX(second.delta_          , VectorXs::Zero(3)         , 1e-8);
-    ASSERT_MATRIX_APPROX(second.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);
+//        capture->process();
+//        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_           , data                      , 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_           , 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_           , 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_           , data                      , 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)
 {
diff --git a/test/gtest_processor_odom_3D.cpp b/test/gtest_processor_odom_3D.cpp
index 3c13449cede9e839e14c50e47d7db78e2134dc1d..df554557cc80013231a7e88cc31c22cfe403cccf 100644
--- a/test/gtest_processor_odom_3D.cpp
+++ b/test/gtest_processor_odom_3D.cpp
@@ -151,423 +151,423 @@ TEST(ProcessorOdom3D, deltaPlusDelta_Jac)
 
 }
 
-TEST(ProcessorOdom3D, Interpolate0) // basic test
-{
-    /* Conditions:
-     * ref d = id
-     * ref D = id
-     * fin d = pos
-     * fin D = id
-     */
-
-    ProcessorOdom3D prc(std::make_shared<ProcessorParamsOdom3D>());
-
-    Motion ref(0.0,6,7,6,0), final(0.0,6,7,6,0), interpolated(0.0,6,7,6,0);
-
-    // set ref
-    ref.ts_ = 1;
-    ref.delta_          << 0,0,0, 0,0,0,1;
-    ref.delta_integr_   << 0,0,0, 0,0,0,1;
-
-    WOLF_DEBUG("ref delta= ", ref.delta_.transpose());
-    WOLF_DEBUG("ref Delta= ", ref.delta_integr_.transpose());
-
-    // set final
-    final.ts_ = 5;
-    final.delta_        << 1,0,0, 0,0,0,1;
-    final.delta_integr_ << 0,0,0, 0,0,0,1;
-    prc.deltaPlusDelta(ref.delta_integr_, final.delta_, (final.ts_ - ref.ts_), final.delta_integr_);
-
-    WOLF_DEBUG("final delta= ", final.delta_.transpose());
-    WOLF_DEBUG("final Delta= ", final.delta_integr_.transpose());
-
-    // interpolate!
-    Motion second = final;
-    TimeStamp t; t = 2;
-    // +--+--------+---> time(s)
-    // 1  2  3  4  5   // 2 = 25% into interpolated, 75% into second
-    interpolated = prc.interpolate(ref, second, t);
-
-    WOLF_DEBUG("interpolated delta= ", interpolated.delta_.transpose());
-    WOLF_DEBUG("interpolated Delta= ", interpolated.delta_integr_.transpose());
-
-    // delta
-    ASSERT_MATRIX_APPROX(interpolated.delta_.head<3>() , 0.25 * final.delta_.head<3>(), Constants::EPS);
-    ASSERT_MATRIX_APPROX(second.delta_.head<3>()       , 0.75 * final.delta_.head<3>(), Constants::EPS);
-
-}
-
-TEST(ProcessorOdom3D, Interpolate1) // delta algebra 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
-     */
-
-    // absolute poses: origin, ref, interp, second=final
-    Vector7s    x_o, x_r, x_i, x_s, x_f;
-    Map<Vector3s>       p_o(x_o.data(), 3);
-    Map<Quaternions>    q_o(x_o.data() +3);
-    Map<Vector3s>       p_r(x_r.data(), 3);
-    Map<Quaternions>    q_r(x_r.data() +3);
-    Map<Vector3s>       p_i(x_i.data(), 3);
-    Map<Quaternions>    q_i(x_i.data() +3);
-    Map<Vector3s>       p_s(x_s.data(), 3);
-    Map<Quaternions>    q_s(x_s.data() +3);
-    Map<Vector3s>       p_f(x_f.data(), 3);
-    Map<Quaternions>    q_f(x_f.data() +3);
-
-    // deltas -- referred to previous delta
-    //         o-r    r-i    i-s    s-f
-    Vector7s dx_or, dx_ri, dx_is, dx_rf;
-    Map<Vector3s>       dp_or(dx_or.data(), 3);
-    Map<Quaternions>    dq_or(dx_or.data() +3);
-    Map<Vector3s>       dp_ri(dx_ri.data(), 3);
-    Map<Quaternions>    dq_ri(dx_ri.data() +3);
-    Map<Vector3s>       dp_is(dx_is.data(), 3);
-    Map<Quaternions>    dq_is(dx_is.data() +3);
-    Map<Vector3s>       dp_rf(dx_rf.data(), 3);
-    Map<Quaternions>    dq_rf(dx_rf.data() +3);
-    Map<Vector7s>       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
-    Vector7s Dx_or, Dx_oi, Dx_os, Dx_of;
-    Map<Vector3s>       Dp_or(Dx_or.data(), 3);
-    Map<Quaternions>    Dq_or(Dx_or.data() +3);
-    Map<Vector3s>       Dp_oi(Dx_oi.data(), 3);
-    Map<Quaternions>    Dq_oi(Dx_oi.data() +3);
-    Map<Vector3s>       Dp_os(Dx_os.data(), 3);
-    Map<Quaternions>    Dq_os(Dx_os.data() +3);
-    Map<Vector3s>       Dp_of(Dx_of.data(), 3);
-    Map<Quaternions>    Dq_of(Dx_of.data() +3);
-
-    // time stamps and intervals
-    TimeStamp t_o(0), t_r(1), t_i(2.3), t_f(5); // t_i=2: 25% of motion; t_i=2.3: a general interpolation point
-    Scalar dt_ri = t_i - t_r;
-    Scalar dt_rf = t_f - t_r;
-
-    WOLF_DEBUG("t_o: ", t_o.get(), "; t_r: ", t_r.get(), "; t_i: ", t_i.get(), "; t_f: ", t_f.get());
-    WOLF_DEBUG("dt_ri: ", dt_ri, "; dt_rf ", dt_rf)
-
-    // Constant velocity model
-    Vector3s v;
-    Vector3s w;
-
-    // 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 origin and ref states
-    x_o << 1,2,3, 4,5,6,7; q_o.normalize();
-    x_r << 7,6,5, 4,3,2,1; q_r.normalize();
-
-    // set constant velocity params
-    v << 3,2,1; // linear velocity
-    w << .1,.2,.3; // angular velocity
-
-    // compute other poses from model
-    p_i = p_r +      v * dt_ri;
-    q_i = q_r * v2q (w * dt_ri);
-    p_f = p_r +      v * dt_rf;
-    q_f = q_r * v2q (w * dt_rf);
-    x_s = x_f;
-
-    WOLF_DEBUG("o   = ", x_o.transpose());
-    WOLF_DEBUG("r   = ", x_r.transpose());
-    WOLF_DEBUG("i   = ", x_i.transpose());
-    WOLF_DEBUG("s   = ", x_s.transpose());
-    WOLF_DEBUG("f   = ", x_f.transpose());
-
-    // deltas -- referred to previous delta
-    dp_or = q_o.conjugate() * (p_r - p_o);
-    dq_or = q_o.conjugate() *  q_r;
-    dp_ri = q_r.conjugate() * (p_i - p_r);
-    dq_ri = q_r.conjugate() *  q_i;
-    dp_is = q_i.conjugate() * (p_s - p_i);
-    dq_is = q_i.conjugate() *  q_s;
-    dp_rf = q_r.conjugate() * (p_f - p_r);
-    dq_rf = q_r.conjugate() *  q_f;
-
-    // Deltas -- always referred to origin
-    Dp_or = q_o.conjugate() * (p_r - p_o);
-    Dq_or = q_o.conjugate() *  q_r;
-    Dp_oi = q_o.conjugate() * (p_i - p_o);
-    Dq_oi = q_o.conjugate() *  q_i;
-    Dp_os = q_o.conjugate() * (p_s - p_o);
-    Dq_os = q_o.conjugate() *  q_s;
-    Dp_of = q_o.conjugate() * (p_f - p_o);
-    Dq_of = q_o.conjugate() *  q_f;
-
-    // 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);
-
-    S = F; // avoid overwriting final
-    WOLF_DEBUG("* S.d = ", S.delta_.transpose());
-    WOLF_DEBUG("  rs  = ", dx_rs.transpose());
-    ASSERT_MATRIX_APPROX(S.delta_        , dx_rs, Constants::EPS);
-
-    WOLF_DEBUG("  S.D = ", S.delta_integr_.transpose());
-    WOLF_DEBUG("  os  = ", Dx_os.transpose());
-    ASSERT_MATRIX_APPROX(S.delta_integr_ , Dx_os, Constants::EPS);
-
-    // interpolate!
-    WOLF_DEBUG("*** INTERPOLATE *** I has been computed; S has changed.");
-    I = prc.interpolate(R, S, t_i);
-
-    WOLF_DEBUG("* I.d = ", I.delta_.transpose());
-    WOLF_DEBUG("  ri  = ", dx_ri.transpose());
-    ASSERT_MATRIX_APPROX(I.delta_        , dx_ri, Constants::EPS);
-
-    WOLF_DEBUG("  I.D = ", I.delta_integr_.transpose());
-    WOLF_DEBUG("  oi  = ", Dx_oi.transpose());
-    ASSERT_MATRIX_APPROX(I.delta_integr_ , Dx_oi, Constants::EPS);
-
-    WOLF_DEBUG("* S.d = ", S.delta_.transpose());
-    WOLF_DEBUG("  is  = ", dx_is.transpose());
-    ASSERT_MATRIX_APPROX(S.delta_        , dx_is, Constants::EPS);
-
-    WOLF_DEBUG("  S.D = ", S.delta_integr_.transpose());
-    WOLF_DEBUG("  os  = ", Dx_os.transpose());
-    ASSERT_MATRIX_APPROX(S.delta_integr_ , Dx_os, Constants::EPS);
-
-}
-
-TEST(ProcessorOdom3D, Interpolate2) // 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_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 final and ref 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);
-
-    S = F; // avoid overwriting final
-    WOLF_DEBUG("* S.d = ", S.delta_.transpose());
-    WOLF_DEBUG("  rs  = ", dx_rs.transpose());
-    ASSERT_MATRIX_APPROX(S.delta_        , dx_rs, Constants::EPS);
-
-    WOLF_DEBUG("  S.D = ", S.delta_integr_.transpose());
-    WOLF_DEBUG("  os  = ", Dx_os.transpose());
-    ASSERT_MATRIX_APPROX(S.delta_integr_ , Dx_os, Constants::EPS);
-
-    // interpolate!
-    t_i = 0.5; /// before ref!
-    WOLF_DEBUG("*** INTERPOLATE *** I has been computed; S has changed.");
-    I = prc.interpolate(R, S, t_i);
-
-    WOLF_DEBUG("* I.d = ", I.delta_.transpose());
-    WOLF_DEBUG("  ri  = ", prc.deltaZero().transpose());
-    ASSERT_MATRIX_APPROX(I.delta_  , prc.deltaZero(), Constants::EPS);
-
-    WOLF_DEBUG("  I.D = ", I.delta_integr_.transpose());
-    WOLF_DEBUG("  oi  = ", Dx_or.transpose());
-    ASSERT_MATRIX_APPROX(I.delta_integr_ , Dx_or, Constants::EPS);
-
-    WOLF_DEBUG("* S.d = ", S.delta_.transpose());
-    WOLF_DEBUG("  is  = ", dx_rf.transpose());
-    ASSERT_MATRIX_APPROX(S.delta_        , dx_rf, Constants::EPS);
-
-    WOLF_DEBUG("  S.D = ", S.delta_integr_.transpose());
-    WOLF_DEBUG("  os  = ", Dx_of.transpose());
-    ASSERT_MATRIX_APPROX(S.delta_integr_ , Dx_of, Constants::EPS);
-
-    // interpolate!
-    t_i = 5.5; /// after ref!
-    S = F;
-    WOLF_DEBUG("*** INTERPOLATE *** I has been computed; S has changed.");
-    I = prc.interpolate(R, S, t_i);
-
-    WOLF_DEBUG("* I.d = ", I.delta_.transpose());
-    WOLF_DEBUG("  ri  = ", dx_rf.transpose());
-    ASSERT_MATRIX_APPROX(I.delta_  , dx_rf, Constants::EPS);
-
-    WOLF_DEBUG("  I.D = ", I.delta_integr_.transpose());
-    WOLF_DEBUG("  oi  = ", Dx_of.transpose());
-    ASSERT_MATRIX_APPROX(I.delta_integr_ , Dx_of, Constants::EPS);
-
-    WOLF_DEBUG("* S.d = ", S.delta_.transpose());
-    WOLF_DEBUG("  is  = ", prc.deltaZero().transpose());
-    ASSERT_MATRIX_APPROX(S.delta_ , prc.deltaZero(), Constants::EPS);
-
-    WOLF_DEBUG("  S.D = ", S.delta_integr_.transpose());
-    WOLF_DEBUG("  os  = ", Dx_of.transpose());
-    ASSERT_MATRIX_APPROX(S.delta_integr_ , Dx_of, Constants::EPS);
-
-}
-
-
-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);
-
-}
+//TEST(ProcessorOdom3D, Interpolate0) // basic test
+//{
+//    /* Conditions:
+//     * ref d = id
+//     * ref D = id
+//     * fin d = pos
+//     * fin D = id
+//     */
+//
+//    ProcessorOdom3D prc(std::make_shared<ProcessorParamsOdom3D>());
+//
+//    Motion ref(0.0,6,7,6,0), final(0.0,6,7,6,0), interpolated(0.0,6,7,6,0);
+//
+//    // set ref
+//    ref.ts_ = 1;
+//    ref.delta_          << 0,0,0, 0,0,0,1;
+//    ref.delta_integr_   << 0,0,0, 0,0,0,1;
+//
+//    WOLF_DEBUG("ref delta= ", ref.delta_.transpose());
+//    WOLF_DEBUG("ref Delta= ", ref.delta_integr_.transpose());
+//
+//    // set final
+//    final.ts_ = 5;
+//    final.delta_        << 1,0,0, 0,0,0,1;
+//    final.delta_integr_ << 0,0,0, 0,0,0,1;
+//    prc.deltaPlusDelta(ref.delta_integr_, final.delta_, (final.ts_ - ref.ts_), final.delta_integr_);
+//
+//    WOLF_DEBUG("final delta= ", final.delta_.transpose());
+//    WOLF_DEBUG("final Delta= ", final.delta_integr_.transpose());
+//
+//    // interpolate!
+//    Motion second = final;
+//    TimeStamp t; t = 2;
+//    // +--+--------+---> time(s)
+//    // 1  2  3  4  5   // 2 = 25% into interpolated, 75% into second
+//    interpolated = prc.interpolate(ref, second, t);
+//
+//    WOLF_DEBUG("interpolated delta= ", interpolated.delta_.transpose());
+//    WOLF_DEBUG("interpolated Delta= ", interpolated.delta_integr_.transpose());
+//
+//    // delta
+//    ASSERT_MATRIX_APPROX(interpolated.delta_.head<3>() , 0.25 * final.delta_.head<3>(), Constants::EPS);
+//    ASSERT_MATRIX_APPROX(second.delta_.head<3>()       , 0.75 * final.delta_.head<3>(), Constants::EPS);
+//
+//}
+//
+//TEST(ProcessorOdom3D, Interpolate1) // delta algebra 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
+//     */
+//
+//    // absolute poses: origin, ref, interp, second=final
+//    Vector7s    x_o, x_r, x_i, x_s, x_f;
+//    Map<Vector3s>       p_o(x_o.data(), 3);
+//    Map<Quaternions>    q_o(x_o.data() +3);
+//    Map<Vector3s>       p_r(x_r.data(), 3);
+//    Map<Quaternions>    q_r(x_r.data() +3);
+//    Map<Vector3s>       p_i(x_i.data(), 3);
+//    Map<Quaternions>    q_i(x_i.data() +3);
+//    Map<Vector3s>       p_s(x_s.data(), 3);
+//    Map<Quaternions>    q_s(x_s.data() +3);
+//    Map<Vector3s>       p_f(x_f.data(), 3);
+//    Map<Quaternions>    q_f(x_f.data() +3);
+//
+//    // deltas -- referred to previous delta
+//    //         o-r    r-i    i-s    s-f
+//    Vector7s dx_or, dx_ri, dx_is, dx_rf;
+//    Map<Vector3s>       dp_or(dx_or.data(), 3);
+//    Map<Quaternions>    dq_or(dx_or.data() +3);
+//    Map<Vector3s>       dp_ri(dx_ri.data(), 3);
+//    Map<Quaternions>    dq_ri(dx_ri.data() +3);
+//    Map<Vector3s>       dp_is(dx_is.data(), 3);
+//    Map<Quaternions>    dq_is(dx_is.data() +3);
+//    Map<Vector3s>       dp_rf(dx_rf.data(), 3);
+//    Map<Quaternions>    dq_rf(dx_rf.data() +3);
+//    Map<Vector7s>       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
+//    Vector7s Dx_or, Dx_oi, Dx_os, Dx_of;
+//    Map<Vector3s>       Dp_or(Dx_or.data(), 3);
+//    Map<Quaternions>    Dq_or(Dx_or.data() +3);
+//    Map<Vector3s>       Dp_oi(Dx_oi.data(), 3);
+//    Map<Quaternions>    Dq_oi(Dx_oi.data() +3);
+//    Map<Vector3s>       Dp_os(Dx_os.data(), 3);
+//    Map<Quaternions>    Dq_os(Dx_os.data() +3);
+//    Map<Vector3s>       Dp_of(Dx_of.data(), 3);
+//    Map<Quaternions>    Dq_of(Dx_of.data() +3);
+//
+//    // time stamps and intervals
+//    TimeStamp t_o(0), t_r(1), t_i(2.3), t_f(5); // t_i=2: 25% of motion; t_i=2.3: a general interpolation point
+//    Scalar dt_ri = t_i - t_r;
+//    Scalar dt_rf = t_f - t_r;
+//
+//    WOLF_DEBUG("t_o: ", t_o.get(), "; t_r: ", t_r.get(), "; t_i: ", t_i.get(), "; t_f: ", t_f.get());
+//    WOLF_DEBUG("dt_ri: ", dt_ri, "; dt_rf ", dt_rf)
+//
+//    // Constant velocity model
+//    Vector3s v;
+//    Vector3s w;
+//
+//    // 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 origin and ref states
+//    x_o << 1,2,3, 4,5,6,7; q_o.normalize();
+//    x_r << 7,6,5, 4,3,2,1; q_r.normalize();
+//
+//    // set constant velocity params
+//    v << 3,2,1; // linear velocity
+//    w << .1,.2,.3; // angular velocity
+//
+//    // compute other poses from model
+//    p_i = p_r +      v * dt_ri;
+//    q_i = q_r * v2q (w * dt_ri);
+//    p_f = p_r +      v * dt_rf;
+//    q_f = q_r * v2q (w * dt_rf);
+//    x_s = x_f;
+//
+//    WOLF_DEBUG("o   = ", x_o.transpose());
+//    WOLF_DEBUG("r   = ", x_r.transpose());
+//    WOLF_DEBUG("i   = ", x_i.transpose());
+//    WOLF_DEBUG("s   = ", x_s.transpose());
+//    WOLF_DEBUG("f   = ", x_f.transpose());
+//
+//    // deltas -- referred to previous delta
+//    dp_or = q_o.conjugate() * (p_r - p_o);
+//    dq_or = q_o.conjugate() *  q_r;
+//    dp_ri = q_r.conjugate() * (p_i - p_r);
+//    dq_ri = q_r.conjugate() *  q_i;
+//    dp_is = q_i.conjugate() * (p_s - p_i);
+//    dq_is = q_i.conjugate() *  q_s;
+//    dp_rf = q_r.conjugate() * (p_f - p_r);
+//    dq_rf = q_r.conjugate() *  q_f;
+//
+//    // Deltas -- always referred to origin
+//    Dp_or = q_o.conjugate() * (p_r - p_o);
+//    Dq_or = q_o.conjugate() *  q_r;
+//    Dp_oi = q_o.conjugate() * (p_i - p_o);
+//    Dq_oi = q_o.conjugate() *  q_i;
+//    Dp_os = q_o.conjugate() * (p_s - p_o);
+//    Dq_os = q_o.conjugate() *  q_s;
+//    Dp_of = q_o.conjugate() * (p_f - p_o);
+//    Dq_of = q_o.conjugate() *  q_f;
+//
+//    // 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);
+//
+//    S = F; // avoid overwriting final
+//    WOLF_DEBUG("* S.d = ", S.delta_.transpose());
+//    WOLF_DEBUG("  rs  = ", dx_rs.transpose());
+//    ASSERT_MATRIX_APPROX(S.delta_        , dx_rs, Constants::EPS);
+//
+//    WOLF_DEBUG("  S.D = ", S.delta_integr_.transpose());
+//    WOLF_DEBUG("  os  = ", Dx_os.transpose());
+//    ASSERT_MATRIX_APPROX(S.delta_integr_ , Dx_os, Constants::EPS);
+//
+//    // interpolate!
+//    WOLF_DEBUG("*** INTERPOLATE *** I has been computed; S has changed.");
+//    I = prc.interpolate(R, S, t_i);
+//
+//    WOLF_DEBUG("* I.d = ", I.delta_.transpose());
+//    WOLF_DEBUG("  ri  = ", dx_ri.transpose());
+//    ASSERT_MATRIX_APPROX(I.delta_        , dx_ri, Constants::EPS);
+//
+//    WOLF_DEBUG("  I.D = ", I.delta_integr_.transpose());
+//    WOLF_DEBUG("  oi  = ", Dx_oi.transpose());
+//    ASSERT_MATRIX_APPROX(I.delta_integr_ , Dx_oi, Constants::EPS);
+//
+//    WOLF_DEBUG("* S.d = ", S.delta_.transpose());
+//    WOLF_DEBUG("  is  = ", dx_is.transpose());
+//    ASSERT_MATRIX_APPROX(S.delta_        , dx_is, Constants::EPS);
+//
+//    WOLF_DEBUG("  S.D = ", S.delta_integr_.transpose());
+//    WOLF_DEBUG("  os  = ", Dx_os.transpose());
+//    ASSERT_MATRIX_APPROX(S.delta_integr_ , Dx_os, Constants::EPS);
+//
+//}
+//
+//TEST(ProcessorOdom3D, Interpolate2) // 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_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 final and ref 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);
+//
+//    S = F; // avoid overwriting final
+//    WOLF_DEBUG("* S.d = ", S.delta_.transpose());
+//    WOLF_DEBUG("  rs  = ", dx_rs.transpose());
+//    ASSERT_MATRIX_APPROX(S.delta_        , dx_rs, Constants::EPS);
+//
+//    WOLF_DEBUG("  S.D = ", S.delta_integr_.transpose());
+//    WOLF_DEBUG("  os  = ", Dx_os.transpose());
+//    ASSERT_MATRIX_APPROX(S.delta_integr_ , Dx_os, Constants::EPS);
+//
+//    // interpolate!
+//    t_i = 0.5; /// before ref!
+//    WOLF_DEBUG("*** INTERPOLATE *** I has been computed; S has changed.");
+//    I = prc.interpolate(R, S, t_i);
+//
+//    WOLF_DEBUG("* I.d = ", I.delta_.transpose());
+//    WOLF_DEBUG("  ri  = ", prc.deltaZero().transpose());
+//    ASSERT_MATRIX_APPROX(I.delta_  , prc.deltaZero(), Constants::EPS);
+//
+//    WOLF_DEBUG("  I.D = ", I.delta_integr_.transpose());
+//    WOLF_DEBUG("  oi  = ", Dx_or.transpose());
+//    ASSERT_MATRIX_APPROX(I.delta_integr_ , Dx_or, Constants::EPS);
+//
+//    WOLF_DEBUG("* S.d = ", S.delta_.transpose());
+//    WOLF_DEBUG("  is  = ", dx_rf.transpose());
+//    ASSERT_MATRIX_APPROX(S.delta_        , dx_rf, Constants::EPS);
+//
+//    WOLF_DEBUG("  S.D = ", S.delta_integr_.transpose());
+//    WOLF_DEBUG("  os  = ", Dx_of.transpose());
+//    ASSERT_MATRIX_APPROX(S.delta_integr_ , Dx_of, Constants::EPS);
+//
+//    // interpolate!
+//    t_i = 5.5; /// after ref!
+//    S = F;
+//    WOLF_DEBUG("*** INTERPOLATE *** I has been computed; S has changed.");
+//    I = prc.interpolate(R, S, t_i);
+//
+//    WOLF_DEBUG("* I.d = ", I.delta_.transpose());
+//    WOLF_DEBUG("  ri  = ", dx_rf.transpose());
+//    ASSERT_MATRIX_APPROX(I.delta_  , dx_rf, Constants::EPS);
+//
+//    WOLF_DEBUG("  I.D = ", I.delta_integr_.transpose());
+//    WOLF_DEBUG("  oi  = ", Dx_of.transpose());
+//    ASSERT_MATRIX_APPROX(I.delta_integr_ , Dx_of, Constants::EPS);
+//
+//    WOLF_DEBUG("* S.d = ", S.delta_.transpose());
+//    WOLF_DEBUG("  is  = ", prc.deltaZero().transpose());
+//    ASSERT_MATRIX_APPROX(S.delta_ , prc.deltaZero(), Constants::EPS);
+//
+//    WOLF_DEBUG("  S.D = ", S.delta_integr_.transpose());
+//    WOLF_DEBUG("  os  = ", Dx_of.transpose());
+//    ASSERT_MATRIX_APPROX(S.delta_integr_ , Dx_of, Constants::EPS);
+//
+//}
+//
+//
+//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);
+//
+//}