diff --git a/test/gtest_processor_odom_icp_3d.cpp b/test/gtest_processor_odom_icp_3d.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..0703ed18b8a2f88e1aab438c37424cf05cee5df8
--- /dev/null
+++ b/test/gtest_processor_odom_icp_3d.cpp
@@ -0,0 +1,327 @@
+// wolf
+#include "laser/processor/processor_odom_icp_3d.h"
+#include "core/processor/factory_processor.h"
+
+// wolf yaml
+#include <core/utils/params_server.h>
+#include <core/yaml/parser_yaml.h>
+#include "core/yaml/yaml_conversion.h"
+
+// testing
+#include <core/utils/utils_gtest.h>
+#include <core/utils/logging.h>
+
+using namespace wolf;
+using namespace laser;
+
+WOLF_REGISTER_STATEBLOCK_WITH_KEY(L, StateVector3d);
+
+class Test_FactorForceTorqueInertialDynamics_yaml : public testing::Test
+{
+ public:
+ ProblemPtr P;
+ SensorLaser3dPtr S;
+ ProcessorOdomIcp3dPtr p;
+ FrameBasePtr F0, F1, F;
+ CaptureLaser3dPtr C0, C1, C;
+ FeatureBasePtr f1;
+ FactorBasePtr c1;
+
+ 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 wolf_root = _WOLF_BODYDYNAMICS_ROOT_DIR;
+ ParserYaml parser = ParserYaml("processor_odom_icp_3d.yaml", wolf_root + "/test/yaml/");
+ ParamsServer server = ParamsServer(parser.getParams());
+ P = Problem::autoSetup(server);
+
+ S = std::static_pointer_cast<SensorForceTorqueInertial>(P->getHardware()->getSensorList().front());
+ p = std::static_pointer_cast<ProcessorForceTorqueInertialPreintDynamics>(S->getProcessorList().front());
+
+ data = VectorXd::Zero(12); // [ a, w, f, t ]
+ data_cov = MatrixXd::Identity(12, 12);
+ C = std::make_shared<CaptureMotion>("CaptureMotion", 0.0, S, data, data_cov, nullptr);
+
+ C->process();
+
+ C0 = std::static_pointer_cast<CaptureMotion>(p->getOrigin());
+ F0 = C0->getFrame();
+
+ ASSERT_EQ(C0->getTimeStamp(), F0->getTimeStamp());
+ ASSERT_EQ(C0->getTimeStamp(), F0->getCaptureOf(S)->getTimeStamp());
+
+ C = std::make_shared<CaptureMotion>("CaptureMotion", 1.0, S, data, data_cov, nullptr);
+ C->process();
+
+ C1 = std::static_pointer_cast<CaptureMotion>(p->getLast());
+ F1 = C1->getFrame();
+
+ F1->link(P); // by the face
+
+ 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);
+
+ c1 = FactorBase::emplace<FactorForceTorqueInertialDynamics>(f1, f1, C0, p, false);
+
+ F1->getStateBlock('P')->setState(Vector3d(0, 0, 0));
+ F1->getStateBlock('V')->setState(Vector3d(0, 0, 0));
+ F1->getStateBlock('O')->setState(Vector4d(0, 0, 0, 1));
+ F1->getStateBlock('L')->setState(Vector3d(0, 0, 0));
+ }
+};
+
+class Test_FactorForceTorqueInertialDynamics : public testing::Test
+{
+ public:
+ ProblemPtr P;
+ SensorForceTorqueInertialPtr S;
+ ProcessorForceTorqueInertialPreintDynamicsPtr p;
+ FrameBasePtr F0, F1;
+ CaptureMotionPtr C0, C1;
+ FeatureMotionPtr f1;
+ FactorForceTorqueInertialDynamicsPtr c1;
+
+ 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);
+
+ // 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);
+
+ // crear un factor
+ c1 = FactorBase::emplace<FactorForceTorqueInertialDynamics>(f1, f1, C0, p, false);
+ }
+};
+
+// TEST(FactorForceTorqueInertialDynamics_yaml, force_registraion)
+// {
+// FactorySensor::registerCreator("SensorForceTorqueInertial", SensorForceTorqueInertial::create);
+// }
+
+// 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 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)
+{
+ testing::InitGoogleTest(&argc, argv);
+ //::testing::GTEST_FLAG(filter) = "Test_FactorForceTorqueInertialDynamics_yaml.residual";
+ return RUN_ALL_TESTS();
+}
\ No newline at end of file
diff --git a/test/yaml/processor_odom_icp_3d.yaml b/test/yaml/processor_odom_icp_3d.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391