diff --git a/.gitignore b/.gitignore
index 2e5e674704c29459650beb9667740661b78e88e9..ee22d7c8146da16da38f11e838fb6523e42dced7 100644
--- a/.gitignore
+++ b/.gitignore
@@ -4,6 +4,7 @@
 README.txt
 bin/
 build/
+build_release/
 lib/
 .idea/
 ./Wolf.user
diff --git a/CMakeLists.txt b/CMakeLists.txt
index e6d799cbe84406acc5e4af82f79335f3464a0754..a7b87c5e9430892f56e7629dd8ab5fd4ada02a2a 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -282,7 +282,7 @@ include/base/state_angle.h
 include/base/state_block.h
 include/base/state_homogeneous_3D.h
 include/base/state_quaternion.h
-include/base/three_D_tools.h
+include/base/SE3.h
 include/base/time_stamp.h
 include/base/track_matrix.h
 include/base/trajectory_base.h
@@ -478,7 +478,7 @@ include/base/state_angle.h
 include/base/state_block.h
 include/base/state_homogeneous_3D.h
 include/base/state_quaternion.h
-include/base/three_D_tools.h
+include/base/SE3.h
 include/base/time_stamp.h
 include/base/track_matrix.h
 include/base/trajectory_base.h
diff --git a/README.md b/README.md
index d8291ed4041e6c171c853ce14c86e67187e5d374..fb08051ca2f57199c0d91870e30db96ebb381b43 100644
--- a/README.md
+++ b/README.md
@@ -139,7 +139,7 @@ We are shipping the CMAKE file `FindYamlCpp.cmake` together with Wolf. Find it a
 
 #### spdlog
 
-Wolf used spdlog macros. To install it:
+Wolf uses spdlog macros. Right now Wolf is only compatible with spdlog version 0.17.0. To install it:
 
 -   Git clone the source:
 
@@ -148,6 +148,7 @@ Wolf used spdlog macros. To install it:
 -   Build and install with:
 
         $ cd spdlog
+        $ git checkout v0.17.0
         $ mkdir build
         $ cd build
         $ cmake -DCMAKE_CXX_FLAGS="${CMAKE_CXX_FLAGS} -fPIC" ..
diff --git a/include/base/IMU_tools.h b/include/base/IMU_tools.h
index 3159a1b9ca7b80b9c5ef018998f2f9e1fc119c49..eecad244c7a57ee2868bcf464e951a0a5108160d 100644
--- a/include/base/IMU_tools.h
+++ b/include/base/IMU_tools.h
@@ -33,10 +33,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
  */
 
@@ -340,7 +340,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);
 
@@ -361,7 +361,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/include/base/SE3.h b/include/base/SE3.h
new file mode 100644
index 0000000000000000000000000000000000000000..cea35460771f0bf91d4f56520dd823ad3a1d505d
--- /dev/null
+++ b/include/base/SE3.h
@@ -0,0 +1,479 @@
+/*
+ * SE3.h
+ *
+ *  Created on: Mar 15, 2018
+ *      Author: jsola
+ */
+
+#ifndef SE3_H_
+#define SE3_H_
+
+#include "base/wolf.h"
+#include "base/rotations.h"
+
+/*
+ * The functions in this file are related to manipulations of Delta motion magnitudes used in 3D motion.
+ *
+ * The Delta is defined as a simple 3D pose with the rotation expressed in quaternion form,
+ *     Delta = [Dp, Dq]
+ * with
+ *     Dp : position delta
+ *     Dq : quaternion delta
+ *
+ * The functions are listed below:
+ *
+ *   - compose:     Dc = D1 (+) D2
+ *   - 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
+ *   - 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)
+ */
+
+namespace wolf
+{
+namespace three_D {
+using namespace Eigen;
+
+template<typename D1, typename D2>
+inline void identity(MatrixBase<D1>& p, QuaternionBase<D2>& q)
+{
+    MatrixSizeCheck<3, 1>::check(p);
+    p = MatrixBase<D1>::Zero(3,1);
+    q = QuaternionBase<D2>::Identity();
+}
+
+template<typename D1, typename D2>
+inline void identity(MatrixBase<D1>& p, MatrixBase<D2>& q)
+{
+    MatrixSizeCheck<3, 1>::check(p);
+    MatrixSizeCheck<4, 1>::check(q);
+    typedef typename D1::Scalar T1;
+    typedef typename D2::Scalar T2;
+    p << T1(0), T1(0), T1(0);
+    q << T2(0), T2(0), T2(0), T2(1);
+}
+
+template<typename T = Scalar>
+inline Matrix<T, 7, 1> identity()
+{
+    Matrix<T, 7, 1> ret;
+    ret<< T(0), T(0), T(0),
+          T(0), T(0), T(0), T(1);
+    return ret;
+}
+
+template<typename D1, typename D2, typename D4, typename D5>
+inline void inverse(const MatrixBase<D1>& dp, const QuaternionBase<D2>& dq,
+                    MatrixBase<D4>& idp, QuaternionBase<D5>& idq)
+{
+    MatrixSizeCheck<3, 1>::check(dp);
+    MatrixSizeCheck<3, 1>::check(idp);
+
+    idp = - dq.conjugate() * dp ;
+    idq =   dq.conjugate() ;
+}
+
+template<typename D1, typename D2>
+inline void inverse(const MatrixBase<D1>& d,
+                    MatrixBase<D2>& id)
+{
+    MatrixSizeCheck<7, 1>::check(d);
+    MatrixSizeCheck<7, 1>::check(id);
+
+    Map<const Matrix<typename D1::Scalar, 3, 1> >   dp   ( & d( 0 ) );
+    Map<const Quaternion<typename D1::Scalar> >     dq   ( & d( 3 ) );
+    Map<Matrix<typename D2::Scalar, 3, 1> >         idp  ( & id( 0 ) );
+    Map<Quaternion<typename D2::Scalar> >           idq  ( & id( 3 ) );
+
+    inverse(dp, dq, idp, idq);
+}
+
+template<typename D>
+inline Matrix<typename D::Scalar, 7, 1> inverse(const MatrixBase<D>& d)
+{
+    Matrix<typename D::Scalar, 7, 1> id;
+    inverse(d, id);
+    return id;
+}
+
+template<typename D1, typename D2, typename D4, typename D5, typename D7, typename D8>
+inline void compose(const MatrixBase<D1>& dp1, const QuaternionBase<D2>& dq1,
+                    const MatrixBase<D4>& dp2, const QuaternionBase<D5>& dq2,
+                    MatrixBase<D7>& sum_p, QuaternionBase<D8>& sum_q )
+{
+        MatrixSizeCheck<3, 1>::check(dp1);
+        MatrixSizeCheck<3, 1>::check(dp2);
+        MatrixSizeCheck<3, 1>::check(sum_p);
+
+        sum_p = dp1 + dq1*dp2;
+        sum_q =       dq1*dq2; // dq here to avoid possible aliasing between d1 and sum
+}
+
+template<typename D1, typename D2, typename D3>
+inline void compose(const MatrixBase<D1>& d1,
+                    const MatrixBase<D2>& d2,
+                    MatrixBase<D3>& sum)
+{
+    MatrixSizeCheck<7, 1>::check(d1);
+    MatrixSizeCheck<7, 1>::check(d2);
+    MatrixSizeCheck<7, 1>::check(sum);
+
+    Map<const Matrix<typename D1::Scalar, 3, 1> >   dp1    ( & d1( 0 ) );
+    Map<const Quaternion<typename D1::Scalar> >     dq1    ( & d1( 3 ) );
+    Map<const Matrix<typename D2::Scalar, 3, 1> >   dp2    ( & d2( 0 ) );
+    Map<const Quaternion<typename D2::Scalar> >     dq2    ( & d2( 3 ) );
+    Map<Matrix<typename D3::Scalar, 3, 1> >         sum_p  ( & sum( 0 ) );
+    Map<Quaternion<typename D3::Scalar> >           sum_q  ( & sum( 3 ) );
+
+    compose(dp1, dq1, dp2, dq2, sum_p, sum_q);
+}
+
+template<typename D1, typename D2>
+inline Matrix<typename D1::Scalar, 7, 1> compose(const MatrixBase<D1>& d1,
+                                                 const MatrixBase<D2>& d2 )
+{
+    Matrix<typename D1::Scalar, 7, 1>  ret;
+    compose(d1, d2, ret);
+    return ret;
+}
+
+template<typename D1, typename D2, typename D3, typename D4, typename D5>
+inline void compose(const MatrixBase<D1>& d1,
+                    const MatrixBase<D2>& d2,
+                    MatrixBase<D3>& sum,
+                    MatrixBase<D4>& J_sum_d1,
+                    MatrixBase<D5>& J_sum_d2)
+{
+    MatrixSizeCheck<7, 1>::check(d1);
+    MatrixSizeCheck<7, 1>::check(d2);
+    MatrixSizeCheck<7, 1>::check(sum);
+    MatrixSizeCheck< 6, 6>::check(J_sum_d1);
+    MatrixSizeCheck< 6, 6>::check(J_sum_d2);
+
+    // Some useful temporaries
+    Matrix<typename D1::Scalar, 3, 3> dR1 = q2R(d1.segment(3,4)); //dq1.matrix(); // First  Delta, DR
+    Matrix<typename D2::Scalar, 3, 3> dR2 = q2R(d2.segment(3,4)); //dq2.matrix(); // Second delta, dR
+
+    // Jac wrt first delta
+    J_sum_d1.setIdentity();                                     // dDp'/dDp = dDv'/dDv = I
+    J_sum_d1.block(0,3,3,3).noalias() = - dR1 * skew(d2.head(3)) ;     // dDp'/dDo
+    J_sum_d1.block(3,3,3,3) = dR2.transpose();                  // dDo'/dDo
+
+    // Jac wrt second delta
+    J_sum_d2.setIdentity();                                     //
+    J_sum_d2.block(0,0,3,3) = dR1;                              // dDp'/ddp
+    // J_sum_d2.block(3,3,3,3) = Matrix3s::Identity();          // dDo'/ddo = I
+
+    // compose deltas -- done here to avoid aliasing when calling with input `d1` and result `sum` referencing the same variable
+    compose(d1, d2, sum);
+}
+
+template<typename D1, typename D2, typename D4, typename D5, typename D7, typename D8>
+inline void between(const MatrixBase<D1>& dp1, const QuaternionBase<D2>& dq1,
+                    const MatrixBase<D4>& dp2, const QuaternionBase<D5>& dq2,
+                    MatrixBase<D7>& dp12, QuaternionBase<D8>& dq12)
+{
+        MatrixSizeCheck<3, 1>::check(dp1);
+        MatrixSizeCheck<3, 1>::check(dp2);
+        MatrixSizeCheck<3, 1>::check(dp12);
+
+        dp12 = dq1.conjugate() * ( dp2 - dp1 );
+        dq12 = dq1.conjugate() *   dq2;
+}
+
+template<typename D1, typename D2, typename D3>
+inline void between(const MatrixBase<D1>& d1,
+                    const MatrixBase<D2>& d2,
+                    MatrixBase<D3>& d2_minus_d1)
+{
+    MatrixSizeCheck<7, 1>::check(d1);
+    MatrixSizeCheck<7, 1>::check(d2);
+    MatrixSizeCheck<7, 1>::check(d2_minus_d1);
+
+    Map<const Matrix<typename D1::Scalar, 3, 1> >   dp1    ( & d1(0) );
+    Map<const Quaternion<typename D1::Scalar> >     dq1    ( & d1(3) );
+    Map<const Matrix<typename D2::Scalar, 3, 1> >   dp2    ( & d2(0) );
+    Map<const Quaternion<typename D2::Scalar> >     dq2    ( & d2(3) );
+    Map<Matrix<typename D3::Scalar, 3, 1> >         dp12 ( & d2_minus_d1(0) );
+    Map<Quaternion<typename D3::Scalar> >           dq12 ( & d2_minus_d1(3) );
+
+    between(dp1, dq1, dp2, dq2, dp12, dq12);
+}
+
+template<typename D1, typename D2>
+inline Matrix<typename D1::Scalar, 7, 1> between(const MatrixBase<D1>& d1,
+                                                 const MatrixBase<D2>& d2 )
+{
+    MatrixSizeCheck<7, 1>::check(d1);
+    MatrixSizeCheck<7, 1>::check(d2);
+    Matrix<typename D1::Scalar, 7, 1> d12;
+    between(d1, d2, d12);
+    return d12;
+}
+
+template<typename Derived>
+Matrix<typename Derived::Scalar, 6, 1> log_SE3(const MatrixBase<Derived>& delta_in)
+{
+    MatrixSizeCheck<7, 1>::check(delta_in);
+
+    Matrix<typename Derived::Scalar, 6, 1> ret;
+
+    Map<const Matrix<typename Derived::Scalar, 3, 1> >   dp_in  ( & delta_in(0) );
+    Map<const Quaternion<typename Derived::Scalar> >     dq_in  ( & delta_in(3) );
+    Map<Matrix<typename Derived::Scalar, 3, 1> >         dp_ret ( & ret(0) );
+    Map<Matrix<typename Derived::Scalar, 3, 1> >         do_ret ( & ret(3) );
+
+    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> 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 = V * dp_in;
+    dq = exp_q(do_in);
+
+    return ret;
+}
+
+template<typename D1, typename D2, typename D4, typename D5, typename D7, typename D8>
+inline void plus(const MatrixBase<D1>& dp1, const QuaternionBase<D2>& dq1,
+                 const MatrixBase<D4>& dp2, const MatrixBase<D5>& do2,
+                 MatrixBase<D7>& plus_p, QuaternionBase<D8>& plus_q)
+{
+    MatrixSizeCheck<3, 1>::check(dp1);
+    MatrixSizeCheck<3, 1>::check(dp2);
+    MatrixSizeCheck<3, 1>::check(plus_p);
+    plus_p = dp1 + dp2;
+    plus_q = dq1 * exp_q(do2);
+}
+
+template<typename D1, typename D2, typename D3>
+inline void plus(const MatrixBase<D1>& d1,
+                 const MatrixBase<D2>& d2,
+                 MatrixBase<D3>& d_plus)
+{
+    Map<const Matrix<typename D1::Scalar, 3, 1> >   dp1    ( & d1(0) );
+    Map<const Quaternion<typename D1::Scalar> >     dq1    ( & d1(3) );
+    Map<const Matrix<typename D2::Scalar, 3, 1> >   dp2    ( & d2(0) );
+    Map<const Matrix<typename D2::Scalar, 3, 1> >   do2    ( & d2(3) );
+    Map<Matrix<typename D3::Scalar, 3, 1> >         dp_p ( & d_plus(0) );
+    Map<Quaternion<typename D3::Scalar> >           dq_p ( & d_plus(3) );
+
+    plus(dp1, dq1, dp2, do2, dp_p, dq_p);
+}
+
+template<typename D1, typename D2>
+inline Matrix<typename D1::Scalar, 7, 1> plus(const MatrixBase<D1>& d1,
+                                              const MatrixBase<D2>& d2)
+{
+    Matrix<typename D1::Scalar, 7, 1> d_plus;
+    plus(d1, d2, d_plus);
+    return d_plus;
+}
+
+template<typename D1, typename D2, typename D4, typename D5, typename D7, typename D8>
+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);
+}
+
+template<typename D1, typename D2, typename D4, typename D5, typename D6, typename D7, typename D8, typename D9>
+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)
+{
+    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);
+}
+
+template<typename D1, typename D2, typename D3>
+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) );
+    Map<const Matrix<typename D2::Scalar, 3, 1> >   dp2    ( & d2(0) );
+    Map<const Quaternion<typename D2::Scalar> >     dq2    ( & d2(3) );
+    Map<Matrix<typename D3::Scalar, 3, 1> >         diff_p ( & err(0) );
+    Map<Matrix<typename D3::Scalar, 3, 1> >         diff_o ( & err(3) );
+
+    minus(dp1, dq1, dp2, dq2, diff_p, diff_o);
+}
+
+template<typename D1, typename D2, typename D3, typename D4, typename D5>
+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) );
+    Map<const Matrix<typename D1::Scalar, 3, 1> >   dv1    ( & d1(7) );
+    Map<const Matrix<typename D2::Scalar, 3, 1> >   dp2    ( & d2(0) );
+    Map<const Quaternion<typename D2::Scalar> >     dq2    ( & d2(3) );
+    Map<const Matrix<typename D2::Scalar, 3, 1> >   dv2    ( & d2(7) );
+    Map<Matrix<typename D3::Scalar, 3, 1> >         diff_p ( & dif(0) );
+    Map<Matrix<typename D3::Scalar, 3, 1> >         diff_o ( & dif(3) );
+    Map<Matrix<typename D3::Scalar, 3, 1> >         diff_v ( & dif(6) );
+
+    Matrix<typename D4::Scalar, 3, 3> 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 = minus(d1, d2) is
+     *   dp = dp2 - dp1
+     *   do = Log(dq1.conj * dq2)
+     *   dv = dv2 - dv1
+     *
+     * With trivial Jacobians for dp and dv, and:
+     *   J_do_dq1 = - J_l_inv(theta)
+     *   J_do_dq2 =   J_r_inv(theta)
+     */
+
+    J_diff_d1 = - Matrix<typename D4::Scalar, 6, 6>::Identity();// d(p2  - p1) / d(p1) = - Identity
+    J_diff_d1.block(3,3,3,3) = J_do_dq1;       // d(R1.tr*R2) / d(R1) = - J_l_inv(theta)
+
+    J_diff_d2.setIdentity(6,6);                                    // d(R1.tr*R2) / d(R2) =   Identity
+    J_diff_d2.block(3,3,3,3) = J_do_dq2;      // d(R1.tr*R2) / d(R1) =   J_r_inv(theta)
+}
+
+template<typename D1, typename D2>
+inline Matrix<typename D1::Scalar, 6, 1> minus(const MatrixBase<D1>& d1,
+                                               const MatrixBase<D2>& d2)
+{
+    Matrix<typename D1::Scalar, 6, 1> 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 /* SE3_H_ */
diff --git a/include/base/capture/capture_base.h b/include/base/capture/capture_base.h
index a3a94e6769a9517e02ff3e7b11395d84a8f3efb4..293ab9d0303aa493930ae11336b7abfe58d7d0f9 100644
--- a/include/base/capture/capture_base.h
+++ b/include/base/capture/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,11 +44,13 @@ 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; }
 
+        bool process();
+
         unsigned int id();
         TimeStamp getTimeStamp() const;
         void setTimeStamp(const TimeStamp& _ts);
@@ -217,6 +218,14 @@ inline void CaptureBase::setTimeStampToNow()
     time_stamp_.setToNow();
 }
 
+inline bool CaptureBase::process()
+{
+    assert (getSensorPtr() != nullptr && "Attempting to process a capture with no associated sensor. Either set the capture's sensor or call sensor->process(capture) instead.");
+
+    return getSensorPtr()->process(shared_from_this());
+}
+
+
 } // namespace wolf
 
 #endif
diff --git a/include/base/ceres_wrapper/ceres_manager.h b/include/base/ceres_wrapper/ceres_manager.h
index 7a0404cec9e50580b52710271e73bf284d38df2c..bd81359681008701c6568a3d5300f5f3ab54c612 100644
--- a/include/base/ceres_wrapper/ceres_manager.h
+++ b/include/base/ceres_wrapper/ceres_manager.h
@@ -55,6 +55,14 @@ class CeresManager : public SolverManager
 
         virtual void computeCovariances(const StateBlockList& st_list) override;
 
+        virtual bool hasConverged() override;
+
+        virtual SizeStd iterations() override;
+
+        virtual Scalar initialCost() override;
+
+        virtual Scalar finalCost() override;
+
         ceres::Solver::Options& getSolverOptions();
 
         void check();
diff --git a/include/base/constraint/constraint_AHP.h b/include/base/constraint/constraint_AHP.h
index 59705efbbafc267d708d60f6ba927e806c79a5bd..a0ab7db1fa497ac7f9a9f87db97ecb128650c306 100644
--- a/include/base/constraint/constraint_AHP.h
+++ b/include/base/constraint/constraint_AHP.h
@@ -184,18 +184,14 @@ inline bool ConstraintAHP::operator ()(const T* const _current_frame_p,
 }
 
 inline ConstraintAHPPtr ConstraintAHP::create(const FeatureBasePtr&   _ftr_ptr,
-                                                    const LandmarkAHPPtr&   _lmk_ahp_ptr,
-                                                    const ProcessorBasePtr& _processor_ptr,
-                                                    bool             _apply_loss_function,
-                                                    ConstraintStatus _status)
+                                              const LandmarkAHPPtr&   _lmk_ahp_ptr,
+                                              const ProcessorBasePtr& _processor_ptr,
+                                              bool             _apply_loss_function,
+                                              ConstraintStatus _status)
 {
     // construct constraint
     ConstraintAHPPtr ctr_ahp = std::make_shared<ConstraintAHP>(_ftr_ptr, _lmk_ahp_ptr, _processor_ptr, _apply_loss_function, _status);
 
-    // link it to wolf tree <-- these pointers cannot be set at construction time
-    _lmk_ahp_ptr->getAnchorFrame()->addConstrainedBy(ctr_ahp);
-    _lmk_ahp_ptr->addConstrainedBy(ctr_ahp);
-
     return ctr_ahp;
 }
 
diff --git a/include/base/constraint/constraint_base.h b/include/base/constraint/constraint_base.h
index 9932bad8d03cb757c87011c39deb30a61bcf1887..1a0b4f14f77f559f983b7dbce0b4028bea45baa9 100644
--- a/include/base/constraint/constraint_base.h
+++ b/include/base/constraint/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/include/base/feature/feature_base.h b/include/base/feature/feature_base.h
index 88521e9a6e9c55ececea8fbf9c81cda2aa8edf13..1ab46cccceebfae070368341aa2edde0000e694a 100644
--- a/include/base/feature/feature_base.h
+++ b/include/base/feature/feature_base.h
@@ -24,7 +24,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_;
@@ -45,7 +44,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/include/base/frame_base.h b/include/base/frame_base.h
index b8430b41fe9624b80227aa39ba6088051393b760..16d13dbee1f0484dee1f911df14b07fd476a007d 100644
--- a/include/base/frame_base.h
+++ b/include/base/frame_base.h
@@ -36,7 +36,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_;
@@ -66,7 +65,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();
 
         // Frame properties -----------------------------------------------
     public:
diff --git a/include/base/landmark/landmark_base.h b/include/base/landmark/landmark_base.h
index f0a61e47496fb8373f6e75ee9cbbb71138191111..8b365e9be506f9271571542970683032326819f9 100644
--- a/include/base/landmark/landmark_base.h
+++ b/include/base/landmark/landmark_base.h
@@ -28,7 +28,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
@@ -47,7 +46,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/include/base/node_base.h b/include/base/node_base.h
index 0c1d60d8603b56026867fd4be70441a42d38a383..2426f53a53357005f9e0fcab1ed0af72b7494b7a 100644
--- a/include/base/node_base.h
+++ b/include/base/node_base.h
@@ -66,6 +66,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 = "");
@@ -75,6 +77,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);
@@ -94,7 +97,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)
 {
     //
 }
@@ -119,6 +123,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/include/base/problem.h b/include/base/problem.h
index c6374623ba7ea4550c1d2dce053026bd9789716f..b15159163f4b13248bca7c1a23fa9ae90b30541c 100644
--- a/include/base/problem.h
+++ b/include/base/problem.h
@@ -43,7 +43,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_;
@@ -59,6 +59,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/include/base/processor/processor_base.h b/include/base/processor/processor_base.h
index 8537db78b20189aef1adeb3bdc2abdf7f41f3681..6a1d23cfb3fd19a625407c4de20eb6d17784e371 100644
--- a/include/base/processor/processor_base.h
+++ b/include/base/processor/processor_base.h
@@ -56,8 +56,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
          *
@@ -145,15 +145,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/include/base/processor/processor_motion.h b/include/base/processor/processor_motion.h
index 5fc43d3887f9277512e67cca351e23edb2107771..0ce05f202e9eb68290871cbcd2319b8c12528a99 100644
--- a/include/base/processor/processor_motion.h
+++ b/include/base/processor/processor_motion.h
@@ -367,6 +367,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
@@ -394,6 +412,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
@@ -428,7 +450,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_;
@@ -442,7 +464,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/include/base/processor/processor_tracker_feature.h b/include/base/processor/processor_tracker_feature.h
index d118388717c322444cac0da6c26c95fc856edeba..52bd493f6870a1424621f5085ebed10b9558df60 100644
--- a/include/base/processor/processor_tracker_feature.h
+++ b/include/base/processor/processor_tracker_feature.h
@@ -114,11 +114,11 @@ class ProcessorTrackerFeature : public ProcessorTracker
         virtual unsigned int processKnown();
 
         /** \brief Track provided features from \b last to \b incoming
-         * \param _feature_list_in input list of features in \b last to track
-         * \param _feature_list_out returned list of features found in \b incoming
+         * \param _features_last_in input list of features in \b last to track
+         * \param _features_incoming_out returned list of features found in \b incoming
          * \param _feature_correspondences returned map of correspondences: _feature_correspondences[feature_out_ptr] = feature_in_ptr
          */
-        virtual unsigned int trackFeatures(const FeatureBaseList& _feature_list_in, FeatureBaseList& _feature_list_out, FeatureMatchMap& _feature_correspondences) = 0;
+        virtual unsigned int trackFeatures(const FeatureBaseList& _features_last_in, FeatureBaseList& _features_incoming_out, FeatureMatchMap& _feature_correspondences) = 0;
 
         /** \brief Correct the drift in incoming feature by re-comparing against the corresponding feature in origin.
          * \param _origin_feature input feature in origin capture tracked
@@ -153,7 +153,7 @@ class ProcessorTrackerFeature : public ProcessorTracker
          *
          * The function sets the member new_features_last_, the list of newly detected features.
          */
-        virtual unsigned int detectNewFeatures(const unsigned int& _max_new_features, FeatureBaseList& _feature_list_out) = 0;
+        virtual unsigned int detectNewFeatures(const unsigned int& _max_new_features, FeatureBaseList& _features_incoming_out) = 0;
 
         /** \brief Create a new constraint and link it to the wolf tree
          * \param _feature_ptr pointer to the parent Feature
diff --git a/include/base/processor/processor_tracker_feature_corner.h b/include/base/processor/processor_tracker_feature_corner.h
index 508867b73ed0dd716f13356182ab94a43d2600e6..5bc33bf47a51b4b267c64ecf916ccc271047c280 100644
--- a/include/base/processor/processor_tracker_feature_corner.h
+++ b/include/base/processor/processor_tracker_feature_corner.h
@@ -82,11 +82,11 @@ class ProcessorTrackerFeatureCorner : public ProcessorTrackerFeature
         void advanceDerived();
 
         /** \brief Track provided features from \b last to \b incoming
-         * \param _feature_list_in input list of features in \b last to track
-         * \param _feature_list_out returned list of features found in \b incoming
+         * \param _features_last_in input list of features in \b last to track
+         * \param _features_incoming_out returned list of features found in \b incoming
          * \param _feature_correspondences returned map of correspondences: _feature_correspondences[feature_out_ptr] = feature_in_ptr
          */
-        virtual unsigned int trackFeatures(const FeatureBaseList& _feature_list_in, FeatureBaseList& _feature_list_out,
+        virtual unsigned int trackFeatures(const FeatureBaseList& _features_last_in, FeatureBaseList& _features_incoming_out,
                                            FeatureMatchMap& _feature_correspondences);
 
         /** \brief Correct the drift in incoming feature by re-comparing against the corresponding feature in origin.
@@ -115,7 +115,7 @@ class ProcessorTrackerFeatureCorner : public ProcessorTrackerFeature
          * The function sets the member new_features_list_, the list of newly detected features,
          * to be used for landmark initialization.
          */
-        virtual unsigned int detectNewFeatures(const unsigned int& _max_features, FeatureBaseList& _feature_list_out);
+        virtual unsigned int detectNewFeatures(const unsigned int& _max_features, FeatureBaseList& _features_incoming_out);
 
         virtual ConstraintBasePtr createConstraint(FeatureBasePtr _feature_ptr, FeatureBasePtr _feature_other_ptr);
 
diff --git a/include/base/processor/processor_tracker_feature_dummy.h b/include/base/processor/processor_tracker_feature_dummy.h
index 7c4c453efe8f11ed429fc68305c91cf879c5b3ec..dbc024b7c1187c5a9cf4b0753aaa8d0fc56fe050 100644
--- a/include/base/processor/processor_tracker_feature_dummy.h
+++ b/include/base/processor/processor_tracker_feature_dummy.h
@@ -32,11 +32,11 @@ class ProcessorTrackerFeatureDummy : public ProcessorTrackerFeature
         unsigned int n_feature_;
 
         /** \brief Track provided features from \b last to \b incoming
-         * \param _feature_list_in input list of features in \b last to track
-         * \param _feature_list_out returned list of features found in \b incoming
+         * \param _features_last_in input list of features in \b last to track
+         * \param _features_incoming_out returned list of features found in \b incoming
          * \param _feature_correspondences returned map of correspondences: _feature_correspondences[feature_out_ptr] = feature_in_ptr
          */
-        virtual unsigned int trackFeatures(const FeatureBaseList& _feature_list_in, FeatureBaseList& _feature_list_out,
+        virtual unsigned int trackFeatures(const FeatureBaseList& _features_last_in, FeatureBaseList& _features_incoming_out,
                                            FeatureMatchMap& _feature_correspondences);
 
         /** \brief Correct the drift in incoming feature by re-comparing against the corresponding feature in origin.
@@ -65,7 +65,7 @@ class ProcessorTrackerFeatureDummy : public ProcessorTrackerFeature
          * The function sets the member new_features_list_, the list of newly detected features,
          * to be used for landmark initialization.
          */
-        virtual unsigned int detectNewFeatures(const unsigned int& _max_features, FeatureBaseList& _feature_list_out);
+        virtual unsigned int detectNewFeatures(const unsigned int& _max_features, FeatureBaseList& _features_incoming_out);
 
         virtual ConstraintBasePtr createConstraint(FeatureBasePtr _feature_ptr, FeatureBasePtr _feature_other_ptr);
 
diff --git a/include/base/processor/processor_tracker_feature_image.h b/include/base/processor/processor_tracker_feature_image.h
index 2bb0d9fb867788e3286bf739f205a17913731e71..48f7e7a705bd0b2006f5ec009653b895e211b11d 100644
--- a/include/base/processor/processor_tracker_feature_image.h
+++ b/include/base/processor/processor_tracker_feature_image.h
@@ -91,7 +91,7 @@ class ProcessorTrackerFeatureImage : public ProcessorTrackerFeature
             image_last_ = image_incoming_;
         }
 
-        virtual unsigned int trackFeatures(const FeatureBaseList& _feature_list_in, FeatureBaseList& _feature_list_out,
+        virtual unsigned int trackFeatures(const FeatureBaseList& _features_last_in, FeatureBaseList& _features_incoming_out,
                                            FeatureMatchMap& _feature_correspondences);
 
         /** \brief Correct the drift in incoming feature by re-comparing against the corresponding feature in origin.
@@ -119,7 +119,7 @@ class ProcessorTrackerFeatureImage : public ProcessorTrackerFeature
          *
          * \return The number of detected Features.
          */
-        virtual unsigned int detectNewFeatures(const unsigned int& _max_new_features, FeatureBaseList& _feature_list_out);
+        virtual unsigned int detectNewFeatures(const unsigned int& _max_new_features, FeatureBaseList& _features_incoming_out);
 
         virtual ConstraintBasePtr createConstraint(FeatureBasePtr _feature_ptr, FeatureBasePtr _feature_other_ptr);
 
diff --git a/include/base/processor/processor_tracker_feature_trifocal.h b/include/base/processor/processor_tracker_feature_trifocal.h
index 8ff87d9a9d1efa9e1b51a43b2bb2a390c28c7384..dca3b791a6885b5e89296b6f988838a511c212fa 100644
--- a/include/base/processor/processor_tracker_feature_trifocal.h
+++ b/include/base/processor/processor_tracker_feature_trifocal.h
@@ -62,11 +62,11 @@ class ProcessorTrackerFeatureTrifocal : public ProcessorTrackerFeature
         virtual void configure(SensorBasePtr _sensor) override;
 
         /** \brief Track provided features from \b last to \b incoming
-         * \param _feature_list_in input list of features in \b last to track
-         * \param _feature_list_out returned list of features found in \b incoming
+         * \param _features_last_in input list of features in \b last to track
+         * \param _features_incoming_out returned list of features found in \b incoming
          * \param _feature_matches returned map of correspondences: _feature_matches[feature_out_ptr] = feature_in_ptr
          */
-        virtual unsigned int trackFeatures(const FeatureBaseList& _feature_list_in, FeatureBaseList& _feature_list_out, FeatureMatchMap& _feature_matches) override;
+        virtual unsigned int trackFeatures(const FeatureBaseList& _features_last_in, FeatureBaseList& _features_incoming_out, FeatureMatchMap& _feature_matches) override;
 
         /** \brief Correct the drift in incoming feature by re-comparing against the corresponding feature in origin.
          * \param _origin_feature input feature in origin capture tracked
@@ -92,7 +92,7 @@ class ProcessorTrackerFeatureTrifocal : public ProcessorTrackerFeature
          *
          * The function sets the member new_features_last_, the list of newly detected features.
          */
-        virtual unsigned int detectNewFeatures(const unsigned int& _max_new_features, FeatureBaseList& _feature_list_out) override;
+        virtual unsigned int detectNewFeatures(const unsigned int& _max_new_features, FeatureBaseList& _features_incoming_out) override;
 
         /** \brief Create a new constraint and link it to the wolf tree
          * \param _feature_ptr pointer to the parent Feature
diff --git a/include/base/processor/processor_tracker_landmark.h b/include/base/processor/processor_tracker_landmark.h
index 961e6f5dcb5004ad3c719aa767f1a070ef0d835d..293570afa29406dfc8515ef9c7d684af2da35cf9 100644
--- a/include/base/processor/processor_tracker_landmark.h
+++ b/include/base/processor/processor_tracker_landmark.h
@@ -93,31 +93,24 @@ class ProcessorTrackerLandmark : public ProcessorTracker
         /** \brief Tracker function
          * \return The number of successful tracks.
          *
+         * Find Features in \b incoming Capture corresponding to known landmarks in the \b Map.
+         *
          * This is the tracker function to be implemented in derived classes.
          * It operates on the \b incoming capture pointed by incoming_ptr_.
          *
-         * This should do one of the following, depending on the design of the tracker:
-         *   - Track Features against other Features in the \b origin Capture. Tips:
-         *     - An intermediary step of matching against Features in the \b last Capture makes tracking easier.
-         *     - Once tracked against last, then the link to Features in \b origin is provided by the Features' Constraints in \b last.
-         *     - If required, correct the drift by re-comparing against the Features in origin.
-         *     - The Constraints in \b last need to be transferred to \b incoming (moved, not copied).
-         *
-         * The function must generate the necessary Features in the \b incoming Capture,
-         * of the correct type, derived from FeatureBase.
-         *
-         * It must also generate the constraints, of the correct type, derived from ConstraintBase
-         * (through ConstraintAnalytic or ConstraintSparse).
+         * The function must populate the list of Features in the \b incoming Capture.
+         * Features need to be of the correct type, derived from FeatureBase.
          */
         virtual unsigned int processKnown();
 
         /** \brief Find provided landmarks in the incoming capture
-         * \param _landmark_list_in input list of landmarks to be found in incoming
-         * \param _feature_list_out returned list of incoming features corresponding to a landmark of _landmark_list_in
+         * \param _landmarks_in input list of landmarks to be found in incoming
+         * \param _features_incoming_out returned list of incoming features corresponding to a landmark of _landmarks_in
          * \param _feature_landmark_correspondences returned map of landmark correspondences: _feature_landmark_correspondences[_feature_out_ptr] = landmark_in_ptr
          */
-        virtual unsigned int findLandmarks(const LandmarkBaseList& _landmark_list_in, FeatureBaseList& _feature_list_out,
-                                           LandmarkMatchMap& _feature_landmark_correspondences) = 0;
+        virtual unsigned int findLandmarks(const LandmarkBaseList&  _landmarks_in,
+                                           FeatureBaseList&         _features_incoming_out,
+                                           LandmarkMatchMap&        _feature_landmark_correspondences) = 0;
 
         /** \brief Vote for KeyFrame generation
          *
@@ -146,7 +139,7 @@ class ProcessorTrackerLandmark : public ProcessorTracker
          * The function sets the member new_features_list_, the list of newly detected features,
          * to be used for landmark initialization.
          */
-        virtual unsigned int detectNewFeatures(const unsigned int& _max_features, FeatureBaseList& _feature_list_out) = 0;
+        virtual unsigned int detectNewFeatures(const unsigned int& _max_features, FeatureBaseList& _features_incoming_out) = 0;
 
         /** \brief Creates a landmark for each of new_features_last_
          **/
diff --git a/include/base/processor/processor_tracker_landmark_corner.h b/include/base/processor/processor_tracker_landmark_corner.h
index a533ce8e32f512d1cd5ac08583b66ce7724d34a1..bd81b1643980f41468180735b85d9dcabd550346 100644
--- a/include/base/processor/processor_tracker_landmark_corner.h
+++ b/include/base/processor/processor_tracker_landmark_corner.h
@@ -118,11 +118,11 @@ class ProcessorTrackerLandmarkCorner : public ProcessorTrackerLandmark
         }
 
         /** \brief Find provided landmarks in the incoming capture
-         * \param _landmark_list_in input list of landmarks to be found in incoming
-         * \param _feature_list_out returned list of incoming features corresponding to a landmark of _landmark_list_in
+         * \param _landmarks_in input list of landmarks to be found in incoming
+         * \param _features_incoming_out returned list of incoming features corresponding to a landmark of _landmarks_in
          * \param _feature_landmark_correspondences returned map of landmark correspondences: _feature_landmark_correspondences[_feature_out_ptr] = landmark_in_ptr
          */
-        virtual unsigned int findLandmarks(const LandmarkBaseList& _landmark_list_in, FeatureBaseList& _feature_list_out,
+        virtual unsigned int findLandmarks(const LandmarkBaseList& _landmarks_in, FeatureBaseList& _features_incoming_out,
                                            LandmarkMatchMap& _feature_landmark_correspondences);
 
         /** \brief Vote for KeyFrame generation
@@ -144,7 +144,7 @@ class ProcessorTrackerLandmarkCorner : public ProcessorTrackerLandmark
          * The function sets the member new_features_list_, the list of newly detected features,
          * to be used for landmark initialization.
          */
-        virtual unsigned int detectNewFeatures(const unsigned int& _max_features, FeatureBaseList& _feature_list_out);
+        virtual unsigned int detectNewFeatures(const unsigned int& _max_features, FeatureBaseList& _features_incoming_out);
 
         /** \brief Create one landmark
          *
diff --git a/include/base/processor/processor_tracker_landmark_dummy.h b/include/base/processor/processor_tracker_landmark_dummy.h
index 612a46a5225cefc56e4edf1026b096d8985e0539..1d9d66bbaeef70cdf9366180ff55b796d78b73ac 100644
--- a/include/base/processor/processor_tracker_landmark_dummy.h
+++ b/include/base/processor/processor_tracker_landmark_dummy.h
@@ -31,11 +31,11 @@ class ProcessorTrackerLandmarkDummy : public ProcessorTrackerLandmark
         virtual void postProcess(); // implemented
 
         /** \brief Find provided landmarks in the incoming capture
-         * \param _landmark_list_in input list of landmarks to be found in incoming
-         * \param _feature_list_out returned list of incoming features corresponding to a landmark of _landmark_list_in
+         * \param _landmarks_in input list of landmarks to be found in incoming
+         * \param _features_incoming_out returned list of incoming features corresponding to a landmark of _landmarks_in
          * \param _feature_landmark_correspondences returned map of landmark correspondences: _feature_landmark_correspondences[_feature_out_ptr] = landmark_in_ptr
          */
-        virtual unsigned int findLandmarks(const LandmarkBaseList& _landmark_list_in, FeatureBaseList& _feature_list_out,
+        virtual unsigned int findLandmarks(const LandmarkBaseList& _landmarks_in, FeatureBaseList& _features_incoming_out,
                                            LandmarkMatchMap& _feature_landmark_correspondences);
 
         /** \brief Vote for KeyFrame generation
@@ -56,7 +56,7 @@ class ProcessorTrackerLandmarkDummy : public ProcessorTrackerLandmark
          * The function sets the member new_features_list_, the list of newly detected features,
          * to be used for landmark initialization.
          */
-        virtual unsigned int detectNewFeatures(const unsigned int& _max_features, FeatureBaseList& _feature_list_out);
+        virtual unsigned int detectNewFeatures(const unsigned int& _max_features, FeatureBaseList& _features_incoming_out);
 
         /** \brief Create one landmark
          *
diff --git a/include/base/processor/processor_tracker_landmark_image.h b/include/base/processor/processor_tracker_landmark_image.h
index 9c80999cc69435b6a854f5ef5f19f15b4efad4d3..1b7a0dbd6d5d33d3adb40742768b5470bb2a3c11 100644
--- a/include/base/processor/processor_tracker_landmark_image.h
+++ b/include/base/processor/processor_tracker_landmark_image.h
@@ -111,11 +111,11 @@ class ProcessorTrackerLandmarkImage : public ProcessorTrackerLandmark
 
         //Pure virtual
         /** \brief Find provided landmarks in the incoming capture
-         * \param _landmark_list_in input list of landmarks to be found in incoming
-         * \param _feature_list_out returned list of incoming features corresponding to a landmark of _landmark_list_in
+         * \param _landmarks_in input list of landmarks to be found in incoming
+         * \param _features_incoming_out returned list of incoming features corresponding to a landmark of _landmarks_in
          * \param _feature_landmark_correspondences returned map of landmark correspondences: _feature_landmark_correspondences[_feature_out_ptr] = landmark_in_ptr
          */
-        virtual unsigned int findLandmarks(const LandmarkBaseList& _landmark_list_in, FeatureBaseList& _feature_list_out,
+        virtual unsigned int findLandmarks(const LandmarkBaseList& _landmarks_in, FeatureBaseList& _features_incoming_out,
                                            LandmarkMatchMap& _feature_landmark_correspondences);
 
         /** \brief Vote for KeyFrame generation
@@ -135,7 +135,7 @@ class ProcessorTrackerLandmarkImage : public ProcessorTrackerLandmark
          *
          * \return The number of detected Features.
          */
-        virtual unsigned int detectNewFeatures(const unsigned int& _max_new_features, FeatureBaseList& _feature_list_out);
+        virtual unsigned int detectNewFeatures(const unsigned int& _max_new_features, FeatureBaseList& _features_incoming_out);
 
         /** \brief Create one landmark
          *
diff --git a/include/base/processor/processor_tracker_landmark_polyline.h b/include/base/processor/processor_tracker_landmark_polyline.h
index 1bcf88ba1f3be2e0dccfcb1fefe51ee559a5d385..e8d70824a898a85130775aed2bb712945ef4e6fc 100644
--- a/include/base/processor/processor_tracker_landmark_polyline.h
+++ b/include/base/processor/processor_tracker_landmark_polyline.h
@@ -129,11 +129,11 @@ class ProcessorTrackerLandmarkPolyline : public ProcessorTrackerLandmark
         void resetDerived();
 
         /** \brief Find provided landmarks in the incoming capture
-         * \param _landmark_list_in input list of landmarks to be found in incoming
-         * \param _feature_list_out returned list of incoming features corresponding to a landmark of _landmark_list_in
+         * \param _landmarks_in input list of landmarks to be found in incoming
+         * \param _features_incoming_out returned list of incoming features corresponding to a landmark of _landmarks_in
          * \param _feature_landmark_correspondences returned map of landmark correspondences: _feature_landmark_correspondences[_feature_out_ptr] = landmark_in_ptr
          */
-        virtual unsigned int findLandmarks(const LandmarkBaseList& _landmark_list_in, FeatureBaseList& _feature_list_out,
+        virtual unsigned int findLandmarks(const LandmarkBaseList& _landmarks_in, FeatureBaseList& _features_incoming_out,
                                            LandmarkMatchMap& _feature_landmark_correspondences);
 
         /** \brief Vote for KeyFrame generation
@@ -155,7 +155,7 @@ class ProcessorTrackerLandmarkPolyline : public ProcessorTrackerLandmark
          * The function sets the member new_features_list_, the list of newly detected features,
          * to be used for landmark initialization.
          */
-        virtual unsigned int detectNewFeatures(const unsigned int& _max_features, FeatureBaseList& _feature_list_out);
+        virtual unsigned int detectNewFeatures(const unsigned int& _max_features, FeatureBaseList& _features_incoming_out);
 
         /** \brief Creates a landmark for each of new_features_last_
          **/
@@ -210,11 +210,11 @@ inline ProcessorTrackerLandmarkPolyline::ProcessorTrackerLandmarkPolyline(Proces
 {
 }
 
-inline unsigned int ProcessorTrackerLandmarkPolyline::detectNewFeatures(const unsigned int& _max_features, FeatureBaseList& _feature_list_out)
+inline unsigned int ProcessorTrackerLandmarkPolyline::detectNewFeatures(const unsigned int& _max_features, FeatureBaseList& _features_incoming_out)
 {
     // already computed since each scan is computed in preprocess()
-    _feature_list_out = std::move(polylines_last_);
-    return _feature_list_out.size();
+    _features_incoming_out = std::move(polylines_last_);
+    return _features_incoming_out.size();
 }
 
 inline void ProcessorTrackerLandmarkPolyline::advanceDerived()
diff --git a/include/base/sensor/sensor_base.h b/include/base/sensor/sensor_base.h
index d2b3b00798dbf1f0134d8cff4cb45307c266a104..8707d13ac8bcac4b288f139dec53045625f02ac9 100644
--- a/include/base/sensor/sensor_base.h
+++ b/include/base/sensor/sensor_base.h
@@ -38,7 +38,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
@@ -90,7 +89,7 @@ class SensorBase : public NodeBase, public std::enable_shared_from_this<SensorBa
                    const bool _intr_dyn = false);
 
         virtual ~SensorBase();
-        void remove();
+        virtual void remove();
 
         unsigned int id();
 
diff --git a/include/base/solver/solver_manager.h b/include/base/solver/solver_manager.h
index e51cf5e44a092dd89fa56fe96b7c12cb1faa0a08..4801fd57f08f9a546b4255f51f9e11e4ca1bd354 100644
--- a/include/base/solver/solver_manager.h
+++ b/include/base/solver/solver_manager.h
@@ -55,6 +55,14 @@ public:
 
   virtual void computeCovariances(const StateBlockList& st_list) = 0;
 
+  virtual bool hasConverged() = 0;
+
+  virtual SizeStd iterations() = 0;
+
+  virtual Scalar initialCost() = 0;
+
+  virtual Scalar finalCost() = 0;
+
   virtual void update();
 
   ProblemPtr getProblemPtr();
diff --git a/src/capture/capture_base.cpp b/src/capture/capture_base.cpp
index ec0b94f43bc67c00c8cff7ba909b28a180627eeb..051b93c9ff33929900026a534e2948120480d30d 100644
--- a/src/capture/capture_base.cpp
+++ b/src/capture/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/ceres_wrapper/ceres_manager.cpp b/src/ceres_wrapper/ceres_manager.cpp
index 78777b627b211d9ca085e86f651ec012f0a62d13..ae8cb7b32fa1388d992d13c477929e991924f29b 100644
--- a/src/ceres_wrapper/ceres_manager.cpp
+++ b/src/ceres_wrapper/ceres_manager.cpp
@@ -359,6 +359,26 @@ void CeresManager::updateStateBlockLocalParametrization(const StateBlockPtr& sta
         addConstraint(ctr);
 }
 
+bool CeresManager::hasConverged()
+{
+    return summary_.termination_type == ceres::CONVERGENCE;
+}
+
+SizeStd CeresManager::iterations()
+{
+    return summary_.iterations.size();
+}
+
+Scalar CeresManager::initialCost()
+{
+    return Scalar(summary_.initial_cost);
+}
+
+Scalar CeresManager::finalCost()
+{
+    return Scalar(summary_.final_cost);
+}
+
 ceres::CostFunctionPtr CeresManager::createCostFunction(const ConstraintBasePtr& _ctr_ptr)
 {
     assert(_ctr_ptr != nullptr);
diff --git a/src/constraint/constraint_base.cpp b/src/constraint/constraint_base.cpp
index 38809e7ded690d7a8ee176237ad9ff888ec62d7b..f521cf214ecd8de80205a7dfbe8572433eca353c 100644
--- a/src/constraint/constraint_base.cpp
+++ b/src/constraint/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/feature/feature_base.cpp b/src/feature/feature_base.cpp
index d97b7c96f9e2ca56f65ab29fdc451487223a812c..314457be284f5fc16bfba370c41c47f8ff963ffa 100644
--- a/src/feature/feature_base.cpp
+++ b/src/feature/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/frame_base.cpp b/src/frame_base.cpp
index 4df54a7f21f1f9bdaa7015941c4d6e9333d8f356..5c1b7fee23ab834725340ac91f5df77c9db4b051 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/landmark/landmark_base.cpp b/src/landmark/landmark_base.cpp
index f99dbff19315462cb165a3ebb9ce68f014d55253..1ae594cb348c66819b3efffdbcf1acf0878e8fa2 100644
--- a/src/landmark/landmark_base.cpp
+++ b/src/landmark/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/problem.cpp b/src/problem.cpp
index 7c471cae6aeffaccf46d69472e2eb1b0b0fead8c..65da6a2279bfcd8959f4267945ee813317aaaafd 100644
--- a/src/problem.cpp
+++ b/src/problem.cpp
@@ -35,25 +35,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.");
@@ -342,6 +343,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/processor/processor_base.cpp b/src/processor/processor_base.cpp
index ec075ffed881383be09e2a91c5ba8096c32dfc2c..aa259d706262904969e9e27245a50e06456584a0 100644
--- a/src/processor/processor_base.cpp
+++ b/src/processor/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());
 }
@@ -96,6 +95,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());
 
@@ -131,33 +136,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/processor_motion.cpp b/src/processor/processor_motion.cpp
index f401cb1202b6feba596d54eedd15e82f41979228..0fe59c59aa026654b2ed5a3d8b924b3d5cfdee48 100644
--- a/src/processor/processor_motion.cpp
+++ b/src/processor/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_)
@@ -243,7 +243,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.
@@ -268,7 +268,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);
         
@@ -290,35 +290,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);
@@ -381,12 +352,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_,
@@ -402,11 +373,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(),
@@ -495,8 +469,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();
@@ -512,7 +531,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
@@ -527,7 +546,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
@@ -578,7 +597,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)
     {
@@ -590,7 +609,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/processor_odom_2D.cpp b/src/processor/processor_odom_2D.cpp
index a0f584296bec01b657c0219d5cf0baf5e531e448..c6031386542c0e92385877a3e5905d5eb9e7684e 100644
--- a/src/processor/processor_odom_2D.cpp
+++ b/src/processor/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/processor/processor_tracker.cpp b/src/processor/processor_tracker.cpp
index 1c017e2095eb9c82ba3895cb1eb9e82249bdf034..f8138e1fc7e1b0d152bf0576ee6fcb454e75ca0b 100644
--- a/src/processor/processor_tracker.cpp
+++ b/src/processor/processor_tracker.cpp
@@ -61,8 +61,9 @@ void ProcessorTracker::process(CaptureBasePtr const _incoming_ptr)
             pack->key_frame->addCapture(incoming_ptr_);
 
             // Process info
+            // TrackerFeature:  We only process new features in Last, here last = nullptr, so we do not have anything to do.
+            // TrackerLandmark: If we have given a map, all landmarks in the map are know. Process them.
             processKnown();
-            // We only process new features in Last, here last = nullptr, so we do not have anything to do.
 
             // Update pointers
             resetDerived();
diff --git a/src/processor/processor_tracker_feature.cpp b/src/processor/processor_tracker_feature.cpp
index c1c2210e393449e82585840a6fa6f82ae73ecdef..cfb6aa984f179b891cfc43f32524625e8d070a16 100644
--- a/src/processor/processor_tracker_feature.cpp
+++ b/src/processor/processor_tracker_feature.cpp
@@ -148,9 +148,20 @@ void ProcessorTrackerFeature::establishConstraints()
         FeatureBasePtr feature_in_origin = pair_trkid_pair.second.first;
         FeatureBasePtr feature_in_last   = pair_trkid_pair.second.second;
 
-        auto constraint  = createConstraint(feature_in_last, feature_in_origin);
-        feature_in_last  ->addConstraint(constraint);
-        feature_in_origin->addConstrainedBy(constraint);
+        auto ctr_ptr  = createConstraint(feature_in_last, feature_in_origin);
+        feature_in_last  ->addConstraint(ctr_ptr);
+        feature_in_origin->addConstrainedBy(ctr_ptr);
+
+        if (ctr_ptr != nullptr) // constraint links
+        {
+            FrameBasePtr frm = ctr_ptr->getFrameOtherPtr();
+            if (frm)
+                frm->addConstrainedBy(ctr_ptr);
+            CaptureBasePtr cap = ctr_ptr->getCaptureOtherPtr();
+            if (cap)
+                cap->addConstrainedBy(ctr_ptr);
+        }
+
 
         WOLF_DEBUG( "Constraint: track: " , feature_in_last->trackId(),
                     " origin: "           , feature_in_origin->id() ,
diff --git a/src/processor/processor_tracker_feature_corner.cpp b/src/processor/processor_tracker_feature_corner.cpp
index b840602c086660a67d50c379afb848933c1dd5f4..0a8e7c7c232fd465f07c3704bbf63f7d615a20cc 100644
--- a/src/processor/processor_tracker_feature_corner.cpp
+++ b/src/processor/processor_tracker_feature_corner.cpp
@@ -70,14 +70,14 @@ void ProcessorTrackerFeatureCorner::advanceDerived()
     corners_last_ = std::move(corners_incoming_);
 }
 
-unsigned int ProcessorTrackerFeatureCorner::trackFeatures(const FeatureBaseList& _feature_list_in,
-                                                         FeatureBaseList& _feature_list_out,
+unsigned int ProcessorTrackerFeatureCorner::trackFeatures(const FeatureBaseList& _features_last_in,
+                                                         FeatureBaseList& _features_incoming_out,
                                                          FeatureMatchMap& _feature_correspondences)
 {
-    std::cout << "tracking " << _feature_list_in.size() << " features..." << std::endl;
+    std::cout << "tracking " << _features_last_in.size() << " features..." << std::endl;
 
     Eigen::Vector3s expected_feature_pose;
-    for (auto feat_in_ptr : _feature_list_in)
+    for (auto feat_in_ptr : _features_last_in)
     {
         expected_feature_pose = R_current_prev_ * feat_in_ptr->getMeasurement().head<3>() + t_current_prev_;
         
@@ -91,7 +91,7 @@ unsigned int ProcessorTrackerFeatureCorner::trackFeatures(const FeatureBaseList&
                 _feature_correspondences[*feat_out_it] = std::make_shared<FeatureMatch>(FeatureMatch({feat_in_ptr,0}));
                 
                 // move matched feature to list
-                _feature_list_out.splice(_feature_list_out.end(), corners_incoming_, feat_out_it);
+                _features_incoming_out.splice(_features_incoming_out.end(), corners_incoming_, feat_out_it);
                 
                 std::cout << "feature " << feat_in_ptr->id() << " tracked!" << std::endl;
             }
@@ -99,7 +99,7 @@ unsigned int ProcessorTrackerFeatureCorner::trackFeatures(const FeatureBaseList&
         }
     }
 
-    return _feature_list_out.size();
+    return _features_incoming_out.size();
 }
 
 bool ProcessorTrackerFeatureCorner::voteForKeyFrame()
@@ -107,11 +107,11 @@ bool ProcessorTrackerFeatureCorner::voteForKeyFrame()
     return incoming_ptr_->getFeatureList().size() < params_tracker_feature_corner_->n_corners_th;
 }
 
-unsigned int ProcessorTrackerFeatureCorner::detectNewFeatures(const unsigned int& _max_features, FeatureBaseList& _feature_list_out)
+unsigned int ProcessorTrackerFeatureCorner::detectNewFeatures(const unsigned int& _max_features, FeatureBaseList& _features_incoming_out)
 {
     // in corners_last_ remain all not tracked corners
-    _feature_list_out = std::move(corners_last_);
-    return _feature_list_out.size();
+    _features_incoming_out = std::move(corners_last_);
+    return _features_incoming_out.size();
 }
 
 ConstraintBasePtr ProcessorTrackerFeatureCorner::createConstraint(FeatureBasePtr _feature_ptr,
diff --git a/src/processor/processor_tracker_feature_dummy.cpp b/src/processor/processor_tracker_feature_dummy.cpp
index c218b04fd3531fbd1aaa9a4ade0acd90e0b8b273..6ce3d4fe18fbabc83d63582a9ac2090c63ce53be 100644
--- a/src/processor/processor_tracker_feature_dummy.cpp
+++ b/src/processor/processor_tracker_feature_dummy.cpp
@@ -13,13 +13,13 @@ namespace wolf
 
 unsigned int ProcessorTrackerFeatureDummy::count_ = 0;
 
-unsigned int ProcessorTrackerFeatureDummy::trackFeatures(const FeatureBaseList& _feature_list_in,
-                                                         FeatureBaseList& _feature_list_out,
+unsigned int ProcessorTrackerFeatureDummy::trackFeatures(const FeatureBaseList& _features_last_in,
+                                                         FeatureBaseList& _features_incoming_out,
                                                          FeatureMatchMap& _feature_correspondences)
 {
-    WOLF_INFO("tracking " , _feature_list_in.size() , " features...");
+    WOLF_INFO("tracking " , _features_last_in.size() , " features...");
 
-    for (auto feat_in : _feature_list_in)
+    for (auto feat_in : _features_last_in)
     {
         if (++count_ % 3 == 2) // lose one every 3 tracks
         {
@@ -28,14 +28,14 @@ unsigned int ProcessorTrackerFeatureDummy::trackFeatures(const FeatureBaseList&
         else
         {
             FeatureBasePtr ftr(std::make_shared<FeatureBase>("POINT IMAGE", feat_in->getMeasurement(), feat_in->getMeasurementCovariance()));
-            _feature_list_out.push_back(ftr);
-            _feature_correspondences[_feature_list_out.back()] = std::make_shared<FeatureMatch>(FeatureMatch({feat_in,0}));
+            _features_incoming_out.push_back(ftr);
+            _feature_correspondences[_features_incoming_out.back()] = std::make_shared<FeatureMatch>(FeatureMatch({feat_in,0}));
 
             WOLF_INFO("track: " , feat_in->trackId() , " last: " , feat_in->id() , " inc: " , ftr->id() , " !" );
         }
     }
 
-    return _feature_list_out.size();
+    return _features_incoming_out.size();
 }
 
 bool ProcessorTrackerFeatureDummy::voteForKeyFrame()
@@ -49,7 +49,7 @@ bool ProcessorTrackerFeatureDummy::voteForKeyFrame()
     return incoming_ptr_->getFeatureList().size() < params_tracker_feature_->min_features_for_keyframe;
 }
 
-unsigned int ProcessorTrackerFeatureDummy::detectNewFeatures(const unsigned int& _max_features, FeatureBaseList& _feature_list_out)
+unsigned int ProcessorTrackerFeatureDummy::detectNewFeatures(const unsigned int& _max_features, FeatureBaseList& _features_incoming_out)
 {
     WOLF_INFO("Detecting " , _max_features , " new features..." );
 
@@ -60,14 +60,14 @@ unsigned int ProcessorTrackerFeatureDummy::detectNewFeatures(const unsigned int&
         FeatureBasePtr ftr(std::make_shared<FeatureBase>("POINT IMAGE",
                                                          n_feature_* Eigen::Vector1s::Ones(),
                                                          Eigen::MatrixXs::Ones(1, 1)));
-        _feature_list_out.push_back(ftr);
+        _features_incoming_out.push_back(ftr);
 
         WOLF_INFO("feature " , ftr->id() , " detected!" );
     }
 
-    WOLF_INFO(_feature_list_out.size() , " features detected!");
+    WOLF_INFO(_features_incoming_out.size() , " features detected!");
 
-    return _feature_list_out.size();
+    return _features_incoming_out.size();
 }
 
 ConstraintBasePtr ProcessorTrackerFeatureDummy::createConstraint(FeatureBasePtr _feature_ptr,
diff --git a/src/processor/processor_tracker_feature_image.cpp b/src/processor/processor_tracker_feature_image.cpp
index 716a2cca77b6a8196955a9549490d138b4d93868..486cd623c75ca91b63ef63abca62d3b0bf552be0 100644
--- a/src/processor/processor_tracker_feature_image.cpp
+++ b/src/processor/processor_tracker_feature_image.cpp
@@ -135,14 +135,14 @@ void ProcessorTrackerFeatureImage::postProcess()
 {
 }
 
-unsigned int ProcessorTrackerFeatureImage::trackFeatures(const FeatureBaseList& _feature_list_in, FeatureBaseList& _feature_list_out,
+unsigned int ProcessorTrackerFeatureImage::trackFeatures(const FeatureBaseList& _features_last_in, FeatureBaseList& _features_incoming_out,
                                            FeatureMatchMap& _feature_matches)
 {
     KeyPointVector candidate_keypoints;
     cv::Mat candidate_descriptors;
     DMatchVector cv_matches;
 
-    for (auto feature_base_ptr : _feature_list_in)
+    for (auto feature_base_ptr : _features_last_in)
     {
         FeaturePointImagePtr feature_ptr = std::static_pointer_cast<FeaturePointImage>(feature_base_ptr);
 
@@ -167,7 +167,7 @@ unsigned int ProcessorTrackerFeatureImage::trackFeatures(const FeatureBaseList&
                         (candidate_descriptors.row(cv_matches[0].trainIdx)),
                         Eigen::Matrix2s::Identity()*params_tracker_feature_image_->pixel_noise_var);
                 incoming_point_ptr->setIsKnown(feature_ptr->isKnown());
-                _feature_list_out.push_back(incoming_point_ptr);
+                _features_incoming_out.push_back(incoming_point_ptr);
 
                 _feature_matches[incoming_point_ptr] = std::make_shared<FeatureMatch>(FeatureMatch({feature_base_ptr, normalized_score}));
             }
@@ -185,8 +185,8 @@ unsigned int ProcessorTrackerFeatureImage::trackFeatures(const FeatureBaseList&
             tracker_roi_.pop_back();
         }
     }
-//    std::cout << "TrackFeatures - Number of Features tracked: " << _feature_list_out.size() << std::endl;
-    return _feature_list_out.size();
+//    std::cout << "TrackFeatures - Number of Features tracked: " << _features_incoming_out.size() << std::endl;
+    return _features_incoming_out.size();
 }
 
 bool ProcessorTrackerFeatureImage::correctFeatureDrift(const FeatureBasePtr _origin_feature, const FeatureBasePtr _last_feature, FeatureBasePtr _incoming_feature)
@@ -244,7 +244,7 @@ bool ProcessorTrackerFeatureImage::correctFeatureDrift(const FeatureBasePtr _ori
     }
 }
 
-unsigned int ProcessorTrackerFeatureImage::detectNewFeatures(const unsigned int& _max_new_features, FeatureBaseList& _feature_list_out)
+unsigned int ProcessorTrackerFeatureImage::detectNewFeatures(const unsigned int& _max_new_features, FeatureBaseList& _features_incoming_out)
 {
     cv::Rect roi;
     KeyPointVector new_keypoints;
@@ -277,7 +277,7 @@ unsigned int ProcessorTrackerFeatureImage::detectNewFeatures(const unsigned int&
                             new_descriptors.row(index),
                             Eigen::Matrix2s::Identity()*params_tracker_feature_image_->pixel_noise_var);
                     point_ptr->setIsKnown(false);
-                    _feature_list_out.push_back(point_ptr);
+                    _features_incoming_out.push_back(point_ptr);
 
                     active_search_ptr_->hitCell(new_keypoints[0]);
 
diff --git a/src/processor/processor_tracker_feature_trifocal.cpp b/src/processor/processor_tracker_feature_trifocal.cpp
index 6a0fa198f7740767641d190b6191e4acb6eb13a8..03cd93f17f9a21b33493cc1cf76a45046061cb4a 100644
--- a/src/processor/processor_tracker_feature_trifocal.cpp
+++ b/src/processor/processor_tracker_feature_trifocal.cpp
@@ -134,7 +134,8 @@ bool ProcessorTrackerFeatureTrifocal::isInlier(const cv::KeyPoint& _kp_last, con
     return inlier;
 }
 
-unsigned int ProcessorTrackerFeatureTrifocal::detectNewFeatures(const unsigned int& _max_new_features, FeatureBaseList& _feature_list_out)
+
+unsigned int ProcessorTrackerFeatureTrifocal::detectNewFeatures(const unsigned int& _max_new_features, FeatureBaseList& _features_incoming_out)
 {
 //    // DEBUG =====================================
 //    clock_t debug_tStart;
@@ -174,7 +175,7 @@ unsigned int ProcessorTrackerFeatureTrifocal::detectNewFeatures(const unsigned i
                                 capture_last_->descriptors_.row(index_last),
                                 Eigen::Matrix2s::Identity() * params_tracker_feature_trifocal_->pixel_noise_std * params_tracker_feature_trifocal_->pixel_noise_std);
 
-                        _feature_list_out.push_back(last_point_ptr);
+                        _features_incoming_out.push_back(last_point_ptr);
 
                         // hit cell to acknowledge there's a tracked point in that cell
                         capture_last_->grid_features_->hitTrackingCell(kp_last);
@@ -192,17 +193,17 @@ unsigned int ProcessorTrackerFeatureTrifocal::detectNewFeatures(const unsigned i
             break; // There are no empty cells
     }
 
-    WOLF_TRACE("DetectNewFeatures - Number of new features detected: " , _feature_list_out.size() );
+    WOLF_TRACE("DetectNewFeatures - Number of new features detected: " , _features_incoming_out.size() );
 
 //    // DEBUG
 //    debug_comp_time_ = (double)(clock() - debug_tStart) / CLOCKS_PER_SEC;
 //    WOLF_TRACE("--> TIME: detect new features: TOTAL ",debug_comp_time_);
 //    WOLF_TRACE("======== ========= =========");
 
-    return _feature_list_out.size();
+    return _features_incoming_out.size();
 }
 
-unsigned int ProcessorTrackerFeatureTrifocal::trackFeatures(const FeatureBaseList& _feature_list_in, FeatureBaseList& _feature_list_out, FeatureMatchMap& _feature_matches)
+unsigned int ProcessorTrackerFeatureTrifocal::trackFeatures(const FeatureBaseList& _features_last_in, FeatureBaseList& _features_incoming_out, FeatureMatchMap& _feature_matches)
 {
 //    // DEBUG =====================================
 //    clock_t debug_tStart;
@@ -211,7 +212,7 @@ unsigned int ProcessorTrackerFeatureTrifocal::trackFeatures(const FeatureBaseLis
 //    WOLF_TRACE("======== TrackFeatures =========");
 //    // ===========================================
 
-    for (auto feature_base_last_ : _feature_list_in)
+    for (auto feature_base_last_ : _features_last_in)
     {
         FeaturePointImagePtr feature_last_ = std::static_pointer_cast<FeaturePointImage>(feature_base_last_);
 
@@ -233,7 +234,7 @@ unsigned int ProcessorTrackerFeatureTrifocal::trackFeatures(const FeatureBaseLis
                             capture_incoming_->descriptors_.row(index_kp_incoming),
                             Eigen::Matrix2s::Identity() * params_tracker_feature_trifocal_->pixel_noise_std * params_tracker_feature_trifocal_->pixel_noise_std);
 
-                    _feature_list_out.push_back(incoming_point_ptr);
+                    _features_incoming_out.push_back(incoming_point_ptr);
 
                     _feature_matches[incoming_point_ptr] = std::make_shared<FeatureMatch>(FeatureMatch({feature_last_, capture_incoming_->matches_normalized_scores_.at(index_kp_incoming)}));
 
@@ -249,9 +250,9 @@ unsigned int ProcessorTrackerFeatureTrifocal::trackFeatures(const FeatureBaseLis
 //    WOLF_TRACE("--> TIME: track: ",debug_comp_time_);
 //    WOLF_TRACE("======== ========= =========");
 
-    WOLF_TRACE("TrAckFeatures - Number of features tracked: " , _feature_list_out.size() );
+    WOLF_TRACE("TrAckFeatures - Number of features tracked: " , _features_incoming_out.size() );
 
-    return _feature_list_out.size();
+    return _features_incoming_out.size();
 }
 
 bool ProcessorTrackerFeatureTrifocal::correctFeatureDrift(const FeatureBasePtr _origin_feature, const FeatureBasePtr _last_feature, FeatureBasePtr _incoming_feature)
diff --git a/src/processor/processor_tracker_landmark.cpp b/src/processor/processor_tracker_landmark.cpp
index 1771f97f422798f6b2d42392b1d677d3d9478e23..08bbf8c52b66b01a49fa49389fce11da4affe0b1 100644
--- a/src/processor/processor_tracker_landmark.cpp
+++ b/src/processor/processor_tracker_landmark.cpp
@@ -115,15 +115,6 @@ void ProcessorTrackerLandmark::createNewLandmarks()
 
 unsigned int ProcessorTrackerLandmark::processKnown()
 {
-
-    //std::cout << "ProcessorTrackerLandmark::processKnown:" << std::endl;
-    //std::cout << "\tlast correspondences: " << matches_landmark_from_last_.size() << std::endl;
-    //std::cout << "\tlast features: " << (last_ptr_ == nullptr ? 0 : last_ptr_->getFeatureList().size()) << std::endl;
-    //std::cout << "\tlast new features: " << new_features_last_.size() << std::endl;
-    //std::cout << "\tincoming correspondences: " << matches_landmark_from_incoming_.size() << std::endl;
-    //std::cout << "\tincoming features: " << (incoming_ptr_ == nullptr ? 0 : incoming_ptr_->getFeatureList().size()) << std::endl;
-    //std::cout << "\tincoming new features: " << new_features_incoming_.size() << std::endl;
-
     // Find landmarks in incoming_ptr_
     FeatureBaseList known_features_list_incoming;
     unsigned int n = findLandmarks(getProblem()->getMapPtr()->getLandmarkList(),
@@ -131,22 +122,12 @@ unsigned int ProcessorTrackerLandmark::processKnown()
     // Append found incoming features
     incoming_ptr_->addFeatureList(known_features_list_incoming);
 
-    //std::cout << "end of processKnown:" << std::endl;
-    //std::cout << "\tlast correspondences: " << matches_landmark_from_last_.size() << std::endl;
-    //std::cout << "\tlast features: " << (last_ptr_ == nullptr ? 0 : last_ptr_->getFeatureList().size()) << std::endl;
-    //std::cout << "\tlast new features: " << new_features_last_.size() << std::endl;
-    //std::cout << "\tincoming correspondences: " << matches_landmark_from_incoming_.size() << std::endl;
-    //std::cout << "\tincoming features: " << (incoming_ptr_ == nullptr ? 0 : incoming_ptr_->getFeatureList().size()) << std::endl;
-    //std::cout << "\tincoming new features: " << new_features_incoming_.size() << std::endl;
     return n;
-
 }
 
 void ProcessorTrackerLandmark::establishConstraints()
 {
-
-    //std::cout << "\tfeatures:" << last_ptr_->getFeatureList().size() << std::endl;
-    //std::cout << "\tcorrespondences: " << matches_landmark_from_last_.size() << std::endl;
+    // Loop all features in last_ptr_
     for (auto last_feature : last_ptr_->getFeatureList())
     {
         auto lmk = matches_landmark_from_last_[last_feature]->landmark_ptr_;
@@ -159,6 +140,9 @@ void ProcessorTrackerLandmark::establishConstraints()
             FrameBasePtr frm = ctr_ptr->getFrameOtherPtr();
             if (frm)
                 frm->addConstrainedBy(ctr_ptr);
+            CaptureBasePtr cap = ctr_ptr->getCaptureOtherPtr();
+            if (cap)
+                cap->addConstrainedBy(ctr_ptr);
         }
     }
 }
diff --git a/src/processor/processor_tracker_landmark_corner.cpp b/src/processor/processor_tracker_landmark_corner.cpp
index 706e12b2b48cad9d8a9b316f69ca4ebbc9b8d2e9..522034204bb5dd4f4b3140d3f55db773faa070af 100644
--- a/src/processor/processor_tracker_landmark_corner.cpp
+++ b/src/processor/processor_tracker_landmark_corner.cpp
@@ -375,11 +375,11 @@ LandmarkBasePtr ProcessorTrackerLandmarkCorner::createLandmark(FeatureBasePtr _f
                                               _feature_ptr->getMeasurement()(3));
 }
 
-unsigned int ProcessorTrackerLandmarkCorner::detectNewFeatures(const unsigned int& _max_features, FeatureBaseList& _feature_list_out)
+unsigned int ProcessorTrackerLandmarkCorner::detectNewFeatures(const unsigned int& _max_features, FeatureBaseList& _features_incoming_out)
 {
     // already computed since each scan is computed in preprocess()
-    _feature_list_out = std::move(corners_last_);
-    return _feature_list_out.size();
+    _features_incoming_out = std::move(corners_last_);
+    return _features_incoming_out.size();
 }
 
 ConstraintBasePtr ProcessorTrackerLandmarkCorner::createConstraint(FeatureBasePtr _feature_ptr,
diff --git a/src/processor/processor_tracker_landmark_dummy.cpp b/src/processor/processor_tracker_landmark_dummy.cpp
index f20af380b631d6cd27539941e761876b2f5a1dea..06c10b37f1f8b9607b773c2c808e101c39dfb412 100644
--- a/src/processor/processor_tracker_landmark_dummy.cpp
+++ b/src/processor/processor_tracker_landmark_dummy.cpp
@@ -26,16 +26,16 @@ ProcessorTrackerLandmarkDummy::~ProcessorTrackerLandmarkDummy()
     //
 }
 
-unsigned int ProcessorTrackerLandmarkDummy::findLandmarks(const LandmarkBaseList& _landmark_list_in,
-                                                          FeatureBaseList& _feature_list_out,
+unsigned int ProcessorTrackerLandmarkDummy::findLandmarks(const LandmarkBaseList& _landmarks_in,
+                                                          FeatureBaseList& _features_incoming_out,
                                                           LandmarkMatchMap& _feature_landmark_correspondences)
 {
     std::cout << "\tProcessorTrackerLandmarkDummy::findLandmarks"  << std::endl;
-    std::cout << "\t\t"  << _landmark_list_in.size() << " landmarks..." << std::endl;
+    std::cout << "\t\t"  << _landmarks_in.size() << " landmarks..." << std::endl;
 
     // loosing the track of the first 2 features
     auto landmarks_lost = 0;
-    for (auto landmark_in_ptr : _landmark_list_in)
+    for (auto landmark_in_ptr : _landmarks_in)
     {
         if (landmark_in_ptr->getDescriptor(0) <= landmark_idx_non_visible_)
         {
@@ -44,15 +44,15 @@ unsigned int ProcessorTrackerLandmarkDummy::findLandmarks(const LandmarkBaseList
         }
         else
         {
-            _feature_list_out.push_back(std::make_shared<FeatureBase>(
+            _features_incoming_out.push_back(std::make_shared<FeatureBase>(
                     "POINT IMAGE",
                     landmark_in_ptr->getDescriptor(),
                     Eigen::MatrixXs::Identity(1,1)));
-            _feature_landmark_correspondences[_feature_list_out.back()] = std::make_shared<LandmarkMatch>(landmark_in_ptr, 1);
+            _feature_landmark_correspondences[_features_incoming_out.back()] = std::make_shared<LandmarkMatch>(landmark_in_ptr, 1);
             std::cout << "\t\tlandmark " << landmark_in_ptr->getDescriptor() << " found!" << std::endl;
         }
     }
-    return _feature_list_out.size();
+    return _features_incoming_out.size();
 }
 
 bool ProcessorTrackerLandmarkDummy::voteForKeyFrame()
@@ -63,7 +63,7 @@ bool ProcessorTrackerLandmarkDummy::voteForKeyFrame()
     return incoming_ptr_->getFeatureList().size() < 4;
 }
 
-unsigned int ProcessorTrackerLandmarkDummy::detectNewFeatures(const unsigned int& _max_features, FeatureBaseList& _feature_list_out)
+unsigned int ProcessorTrackerLandmarkDummy::detectNewFeatures(const unsigned int& _max_features, FeatureBaseList& _features_incoming_out)
 {
     std::cout << "\tProcessorTrackerLandmarkDummy::detectNewFeatures" << std::endl;
 
@@ -71,11 +71,11 @@ unsigned int ProcessorTrackerLandmarkDummy::detectNewFeatures(const unsigned int
     for (unsigned int i = 1; i <= _max_features; i++)
     {
         n_feature_++;
-        _feature_list_out.push_back(
+        _features_incoming_out.push_back(
                 std::make_shared<FeatureBase>("POINT IMAGE", n_feature_ * Eigen::Vector1s::Ones(), Eigen::MatrixXs::Ones(1, 1)));
-        std::cout << "\t\tfeature " << _feature_list_out.back()->getMeasurement() << " detected!" << std::endl;
+        std::cout << "\t\tfeature " << _features_incoming_out.back()->getMeasurement() << " detected!" << std::endl;
     }
-    return _feature_list_out.size();
+    return _features_incoming_out.size();
 }
 
 LandmarkBasePtr ProcessorTrackerLandmarkDummy::createLandmark(FeatureBasePtr _feature_ptr)
diff --git a/src/processor/processor_tracker_landmark_image.cpp b/src/processor/processor_tracker_landmark_image.cpp
index 3835cbf60595dc497e98ebd9d68f0b2f577196d0..b055f1353a95302d17f11824744e6c0df482a7db 100644
--- a/src/processor/processor_tracker_landmark_image.cpp
+++ b/src/processor/processor_tracker_landmark_image.cpp
@@ -144,8 +144,8 @@ void ProcessorTrackerLandmarkImage::postProcess()
     feat_lmk_found_.clear();
 }
 
-unsigned int ProcessorTrackerLandmarkImage::findLandmarks(const LandmarkBaseList& _landmark_list_in,
-                                                         FeatureBaseList&  _feature_list_out,
+unsigned int ProcessorTrackerLandmarkImage::findLandmarks(const LandmarkBaseList& _landmarks_in,
+                                                         FeatureBaseList&  _features_incoming_out,
                                                          LandmarkMatchMap& _feature_landmark_correspondences)
 {
     KeyPointVector candidate_keypoints;
@@ -154,7 +154,7 @@ unsigned int ProcessorTrackerLandmarkImage::findLandmarks(const LandmarkBaseList
 
     Eigen::VectorXs current_state = getProblem()->getState(incoming_ptr_->getTimeStamp());
 
-    for (auto landmark_in_ptr : _landmark_list_in)
+    for (auto landmark_in_ptr : _landmarks_in)
     {
 
         // project landmark into incoming capture
@@ -194,7 +194,7 @@ unsigned int ProcessorTrackerLandmarkImage::findLandmarks(const LandmarkBaseList
                     incoming_point_ptr->setScore(normalized_score);
                     incoming_point_ptr->setExpectation(pixel);
 
-                    _feature_list_out.push_back(incoming_point_ptr);
+                    _features_incoming_out.push_back(incoming_point_ptr);
 
                     _feature_landmark_correspondences[incoming_point_ptr] = std::make_shared<LandmarkMatch>(landmark_in_ptr, normalized_score);
 
@@ -214,9 +214,9 @@ unsigned int ProcessorTrackerLandmarkImage::findLandmarks(const LandmarkBaseList
 //        else
 //            std::cout << "NOT in the image" << std::endl;
     }
-//    std::cout << "\tNumber of Features tracked: " << _feature_list_out.size() << std::endl;
-    landmarks_tracked_ = _feature_list_out.size();
-    return _feature_list_out.size();
+//    std::cout << "\tNumber of Features tracked: " << _features_incoming_out.size() << std::endl;
+    landmarks_tracked_ = _features_incoming_out.size();
+    return _features_incoming_out.size();
 }
 
 bool ProcessorTrackerLandmarkImage::voteForKeyFrame()
@@ -225,7 +225,7 @@ bool ProcessorTrackerLandmarkImage::voteForKeyFrame()
 //    return landmarks_tracked_ < params_tracker_landmark_image_->min_features_for_keyframe;
 }
 
-unsigned int ProcessorTrackerLandmarkImage::detectNewFeatures(const unsigned int& _max_features, FeatureBaseList& _feature_list_out)
+unsigned int ProcessorTrackerLandmarkImage::detectNewFeatures(const unsigned int& _max_features, FeatureBaseList& _features_incoming_out)
 {
     cv::Rect roi;
     KeyPointVector new_keypoints;
@@ -260,7 +260,7 @@ unsigned int ProcessorTrackerLandmarkImage::detectNewFeatures(const unsigned int
                     point_ptr->setIsKnown(false);
                     point_ptr->setTrackId(point_ptr->id());
                     point_ptr->setExpectation(Eigen::Vector2s(new_keypoints[0].pt.x,new_keypoints[0].pt.y));
-                    _feature_list_out.push_back(point_ptr);
+                    _features_incoming_out.push_back(point_ptr);
 
                     active_search_ptr_->hitCell(new_keypoints[0]);
 
diff --git a/src/sensor/sensor_base.cpp b/src/sensor/sensor_base.cpp
index 2367c480d373a7bab8541fcb610bee2c08e45aea..a05fec1ceb4b1b90912dd4ec380212848c68f98b 100644
--- a/src/sensor/sensor_base.cpp
+++ b/src/sensor/sensor_base.cpp
@@ -16,7 +16,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),
@@ -43,7 +42,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/sensor_odom_2D.cpp b/src/sensor/sensor_odom_2D.cpp
index ac9e1bf771adbda48bbb41b248ba461dbe8b0560..2790ebb568673acff94b1e246d8f8a0ec35f5de4 100644
--- a/src/sensor/sensor_odom_2D.cpp
+++ b/src/sensor/sensor_odom_2D.cpp
@@ -80,8 +80,6 @@ SensorBasePtr SensorOdom2D::createNew(const std::string& _unique_name, const par
     return odo;
 }
 
-} // namespace wolf
-
 // Register in the SensorFactory
 #include "base/sensor/sensor_factory.h"
 namespace wolf {
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index 8126f71f84c5efe5ca2a171483917e179c9b2ffc..454a7bf205facc449a614f802a72cc00bb6ba09b 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -98,7 +98,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/test/gtest_IMU_tools.cpp b/test/gtest_IMU_tools.cpp
index 83f4000caff84d4d4c5c326a34e24e6ad189adec..e41007154f27c8c6de6845db934215df22d7c382 100644
--- a/test/gtest_IMU_tools.cpp
+++ b/test/gtest_IMU_tools.cpp
@@ -128,7 +128,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);
@@ -138,11 +138,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/test/gtest_SE3.cpp b/test/gtest_SE3.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..d6e3b9b1c2a2154677f4a2a77094b4bac93146f0
--- /dev/null
+++ b/test/gtest_SE3.cpp
@@ -0,0 +1,236 @@
+/**
+ * \file gtest_SE3.cpp
+ *
+ *  Created on: Feb 2, 2019
+ *      \author: jsola
+ */
+
+
+#include "base/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/test/gtest_capture_base.cpp b/test/gtest_capture_base.cpp
index 6faccee27cc478702ac1ac6a3f62e820e5dcba9e..d1ec1884cc79fb858f4b97f8078ba8281c508753 100644
--- a/test/gtest_capture_base.cpp
+++ b/test/gtest_capture_base.cpp
@@ -102,6 +102,15 @@ TEST(CaptureBase, addFeatureList)
     ASSERT_EQ(C->getFeatureList().back(), f_last);
 }
 
+TEST(CaptureBase, process)
+{
+    SensorBasePtr S(std::make_shared<SensorBase>("DUMMY", nullptr, nullptr, nullptr, 2));
+    CaptureBasePtr C(std::make_shared<CaptureBase>("DUMMY", 1.5, nullptr));
+    ASSERT_DEATH({C->process();},""); // No sensor in the capture should fail
+    C->setSensorPtr(S);
+    ASSERT_TRUE(C->process()); // This should not fail (although it does nothing)
+}
+
 int main(int argc, char **argv)
 {
   testing::InitGoogleTest(&argc, argv);
diff --git a/test/gtest_odom_2D.cpp b/test/gtest_odom_2D.cpp
index 65c94f02bc7efd810829187d0e9c2501721a5689..fcf60798e88d353bf18371d2a5c3f455b98f597e 100644
--- a/test/gtest_odom_2D.cpp
+++ b/test/gtest_odom_2D.cpp
@@ -122,7 +122,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/test/gtest_pack_KF_buffer.cpp b/test/gtest_pack_KF_buffer.cpp
index 919727a4fe2c7e6d19db945e5f70e8b09f65b1c3..8f68a5bd532c5a1408d0ba9654ec9f27283c7ba3 100644
--- a/test/gtest_pack_KF_buffer.cpp
+++ b/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/test/gtest_processor_motion.cpp b/test/gtest_processor_motion.cpp
index 573b7b1791459c03c52dd89ffd98d4d44da52413..3e379c9a5dd72300a5c2cfa614d2d5b5933f9542 100644
--- a/test/gtest_processor_motion.cpp
+++ b/test/gtest_processor_motion.cpp
@@ -28,13 +28,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()
@@ -51,18 +55,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);
-        }
-
 };
 
 TEST_F(ProcessorMotion_test, IntegrateStraight)
@@ -90,7 +82,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);
@@ -105,13 +97,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);
diff --git a/test/gtest_solver_manager.cpp b/test/gtest_solver_manager.cpp
index 076e5b82d7d7d69e02809090c7f2d4bc474b6034..9d5fe610b2ef612d2f21d8be00bfafd55e2fbca9 100644
--- a/test/gtest_solver_manager.cpp
+++ b/test/gtest_solver_manager.cpp
@@ -63,6 +63,14 @@ class SolverManagerWrapper : public SolverManager
         virtual void computeCovariances(const CovarianceBlocksToBeComputed blocks){};
         virtual void computeCovariances(const StateBlockList& st_list){};
 
+        // The following are dummy implementations
+        bool    hasConverged()  { return true;      }
+        SizeStd iterations()    { return 1;         }
+        Scalar  initialCost()   { return Scalar(1); }
+        Scalar  finalCost()     { return Scalar(0); }
+
+
+
     protected:
 
         virtual std::string solveImpl(const ReportVerbosity report_level){ return std::string("");};