diff --git a/CMakeLists.txt b/CMakeLists.txt
index f688e52678b509edbbd9b6299da740fd86981ba6..b65f0c839ff9a61338f4a5fd267cbf32fb28b116 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -273,6 +273,9 @@ SET(HDRS_UTILS
   include/base/utils/logging.h
   include/base/utils/singleton.h
   include/base/utils/make_unique.h
+  include/base/utils/params_server.hpp
+  include/base/utils/converter.h
+  include/base/utils/loader.hpp
   )
 SET(HDRS_PROBLEM
   include/base/problem/problem.h
@@ -429,15 +432,15 @@ SET(HDRS_DTASSC
   include/base/association/association_nnls.h
   include/base/association/matrix.h
   )
+SET(HDRS_YAML
+  include/base/yaml/parser_yaml.hpp
+  )
 
 
 #SOURCES
 SET(SRCS_PROBLEM
   src/problem/problem.cpp
   )
-SET(HDRS_YAML
-  include/base/yaml/parser_yaml.hpp
-  )
 SET(SRCS_HARDWARE
   src/hardware/hardware_base.cpp
   )
@@ -480,6 +483,7 @@ SET(SRCS_CAPTURE
   src/capture/capture_odom_3D.cpp
   src/capture/capture_velocity.cpp
   src/capture/capture_wheel_joint_position.cpp
+  src/capture/capture_image.cpp
   )
 SET(SRCS_FACTOR
   src/factor/factor_analytic.cpp
@@ -495,6 +499,7 @@ SET(SRCS_FEATURE
   src/feature/feature_odom_2D.cpp
   src/feature/feature_polyline_2D.cpp
   src/feature/feature_pose.cpp
+  src/feature/feature_point_image.cpp
   )
 SET(SRCS_LANDMARK
   src/landmark/landmark_corner_2D.cpp
@@ -614,10 +619,6 @@ IF (vision_utils_FOUND)
     include/base/landmark/landmark_point_3D.h
     include/base/landmark/landmark_AHP.h
     )
-  SET(SRCS ${SRCS}
-    src/capture/capture_image.cpp
-    src/feature/feature_point_image.cpp
-    )
   SET(SRCS_LANDMARK ${SRCS_LANDMARK}
     src/landmark/landmark_point_3D.cpp
     src/landmark/landmark_AHP.cpp
@@ -655,7 +656,7 @@ IF(YAMLCPP_FOUND)
     src/yaml/sensor_IMU_yaml.cpp
     )
   IF(laser_scan_utils_FOUND)
-    SET(SRCS ${SRCS_YAML}
+    SET(SRCS_YAML ${SRCS_YAML}
       src/yaml/sensor_laser_2D_yaml.cpp
       )
   ENDIF(laser_scan_utils_FOUND)
@@ -670,6 +671,7 @@ ENDIF(YAMLCPP_FOUND)
 # create the shared library
 ADD_LIBRARY(${PROJECT_NAME} 
   SHARED 
+  ${SRCS}
   ${SRCS_BASE} 
   ${SRCS_CAPTURE}
   ${SRCS_COMMON}
diff --git a/hello_plugin/hello_plugin.cpp b/hello_plugin/hello_plugin.cpp
index fbe3955dc557f20626e8f316d28d7d9aff638cbd..e02c232a82298adab7fef850e3bda956903b89d2 100644
--- a/hello_plugin/hello_plugin.cpp
+++ b/hello_plugin/hello_plugin.cpp
@@ -5,17 +5,17 @@
  *      Author: jcasals
  */
 #include "base/sensor/sensor_base.h"
-#include "base/wolf.h"
+#include "base/common/wolf.h"
 // #include "sensor_odom_2D.cpp"
 #include <yaml-cpp/yaml.h>
 #include "base/yaml/parser_yaml.hpp"
-#include "base/params_server.hpp"
+#include "base/utils/params_server.hpp"
 
 #include "../hello_wolf/capture_range_bearing.h"
 #include "../hello_wolf/feature_range_bearing.h"
 #include "../hello_wolf/factor_range_bearing.h"
 #include "../hello_wolf/landmark_point_2D.h"
-#include "base/loader.hpp"
+#include "base/utils/loader.hpp"
 #include "base/processor/processor_odom_2D.h"
 
 #include "base/solver/solver_factory.h"
@@ -162,11 +162,17 @@ int main(int argc, char** argv) {
     // GET COVARIANCES of all states
     WOLF_TRACE("======== COVARIANCES OF SOLVED PROBLEM =======")
     ceres->computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS);
-    for (auto kf : problem->getTrajectory()->getFrameList())
+    for (auto kf : problem->getTrajectory()->getFrameList()){
         if (kf->isKey())
-            WOLF_TRACE("KF", kf->id(), "_cov = \n", kf->getCovariance());
-    for (auto lmk : problem->getMap()->getLandmarkList())
-        WOLF_TRACE("L", lmk->id(), "_cov = \n", lmk->getCovariance());
+            {
+                Eigen::MatrixXs cov;
+                WOLF_TRACE("KF", kf->id(), "_cov = \n", kf->getCovariance(cov));
+            }
+        for (auto lmk : problem->getMap()->getLandmarkList()) {
+            Eigen::MatrixXs cov;
+            WOLF_TRACE("L", lmk->id(), "_cov = \n", lmk->getCovariance(cov));
+        }
+    }
     std::cout << std::endl;
 
     WOLF_TRACE("======== FINAL PRINT FOR INTERPRETATION =======")
diff --git a/hello_plugin/params_autoconf.cpp b/hello_plugin/params_autoconf.cpp
index 70f562ffc4b9a255086fc9af3a034e11b6a2bf50..189bc777fb28040e66c1622ab2fa33ed52ee8a0a 100644
--- a/hello_plugin/params_autoconf.cpp
+++ b/hello_plugin/params_autoconf.cpp
@@ -5,17 +5,17 @@
  *      Author: jcasals
  */
 #include "base/sensor/sensor_base.h"
-#include "base/wolf.h"
+#include "base/common/wolf.h"
 // #include "sensor_odom_2D.cpp"
 #include <yaml-cpp/yaml.h>
 #include "base/yaml/parser_yaml.hpp"
-#include "base/params_server.hpp"
+#include "base/utils/params_server.hpp"
 
 #include "../hello_wolf/capture_range_bearing.h"
 #include "../hello_wolf/feature_range_bearing.h"
 #include "../hello_wolf/factor_range_bearing.h"
 #include "../hello_wolf/landmark_point_2D.h"
-#include "base/loader.hpp"
+#include "base/utils/loader.hpp"
 #include "base/processor/processor_odom_2D.h"
 
 #include "base/solver/solver_factory.h"
diff --git a/hello_wolf/sensor_range_bearing.h b/hello_wolf/sensor_range_bearing.h
index 29adf8ae1e654c02b481271360833b0e587e95f2..5eaebc6b30d379e64c698bc19d89cbe6e9c20f3c 100644
--- a/hello_wolf/sensor_range_bearing.h
+++ b/hello_wolf/sensor_range_bearing.h
@@ -9,7 +9,7 @@
 #define HELLO_WOLF_SENSOR_RANGE_BEARING_H_
 
 #include "base/sensor/sensor_base.h"
-#include "base/params_server.hpp"
+#include "base/utils/params_server.hpp"
 
 namespace wolf
 {
diff --git a/include/base/common/wolf.h b/include/base/common/wolf.h
index c0a9a9a2d0b7ba3342f13692b5a0beb06e30e620..3d5ea9efc34f855250fd83cebf738a889f9884d7 100644
--- a/include/base/common/wolf.h
+++ b/include/base/common/wolf.h
@@ -10,7 +10,7 @@
 
 // Enable project-specific definitions and macros
 #include "internal/config.h"
-#include "base/params_server.hpp"
+#include "base/utils/params_server.hpp"
 #include "base/utils/logging.h"
 
 //includes from Eigen lib
diff --git a/include/base/problem/problem.h b/include/base/problem/problem.h
index d79f730cd190ae3b5137cf586873aa4c623d4485..156077ec8048fe77e9ba10567969aa64a57ac82a 100644
--- a/include/base/problem/problem.h
+++ b/include/base/problem/problem.h
@@ -14,7 +14,7 @@ struct ProcessorParamsBase;
 }
 
 //wolf includes
-#include "base/params_server.hpp"
+#include "base/utils/params_server.hpp"
 #include "base/sensor/new_sensor_factory.h"
 #include "base/processor/new_processor_factory.h"
 #include "base/common/wolf.h"
diff --git a/include/base/processor/processor_base.h b/include/base/processor/processor_base.h
index aa6aaf7ac5b0bbfb8e0388cf3a2bcdcdf590c79b..a188d81780a48ddb1d3681b79152c14840b76ad5 100644
--- a/include/base/processor/processor_base.h
+++ b/include/base/processor/processor_base.h
@@ -7,7 +7,7 @@ class SensorBase;
 }
 
 // Wolf includes
-#include "base/params_server.hpp"
+#include "base/utils/params_server.hpp"
 #include "base/common/wolf.h"
 #include "base/common/node_base.h"
 #include "base/common/time_stamp.h"
diff --git a/include/base/processor/processor_diff_drive.h b/include/base/processor/processor_diff_drive.h
index a486c721865fc21ed05135ccfb8d1717cfa738a9..87fc7f6bcf6da05531dd26185fe69edd042900e3 100644
--- a/include/base/processor/processor_diff_drive.h
+++ b/include/base/processor/processor_diff_drive.h
@@ -9,7 +9,7 @@
 #define _WOLF_PROCESSOR_DIFF_DRIVE_H_
 
 #include "base/processor/processor_motion.h"
-#include "base/params_server.hpp"
+#include "base/utils/params_server.hpp"
 #include "base/processor/diff_drive_tools.h"
 
 namespace wolf {
diff --git a/include/base/processor/processor_frame_nearest_neighbor_filter.h b/include/base/processor/processor_frame_nearest_neighbor_filter.h
index 619b88b7990cf365fa2616546be2d88821bac528..219d038642eb6f9f66899605b55f1271a0a37d94 100644
--- a/include/base/processor/processor_frame_nearest_neighbor_filter.h
+++ b/include/base/processor/processor_frame_nearest_neighbor_filter.h
@@ -3,7 +3,7 @@
 
 // Wolf related headers
 #include "base/processor/processor_loopclosure_base.h"
-#include "base/params_server.hpp"
+#include "base/utils/params_server.hpp"
 #include "base/state_block/state_block.h"
 
 namespace wolf{
diff --git a/include/base/processor/processor_motion.h b/include/base/processor/processor_motion.h
index 805ef9261d3b6b76c9df1afe1a2631ba64162b5d..7faf2ecc0dbf67f2fe5ce415b1f71057dc718dca 100644
--- a/include/base/processor/processor_motion.h
+++ b/include/base/processor/processor_motion.h
@@ -11,7 +11,7 @@
 // Wolf
 #include "base/capture/capture_motion.h"
 #include "base/processor/processor_base.h"
-#include "base/params_server.hpp"
+#include "base/utils/params_server.hpp"
 #include "base/common/time_stamp.h"
 
 // std
diff --git a/include/base/processor/processor_odom_2D.h b/include/base/processor/processor_odom_2D.h
index 5f4718c233f3ae9db651b661a9f146f87ba67116..5e0eff3a58c7966d34fa582a774d8df83443b46a 100644
--- a/include/base/processor/processor_odom_2D.h
+++ b/include/base/processor/processor_odom_2D.h
@@ -11,7 +11,7 @@
 #include "base/processor/processor_motion.h"
 #include "base/capture/capture_odom_2D.h"
 #include "base/factor/factor_odom_2D.h"
-#include "base/params_server.hpp"
+#include "base/utils/params_server.hpp"
 #include "base/math/rotations.h"
 
 namespace wolf {
diff --git a/include/base/processor/processor_params_image.h b/include/base/processor/processor_params_image.h
index 33e92c38e204aa62871eb7c8f540e30a89937fd6..f493db6ec9138cb510171e1986c439dc4a484ede 100644
--- a/include/base/processor/processor_params_image.h
+++ b/include/base/processor/processor_params_image.h
@@ -4,7 +4,7 @@
 // wolf
 #include "base/processor/processor_tracker_feature.h"
 #include "base/processor/processor_tracker_landmark.h"
-#include "base/params_server.hpp"
+#include "base/utils/params_server.hpp"
 
 namespace wolf
 {
diff --git a/include/base/processor/processor_tracker.h b/include/base/processor/processor_tracker.h
index ea414cfffc484b73e1025f8e32aa520e4a7ffeb0..b583db352df4dbfb6460a161a93a4d95bcef4ff7 100644
--- a/include/base/processor/processor_tracker.h
+++ b/include/base/processor/processor_tracker.h
@@ -18,13 +18,14 @@ WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsTracker);
 struct ProcessorParamsTracker : public ProcessorParamsBase
 {
     unsigned int min_features_for_keyframe; ///< minimum nbr. of features to vote for keyframe
+    int max_new_features;                   ///< maximum nbr. of new features to be processed when adding a keyframe (-1: unlimited. 0: none.)
+
     ProcessorParamsTracker() = default;
     ProcessorParamsTracker(std::string _unique_name, const wolf::paramsServer & _server):
         ProcessorParamsBase(_unique_name, _server)
     {
         min_features_for_keyframe = _server.getParam<unsigned int>(_unique_name + "/min_features_for_keyframe", "1");
-        max_new_features = _server.getParam<unsigned int>(_unique_name + "/max_new_features", "10");
-    }
+        max_new_features = _server.getParam<int>(_unique_name + "/max_new_features", "-1");
     }
 };
 
diff --git a/include/base/processor/processor_tracker_feature_corner.h b/include/base/processor/processor_tracker_feature_corner.h
index 5ab8b2027d35a29bce529103a9ecf522a4d527f0..a5b7e107f4283f5c36207cccb550ba4dc7d163fc 100644
--- a/include/base/processor/processor_tracker_feature_corner.h
+++ b/include/base/processor/processor_tracker_feature_corner.h
@@ -17,7 +17,7 @@
 #include "base/state_block/state_block.h"
 #include "base/association/association_tree.h"
 #include "base/processor/processor_tracker_feature.h"
-#include "base/params_server.hpp"
+#include "base/utils/params_server.hpp"
 
 //laser_scan_utils
 //#include "laser_scan_utils/scan_basics.h"
diff --git a/include/base/processor/processor_tracker_feature_trifocal.h b/include/base/processor/processor_tracker_feature_trifocal.h
index a017004cdfa9634a4b36b31926002134a3931160..7a71a038fa48af53e1f9a10d54dc1651f1f572c3 100644
--- a/include/base/processor/processor_tracker_feature_trifocal.h
+++ b/include/base/processor/processor_tracker_feature_trifocal.h
@@ -4,7 +4,7 @@
 //Wolf includes
 #include "base/processor/processor_tracker_feature.h"
 #include "base/capture/capture_image.h"
-#include "base/params_server.hpp"
+#include "base/utils/params_server.hpp"
 
 // Vision utils
 #include <vision_utils.h>
diff --git a/include/base/sensor/sensor_GPS_fix.h b/include/base/sensor/sensor_GPS_fix.h
index dd2e8570054406203a23139a67aeba521c0098ec..88b2e54d8c6c654ca6e043d61543050723a1192d 100644
--- a/include/base/sensor/sensor_GPS_fix.h
+++ b/include/base/sensor/sensor_GPS_fix.h
@@ -4,7 +4,7 @@
 
 //wolf includes
 #include "base/sensor/sensor_base.h"
-#include "base/params_server.hpp"
+#include "base/utils/params_server.hpp"
 
 // std includes
 
diff --git a/include/base/sensor/sensor_IMU.h b/include/base/sensor/sensor_IMU.h
index 4ae415566560ccb9ee3263890ee11d7dcc375833..c9dc9ad4b308b93c6bf8de2f25f5460fce94e536 100644
--- a/include/base/sensor/sensor_IMU.h
+++ b/include/base/sensor/sensor_IMU.h
@@ -3,7 +3,7 @@
 
 //wolf includes
 #include "base/sensor/sensor_base.h"
-#include "base/params_server.hpp"
+#include "base/utils/params_server.hpp"
 #include <iostream>
 
 namespace wolf {
diff --git a/include/base/sensor/sensor_camera.h b/include/base/sensor/sensor_camera.h
index 44bd1dd0b55361942e5a8ab06c34b18495b5dceb..04d2bd8aa585ea300ecb0472033fad907bf7dd97 100644
--- a/include/base/sensor/sensor_camera.h
+++ b/include/base/sensor/sensor_camera.h
@@ -3,7 +3,7 @@
 
 //wolf includes
 #include "base/sensor/sensor_base.h"
-#include "base/params_server.hpp"
+#include "base/utils/params_server.hpp"
 
 namespace wolf
 {
diff --git a/include/base/sensor/sensor_diff_drive.h b/include/base/sensor/sensor_diff_drive.h
index fd0a801105af5b0fe7f4707fa1088b63ccc38efd..e84be0627ac7d04fe6cfec463cd9ebe5edd77504 100644
--- a/include/base/sensor/sensor_diff_drive.h
+++ b/include/base/sensor/sensor_diff_drive.h
@@ -10,7 +10,7 @@
 
 //wolf includes
 #include "base/sensor/sensor_base.h"
-#include "base/params_server.hpp"
+#include "base/utils/params_server.hpp"
 #include "base/processor/diff_drive_tools.h"
 
 namespace wolf {
diff --git a/include/base/sensor/sensor_odom_2D.h b/include/base/sensor/sensor_odom_2D.h
index 934ef26150ced15aac4a8d18aa0cba24b755b483..92b6bb111cb35b033b27ebe3d0ca2d220f573137 100644
--- a/include/base/sensor/sensor_odom_2D.h
+++ b/include/base/sensor/sensor_odom_2D.h
@@ -3,7 +3,7 @@
 
 //wolf includes
 #include "base/sensor/sensor_base.h"
-#include "base/params_server.hpp"
+#include "base/utils/params_server.hpp"
 
 namespace wolf {
 
diff --git a/include/base/sensor/sensor_odom_3D.h b/include/base/sensor/sensor_odom_3D.h
index c1128098e4259026bd9dcb61f8608b21f6a45585..dfa736fe93853a31f5fe30ee7c8e21329a1c2ec1 100644
--- a/include/base/sensor/sensor_odom_3D.h
+++ b/include/base/sensor/sensor_odom_3D.h
@@ -10,7 +10,7 @@
 
 //wolf includes
 #include "base/sensor/sensor_base.h"
-#include "base/params_server.hpp"
+#include "base/utils/params_server.hpp"
 
 namespace wolf {
 
diff --git a/include/base/solver/solver_factory.h b/include/base/solver/solver_factory.h
index 5a0ce6417ab1c90ecb3fd801234785a3e7db8ecf..8d37673fecf9cb61b87739c8bdfcb8d1a26238da 100644
--- a/include/base/solver/solver_factory.h
+++ b/include/base/solver/solver_factory.h
@@ -15,7 +15,7 @@ struct IntrinsicsBase;
 }
 
 // wolf
-#include "base/factory.h"
+#include "base/common/factory.h"
 #include "base/solver/solver_manager.h"
 
 namespace wolf
diff --git a/include/base/three_D_tools.h b/include/base/three_D_tools.h
deleted file mode 100644
index 86e2086f6a5745a2ae8adce7339e5104a5321f64..0000000000000000000000000000000000000000
--- a/include/base/three_D_tools.h
+++ /dev/null
@@ -1,375 +0,0 @@
-/*
- * three_D_tools.h
- *
- *  Created on: Mar 15, 2018
- *      Author: jsola
- */
-
-#ifndef THREE_D_TOOLS_H_
-#define THREE_D_TOOLS_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
- *   - lift:        go from Delta manifold to tangent space (equivalent to log() in rotations)
- *   - retract:     go from tangent space to delta manifold (equivalent to exp() in rotations)
- *   - plus:        D2 = D1 (+) retract(d)
- *   - diff:        d  = lift( D2 (-) D1 )
- */
-
-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> lift(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) );
-
-    dp_ret = dp_in;
-    do_ret = log_q(dq_in);
-
-    return ret;
-}
-
-template<typename Derived>
-Matrix<typename Derived::Scalar, 7, 1> retract(const MatrixBase<Derived>& d_in)
-{
-    MatrixSizeCheck<6, 1>::check(d_in);
-
-    Matrix<typename Derived::Scalar, 7, 1> ret;
-
-    Map<const Matrix<typename Derived::Scalar, 3, 1> >   dp_in  ( & d_in(0) );
-    Map<const Matrix<typename Derived::Scalar, 3, 1> >   do_in  ( & d_in(3) );
-    Map<Matrix<typename Derived::Scalar, 3, 1> >         dp     ( &  ret(0) );
-    Map<Quaternion<typename Derived::Scalar> >           dq     ( &  ret(3) );
-
-    dp = 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 diff(const MatrixBase<D1>& dp1, const QuaternionBase<D2>& dq1,
-                 const MatrixBase<D4>& dp2, const QuaternionBase<D5>& dq2,
-                 MatrixBase<D7>& diff_p, MatrixBase<D8>& diff_o )
-{
-        diff_p = dp2 - dp1;
-        diff_o = log_q(dq1.conjugate() * dq2);
-}
-
-template<typename D1, typename D2, typename D4, typename D5, typename D6, typename D7, typename D8, typename D9>
-inline void diff(const MatrixBase<D1>& dp1, const QuaternionBase<D2>& dq1,
-                 const MatrixBase<D4>& dp2, const QuaternionBase<D5>& dq2,
-                 MatrixBase<D6>& diff_p, MatrixBase<D7>& diff_o,
-                 MatrixBase<D8>& J_do_dq1, MatrixBase<D9>& J_do_dq2)
-{
-    diff(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 diff(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) );
-
-    diff(dp1, dq1, dp2, dq2, diff_p, diff_o);
-}
-
-template<typename D1, typename D2, typename D3, typename D4, typename D5>
-inline void diff(const MatrixBase<D1>& d1,
-                 const MatrixBase<D2>& d2,
-                 MatrixBase<D3>& dif,
-                 MatrixBase<D4>& J_diff_d1,
-                 MatrixBase<D5>& J_diff_d2)
-{
-    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;
-
-    diff(dp1, dq1, dv1, dp2, dq2, dv2, diff_p, diff_o, diff_v, J_do_dq1, J_do_dq2);
-
-    /* d = diff(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> diff(const MatrixBase<D1>& d1,
-                                              const MatrixBase<D2>& d2)
-{
-    Matrix<typename D1::Scalar, 6, 1> ret;
-    diff(d1, d2, ret);
-    return ret;
-}
-
-} // namespace three_d
-} // namespace wolf
-
-#endif /* THREE_D_TOOLS_H_ */
diff --git a/include/base/converter.h b/include/base/utils/converter.h
similarity index 100%
rename from include/base/converter.h
rename to include/base/utils/converter.h
diff --git a/include/base/loader.hpp b/include/base/utils/loader.hpp
similarity index 100%
rename from include/base/loader.hpp
rename to include/base/utils/loader.hpp
diff --git a/include/base/params_server.hpp b/include/base/utils/params_server.hpp
similarity index 99%
rename from include/base/params_server.hpp
rename to include/base/utils/params_server.hpp
index 55f268d78a5e573eb1eb48d187cdc7690e9805b1..186bd82869060f421733ccd4bdcd11479e389366 100644
--- a/include/base/params_server.hpp
+++ b/include/base/utils/params_server.hpp
@@ -3,7 +3,7 @@
 #include <vector>
 #include <regex>
 #include <map>
-#include "base/converter.h"
+#include "base/utils/converter.h"
 namespace wolf{
 class paramsServer{
     struct ParamsInitSensor{
diff --git a/test/gtest_converter.cpp b/test/gtest_converter.cpp
index 6f821cba2fa0faa6241ab180c8f846f75699239f..e594458e9d23ba85a7b0c008e700045c240b8ccd 100644
--- a/test/gtest_converter.cpp
+++ b/test/gtest_converter.cpp
@@ -1,5 +1,5 @@
 #include "utils_gtest.h"
-#include "base/converter.h"
+#include "base/utils/converter.h"
 
 using namespace std;
 using namespace wolf;
diff --git a/test/gtest_param_server.cpp b/test/gtest_param_server.cpp
index a17791973925eb6a9adfccb9ab7548ad3f85cffa..9b80933a1daa7eb1a1a0c6bb4f3f3059fdc0e75f 100644
--- a/test/gtest_param_server.cpp
+++ b/test/gtest_param_server.cpp
@@ -1,8 +1,8 @@
 #include "utils_gtest.h"
-#include "base/converter.h"
-#include "base/wolf.h"
+#include "base/utils/converter.h"
+#include "base/common/wolf.h"
 #include "base/yaml/parser_yaml.hpp"
-#include "base/params_server.hpp"
+#include "base/utils/params_server.hpp"
 
 using namespace std;
 using namespace wolf;
diff --git a/test/gtest_parser_yaml.cpp b/test/gtest_parser_yaml.cpp
index 437879ad284aac2745467723b913b3de51096da9..006bd5729952e976e66089a6148cc6feb960b00a 100644
--- a/test/gtest_parser_yaml.cpp
+++ b/test/gtest_parser_yaml.cpp
@@ -1,6 +1,6 @@
 #include "utils_gtest.h"
-#include "base/converter.h"
-#include "base/wolf.h"
+#include "base/utils/converter.h"
+#include "base/common/wolf.h"
 #include "base/yaml/parser_yaml.hpp"
 
 using namespace std;