diff --git a/src/IMU_tools.h b/src/IMU_tools.h
index 4f6ecda67b6695723b11ddecc56403da57a931a0..6d231223554ec4ea5e196cb7220a40e0eb42c001 100644
--- a/src/IMU_tools.h
+++ b/src/IMU_tools.h
@@ -34,10 +34,10 @@
  *   - between: Db = D2 (-) D1, so that D2 = D1 (+) Db
  *   - composeOverState: x2 = x1 (+) D
  *   - betweenStates: D = x2 (-) x1, so that x2 = x1 (+) D
- *   - lift: got from Delta manifold to tangent space (equivalent to log() in rotations)
- *   - retract: go from tangent space to delta manifold (equivalent to exp() in rotations)
- *   - plus: D2 = D1 (+) retract(d)
- *   - diff: d = lift( D2 (-) D1 )
+ *   - log: got from Delta manifold to tangent space (equivalent to log() in rotations)
+ *   - exp_IMU: go from tangent space to delta manifold (equivalent to exp() in rotations)
+ *   - plus: D2 = D1 (+) exp_IMU(d)
+ *   - diff: d = log_IMU( D2 (-) D1 )
  *   - body2delta: construct a delta from body magnitudes of linAcc and angVel
  */
 
@@ -345,7 +345,7 @@ inline Matrix<typename D1::Scalar, 10, 1> betweenStates(const MatrixBase<D1>& x1
 }
 
 template<typename Derived>
-Matrix<typename Derived::Scalar, 9, 1> lift(const MatrixBase<Derived>& delta_in)
+Matrix<typename Derived::Scalar, 9, 1> log_IMU(const MatrixBase<Derived>& delta_in)
 {
     MatrixSizeCheck<10, 1>::check(delta_in);
 
@@ -366,7 +366,7 @@ Matrix<typename Derived::Scalar, 9, 1> lift(const MatrixBase<Derived>& delta_in)
 }
 
 template<typename Derived>
-Matrix<typename Derived::Scalar, 10, 1> retract(const MatrixBase<Derived>& d_in)
+Matrix<typename Derived::Scalar, 10, 1> exp_IMU(const MatrixBase<Derived>& d_in)
 {
     MatrixSizeCheck<9, 1>::check(d_in);
 
diff --git a/src/three_D_tools.h b/src/SE3.h
similarity index 70%
rename from src/three_D_tools.h
rename to src/SE3.h
index 73ec2d413475ccd8ca82a4b9db14f84787d38509..cecd71757360a0bee2d36d50bbf225eda914e606 100644
--- a/src/three_D_tools.h
+++ b/src/SE3.h
@@ -1,12 +1,12 @@
 /*
- * three_D_tools.h
+ * SE3.h
  *
  *  Created on: Mar 15, 2018
  *      Author: jsola
  */
 
-#ifndef THREE_D_TOOLS_H_
-#define THREE_D_TOOLS_H_
+#ifndef SE3_H_
+#define SE3_H_
 
 
 #include "wolf.h"
@@ -27,10 +27,11 @@
  *   - identity:    I  = Delta at the origin, with Dp = [0,0,0]; Dq = [0,0,0,1], so that D (+) I = I (+) D = D
  *   - inverse:     so that D (+) D.inv = D.inv (+) D = I
  *   - between:     Db = D2 (-) D1 := D1.inv (+) D2, so that D2 = D1 (+) Db
- *   - lift:        go from Delta manifold to tangent space (equivalent to log() in rotations)
- *   - retract:     go from tangent space to delta manifold (equivalent to exp() in rotations)
- *   - plus:        D2 = D1 (+) retract(d)
- *   - diff:        d  = lift( D2 (-) D1 )
+ *   - log_SE3:     go from Delta manifold to tangent space (equivalent to log() in rotations)
+ *   - exp_SE3:     go from tangent space to delta manifold (equivalent to exp() in rotations)
+ *   - plus:        D2 = D1 * exp_SE3(d)
+ *   - minus:       d  = log_SE3( D1.inv() * D2 )
+ *   - interpolate: dd = D1 * exp ( log( D1.inv() * D2 ) * t ) = D1 (+) ( (D2 (-) D1) * t)
  */
 
 
@@ -220,7 +221,7 @@ inline Matrix<typename D1::Scalar, 7, 1> between(const MatrixBase<D1>& d1,
 }
 
 template<typename Derived>
-Matrix<typename Derived::Scalar, 6, 1> lift(const MatrixBase<Derived>& delta_in)
+Matrix<typename Derived::Scalar, 6, 1> log_SE3(const MatrixBase<Derived>& delta_in)
 {
     MatrixSizeCheck<7, 1>::check(delta_in);
 
@@ -231,25 +232,32 @@ Matrix<typename Derived::Scalar, 6, 1> lift(const MatrixBase<Derived>& delta_in)
     Map<Matrix<typename Derived::Scalar, 3, 1> >         dp_ret ( & ret(0) );
     Map<Matrix<typename Derived::Scalar, 3, 1> >         do_ret ( & ret(3) );
 
-    dp_ret = dp_in;
-    do_ret = log_q(dq_in);
+    Matrix<typename Derived::Scalar, 3, 3> V_inv;
+
+    do_ret  = log_q(dq_in);
+    V_inv   = jac_SO3_left_inv(do_ret);
+    dp_ret  = V_inv * dp_in;
 
     return ret;
 }
 
 template<typename Derived>
-Matrix<typename Derived::Scalar, 7, 1> retract(const MatrixBase<Derived>& d_in)
+Matrix<typename Derived::Scalar, 7, 1> exp_SE3(const MatrixBase<Derived>& d_in)
 {
     MatrixSizeCheck<6, 1>::check(d_in);
 
     Matrix<typename Derived::Scalar, 7, 1> ret;
 
+    Matrix<typename Derived::Scalar, 3, 3> V;
+
+    V = jac_SO3_left(d_in.template tail<3>());
+
     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<Matrix<typename Derived::Scalar, 3, 1> >         dp     ( &  ret(0) );
     Map<Quaternion<typename Derived::Scalar> >           dq     ( &  ret(3) );
 
-    dp = dp_in;
+    dp = V * dp_in;
     dq = exp_q(do_in);
 
     return ret;
@@ -292,21 +300,21 @@ inline Matrix<typename D1::Scalar, 7, 1> plus(const MatrixBase<D1>& d1,
 }
 
 template<typename D1, typename D2, typename D4, typename D5, typename D7, typename D8>
-inline void diff(const MatrixBase<D1>& dp1, const QuaternionBase<D2>& dq1,
-                 const MatrixBase<D4>& dp2, const QuaternionBase<D5>& dq2,
-                 MatrixBase<D7>& diff_p, MatrixBase<D8>& diff_o )
+inline void minus(const MatrixBase<D1>& dp1, const QuaternionBase<D2>& dq1,
+                  const MatrixBase<D4>& dp2, const QuaternionBase<D5>& dq2,
+                  MatrixBase<D7>& diff_p, MatrixBase<D8>& diff_o )
 {
-        diff_p = dp2 - dp1;
-        diff_o = log_q(dq1.conjugate() * dq2);
+    diff_p = dp2 - dp1;
+    diff_o = log_q(dq1.conjugate() * dq2);
 }
 
 template<typename D1, typename D2, typename D4, typename D5, typename D6, typename D7, typename D8, typename D9>
-inline void diff(const MatrixBase<D1>& dp1, const QuaternionBase<D2>& dq1,
-                 const MatrixBase<D4>& dp2, const QuaternionBase<D5>& dq2,
-                 MatrixBase<D6>& diff_p, MatrixBase<D7>& diff_o,
-                 MatrixBase<D8>& J_do_dq1, MatrixBase<D9>& J_do_dq2)
+inline void minus(const MatrixBase<D1>& dp1, const QuaternionBase<D2>& dq1,
+                  const MatrixBase<D4>& dp2, const QuaternionBase<D5>& dq2,
+                  MatrixBase<D6>& diff_p, MatrixBase<D7>& diff_o,
+                  MatrixBase<D8>& J_do_dq1, MatrixBase<D9>& J_do_dq2)
 {
-    diff(dp1, dq1, dp2, dq2, diff_p, diff_o);
+    minus(dp1, dq1, dp2, dq2, diff_p, diff_o);
 
     J_do_dq1    = - jac_SO3_left_inv(diff_o);
     J_do_dq2    =   jac_SO3_right_inv(diff_o);
@@ -314,9 +322,9 @@ inline void diff(const MatrixBase<D1>& dp1, const QuaternionBase<D2>& dq1,
 
 
 template<typename D1, typename D2, typename D3>
-inline void diff(const MatrixBase<D1>& d1,
-                 const MatrixBase<D2>& d2,
-                 MatrixBase<D3>& err)
+inline void minus(const MatrixBase<D1>& d1,
+                  const MatrixBase<D2>& d2,
+                  MatrixBase<D3>& err)
 {
     Map<const Matrix<typename D1::Scalar, 3, 1> >   dp1    ( & d1(0) );
     Map<const Quaternion<typename D1::Scalar> >     dq1    ( & d1(3) );
@@ -325,15 +333,15 @@ inline void diff(const MatrixBase<D1>& d1,
     Map<Matrix<typename D3::Scalar, 3, 1> >         diff_p ( & err(0) );
     Map<Matrix<typename D3::Scalar, 3, 1> >         diff_o ( & err(3) );
 
-    diff(dp1, dq1, dp2, dq2, diff_p, diff_o);
+    minus(dp1, dq1, dp2, dq2, diff_p, diff_o);
 }
 
 template<typename D1, typename D2, typename D3, typename D4, typename D5>
-inline void diff(const MatrixBase<D1>& d1,
-                 const MatrixBase<D2>& d2,
-                 MatrixBase<D3>& dif,
-                 MatrixBase<D4>& J_diff_d1,
-                 MatrixBase<D5>& J_diff_d2)
+inline void minus(const MatrixBase<D1>& d1,
+                  const MatrixBase<D2>& d2,
+                  MatrixBase<D3>& dif,
+                  MatrixBase<D4>& J_diff_d1,
+                  MatrixBase<D5>& J_diff_d2)
 {
     Map<const Matrix<typename D1::Scalar, 3, 1> >   dp1    ( & d1(0) );
     Map<const Quaternion<typename D1::Scalar> >     dq1    ( & d1(3) );
@@ -347,9 +355,9 @@ inline void diff(const MatrixBase<D1>& d1,
 
     Matrix<typename D4::Scalar, 3, 3> J_do_dq1, J_do_dq2;
 
-    diff(dp1, dq1, dv1, dp2, dq2, dv2, diff_p, diff_o, diff_v, J_do_dq1, J_do_dq2);
+    minus(dp1, dq1, dv1, dp2, dq2, dv2, diff_p, diff_o, diff_v, J_do_dq1, J_do_dq2);
 
-    /* d = diff(d1, d2) is
+    /* d = minus(d1, d2) is
      *   dp = dp2 - dp1
      *   do = Log(dq1.conj * dq2)
      *   dv = dv2 - dv1
@@ -367,17 +375,111 @@ inline void diff(const MatrixBase<D1>& d1,
 }
 
 template<typename D1, typename D2>
-inline Matrix<typename D1::Scalar, 6, 1> diff(const MatrixBase<D1>& d1,
-                                              const MatrixBase<D2>& d2)
+inline Matrix<typename D1::Scalar, 6, 1> minus(const MatrixBase<D1>& d1,
+                                               const MatrixBase<D2>& d2)
 {
     Matrix<typename D1::Scalar, 6, 1> ret;
-    diff(d1, d2, ret);
+    minus(d1, d2, ret);
     return ret;
 }
 
+template<typename D1, typename D2, typename D3>
+inline void interpolate(const MatrixBase<D1>& d1,
+                        const MatrixBase<D2>& d2,
+                        const typename D1::Scalar& t,
+                        MatrixBase<D3>& ret)
+{
+    Matrix<typename D1::Scalar, 7, 1> dd = between(d1, d2);
+
+    Matrix<typename D1::Scalar, 6, 1> tau = t * log_SE3(dd);
+
+    ret = compose(d1, exp_SE3(tau));
+}
+
+template<typename D1, typename D2>
+inline void toSE3(const MatrixBase<D1>& pose,
+                        MatrixBase<D2>& SE3)
+{
+    MatrixSizeCheck<4,4>::check(SE3);
+
+    typedef typename D1::Scalar T;
+
+    SE3.template block<3,1>(0,3) = pose.template head<3>();
+    SE3.template block<3,3>(0,0) = q2R(pose.template tail<4>());
+    SE3.template block<1,3>(3,0).setZero();
+    SE3(3,3) = (T)1.0;
+}
+
+template<typename D1, typename D2>
+inline Matrix<typename D1::Scalar, 6, 6> Q_helper(const MatrixBase<D1>& v, const MatrixBase<D1>& w)
+{
+    typedef typename D1::Scalar T;
+
+    Matrix<T, 3, 3> V   = skew(v);
+    Matrix<T, 3, 3> W   = skew(w);
+    Matrix<T, 3, 3> VW  = V * W;
+    Matrix<T, 3, 3> WV  = VW.transpose();       // Note on this change wrt. Barfoot: it happens that V*W = (W*V).transpose() !!!
+    Matrix<T, 3, 3> WVW = WV * W;
+    Matrix<T, 3, 3> VWW = VW * W;
+
+    T th_2      = w.squaredNorm();
+
+    T A(T(0.5)), B, C, D;
+
+    // Small angle approximation
+    if (th_2 <= T(1e-8))
+    {
+        B =  Scalar(1./6.)  + Scalar(1./120.)  * th_2;
+        C = -Scalar(1./24.) + Scalar(1./720.)  * th_2;
+        D = -Scalar(1./60.);
+    }
+    else
+    {
+        T th        = sqrt(th_2);
+        T th_3      = th_2*th;
+        T th_4      = th_2*th_2;
+        T th_5      = th_3*th_2;
+        T sin_th    = sin(th);
+        T cos_th    = cos(th);
+        B           = (th-sin_th)/th_3;
+        C           = (T(1.0) - th_2/T(2.0) - cos_th)/th_4;
+        D           = (th - sin_th - th_3/T(6.0))/th_5;
+    }
+    Matrix<T, 3, 3> Q;
+    Q.noalias() =
+            + A * V
+            + B * (WV + VW + WVW)
+            - C * (VWW - VWW.transpose() - Scalar(3) * WVW)           // Note on this change wrt. Barfoot: it happens that V*W*W = -(W*W*V).transpose() !!!
+            - D * WVW * W;                                            // Note on this change wrt. Barfoot: it happens that W*V*W*W = W*W*V*W !!!
+
+    return Q;
+}
+
+template<typename D1>
+inline Matrix<typename D1::Scalar, 6, 6> jac_SE3_left(const MatrixBase<D1>& tangent)
+{
+    typedef typename D1::Scalar T;
+    Map<Matrix<T, 3, 1>> v(tangent.data() + 0); // linear
+    Map<Matrix<T, 3, 1>> w(tangent.data() + 3); // angular
+
+    Matrix<T, 3, 3> Jl(jac_SO3_left(w));
+    Matrix<T, 3, 3> Q = Q_helper(v,w);
+
+    Matrix<T, 6, 6> Jl_SE3;
+    Jl_SE3.topLeftCorner(3,3)     = Jl;
+    Jl_SE3.bottomRightCorner(3,3) = Jl;
+    Jl_SE3.topRightCorner(3,3)    = Q;
+    Jl_SE3.bottomLeftCorner(3,3)  .setZero();
+}
+
+template<typename D1>
+inline Matrix<typename D1::Scalar, 6, 6> jac_SE3_right(const MatrixBase<D1>& tangent)
+{
+    return jac_SE3_left(-tangent);
+}
 
 } // namespace three_d
 } // namespace wolf
 
 
-#endif /* THREE_D_TOOLS_H_ */
+#endif /* SE3_H_ */
diff --git a/src/capture_base.cpp b/src/capture_base.cpp
index d5d76ba8e37c92d2d17a00cf40b59fea18157b33..880303f494e5e7fc2e00d3471a04c2c8c6a09dde 100644
--- a/src/capture_base.cpp
+++ b/src/capture_base.cpp
@@ -18,7 +18,6 @@ CaptureBase::CaptureBase(const std::string& _type,
     sensor_ptr_(_sensor_ptr),
     state_block_vec_(3),
     calib_size_(0),
-    is_removing_(false),
     capture_id_(++capture_id_count_),
     time_stamp_(_ts)
 {
diff --git a/src/capture_base.h b/src/capture_base.h
index d705be3764535be62e13f5067867de4a2e7a29b0..9da734e64f3b60ed580c69b4f970cf650b9cf616 100644
--- a/src/capture_base.h
+++ b/src/capture_base.h
@@ -29,7 +29,6 @@ class CaptureBase : public NodeBase, public std::enable_shared_from_this<Capture
         SizeEigen calib_size_;           ///< size of the calibration parameters (dynamic or static sensor params that are not fixed)
 
         static unsigned int capture_id_count_;
-        bool is_removing_;          ///< A flag for safely removing nodes from the Wolf tree. See remove().
 
     protected:
         unsigned int capture_id_;
@@ -45,7 +44,7 @@ class CaptureBase : public NodeBase, public std::enable_shared_from_this<Capture
                     StateBlockPtr _intr_ptr     = nullptr);
 
         virtual ~CaptureBase();
-        void remove();
+        virtual void remove();
 
         // Type
         virtual bool isMotion() const { return false; }
diff --git a/src/ceres_wrapper/ceres_manager.h b/src/ceres_wrapper/ceres_manager.h
index efd9861bf380710ea027a06570f08910107a397a..8852bc06d3a0fe24fa2b760aa3766cb2e24cbf66 100644
--- a/src/ceres_wrapper/ceres_manager.h
+++ b/src/ceres_wrapper/ceres_manager.h
@@ -53,13 +53,13 @@ class CeresManager : public SolverManager
 
         virtual void computeCovariances(const StateBlockList& st_list) override;
 
-        virtual bool hasConverged();
+        virtual bool hasConverged() override;
 
-        virtual SizeStd iterations();
+        virtual SizeStd iterations() override;
 
-        virtual Scalar initialCost();
+        virtual Scalar initialCost() override;
 
-        virtual Scalar finalCost();
+        virtual Scalar finalCost() override;
 
         ceres::Solver::Options& getSolverOptions();
 
diff --git a/src/constraint_base.cpp b/src/constraint_base.cpp
index d88dedbf05f89fcf7386b447520c31a4752a71a0..3e33cc6cc6d84dc1bded350956e29a548dacef22 100644
--- a/src/constraint_base.cpp
+++ b/src/constraint_base.cpp
@@ -11,7 +11,6 @@ ConstraintBase::ConstraintBase(const std::string&  _tp,
                                ConstraintStatus _status) :
     NodeBase("CONSTRAINT", _tp),
     feature_ptr_(), // nullptr
-    is_removing_(false),
     constraint_id_(++constraint_id_count_),
     status_(_status),
     apply_loss_function_(_apply_loss_function),
@@ -32,7 +31,6 @@ ConstraintBase::ConstraintBase(const std::string&  _tp,
                                bool _apply_loss_function, ConstraintStatus _status) :
     NodeBase("CONSTRAINT", _tp),
     feature_ptr_(),
-    is_removing_(false),
     constraint_id_(++constraint_id_count_),
     status_(_status),
     apply_loss_function_(_apply_loss_function),
diff --git a/src/constraint_base.h b/src/constraint_base.h
index 838d50c23659e1da94c60e23580c8ceb1a0feda2..3341a55c882d64262fb2bc6f6a7251a0f3acf6c9 100644
--- a/src/constraint_base.h
+++ b/src/constraint_base.h
@@ -42,7 +42,6 @@ class ConstraintBase : public NodeBase, public std::enable_shared_from_this<Cons
         FeatureBaseWPtr feature_ptr_;                    ///< FeatureBase pointer (upper node)
 
         static unsigned int constraint_id_count_;
-        bool is_removing_; ///< A flag for safely removing nodes from the Wolf tree. See remove().
 
     protected:
         unsigned int constraint_id_;
@@ -75,7 +74,7 @@ class ConstraintBase : public NodeBase, public std::enable_shared_from_this<Cons
 
         virtual ~ConstraintBase() = default;
 
-        void remove();
+        virtual void remove();
 
         unsigned int id() const;
 
diff --git a/src/feature_base.cpp b/src/feature_base.cpp
index 3cc0b6ca3d64b7299641de401fb67068c0af1b08..6459ffc244964171a58b839590fc1a5b392fa21b 100644
--- a/src/feature_base.cpp
+++ b/src/feature_base.cpp
@@ -9,7 +9,6 @@ unsigned int FeatureBase::feature_id_count_ = 0;
 FeatureBase::FeatureBase(const std::string& _type, const Eigen::VectorXs& _measurement, const Eigen::MatrixXs& _meas_covariance) :
 	NodeBase("FEATURE", _type),
     capture_ptr_(),
-    is_removing_(false),
     feature_id_(++feature_id_count_),
     track_id_(0),
     landmark_id_(0),
diff --git a/src/feature_base.h b/src/feature_base.h
index 3bd60033a40929c7e6a18705acbd46a2a56a94e3..1eb4fbf851feb0257b11b210dd597c1ab3710286 100644
--- a/src/feature_base.h
+++ b/src/feature_base.h
@@ -25,7 +25,6 @@ class FeatureBase : public NodeBase, public std::enable_shared_from_this<Feature
         ConstraintBaseList constrained_by_list_;
 
         static unsigned int feature_id_count_;
-        bool is_removing_; ///< A flag for safely removing nodes from the Wolf tree. See remove().
 
     protected:
         unsigned int feature_id_;
@@ -46,7 +45,7 @@ class FeatureBase : public NodeBase, public std::enable_shared_from_this<Feature
         FeatureBase(const std::string& _type, const Eigen::VectorXs& _measurement, const Eigen::MatrixXs& _meas_covariance);
 
         virtual ~FeatureBase();
-        void remove();
+        virtual void remove();
 
         virtual void setProblem(ProblemPtr _problem) final;
 
diff --git a/src/frame_base.cpp b/src/frame_base.cpp
index c605ae015c384e235a5409d215a3b2903c1bb439..4f0f619fe36767466f7db210e36ac4da03452892 100644
--- a/src/frame_base.cpp
+++ b/src/frame_base.cpp
@@ -15,7 +15,6 @@ FrameBase::FrameBase(const TimeStamp& _ts, StateBlockPtr _p_ptr, StateBlockPtr _
             NodeBase("FRAME", "Base"),
             trajectory_ptr_(),
             state_block_vec_(3), // allow for 3 state blocks by default. Resize in derived constructors if needed.
-            is_removing_(false),
             frame_id_(++frame_id_count_),
             type_(NON_KEY_FRAME),
             time_stamp_(_ts)
@@ -29,7 +28,6 @@ FrameBase::FrameBase(const FrameType & _tp, const TimeStamp& _ts, StateBlockPtr
             NodeBase("FRAME", "Base"),
             trajectory_ptr_(),
             state_block_vec_(3), // allow for 3 state blocks by default. Resize in derived constructors if needed.
-            is_removing_(false),
             frame_id_(++frame_id_count_),
             type_(_tp),
             time_stamp_(_ts)
diff --git a/src/frame_base.h b/src/frame_base.h
index d5202bb69e636d7b3f9a8509a0af2ebcb57b6f28..f7e5f8379772502c5169d03aa11a7cbdae39f2d3 100644
--- a/src/frame_base.h
+++ b/src/frame_base.h
@@ -37,7 +37,6 @@ class FrameBase : public NodeBase, public std::enable_shared_from_this<FrameBase
         std::vector<StateBlockPtr> state_block_vec_; ///< vector of state blocks, in the order: Position, Orientation, Velocity.
 
         static unsigned int frame_id_count_;
-        bool is_removing_; ///< A flag for safely removing nodes from the Wolf tree. See remove().
 
     protected:
         unsigned int frame_id_;
@@ -67,7 +66,7 @@ class FrameBase : public NodeBase, public std::enable_shared_from_this<FrameBase
         FrameBase(const FrameType & _tp, const TimeStamp& _ts, StateBlockPtr _p_ptr, StateBlockPtr _o_ptr = nullptr, StateBlockPtr _v_ptr = nullptr);
 
         virtual ~FrameBase();
-        void remove();
+        virtual void remove();
 
 
 
diff --git a/src/landmark_base.cpp b/src/landmark_base.cpp
index 750911457c058acf179d635d24bc289df8237bad..830ae2940f201c291d9d12f6be529a591320f8ca 100644
--- a/src/landmark_base.cpp
+++ b/src/landmark_base.cpp
@@ -13,7 +13,6 @@ LandmarkBase::LandmarkBase(const std::string& _type, StateBlockPtr _p_ptr, State
             NodeBase("LANDMARK", _type),
             map_ptr_(),
             state_block_vec_(2), // allow for 2 state blocks by default. Resize in derived constructors if needed.
-            is_removing_(false),
             landmark_id_(++landmark_id_count_)
 {
     state_block_vec_[0] = _p_ptr;
diff --git a/src/landmark_base.h b/src/landmark_base.h
index e4b721db95a7fe30db760f3a517a10e11f4f6872..66485387abcbdfdaec03f0f6fa93cc92d7998f1b 100644
--- a/src/landmark_base.h
+++ b/src/landmark_base.h
@@ -29,7 +29,6 @@ class LandmarkBase : public NodeBase, public std::enable_shared_from_this<Landma
         std::vector<StateBlockPtr> state_block_vec_; ///< vector of state blocks, in the order P, O.
 
         static unsigned int landmark_id_count_;
-        bool is_removing_; ///< A flag for safely removing nodes from the Wolf tree. See remove().
 
     protected:
         unsigned int landmark_id_; ///< landmark unique id
@@ -48,7 +47,7 @@ class LandmarkBase : public NodeBase, public std::enable_shared_from_this<Landma
          **/
         LandmarkBase(const std::string& _type, StateBlockPtr _p_ptr, StateBlockPtr _o_ptr = nullptr);
         virtual ~LandmarkBase();
-        void remove();
+        virtual void remove();
         virtual YAML::Node saveToYaml() const;
 
         // Properties
diff --git a/src/node_base.h b/src/node_base.h
index 1cf31eb69c4cbddaa87b20a208abd5dcd317110a..bedc866122037c8be8a1d8cee8db2dd009d28f88 100644
--- a/src/node_base.h
+++ b/src/node_base.h
@@ -67,6 +67,8 @@ class NodeBase
         std::string node_type_;  ///< Text label identifying the type or subcategory of node ("Pin Hole", "Point 2D", etc)
         std::string node_name_;  ///< Text label identifying each specific object ("left camera", "LIDAR 1", "PointGrey", "Andrew", etc)
 
+        bool is_removing_; ///< A flag for safely removing nodes from the Wolf tree. See remove().
+
     public: 
 
         NodeBase(const std::string& _category, const std::string& _type = "Undefined", const std::string& _name = "");
@@ -76,6 +78,7 @@ class NodeBase
         std::string getCategory() const;
         std::string getType()  const;
         std::string getName()  const;
+        bool isRemoving() const;
 
         void setType(const std::string& _type);
         void setName(const std::string& _name);
@@ -95,7 +98,8 @@ inline NodeBase::NodeBase(const std::string& _category, const std::string& _type
         node_id_(++node_id_count_),
         node_category_(_category),
         node_type_(_type),
-        node_name_(_name)
+        node_name_(_name),
+        is_removing_(false)
 {
     //
 }
@@ -120,6 +124,11 @@ inline std::string NodeBase::getName() const
     return node_name_;
 }
 
+inline bool NodeBase::isRemoving() const
+{
+    return is_removing_;
+}
+
 inline void NodeBase::setType(const std::string& _type)
 {
     node_type_ = _type;
diff --git a/src/problem.cpp b/src/problem.cpp
index 324ef036afab9b122a8a951da0a2cb5907af730c..fec23b17497b920b575f3f80c05a9db5882e9bc8 100644
--- a/src/problem.cpp
+++ b/src/problem.cpp
@@ -36,25 +36,26 @@ Problem::Problem(const std::string& _frame_structure) :
         trajectory_ptr_(std::make_shared<TrajectoryBase>(_frame_structure)),
         map_ptr_(std::make_shared<MapBase>()),
         processor_motion_ptr_(),
-        state_size_(0),
-        state_cov_size_(0),
         prior_is_set_(false)
 {
     if (_frame_structure == "PO 2D")
     {
         state_size_ = 3;
         state_cov_size_ = 3;
+        dim_ = 2;
     }
 
     else if (_frame_structure == "PO 3D")
     {
         state_size_ = 7;
         state_cov_size_ = 6;
+        dim_ = 3;
     }
     else if (_frame_structure == "POV 3D")
     {
         state_size_ = 10;
         state_cov_size_ = 9;
+        dim_ = 3;
     }
     else std::runtime_error(
             "Problem::Problem(): Unknown frame structure. Add appropriate frame structure to the switch statement.");
@@ -315,6 +316,11 @@ void Problem::getFrameStructureSize(SizeEigen& _x_size, SizeEigen& _cov_size) co
     _cov_size = state_cov_size_;
 }
 
+SizeEigen Problem::getDim() const
+{
+    return dim_;
+}
+
 Eigen::VectorXs Problem::zeroState()
 {
     Eigen::VectorXs state = Eigen::VectorXs::Zero(getFrameStructureSize());
diff --git a/src/problem.h b/src/problem.h
index 3b5988333bcd7474a90b67bd5013c1863ae4b0f9..70cb8b7c4e58e31ad0a329056cffd411e537ead7 100644
--- a/src/problem.h
+++ b/src/problem.h
@@ -42,7 +42,7 @@ class Problem : public std::enable_shared_from_this<Problem>
         ProcessorMotionPtr  processor_motion_ptr_;
         StateBlockList      state_block_list_;
         std::map<std::pair<StateBlockPtr, StateBlockPtr>, Eigen::MatrixXs> covariances_;
-        SizeEigen state_size_, state_cov_size_;
+        SizeEigen state_size_, state_cov_size_, dim_;
         std::map<ConstraintBasePtr, Notification> constraint_notification_map_;
         std::map<StateBlockPtr, Notification> state_block_notification_map_;
         bool prior_is_set_;
@@ -58,6 +58,7 @@ class Problem : public std::enable_shared_from_this<Problem>
         // Properties -----------------------------------------
         SizeEigen getFrameStructureSize() const;
         void getFrameStructureSize(SizeEigen& _x_size, SizeEigen& _cov_size) const;
+        SizeEigen getDim() const;
 
         // Hardware branch ------------------------------------
         HardwareBasePtr getHardwarePtr();
diff --git a/src/processor_base.cpp b/src/processor_base.cpp
index 28a6f3dde73ebba6bba0e9c632660d9d13180803..ea4bdc4e82e1edbcb83c9500656f0b0d05de6ab8 100644
--- a/src/processor_base.cpp
+++ b/src/processor_base.cpp
@@ -11,8 +11,7 @@ ProcessorBase::ProcessorBase(const std::string& _type, ProcessorParamsBasePtr _p
         NodeBase("PROCESSOR", _type, _params->name),
         processor_id_(++processor_id_count_),
         params_(_params),
-        sensor_ptr_(),
-        is_removing_(false)
+        sensor_ptr_()
 {
 //    WOLF_DEBUG("constructed    +p" , id());
 }
@@ -98,6 +97,12 @@ PackKeyFramePtr PackKeyFrameBuffer::selectPack(const TimeStamp& _time_stamp, con
 
     PackKeyFrameBuffer::Iterator post = container_.upper_bound(_time_stamp);
 
+    // remove packs corresponding to removed KFs (keeping the next iterator in post)
+    while (post != container_.end() && post->second->key_frame->isRemoving())
+        post = container_.erase(post);
+    while (post != container_.begin() && std::prev(post)->second->key_frame->isRemoving())
+        container_.erase(std::prev(post));
+
     bool prev_exists = (post != container_.begin());
     bool post_exists = (post != container_.end());
 
@@ -133,33 +138,33 @@ PackKeyFramePtr PackKeyFrameBuffer::selectPack(const CaptureBasePtr _capture, co
     return selectPack(_capture->getTimeStamp(), _time_tolerance);
 }
 
-PackKeyFramePtr PackKeyFrameBuffer::selectPackBefore(const TimeStamp& _time_stamp, const Scalar& _time_tolerance)
+PackKeyFramePtr PackKeyFrameBuffer::selectFirstPackBefore(const TimeStamp& _time_stamp, const Scalar& _time_tolerance)
 {
-    if (container_.empty())
-        return nullptr;
+    // remove packs corresponding to removed KFs
+    while (!container_.empty() && container_.begin()->second->key_frame->isRemoving())
+        container_.erase(container_.begin());
 
-    PackKeyFrameBuffer::Iterator post = container_.upper_bound(_time_stamp);
+    // There is no pack
+    if (container_.empty())
+         return nullptr;
 
-    bool prev_exists = (post != container_.begin());
+    // Checking on begin() since packs are ordered in time
+    // Return first pack if is older than time stamp
+    if (container_.begin()->first < _time_stamp)
+         return container_.begin()->second;
 
-    if (prev_exists)
+    // Return first pack if despite being newer, it is within the time tolerance
+    if (checkTimeTolerance(container_.begin()->first, container_.begin()->second->time_tolerance, _time_stamp, _time_tolerance))
         return container_.begin()->second;
 
-    else
-    {
-        bool post_exists = (post != container_.end());
-        bool post_ok     = post_exists && checkTimeTolerance(post->first, post->second->time_tolerance, _time_stamp, _time_tolerance);
-
-        if (post_ok)
-            return post->second;
-    }
-
+    // otherwise return nullptr (no pack before the provided ts or within the tolerance was found)
     return nullptr;
+
 }
 
-PackKeyFramePtr PackKeyFrameBuffer::selectPackBefore(const CaptureBasePtr _capture, const Scalar& _time_tolerance)
+PackKeyFramePtr PackKeyFrameBuffer::selectFirstPackBefore(const CaptureBasePtr _capture, const Scalar& _time_tolerance)
 {
-    return selectPackBefore(_capture->getTimeStamp(), _time_tolerance);
+    return selectFirstPackBefore(_capture->getTimeStamp(), _time_tolerance);
 }
 
 void PackKeyFrameBuffer::print(void)
diff --git a/src/processor_base.h b/src/processor_base.h
index b9dc4b8d6464062e21a91e8d4133462731ac1be6..bcf5c46b62730d593165f99fdb8d021296964812 100644
--- a/src/processor_base.h
+++ b/src/processor_base.h
@@ -58,8 +58,8 @@ class PackKeyFrameBuffer
         PackKeyFramePtr selectPack(const TimeStamp& _time_stamp, const Scalar& _time_tolerance);
         PackKeyFramePtr selectPack(const CaptureBasePtr _capture, const Scalar& _time_tolerance);
 
-        PackKeyFramePtr selectPackBefore(const TimeStamp& _time_stamp, const Scalar& _time_tolerance);
-        PackKeyFramePtr selectPackBefore(const CaptureBasePtr _capture, const Scalar& _time_tolerance);
+        PackKeyFramePtr selectFirstPackBefore(const TimeStamp& _time_stamp, const Scalar& _time_tolerance);
+        PackKeyFramePtr selectFirstPackBefore(const CaptureBasePtr _capture, const Scalar& _time_tolerance);
 
         /**\brief Buffer size
          *
@@ -147,15 +147,13 @@ class ProcessorBase : public NodeBase, public std::enable_shared_from_this<Proce
     private:
         SensorBaseWPtr sensor_ptr_;
 
-        bool is_removing_; ///< A flag for safely removing nodes from the Wolf tree. See remove().
-
         static unsigned int processor_id_count_;
 
     public:
         ProcessorBase(const std::string& _type, ProcessorParamsBasePtr _params);
         virtual ~ProcessorBase();
         virtual void configure(SensorBasePtr _sensor) = 0;
-        void remove();
+        virtual void remove();
 
         unsigned int id();
 
diff --git a/src/processor_motion.cpp b/src/processor_motion.cpp
index 450cd9d8c2333e06ef76a269939430fe42a18705..0abab214309a12779c0d916ac559ca4afbbd1339 100644
--- a/src/processor_motion.cpp
+++ b/src/processor_motion.cpp
@@ -26,7 +26,7 @@ ProcessorMotion::ProcessorMotion(const std::string& _type,
         delta_cov_(_delta_cov_size, _delta_cov_size),
         delta_integrated_(_delta_size),
         delta_integrated_cov_(_delta_cov_size, _delta_cov_size),
-        calib_(_calib_size),
+        calib_preint_(_calib_size),
         jacobian_delta_preint_(delta_cov_size_, delta_cov_size_),
         jacobian_delta_(delta_cov_size_, delta_cov_size_),
         jacobian_calib_(delta_size_, calib_size_)
@@ -248,7 +248,7 @@ void ProcessorMotion::process(CaptureBasePtr _incoming_ptr)
         getProblem()->keyFrameCallback(key_frame_ptr, shared_from_this(), params_motion_->time_tolerance);
     }
 
-    resetDerived(); // TODO see where to put this
+    resetDerived();
 
     // clear incoming just in case
     incoming_ptr_ = nullptr; // This line is not really needed, but it makes things clearer.
@@ -274,7 +274,7 @@ bool ProcessorMotion::getState(const TimeStamp& _ts, Eigen::VectorXs& _x)
         CaptureBasePtr cap_orig   = capture_motion->getOriginFramePtr()->getCaptureOf(getSensorPtr());
         VectorXs calib            = cap_orig->getCalibration();
 
-        // Get delta and correct it with new bias
+        // Get delta and correct it with new calibration params
         VectorXs calib_preint     = capture_motion->getBuffer().getCalibrationPreint();
         Motion   motion           = capture_motion->getBuffer().getMotion(_ts);
         
@@ -296,35 +296,6 @@ bool ProcessorMotion::getState(const TimeStamp& _ts, Eigen::VectorXs& _x)
     return true;
 }
 
-//CaptureMotionPtr ProcessorMotion::findCaptureContainingTimeStamp(const TimeStamp& _ts) const
-//{
-//    //std::cout << "ProcessorMotion::findCaptureContainingTimeStamp: ts = " << _ts.getSeconds() << "." << _ts.getNanoSeconds() << std::endl;
-//    CaptureMotionPtr capture_ptr = last_ptr_;
-//    while (capture_ptr != nullptr)
-//    {
-//        // capture containing motion previous than the ts found:
-//        if (capture_ptr->getBuffer().get().front().ts_ < _ts)
-//            return capture_ptr;
-//        else
-//        {
-//            // go to the previous motion capture
-//            if (capture_ptr == last_ptr_)
-//                capture_ptr = origin_ptr_;
-//            else if (capture_ptr->getOriginFramePtr() == nullptr)
-//                return nullptr;
-//            else
-//            {
-//                CaptureBasePtr capture_base_ptr = capture_ptr->getOriginFramePtr()->getCaptureOf(getSensorPtr());
-//                if (capture_base_ptr == nullptr)
-//                    return nullptr;
-//                else
-//                    capture_ptr = std::static_pointer_cast<CaptureMotion>(capture_base_ptr);
-//            }
-//        }
-//    }
-//    return capture_ptr;
-//}
-
 FrameBasePtr ProcessorMotion::setOrigin(const Eigen::VectorXs& _x_origin, const TimeStamp& _ts_origin)
 {
     FrameBasePtr key_frame_ptr = getProblem()->emplaceFrame(KEY_FRAME, _x_origin, _ts_origin);
@@ -387,12 +358,12 @@ void ProcessorMotion::integrateOneStep()
     assert(dt_ >= 0 && "Time interval _dt is negative!");
 
     // get vector of parameters to calibrate
-    calib_ = getBuffer().getCalibrationPreint();
+    calib_preint_ = getBuffer().getCalibrationPreint();
 
     // get data and convert it to delta, and obtain also the delta covariance
     computeCurrentDelta(incoming_ptr_->getData(),
                         incoming_ptr_->getDataCovariance(),
-                        calib_,
+                        calib_preint_,
                         dt_,
                         delta_,
                         delta_cov_,
@@ -408,11 +379,14 @@ void ProcessorMotion::integrateOneStep()
 
     // integrate Jacobian wrt calib
     if ( hasCalibration() )
-        jacobian_calib_ = jacobian_delta_preint_ * getBuffer().get().back().jacobian_calib_ + jacobian_delta_ * jacobian_delta_calib_;
+        jacobian_calib_.noalias()
+            = jacobian_delta_preint_ * getBuffer().get().back().jacobian_calib_
+            + jacobian_delta_ * jacobian_delta_calib_;
 
     // Integrate covariance
-    delta_integrated_cov_ = jacobian_delta_preint_ * getBuffer().get().back().delta_integr_cov_ * jacobian_delta_preint_.transpose()
-                          + jacobian_delta_        * delta_cov_                                 * jacobian_delta_.transpose();
+    delta_integrated_cov_.noalias()
+            = jacobian_delta_preint_ * getBuffer().get().back().delta_integr_cov_ * jacobian_delta_preint_.transpose()
+            + jacobian_delta_        * delta_cov_                                 * jacobian_delta_.transpose();
 
     // push all into buffer
     getBuffer().get().emplace_back(incoming_ptr_->getTimeStamp(),
@@ -501,8 +475,53 @@ Motion ProcessorMotion::interpolate(const Motion& _ref, Motion& _second, TimeSta
     else
     {
         // _ts is closest to _second
-        Motion interpolated             ( _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_);
+
+    if (tau < 0.5)
+    {
+        // _ts is closest to _ref1
+        Motion interpolated                 ( _ref1 );
+        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();
+
+        _second = _ref2;
+
+        return interpolated;
+    }
+    else
+    {
+        // _ts is closest to _ref2
+        Motion interpolated                 ( _ref2 );
         interpolated.ts_                    = _ts;
+
+        _second                             = _ref2;
         _second.data_                       . setZero();
         _second.data_cov_                   . setZero();
         _second.delta_                      = deltaZero();
@@ -518,7 +537,7 @@ Motion ProcessorMotion::interpolate(const Motion& _ref, Motion& _second, TimeSta
 CaptureMotionPtr ProcessorMotion::findCaptureContainingTimeStamp(const TimeStamp& _ts) const
 {
     // We need to search in previous keyframes for the capture containing a motion buffer with the queried time stamp
-    // Note: since the buffer goes from a FK through the past until the previous KF, we need to:
+    // Note: since the buffer goes from a KF in the past until the next KF, we need to:
     //  1. See that the KF contains a CaptureMotion
     //  2. See that the TS is smaller than the KF's TS
     //  3. See that the TS is bigger than the KF's first Motion in the CaptureMotion's buffer
@@ -533,7 +552,7 @@ CaptureMotionPtr ProcessorMotion::findCaptureContainingTimeStamp(const TimeStamp
         capture = frame->getCaptureOf(getSensorPtr());
         if (capture != nullptr)
         {
-            // We found a Capture belonging to this processor's Sensor ==> it is a CaptureMotion
+            // Rule 1 satisfied! We found a Capture belonging to this processor's Sensor ==> it is a CaptureMotion
             capture_motion = std::static_pointer_cast<CaptureMotion>(capture);
             if (_ts >= capture_motion->getBuffer().get().front().ts_)
                 // Found time stamp satisfying rule 3 above !! ==> break for loop
@@ -584,7 +603,7 @@ PackKeyFramePtr ProcessorMotion::computeProcessingStep()
         throw std::runtime_error("ProcessorMotion received data before being initialized.");
     }
 
-    PackKeyFramePtr pack = kf_pack_buffer_.selectPackBefore(last_ptr_, params_motion_->time_tolerance);
+    PackKeyFramePtr pack = kf_pack_buffer_.selectFirstPackBefore(last_ptr_, params_motion_->time_tolerance);
 
     if (pack)
     {
@@ -596,7 +615,7 @@ PackKeyFramePtr ProcessorMotion::computeProcessingStep()
             //            throw std::runtime_error("Pack's KF and origin's KF have matching time stamps (i.e. below time tolerances)");
             processing_step_ = RUNNING_WITH_PACK_ON_ORIGIN;
         }
-        else if (pack->key_frame->getTimeStamp() < origin_ptr_->getTimeStamp() - params_motion_->time_tolerance)
+        else if (pack->key_frame->getTimeStamp() < origin_ptr_->getTimeStamp())
             processing_step_ = RUNNING_WITH_PACK_BEFORE_ORIGIN;
 
         else
diff --git a/src/processor_motion.h b/src/processor_motion.h
index 4e20cab461bfb9cb5555f3091b1abd5d086d6573..e399bddcce6562e265d540c9e5699cd8e7f64193 100644
--- a/src/processor_motion.h
+++ b/src/processor_motion.h
@@ -372,6 +372,24 @@ class ProcessorMotion : public ProcessorBase
          */
         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 create a CaptureMotion and add it to a Frame
          * \param _ts time stamp
          * \param _sensor Sensor that produced the Capture
@@ -399,6 +417,10 @@ class ProcessorMotion : public ProcessorBase
          * \param _capture_motion: the parent capture
          */
         FeatureBasePtr emplaceFeature(CaptureMotionPtr _capture_own);
+
+        /** \brief create a feature corresponding to given capture
+         * \param _capture_motion: the parent capture
+         */
         virtual FeatureBasePtr createFeature(CaptureMotionPtr _capture_own) = 0;
 
         /** \brief create a constraint and link it in the wolf tree
@@ -435,7 +457,7 @@ class ProcessorMotion : public ProcessorBase
         SizeEigen data_size_;        ///< the size of the incoming data
         SizeEigen delta_size_;       ///< the size of the deltas
         SizeEigen delta_cov_size_;   ///< the size of the delta covariances matrix
-        SizeEigen calib_size_;       ///< size of the extra parameters (TBD in derived classes)
+        SizeEigen calib_size_;       ///< the size of the calibration parameters (TBD in derived classes)
         CaptureMotionPtr origin_ptr_;
         CaptureMotionPtr last_ptr_;
         CaptureMotionPtr incoming_ptr_;
@@ -449,7 +471,7 @@ class ProcessorMotion : public ProcessorBase
         Eigen::MatrixXs delta_cov_;             ///< current delta covariance
         Eigen::VectorXs delta_integrated_;      ///< integrated delta
         Eigen::MatrixXs delta_integrated_cov_;  ///< integrated delta covariance
-        Eigen::VectorXs calib_;                 ///< calibration vector
+        Eigen::VectorXs calib_preint_;          ///< calibration vector used during pre-integration
         Eigen::MatrixXs jacobian_delta_preint_; ///< jacobian of delta composition w.r.t previous delta integrated
         Eigen::MatrixXs jacobian_delta_;        ///< jacobian of delta composition w.r.t current delta
         Eigen::MatrixXs jacobian_calib_;        ///< jacobian of delta preintegration wrt calibration params
diff --git a/src/processor_odom_2D.cpp b/src/processor_odom_2D.cpp
index fb1f567761e1a8d35b15aa019a78524fd47bfd16..19818dffcc1aee098c316f1b13bf603703333c39 100644
--- a/src/processor_odom_2D.cpp
+++ b/src/processor_odom_2D.cpp
@@ -5,10 +5,6 @@ namespace wolf
 ProcessorOdom2D::ProcessorOdom2D(ProcessorParamsOdom2DPtr _params) :
                 ProcessorMotion("ODOM 2D", 3, 3, 3, 2, 0, _params),
                 params_odom_2D_(_params)
-//                dist_traveled_th_(_params.dist_traveled_th_),
-//                theta_traveled_th_(_params.theta_traveled_th_),
-//                cov_det_th_(_params->cov_det_th)//,
-//                elapsed_time_th_(_params.elapsed_time_th_)
 {
     unmeasured_perturbation_cov_ = _params->unmeasured_perturbation_std * _params->unmeasured_perturbation_std * Matrix3s::Identity();
 }
@@ -21,16 +17,17 @@ void ProcessorOdom2D::computeCurrentDelta(const Eigen::VectorXs& _data, const Ei
                                           const Eigen::VectorXs& _calib, const Scalar _dt, Eigen::VectorXs& _delta,
                                           Eigen::MatrixXs& _delta_cov, Eigen::MatrixXs& _jacobian_calib)
 {
-    //std::cout << "ProcessorOdom2d::data2delta" << std::endl;
     assert(_data.size() == data_size_ && "Wrong _data vector size");
     assert(_data_cov.rows() == data_size_ && "Wrong _data_cov size");
     assert(_data_cov.cols() == data_size_ && "Wrong _data_cov size");
+
     // data  is [dtheta, dr]
     // delta is [dx, dy, dtheta]
     // motion model is 1/2 turn + straight + 1/2 turn
     _delta(0) = cos(_data(1) / 2) * _data(0);
     _delta(1) = sin(_data(1) / 2) * _data(0);
     _delta(2) = _data(1);
+
     // Fill delta covariance
     Eigen::MatrixXs J(delta_cov_size_, data_size_);
     J(0, 0) = cos(_data(1) / 2);
@@ -39,35 +36,29 @@ void ProcessorOdom2D::computeCurrentDelta(const Eigen::VectorXs& _data, const Ei
     J(0, 1) = -_data(0) * sin(_data(1) / 2) / 2;
     J(1, 1) = _data(0) * cos(_data(1) / 2) / 2;
     J(2, 1) = 1;
+
     // Since input data is size 2, and delta is size 3, the noise model must be given by:
     // 1. Covariance of the input data:  J*Q*J.tr
-    // 2. Fix variance term to be added: var*Id
+    // 2. Fixed variance term to be added: var*Id
     _delta_cov = J * _data_cov * J.transpose() + unmeasured_perturbation_cov_;
-    //std::cout << "data      :" << _data.transpose() << std::endl;
-    //std::cout << "data cov  :" << std::endl << _data_cov << std::endl;
-    //std::cout << "delta     :" << delta_.transpose() << std::endl;
-    //std::cout << "delta cov :" << std::endl << delta_cov_ << std::endl;
-    // jacobian_delta_calib_ not used in this class yet. In any case, set to zero with:
-    //    jacobian_delta_calib_.setZero();
 }
 
 void ProcessorOdom2D::deltaPlusDelta(const Eigen::VectorXs& _delta1, const Eigen::VectorXs& _delta2, const Scalar _Dt2,
                                      Eigen::VectorXs& _delta1_plus_delta2)
 {
-    // This is just a frame composition in 2D
-    //std::cout << "ProcessorOdom2d::deltaPlusDelta" << std::endl;
     assert(_delta1.size() == delta_size_ && "Wrong _delta1 vector size");
     assert(_delta2.size() == delta_size_ && "Wrong _delta2 vector size");
     assert(_delta1_plus_delta2.size() == delta_size_ && "Wrong _delta1_plus_delta2 vector size");
-    _delta1_plus_delta2.head<2>() = _delta1.head<2>() + Eigen::Rotation2Ds(_delta1(2)).matrix() * _delta2.head<2>();
-    _delta1_plus_delta2(2) = pi2pi(_delta1(2) + _delta2(2));
+
+    // This is just a frame composition in 2D
+    _delta1_plus_delta2.head<2>()   = _delta1.head<2>() + Eigen::Rotation2Ds(_delta1(2)).matrix() * _delta2.head<2>();
+    _delta1_plus_delta2(2)          = pi2pi(_delta1(2) + _delta2(2));
 }
 
 void ProcessorOdom2D::deltaPlusDelta(const Eigen::VectorXs& _delta1, const Eigen::VectorXs& _delta2, const Scalar _Dt2,
                                      Eigen::VectorXs& _delta1_plus_delta2, Eigen::MatrixXs& _jacobian1,
                                      Eigen::MatrixXs& _jacobian2)
 {
-    //std::cout << "ProcessorOdom2d::deltaPlusDelta jacobians" << std::endl;
     assert(_delta1.size() == delta_size_ && "Wrong _delta1 vector size");
     assert(_delta2.size() == delta_size_ && "Wrong _delta2 vector size");
     assert(_delta1_plus_delta2.size() == delta_size_ && "Wrong _delta1_plus_delta2 vector size");
@@ -75,12 +66,16 @@ void ProcessorOdom2D::deltaPlusDelta(const Eigen::VectorXs& _delta1, const Eigen
     assert(_jacobian1.cols() == delta_cov_size_ && "Wrong _jacobian1 size");
     assert(_jacobian2.rows() == delta_cov_size_ && "Wrong _jacobian2 size");
     assert(_jacobian2.cols() == delta_cov_size_ && "Wrong _jacobian2 size");
+
+    // This is just a frame composition in 2D
     _delta1_plus_delta2.head<2>() = _delta1.head<2>() + Eigen::Rotation2Ds(_delta1(2)).matrix() * _delta2.head<2>();
     _delta1_plus_delta2(2) = pi2pi(_delta1(2) + _delta2(2));
+
     // Jac wrt delta_integrated
     _jacobian1 = Eigen::MatrixXs::Identity(delta_cov_size_, delta_cov_size_);
     _jacobian1(0, 2) = -sin(_delta1(2)) * _delta2(0) - cos(_delta1(2)) * _delta2(1);
     _jacobian1(1, 2) = cos(_delta1(2)) * _delta2(0) - sin(_delta1(2)) * _delta2(1);
+
     // jac wrt delta
     _jacobian2 = Eigen::MatrixXs::Identity(delta_cov_size_, delta_cov_size_);
     _jacobian2.topLeftCorner<2, 2>() = Eigen::Rotation2Ds(_delta1(2)).matrix();
@@ -89,10 +84,10 @@ void ProcessorOdom2D::deltaPlusDelta(const Eigen::VectorXs& _delta1, const Eigen
 void ProcessorOdom2D::statePlusDelta(const Eigen::VectorXs& _x, const Eigen::VectorXs& _delta, const Scalar _Dt,
                                      Eigen::VectorXs& _x_plus_delta)
 {
-    // This is just a frame composition in 2D
-    //std::cout << "ProcessorOdom2d::statePlusDelta" << std::endl;
     assert(_x.size() == x_size_ && "Wrong _x vector size");
     assert(_x_plus_delta.size() == x_size_ && "Wrong _x_plus_delta vector size");
+
+    // This is just a frame composition in 2D
     _x_plus_delta.head<2>() = _x.head<2>() + Eigen::Rotation2Ds(_x(2)).matrix() * _delta.head<2>();
     _x_plus_delta(2) = pi2pi(_x(2) + _delta(2));
 }
diff --git a/src/sensor_base.cpp b/src/sensor_base.cpp
index 96f0403f678e92204a5585dd73545646901bc6ec..9bff803ed457149143ceaec76dd6b56f8bc4d415 100644
--- a/src/sensor_base.cpp
+++ b/src/sensor_base.cpp
@@ -20,7 +20,6 @@ SensorBase::SensorBase(const std::string& _type,
         hardware_ptr_(),
         state_block_vec_(3), // allow for 3 state blocks by default. Resize in derived constructors if needed.
         calib_size_(0),
-        is_removing_(false),
         sensor_id_(++sensor_id_count_), // simple ID factory
         extrinsic_dynamic_(_extr_dyn),
         intrinsic_dynamic_(_intr_dyn),
@@ -47,7 +46,6 @@ SensorBase::SensorBase(const std::string& _type,
         hardware_ptr_(),
         state_block_vec_(3), // allow for 3 state blocks by default. Resize in derived constructors if needed.
         calib_size_(0),
-        is_removing_(false),
         sensor_id_(++sensor_id_count_), // simple ID factory
         extrinsic_dynamic_(_extr_dyn),
         intrinsic_dynamic_(_intr_dyn),
diff --git a/src/sensor_base.h b/src/sensor_base.h
index b97c65976c5baebc7c1e5614e504e79c26c16343..1e690affe078716a233fb87e817fe431a9964d76 100644
--- a/src/sensor_base.h
+++ b/src/sensor_base.h
@@ -39,7 +39,6 @@ class SensorBase : public NodeBase, public std::enable_shared_from_this<SensorBa
         SizeEigen calib_size_;
 
         static unsigned int sensor_id_count_; ///< Object counter (acts as simple ID factory)
-        bool is_removing_; ///< A flag for safely removing nodes from the Wolf tree. See remove().
 
     protected:
         unsigned int sensor_id_;   // sensor ID
diff --git a/src/test/CMakeLists.txt b/src/test/CMakeLists.txt
index a1ef79cb4faccbb63b81f0cf12cee6f876580d1c..917c927554a76dff36317ca607ade99f17b0a807 100644
--- a/src/test/CMakeLists.txt
+++ b/src/test/CMakeLists.txt
@@ -94,7 +94,9 @@ target_link_libraries(gtest_processor_motion ${PROJECT_NAME})
   
 # Rotation test
 wolf_add_gtest(gtest_rotation gtest_rotation.cpp)
-#target_link_libraries(gtest_rotation ${PROJECT_NAME})
+
+# SE3 test
+wolf_add_gtest(gtest_SE3 gtest_SE3.cpp)
 
 # SensorBase test
 wolf_add_gtest(gtest_sensor_base gtest_sensor_base.cpp)
diff --git a/src/test/gtest_IMU_tools.cpp b/src/test/gtest_IMU_tools.cpp
index 14957c9791a4754c254319b90f1d93c4e7763225..9f0f8c398e1f7b51690228f5ff3f75be4e66b01d 100644
--- a/src/test/gtest_IMU_tools.cpp
+++ b/src/test/gtest_IMU_tools.cpp
@@ -129,7 +129,7 @@ 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 = retract(d_min);
+    VectorXs delta = exp_IMU(d_min);
 
     // expected delta
     Vector3s dp = d_min.head(3);
@@ -139,11 +139,11 @@ TEST(IMU_tools, lift_retract)
     ASSERT_MATRIX_APPROX(delta, delta_true, 1e-10);
 
     // transform to a new tangent -- should be the original tangent
-    VectorXs d_from_delta = lift(delta);
+    VectorXs d_from_delta = log_IMU(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 = retract(d_from_delta);
+    VectorXs delta_from_d = exp_IMU(d_from_delta);
     ASSERT_MATRIX_APPROX(delta_from_d, delta, 1e-10);
 }
 
diff --git a/src/test/gtest_SE3.cpp b/src/test/gtest_SE3.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..b734385b4a997e78d8b0de73573bbbe331009715
--- /dev/null
+++ b/src/test/gtest_SE3.cpp
@@ -0,0 +1,236 @@
+/**
+ * \file gtest_SE3.cpp
+ *
+ *  Created on: Feb 2, 2019
+ *      \author: jsola
+ */
+
+
+#include "../SE3.h"
+#include "utils_gtest.h"
+
+
+
+using namespace Eigen;
+using namespace wolf;
+using namespace three_D;
+
+TEST(SE3, exp_0)
+{
+    Vector6s tau = Vector6s::Zero();
+    Vector7s pose; pose << 0,0,0, 0,0,0,1;
+
+    ASSERT_MATRIX_APPROX(pose, exp_SE3(tau), 1e-8);
+}
+
+TEST(SE3, log_I)
+{
+    Vector7s pose; pose << 0,0,0, 0,0,0,1;
+    Vector6s tau = Vector6s::Zero();
+
+    ASSERT_MATRIX_APPROX(tau, log_SE3(pose), 1e-8);
+}
+
+TEST(SE3, exp_log)
+{
+    Vector6s tau = Vector6s::Random() / 5.0;
+    Vector7s pose = exp_SE3(tau);
+
+    ASSERT_MATRIX_APPROX(tau, log_SE3(exp_SE3(tau)), 1e-8);
+    ASSERT_MATRIX_APPROX(pose, exp_SE3(log_SE3(pose)), 1e-8);
+}
+
+TEST(SE3, exp_of_multiple)
+{
+    Vector6s tau, tau2, tau3;
+    Vector7s pose, pose2, pose3;
+    tau = Vector6s::Random() / 5;
+    pose << exp_SE3(tau);
+    tau2  = 2*tau;
+    tau3  = 3*tau;
+    pose2 = compose(pose, pose);
+    pose3 = compose(pose2, pose);
+
+    // 1
+    ASSERT_MATRIX_APPROX(exp_SE3(tau), pose, 1e-8);
+
+    // 2
+    ASSERT_MATRIX_APPROX(exp_SE3(tau2), pose2, 1e-8);
+
+    // 3
+    ASSERT_MATRIX_APPROX(exp_SE3(tau3), pose3, 1e-8);
+}
+
+TEST(SE3, log_of_power)
+{
+    Vector6s tau, tau2, tau3;
+    Vector7s pose, pose2, pose3;
+    tau = Vector6s::Random() / 5;
+    pose << exp_SE3(tau);
+    tau2 = 2*tau;
+    tau3 = 3*tau;
+    pose2 = compose(pose, pose);
+    pose3 = compose(pose2, pose);
+
+    // 1
+    ASSERT_MATRIX_APPROX(tau, log_SE3(pose), 1e-8);
+
+    // 2
+    ASSERT_MATRIX_APPROX(tau2, log_SE3(pose2), 1e-8);
+
+    // 3
+    ASSERT_MATRIX_APPROX(tau3, log_SE3(pose3), 1e-8);
+}
+
+TEST(SE3, interpolate_I_xyz)
+{
+    Vector7s p1, p2, p_pre;
+
+    p1 << 0,0,0, 0,0,0,1;
+    p2 << 1,2,3, 0,0,0,1;
+    Scalar t = 0.2;
+
+    interpolate(p1, p2, t, p_pre);
+
+    Vector7s p_gt; p_gt << 0.2,0.4,0.6, 0,0,0,1;
+
+    ASSERT_MATRIX_APPROX(p_pre, p_gt, 1e-8);
+}
+
+TEST(SE3, interpolate_xyz_xyz)
+{
+    Vector7s p1, p2, p_pre;
+
+    p1 << 1,2,3, 0,0,0,1;
+    p2 << 2,4,6, 0,0,0,1;
+    Scalar t = 0.2;
+
+    interpolate(p1, p2, t, p_pre);
+
+    Vector7s p_gt; p_gt << 1.2,2.4,3.6, 0,0,0,1;
+
+    ASSERT_MATRIX_APPROX(p_pre, p_gt, 1e-8);
+}
+
+TEST(SE3, interpolate_I_qx)
+{
+    Vector7s p1, p2, p_pre;
+
+    p1 << 0,0,0, 0,0,0,1;
+    p2 << 0,0,0, 1,0,0,0;
+    Scalar t = 0.5;
+
+    interpolate(p1, p2, t, p_pre);
+
+    Vector7s p_gt; p_gt << 0,0,0, sqrt(2)/2,0,0,sqrt(2)/2;
+
+    ASSERT_MATRIX_APPROX(p_pre, p_gt, 1e-8);
+}
+
+TEST(SE3, interpolate_I_qy)
+{
+    Vector7s p1, p2, p_pre;
+
+    p1 << 0,0,0, 0,0,0,1;
+    p2 << 0,0,0, 0,1,0,0;
+    Scalar t = 0.5;
+
+    interpolate(p1, p2, t, p_pre);
+
+    Vector7s p_gt; p_gt << 0,0,0, 0,sqrt(2)/2,0,sqrt(2)/2;
+
+    ASSERT_MATRIX_APPROX(p_pre, p_gt, 1e-8);
+}
+
+TEST(SE3, interpolate_I_qz)
+{
+    Vector7s p1, p2, p_pre;
+
+    p1 << 0,0,0, 0,0,0,1;
+    p2 << 0,0,0, 0,0,1,0;
+    Scalar t = 0.5;
+
+    interpolate(p1, p2, t, p_pre);
+
+    Vector7s p_gt; p_gt << 0,0,0, 0,0,sqrt(2)/2,sqrt(2)/2;
+
+    ASSERT_MATRIX_APPROX(p_pre, p_gt, 1e-8);
+}
+
+TEST(SE3, interpolate_I_qxyz)
+{
+    Vector7s p1, p2, dp, p_pre, p_gt;
+    Vector3s da;
+
+    da.setRandom(); da /= 5;
+
+    p1 << 0,0,0, 0,0,0,1;
+    dp << 0,0,0, exp_q(da).coeffs();
+    p_gt = compose(p1, dp);
+    p2   = compose(p_gt, dp);
+    Scalar t = 0.5;
+
+    interpolate(p1, p2, t, p_pre);
+
+    ASSERT_MATRIX_APPROX(p_pre, p_gt, 1e-8);
+}
+
+TEST(SE3, interpolate_half)
+{
+    Vector7s p1, p2, dp, p_pre, p_gt;
+    Vector6s da;
+
+    p1.setRandom(); p1.tail(4).normalize();
+
+    da.setRandom(); da /= 5; // small rotation
+    dp  << exp_SE3(da);
+
+    // compose double, then interpolate 1/2
+
+    p_gt = compose(p1, dp);
+    p2   = compose(p_gt, dp);
+
+    Scalar t = 0.5;
+    interpolate(p1, p2, t, p_pre);
+
+    ASSERT_MATRIX_APPROX(p_pre, p_gt, 1e-8);
+}
+
+TEST(SE3, interpolate_third)
+{
+    Vector7s p1, p2, dp, dp2, dp3, p_pre, p_gt;
+    Vector6s da;
+
+    p1.setRandom(); p1.tail(4).normalize();
+
+    da.setRandom(); da /= 5; // small rotation
+    dp  << exp_SE3(da);
+    dp2 = compose(dp, dp);
+    dp3 = compose(dp2, dp);
+
+    // compose triple, then interpolate 1/3
+
+    p_gt = compose(p1, dp);
+    p2   = compose(p1, dp3);
+
+    Scalar t = 1.0/3.0;
+    interpolate(p1, p2, t, p_pre);
+
+    ASSERT_MATRIX_APPROX(p_pre, p_gt, 1e-8);
+}
+
+TEST(SE3, toSE3_I)
+{
+    Vector7s pose; pose << 0,0,0, 0,0,0,1;
+    Matrix4s R;
+    toSE3(pose, R);
+
+    ASSERT_MATRIX_APPROX(R, Matrix4s::Identity(), 1e-8);
+}
+
+int main(int argc, char **argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}
+
diff --git a/src/test/gtest_odom_2D.cpp b/src/test/gtest_odom_2D.cpp
index da1e5ca48bf5fd9f0e96217426cbb31f73be5772..d8e10b949a137e718d1ca51502e94671d8379e09 100644
--- a/src/test/gtest_odom_2D.cpp
+++ b/src/test/gtest_odom_2D.cpp
@@ -16,6 +16,7 @@
 #include "../state_block.h"
 #include "../wolf.h"
 #include "../ceres_wrapper/ceres_manager.h"
+#include "../capture_pose.h"
 
 // STL includes
 #include <map>
@@ -26,7 +27,6 @@
 // General includes
 #include <iostream>
 #include <iomanip>      // std::setprecision
-#include "../capture_pose.h"
 
 using namespace wolf;
 using namespace Eigen;
@@ -124,7 +124,7 @@ TEST(Odom2D, ConstraintFix_and_ConstraintOdom2D)
     Matrix3s delta_cov; delta_cov << .02, 0, 0, 0, .025, .02, 0, .02, .02;
 
     ProblemPtr          Pr = Problem::create("PO 2D");
-    CeresManager ceres_manager(Pr);
+    CeresManager        ceres_manager(Pr);
 
     // KF0 and absolute prior
     FrameBasePtr        F0 = Pr->setPrior(x0, P0,t0, dt/2);
diff --git a/src/test/gtest_pack_KF_buffer.cpp b/src/test/gtest_pack_KF_buffer.cpp
index dab222c29248b5be35c61d9de10ffb041647217f..1204254c1024266c98cfe2c8e3764f76453fe2ee 100644
--- a/src/test/gtest_pack_KF_buffer.cpp
+++ b/src/test/gtest_pack_KF_buffer.cpp
@@ -144,7 +144,7 @@ TEST_F(BufferPackKeyFrameTest, selectPack)
     }
 }
 
-TEST_F(BufferPackKeyFrameTest, selectPackBefore)
+TEST_F(BufferPackKeyFrameTest, selectFirstPackBefore)
 {
     pack_kf_buffer.clear();
 
@@ -194,7 +194,7 @@ TEST_F(BufferPackKeyFrameTest, selectPackBefore)
         int j = 0;
         for (auto ts : q_ts)
         {
-            packQ = pack_kf_buffer.selectPackBefore(ts, tt);
+            packQ = pack_kf_buffer.selectFirstPackBefore(ts, tt);
             if (packQ)
                 res(i,j) = packQ->key_frame->getTimeStamp().get();
 
diff --git a/src/test/gtest_processor_motion.cpp b/src/test/gtest_processor_motion.cpp
index 77d2a55186ae6084f54e6832c71bc88067edc081..94a2f0c15f61439d8f5d800d893d6ae95f45016a 100644
--- a/src/test/gtest_processor_motion.cpp
+++ b/src/test/gtest_processor_motion.cpp
@@ -29,13 +29,17 @@ class ProcessorMotion_test : public ProcessorOdom2D, public testing::Test{
         Vector2s            data;
         Matrix2s            data_cov;
 
-        ProcessorMotion_test() : ProcessorOdom2D(std::make_shared<ProcessorParamsOdom2D>()) { }
+        ProcessorMotion_test() :
+            ProcessorOdom2D(std::make_shared<ProcessorParamsOdom2D>()),
+            dt(0)
+        { }
 
         virtual void SetUp()
         {
-            dt                          = 1.0;
+            dt                      = 1.0;
             problem = Problem::create("PO 2D");
             ProcessorParamsOdom2DPtr params(std::make_shared<ProcessorParamsOdom2D>());
+            params->time_tolerance  = 0.25;
             params->dist_traveled   = 100;
             params->angle_turned    = 6.28;
             params->max_time_span   = 2.5*dt; // force KF at every third process()
@@ -52,19 +56,6 @@ class ProcessorMotion_test : public ProcessorOdom2D, public testing::Test{
 
         virtual void TearDown(){}
 
-        virtual Motion interpolate(const Motion& _ref,
-                                   Motion& _second,
-                                   TimeStamp& _ts)
-        {
-            return ProcessorMotion::interpolate(_ref, _second, _ts);
-        }
-
-        virtual Motion motionZero(TimeStamp& t)
-        {
-            return ProcessorOdom2D::motionZero(t);
-        }
-
-
 };
 
 
@@ -93,7 +84,7 @@ TEST_F(ProcessorMotion_test, IntegrateCircle)
     data_cov.setIdentity();
     TimeStamp t(0.0);
 
-    for (int i = 0; i<10; i++) // one full turn
+    for (int i = 0; i<10; i++) // one full turn exactly
     {
         t += dt;
         capture->setTimeStamp(t);
@@ -108,13 +99,13 @@ TEST_F(ProcessorMotion_test, IntegrateCircle)
 
 TEST_F(ProcessorMotion_test, Interpolate)
 {
-    data << 1, 2*M_PI/10; // advance straight
+    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++)
+    for (int i = 0; i<10; i++) // one full turn exactly
     {
         t += dt;
         capture->setTimeStamp(t);