diff --git a/CMakeLists.txt b/CMakeLists.txt
index 1886fd5119aa1aaba3259fb5fcbfc1832e34bdbc..a90ee5e67b96e6cca9df6bc5db611725df3abd46 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -233,8 +233,11 @@ if(PCL_FOUND)
   SET(SRCS_CAPTURE ${SRCS_CAPTURE}
     src/capture/capture_laser_3d.cpp
     )
+    SET(SRCS_PROCESSOR ${SRCS_PROCESSOR}
+    src/processor/processor_odom_icp_3d.cpp
+  )
   SET(SRCS_PROCESSOR ${SRCS_PROCESSOR}
-      src/processor/processor_odom_icp_3d.cpp
+      src/sensor/sensor_laser_3d.cpp
     )
 endif(PCL_FOUND)
 
diff --git a/include/laser/sensor/sensor_laser_3d.h b/include/laser/sensor/sensor_laser_3d.h
index 9b9ca106bfbe6f220ea1e429ee140807d32528ec..bf077535c64c7c1f2638c8947816dc2c31b78306 100644
--- a/include/laser/sensor/sensor_laser_3d.h
+++ b/include/laser/sensor/sensor_laser_3d.h
@@ -33,7 +33,7 @@ WOLF_STRUCT_PTR_TYPEDEFS(ParamsSensorLaser3d);
 struct ParamsSensorLaser3d : public ParamsSensorBase
 {
     ParamsSensorLaser3d() = default;
-    ParamsSensorLaser3d(std::string _unique_name, const wolf::ParamsServer& _server)
+    ParamsSensorLaser3d(std::string _unique_name, const ParamsServer& _server)
         : ParamsSensorBase(_unique_name, _server)
     {
     }
@@ -46,7 +46,7 @@ class SensorLaser3d : public SensorBase
 {
   public:
     SensorLaser3d(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr);
-    SensorLaser3d(const Eigen::VectorXd& _extrinsics, const std::shared_ptr<ParamsSensorLaser3d> _params);
+    SensorLaser3d(const Eigen::VectorXd& _extrinsics, ParamsSensorLaser3dPtr _params);
     ~SensorLaser3d();
     WOLF_SENSOR_CREATE(SensorLaser3d, ParamsSensorLaser3d, 7);
     ParamsSensorLaser3dPtr params_laser3d_;
diff --git a/src/processor/processor_odom_icp_3d.cpp b/src/processor/processor_odom_icp_3d.cpp
index c9104c9026b20e1f28ccb4f1d408851248e2c255..20dde002124334b5c64cf8241cd3fbdbba92a3e0 100644
--- a/src/processor/processor_odom_icp_3d.cpp
+++ b/src/processor/processor_odom_icp_3d.cpp
@@ -86,11 +86,14 @@ void ProcessorOdomIcp3d::preProcess()
  */
 unsigned int ProcessorOdomIcp3d::processKnown()
 {
-    pairAlign(origin_laser_->getPointCloud(),
-              incoming_laser_->getPointCloud(),
-              T_origin_last_,
-              T_origin_incoming_,
-              registration_solver_);
+    if (origin_ptr_ && (incoming_ptr_ != origin_ptr_))
+    {
+        pairAlign(origin_laser_->getPointCloud(),
+                  incoming_laser_->getPointCloud(),
+                  T_origin_last_,
+                  T_origin_incoming_,
+                  registration_solver_);
+    }
     return 0;
 };
 
@@ -186,4 +189,10 @@ void ProcessorOdomIcp3d::establishFactors()
         feature, feature, origin_laser_->getFrame(), shared_from_this(), false, TOP_MOTION);
 };
 
-}  // namespace wolf
\ No newline at end of file
+}  // namespace wolf
+#include "core/processor/factory_processor.h"
+namespace wolf
+{
+WOLF_REGISTER_PROCESSOR(ProcessorOdomIcp3d);
+WOLF_REGISTER_PROCESSOR_AUTO(ProcessorOdomIcp3d);
+}  // namespace wolf
diff --git a/src/sensor/sensor_laser_3d.cpp b/src/sensor/sensor_laser_3d.cpp
index 1cb46b8b355aa6df5d6b37e9228d10e4228f1fc2..40d0bda87dc37a8efa5a7378a085f3bd788ce5fd 100644
--- a/src/sensor/sensor_laser_3d.cpp
+++ b/src/sensor/sensor_laser_3d.cpp
@@ -21,6 +21,9 @@
 //--------LICENSE_END--------
 #include "laser/sensor/sensor_laser_3d.h"
 
+#include <core/state_block/state_block_derived.h>
+#include <core/state_block/state_quaternion.h>
+
 namespace wolf
 {
 
@@ -29,17 +32,25 @@ SensorLaser3d::SensorLaser3d(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr)
 {
 }
 
-SensorLaser3d::SensorLaser3d(const Eigen::VectorXd& _extrinsics, const std::shared_ptr<ParamsSensorLaser3d> _params);
-SensorBase("SensorLaser3d",
-           std::make_shared<StatePoint3d>(_extrinsics.head(3), true),
-           std::make_shared<StateQuaternion>(_extrinsics.tail(4), true),
-           nullptr,
-           0),
-    params_laser3d_(_params)
+SensorLaser3d::SensorLaser3d(const Eigen::VectorXd& _extrinsics, ParamsSensorLaser3dPtr _params)
+    : SensorBase("SensorLaser3d",
+                 std::make_shared<StatePoint3d>(_extrinsics.head(3), true),
+                 std::make_shared<StateQuaternion>(_extrinsics.tail(4), true),
+                 nullptr,
+                 0),
+      params_laser3d_(_params)
 {
     //
 }
 
 SensorLaser3d::~SensorLaser3d() {}
 
-}  // namespace wolf
\ No newline at end of file
+}  // namespace wolf
+
+// Register in the FactorySensor and the ParameterFactory
+#include "core/sensor/factory_sensor.h"
+namespace wolf {
+WOLF_REGISTER_SENSOR(SensorLaser3d)
+WOLF_REGISTER_SENSOR_AUTO(SensorLaser3d)
+} // namespace wolf
+
diff --git a/test/gtest_processor_odom_icp_3d.cpp b/test/gtest_processor_odom_icp_3d.cpp
index 0cb18a1fb3a270ef879cac22b38f1b1455596018..2a3378532ad8041c9664483496f4f1dee22a8956 100644
--- a/test/gtest_processor_odom_icp_3d.cpp
+++ b/test/gtest_processor_odom_icp_3d.cpp
@@ -19,10 +19,16 @@
 // along with this program.  If not, see <http://www.gnu.org/licenses/>.
 //
 //--------LICENSE_END--------
-// wolf
+
+// wolf laser
+#include "laser/sensor/sensor_laser_3d.h"
 #include "laser/processor/processor_odom_icp_3d.h"
-#include "core/processor/factory_processor.h"
+#include "laser/internal/config.h"
+
+// wolf core
 #include "core/common/wolf.h"
+#include <core/sensor/factory_sensor.h>
+#include "core/processor/factory_processor.h"
 
 // wolf yaml
 #include <core/utils/params_server.h>
@@ -33,10 +39,23 @@
 #include <core/utils/utils_gtest.h>
 #include <core/utils/logging.h>
 
+
+// pcl includes
+#include <pcl/point_cloud.h>
+#include <pcl/io/pcd_io.h>
+#include <pcl/filters/voxel_grid.h>
+
 using namespace wolf;
-using namespace laser;
 
-WOLF_REGISTER_STATEBLOCK_WITH_KEY(L, StateVector3d);
+std::string laser_root_dir = _WOLF_LASER_ROOT_DIR;
+
+void loadData(std::string fname, pcl::PointCloud<pcl::PointXYZ>& cloud)
+{
+    pcl::io::loadPCDFile(fname, cloud);
+    // remove NAN points from the cloud
+    pcl::Indices indices;
+    pcl::removeNaNFromPointCloud(cloud, cloud, indices);
+};
 
 class Test_ProcessorOdomIcp3D_yaml : public testing::Test
 {
@@ -46,22 +65,17 @@ class Test_ProcessorOdomIcp3D_yaml : public testing::Test
     ProcessorOdomIcp3dPtr                p;
     FrameBasePtr                         F0, F1, F;
     CaptureLaser3dPtr                    C0, C1, C2;
-    FeatureBasePtr                       f1, f2, f3;
-    FactorBasePtr                        c1, c2;
-
+    
     VectorXd              data;
     MatrixXd              data_cov;
-    VectorXd              delta;
-    MatrixXd              delta_cov;
-    Matrix<double, 13, 1> calib;
-    MatrixXd              J_delta_calib;
 
   protected:
     void SetUp() override
     {
-        std::string laser_root_dir = _WOLF_LASER_ROOT_DIR;
 
-        ParserYaml   parser    = ParserYaml("processor_odom_icp_3d.yaml", laser_root_dir + "/test/yaml/");
+        WOLF_INFO(laser_root_dir);
+
+        ParserYaml   parser    = ParserYaml("problem_odom_icp_3d.yaml", laser_root_dir + "/test/yaml/");
         ParamsServer server    = ParamsServer(parser.getParams());
         P                      = Problem::autoSetup(server);
 
@@ -112,239 +126,197 @@ class Test_ProcessorOdomIcp3D_yaml : public testing::Test
         */
     }
 };
+    
 
-class Test_ProcessorOdomIcp3D : public testing::Test
+TEST(Init, register_in_factories)
 {
-  public:
-    ProblemPtr                                    P;
-    SensorForceTorqueInertialPtr                  S;
-    ProcessorForceTorqueInertialPreintDynamicsPtr p;
-    FrameBasePtr                                  F0, F1;
-    CaptureMotionPtr                              C0, C1;
-    FeatureMotionPtr                              f1;
-    FactorForceTorqueInertialDynamicsPtr          c1;
+FactorySensor::registerCreator("SensorLaser3d", SensorLaser3d::create);
+AutoConfFactorySensor::registerCreator("SensorLaser3d", SensorLaser3d::create);
+FactoryProcessor::registerCreator("ProcessorOdomIcp3d", ProcessorOdomIcp3d::create);
+AutoConfFactoryProcessor::registerCreator("ProcessorOdomIcp3d", ProcessorOdomIcp3d::create);
 
-    VectorXd              data;
-    MatrixXd              data_cov;
-    VectorXd              delta;
-    MatrixXd              delta_cov;
-    Matrix<double, 13, 1> calib;
-    MatrixXd              J_delta_calib;
+}
 
-  protected:
-    void SetUp() override
-    {
-        data     = VectorXd::Zero(12);  // [ a, w, f, t ]
-        data_cov = MatrixXd::Identity(12, 12);
+TEST_F(Test_ProcessorOdomIcp3D_yaml, constructor)
+{
+    // Load data
+    pcl::PointCloud<pcl::PointXYZ>::Ptr pcl_ref1 = pcl::make_shared<pcl::PointCloud<pcl::PointXYZ>>();
+    loadData(laser_root_dir + "/test/data/1.pcd", *pcl_ref1);
 
-        // crear un problem
-        P = Problem::create("POVL", 3);
-
-        // crear un sensor
-        auto     params_sensor = std::make_shared<ParamsSensorForceTorqueInertial>();
-        Vector7d extrinsics;
-        extrinsics << 0, 0, 0, 0, 0, 0, 1;
-        S = SensorBase::emplace<SensorForceTorqueInertial>(P->getHardware(), extrinsics, params_sensor);
-        S->setStateBlockDynamic('I', true);
-
-        // crear processador
-        auto params_processor = std::make_shared<ParamsProcessorForceTorqueInertialPreintDynamics>();
-        p = ProcessorBase::emplace<ProcessorForceTorqueInertialPreintDynamics>(S, params_processor);
-
-        // crear dos frame
-        VectorXd state(13);
-        state << 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0;
-        VectorComposite state_c(state, "POVL", {3, 4, 3, 3});
-        F0 = FrameBase::emplace<FrameBase>(P->getTrajectory(), 0.0, "POVL", state_c);
-        F1 = FrameBase::emplace<FrameBase>(P->getTrajectory(), 1.0, "POVL", state_c);
-
-        // crear dues capture
-        VectorXd data(VectorXd::Zero(12));
-        MatrixXd data_cov(MatrixXd::Identity(12, 12));
-        Vector6d bias(Vector6d::Zero());
-        auto     sb_bias = std::make_shared<StateParams6>(bias);
-        C0               = CaptureBase::emplace<CaptureMotion>(
-            F0, "CaptureMotion", 0.0, S, data, data_cov, nullptr, nullptr, nullptr, sb_bias);
-        C1 = CaptureBase::emplace<CaptureMotion>(
-            F1, "CaptureMotion", 1.0, S, data, data_cov, C0, nullptr, nullptr, sb_bias);
-
-        // crear un feature
-        VectorXd delta_preint(VectorXd::Zero(19));
-        delta_preint.head<3>()     = -0.5 * gravity();
-        delta_preint.segment<3>(3) = -gravity();
-        delta_preint.segment<3>(6) = -0.5 * gravity();
-        delta_preint.segment<3>(9) = -gravity();
-        delta_preint(18)           = 1;
-        MatrixXd delta_preint_cov(MatrixXd::Identity(18, 18));
-        VectorXd calib_preint(VectorXd::Zero(13));
-        MatrixXd jacobian_calib(MatrixXd::Zero(18, 13));
-        f1 = FeatureBase::emplace<FeatureMotion>(
-            C1, "FeatureMotion", delta_preint, delta_preint_cov, calib_preint, jacobian_calib);
+    C0 = std::make_shared<CaptureLaser3d>(0.0, S, pcl_ref1);
 
-        // crear un factor
-        c1 = FactorBase::emplace<FactorForceTorqueInertialDynamics>(f1, f1, C0, p, false);
-    }
-};
+    C0->process();
 
-// TEST(FactorForceTorqueInertialDynamics_yaml, force_registraion)
-// {
-//     FactorySensor::registerCreator("SensorForceTorqueInertial", SensorForceTorqueInertial::create);
-// }
+    P->print(4,1,1,1);
 
-// Test to see if the constructor works (not yaml files)
-TEST_F(Test_FactorForceTorqueInertialDynamics, constructor)
-{
-    ASSERT_EQ(c1->getCapture(), C1);
-    ASSERT_EQ(c1->getCalibPreint().size(), 13);
-}
+    // Load data
+    pcl::PointCloud<pcl::PointXYZ>::Ptr pcl_ref2 = pcl::make_shared<pcl::PointCloud<pcl::PointXYZ>>();
+    loadData(laser_root_dir + "/test/data/2.pcd", *pcl_ref2);
 
-// Test to see if the constructor works (yaml files)
-TEST_F(Test_FactorForceTorqueInertialDynamics_yaml, constructor)
-{
-    ASSERT_EQ(c1->getCapture(), C1);
-    ASSERT_EQ(c1->getCalibPreint().size(), 13);
-}
+    C1 = std::make_shared<CaptureLaser3d>(1.0, S, pcl_ref2);
 
-// Test en el que no hi ha moviment (not yaml files)
-TEST_F(Test_FactorForceTorqueInertialDynamics, residual)
-{
-    VectorXd              res_exp = VectorXd::Zero(18);
-    Matrix<double, 18, 1> res;
-    VectorXd              bias = VectorXd::Zero(6);
-
-    P->print(4, 1, 1, 1);
-
-    c1->residual(F0->getStateBlock('P')->getState(),                      // p0
-                 Quaterniond(F0->getStateBlock('O')->getState().data()),  // q0
-                 F0->getStateBlock('V')->getState(),                      // v0
-                 F0->getStateBlock('L')->getState(),                      // L0
-                 bias,                                                    // I
-                 F1->getStateBlock('P')->getState(),                      // p1
-                 Quaterniond(F1->getStateBlock('O')->getState().data()),  // q1
-                 F1->getStateBlock('V')->getState(),                      // v1
-                 F1->getStateBlock('L')->getState(),                      // L1
-                 S->getCom(),                                             // C
-                 S->getInertia(),                                         // i
-                 S->getMass(),                                            // m
-                 res);
-
-    ASSERT_MATRIX_APPROX(res_exp, res, 1e-8);
-}
+    C1->process();
 
-// Test en el que no hi ha moviment (yaml files)
-TEST_F(Test_FactorForceTorqueInertialDynamics_yaml, residual)
-{
-    VectorXd              res_exp = VectorXd::Zero(18);
-    Matrix<double, 18, 1> res;
-    VectorXd              bias = VectorXd::Zero(6);
-
-    c1->residual(F0->getStateBlock('P')->getState(),                      // p0
-                 Quaterniond(F0->getStateBlock('O')->getState().data()),  // q0
-                 F0->getStateBlock('V')->getState(),                      // v0
-                 F0->getStateBlock('L')->getState(),                      // L0
-                 bias,                                                    // I
-                 F1->getStateBlock('P')->getState(),                      // p1
-                 Quaterniond(F1->getStateBlock('O')->getState().data()),  // q1
-                 F1->getStateBlock('V')->getState(),                      // v1
-                 F1->getStateBlock('L')->getState(),                      // L1
-                 S->getCom(),                                             // C
-                 S->getInertia(),                                         // i
-                 S->getMass(),                                            // m
-                 res);
-
-    ASSERT_MATRIX_APPROX(res_exp, res, 1e-8);
-}
+    P->print(4,1,1,1);
 
-// Test en el que s'avança 1m en direcció x (not yaml files)
-TEST_F(Test_FactorForceTorqueInertialDynamics, residualx)
-{
-    VectorXd              res_exp = VectorXd::Zero(18);
-    Matrix<double, 18, 1> res;
-    VectorXd              bias = VectorXd::Zero(6);
-
-    // provem el moviment de P=(0,0,0) a P=(1,0,0) i v, o i L queden igual
-    F1->getStateBlock('P')->setState(Vector3d(1, 0, 0));
-
-    // Hem de crear un nou feature i un nou factor ja que la delta preint canvia.
-    VectorXd delta_preint(VectorXd::Zero(19));
-    delta_preint.head<3>()     = -0.5 * gravity();
-    delta_preint(0)            = 1;
-    delta_preint.segment<3>(3) = -gravity();
-    delta_preint.segment<3>(6) = -0.5 * gravity();
-    delta_preint(6)            = 1;
-    delta_preint.segment<3>(9) = -gravity();
-    delta_preint(18)           = 1;
-    MatrixXd         delta_preint_cov(MatrixXd::Identity(18, 18));
-    VectorXd         calib_preint(VectorXd::Zero(13));
-    MatrixXd         jacobian_calib(MatrixXd::Zero(18, 13));
-    FeatureMotionPtr f2 = FeatureBase::emplace<FeatureMotion>(
-        C1, "FeatureMotion", delta_preint, delta_preint_cov, calib_preint, jacobian_calib);
-
-    FactorForceTorqueInertialDynamicsPtr c2 =
-        FactorBase::emplace<FactorForceTorqueInertialDynamics>(f2, f2, C0, p, false);
-
-    c2->residual(F0->getStateBlock('P')->getState(),                      // p0
-                 Quaterniond(F0->getStateBlock('O')->getState().data()),  // q0
-                 F0->getStateBlock('V')->getState(),                      // v0
-                 F0->getStateBlock('L')->getState(),                      // L0
-                 bias,                                                    // I
-                 F1->getStateBlock('P')->getState(),                      // p1
-                 Quaterniond(F1->getStateBlock('O')->getState().data()),  // q1
-                 F1->getStateBlock('V')->getState(),                      // v1
-                 F1->getStateBlock('L')->getState(),                      // L1
-                 S->getCom(),                                             // C
-                 S->getInertia(),                                         // i
-                 S->getMass(),                                            // m
-                 res);
-
-    ASSERT_MATRIX_APPROX(res_exp, res, 1e-8);
+
+    
 }
 
-// Test en el que s'avança 1m en direcció x (fitxers yaml)
+// // Test to see if the constructor works (not yaml files)
+// TEST_F(Test_FactorForceTorqueInertialDynamics, constructor)
+// {
+//     ASSERT_EQ(c1->getCapture(), C1);
+//     ASSERT_EQ(c1->getCalibPreint().size(), 13);
+// }
 
-TEST_F(Test_FactorForceTorqueInertialDynamics_yaml, residualx)
-{
-    VectorXd              res_exp = VectorXd::Zero(18);
-    Matrix<double, 18, 1> res;
-    VectorXd              bias = VectorXd::Zero(6);
-
-    // provem el moviment de P=(0,0,0) a P=(1,0,0) i v, o i L queden igual
-    F1->getStateBlock('P')->setState(Vector3d(1, 0, 0));
-
-    // Hem de crear un nou feature i un nou factor ja que la delta preint canvia.
-    VectorXd delta_preint(VectorXd::Zero(19));
-    delta_preint.head<3>()     = -0.5 * gravity();
-    delta_preint(0)            = 1;
-    delta_preint.segment<3>(3) = -gravity();
-    delta_preint.segment<3>(6) = -0.5 * gravity();
-    delta_preint(6)            = 1;
-    delta_preint.segment<3>(9) = -gravity();
-    delta_preint(18)           = 1;
-    MatrixXd         delta_preint_cov(MatrixXd::Identity(18, 18));
-    VectorXd         calib_preint(VectorXd::Zero(13));
-    MatrixXd         jacobian_calib(MatrixXd::Zero(18, 13));
-    FeatureMotionPtr f2 = FeatureBase::emplace<FeatureMotion>(
-        C1, "FeatureMotion", delta_preint, delta_preint_cov, calib_preint, jacobian_calib);
-
-    FactorForceTorqueInertialDynamicsPtr c2 =
-        FactorBase::emplace<FactorForceTorqueInertialDynamics>(f2, f2, C0, p, false);
-
-    c2->residual(F0->getStateBlock('P')->getState(),                      // p0
-                 Quaterniond(F0->getStateBlock('O')->getState().data()),  // q0
-                 F0->getStateBlock('V')->getState(),                      // v0
-                 F0->getStateBlock('L')->getState(),                      // L0
-                 bias,                                                    // I
-                 F1->getStateBlock('P')->getState(),                      // p1
-                 Quaterniond(F1->getStateBlock('O')->getState().data()),  // q1
-                 F1->getStateBlock('V')->getState(),                      // v1
-                 F1->getStateBlock('L')->getState(),                      // L1
-                 S->getCom(),                                             // C
-                 S->getInertia(),                                         // i
-                 S->getMass(),                                            // m
-                 res);
-
-    ASSERT_MATRIX_APPROX(res_exp, res, 1e-8);
-}
+// // Test to see if the constructor works (yaml files)
+// TEST_F(Test_FactorForceTorqueInertialDynamics_yaml, constructor)
+// {
+//     ASSERT_EQ(c1->getCapture(), C1);
+//     ASSERT_EQ(c1->getCalibPreint().size(), 13);
+// }
+
+// // Test en el que no hi ha moviment (not yaml files)
+// TEST_F(Test_FactorForceTorqueInertialDynamics, residual)
+// {
+//     VectorXd              res_exp = VectorXd::Zero(18);
+//     Matrix<double, 18, 1> res;
+//     VectorXd              bias = VectorXd::Zero(6);
+
+//     P->print(4, 1, 1, 1);
+
+//     c1->residual(F0->getStateBlock('P')->getState(),                      // p0
+//                  Quaterniond(F0->getStateBlock('O')->getState().data()),  // q0
+//                  F0->getStateBlock('V')->getState(),                      // v0
+//                  F0->getStateBlock('L')->getState(),                      // L0
+//                  bias,                                                    // I
+//                  F1->getStateBlock('P')->getState(),                      // p1
+//                  Quaterniond(F1->getStateBlock('O')->getState().data()),  // q1
+//                  F1->getStateBlock('V')->getState(),                      // v1
+//                  F1->getStateBlock('L')->getState(),                      // L1
+//                  S->getCom(),                                             // C
+//                  S->getInertia(),                                         // i
+//                  S->getMass(),                                            // m
+//                  res);
+
+//     ASSERT_MATRIX_APPROX(res_exp, res, 1e-8);
+// }
+
+// // Test en el que no hi ha moviment (yaml files)
+// TEST_F(Test_FactorForceTorqueInertialDynamics_yaml, residual)
+// {
+//     VectorXd              res_exp = VectorXd::Zero(18);
+//     Matrix<double, 18, 1> res;
+//     VectorXd              bias = VectorXd::Zero(6);
+
+//     c1->residual(F0->getStateBlock('P')->getState(),                      // p0
+//                  Quaterniond(F0->getStateBlock('O')->getState().data()),  // q0
+//                  F0->getStateBlock('V')->getState(),                      // v0
+//                  F0->getStateBlock('L')->getState(),                      // L0
+//                  bias,                                                    // I
+//                  F1->getStateBlock('P')->getState(),                      // p1
+//                  Quaterniond(F1->getStateBlock('O')->getState().data()),  // q1
+//                  F1->getStateBlock('V')->getState(),                      // v1
+//                  F1->getStateBlock('L')->getState(),                      // L1
+//                  S->getCom(),                                             // C
+//                  S->getInertia(),                                         // i
+//                  S->getMass(),                                            // m
+//                  res);
+
+//     ASSERT_MATRIX_APPROX(res_exp, res, 1e-8);
+// }
+
+// // Test en el que s'avança 1m en direcció x (not yaml files)
+// TEST_F(Test_FactorForceTorqueInertialDynamics, residualx)
+// {
+//     VectorXd              res_exp = VectorXd::Zero(18);
+//     Matrix<double, 18, 1> res;
+//     VectorXd              bias = VectorXd::Zero(6);
+
+//     // provem el moviment de P=(0,0,0) a P=(1,0,0) i v, o i L queden igual
+//     F1->getStateBlock('P')->setState(Vector3d(1, 0, 0));
+
+//     // Hem de crear un nou feature i un nou factor ja que la delta preint canvia.
+//     VectorXd delta_preint(VectorXd::Zero(19));
+//     delta_preint.head<3>()     = -0.5 * gravity();
+//     delta_preint(0)            = 1;
+//     delta_preint.segment<3>(3) = -gravity();
+//     delta_preint.segment<3>(6) = -0.5 * gravity();
+//     delta_preint(6)            = 1;
+//     delta_preint.segment<3>(9) = -gravity();
+//     delta_preint(18)           = 1;
+//     MatrixXd         delta_preint_cov(MatrixXd::Identity(18, 18));
+//     VectorXd         calib_preint(VectorXd::Zero(13));
+//     MatrixXd         jacobian_calib(MatrixXd::Zero(18, 13));
+//     FeatureMotionPtr f2 = FeatureBase::emplace<FeatureMotion>(
+//         C1, "FeatureMotion", delta_preint, delta_preint_cov, calib_preint, jacobian_calib);
+
+//     FactorForceTorqueInertialDynamicsPtr c2 =
+//         FactorBase::emplace<FactorForceTorqueInertialDynamics>(f2, f2, C0, p, false);
+
+//     c2->residual(F0->getStateBlock('P')->getState(),                      // p0
+//                  Quaterniond(F0->getStateBlock('O')->getState().data()),  // q0
+//                  F0->getStateBlock('V')->getState(),                      // v0
+//                  F0->getStateBlock('L')->getState(),                      // L0
+//                  bias,                                                    // I
+//                  F1->getStateBlock('P')->getState(),                      // p1
+//                  Quaterniond(F1->getStateBlock('O')->getState().data()),  // q1
+//                  F1->getStateBlock('V')->getState(),                      // v1
+//                  F1->getStateBlock('L')->getState(),                      // L1
+//                  S->getCom(),                                             // C
+//                  S->getInertia(),                                         // i
+//                  S->getMass(),                                            // m
+//                  res);
+
+//     ASSERT_MATRIX_APPROX(res_exp, res, 1e-8);
+// }
+
+// // Test en el que s'avança 1m en direcció x (fitxers yaml)
+
+// TEST_F(Test_FactorForceTorqueInertialDynamics_yaml, residualx)
+// {
+//     VectorXd              res_exp = VectorXd::Zero(18);
+//     Matrix<double, 18, 1> res;
+//     VectorXd              bias = VectorXd::Zero(6);
+
+//     // provem el moviment de P=(0,0,0) a P=(1,0,0) i v, o i L queden igual
+//     F1->getStateBlock('P')->setState(Vector3d(1, 0, 0));
+
+//     // Hem de crear un nou feature i un nou factor ja que la delta preint canvia.
+//     VectorXd delta_preint(VectorXd::Zero(19));
+//     delta_preint.head<3>()     = -0.5 * gravity();
+//     delta_preint(0)            = 1;
+//     delta_preint.segment<3>(3) = -gravity();
+//     delta_preint.segment<3>(6) = -0.5 * gravity();
+//     delta_preint(6)            = 1;
+//     delta_preint.segment<3>(9) = -gravity();
+//     delta_preint(18)           = 1;
+//     MatrixXd         delta_preint_cov(MatrixXd::Identity(18, 18));
+//     VectorXd         calib_preint(VectorXd::Zero(13));
+//     MatrixXd         jacobian_calib(MatrixXd::Zero(18, 13));
+//     FeatureMotionPtr f2 = FeatureBase::emplace<FeatureMotion>(
+//         C1, "FeatureMotion", delta_preint, delta_preint_cov, calib_preint, jacobian_calib);
+
+//     FactorForceTorqueInertialDynamicsPtr c2 =
+//         FactorBase::emplace<FactorForceTorqueInertialDynamics>(f2, f2, C0, p, false);
+
+//     c2->residual(F0->getStateBlock('P')->getState(),                      // p0
+//                  Quaterniond(F0->getStateBlock('O')->getState().data()),  // q0
+//                  F0->getStateBlock('V')->getState(),                      // v0
+//                  F0->getStateBlock('L')->getState(),                      // L0
+//                  bias,                                                    // I
+//                  F1->getStateBlock('P')->getState(),                      // p1
+//                  Quaterniond(F1->getStateBlock('O')->getState().data()),  // q1
+//                  F1->getStateBlock('V')->getState(),                      // v1
+//                  F1->getStateBlock('L')->getState(),                      // L1
+//                  S->getCom(),                                             // C
+//                  S->getInertia(),                                         // i
+//                  S->getMass(),                                            // m
+//                  res);
+
+//     ASSERT_MATRIX_APPROX(res_exp, res, 1e-8);
+// }
 
 int main(int argc, char **argv)
 {
diff --git a/test/yaml/processor_odom_icp_3d.yaml b/test/yaml/problem_odom_icp_3d.yaml
similarity index 66%
rename from test/yaml/processor_odom_icp_3d.yaml
rename to test/yaml/problem_odom_icp_3d.yaml
index 82386dd0f0291e5a604a24bb9d5a0902a880b16c..835d2077f19bdb6af2b96f9e85ff6f3903eac19a 100644
--- a/test/yaml/processor_odom_icp_3d.yaml
+++ b/test/yaml/problem_odom_icp_3d.yaml
@@ -7,6 +7,9 @@ config:
       $state:
         P: [0,0,0]
         O: [0,0,0,1]
+      $sigma:
+        P: [.1,.1,.1]
+        O: [.1,.1,.1]
       time_tolerance: 0.05
     tree_manager: 
       type: "None"
@@ -31,4 +34,15 @@ config:
     icp_max_iterations:  20
     icp_transformation_rotation_epsilon:  0.99
     icp_transformation_translation_epsilon: 1e-6
-    icp_max_correspondence_distance:  0.5
\ No newline at end of file
+    icp_max_correspondence_distance:  0.5
+    keyframe_vote:
+      voting_active: false
+      min_features_for_keyframe : 0
+    apply_loss_function: false
+    max_new_features: 0
+    state_getter: true
+    state_priority: 1
+    pcl_downsample: true
+    vote_elapsed_time: 0.99
+
+