diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index 812ee1f8e4233825bb78aae2029cbd349c34afc1..6fa50395a7ee2af50d290b32e543b18bac534869 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -172,7 +172,6 @@ wolf_add_gtest(gtest_factor_relative_pose_3d gtest_factor_relative_pose_3d.cpp)
 
 # FactorRelativePose3dWithExtrinsics class test
 wolf_add_gtest(gtest_factor_relative_pose_3d_with_extrinsics gtest_factor_relative_pose_3d_with_extrinsics.cpp)
-wolf_add_gtest(gtest_factor_relative_pose_3d_with_extrinsics_new gtest_factor_relative_pose_3d_with_extrinsics_new.cpp)
 
 # FactorVelocityLocalDirection3d class test
 wolf_add_gtest(gtest_factor_velocity_local_direction_3d gtest_factor_velocity_local_direction_3d.cpp)
diff --git a/test/gtest_factor_relative_pose_3d_with_extrinsics.cpp b/test/gtest_factor_relative_pose_3d_with_extrinsics.cpp
index f4755e11139f07ba42d537e0fc4e070c300cf9a3..54f4fc527a9d470442b2a1d6352ef064af6c21d4 100644
--- a/test/gtest_factor_relative_pose_3d_with_extrinsics.cpp
+++ b/test/gtest_factor_relative_pose_3d_with_extrinsics.cpp
@@ -1,326 +1,378 @@
-//--------LICENSE_START--------
-//
-// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
-// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
-// All rights reserved.
-//
-// This file is part of WOLF
-// WOLF is free software: you can redistribute it and/or modify
-// it under the terms of the GNU Lesser General Public License as published by
-// the Free Software Foundation, either version 3 of the License, or
-// at your option) any later version.
-//
-// This program is distributed in the hope that it will be useful,
-// but WITHOUT ANY WARRANTY; without even the implied warranty of
-// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
-// GNU Lesser General Public License for more details.
-//
-// You should have received a copy of the GNU Lesser General Public License
-// along with this program.  If not, see <http://www.gnu.org/licenses/>.
-//
-//--------LICENSE_END--------
-#include <core/factor/factor_relative_pose_3d_with_extrinsics.h>
-#include <core/utils/utils_gtest.h>
-
-#include "core/common/wolf.h"
-#include "core/utils/logging.h"
-
-#include "core/state_block/state_block_derived.h"
-#include "core/state_block/state_quaternion.h"
-#include "core/ceres_wrapper/solver_ceres.h"
-#include "core/capture/capture_pose.h"
-#include "core/feature/feature_pose.h"
-
-
-
-using namespace Eigen;
-using namespace wolf;
-using std::static_pointer_cast;
-
-
-
-// Use the following in case you want to initialize tests with predefines variables or methods.
-class FactorRelativePose3dWithExtrinsics_class : public testing::Test{
-    public:
-        Vector3d    pos_camera,   pos_robot,   pos_landmark;
-        Vector3d    euler_camera, euler_robot, euler_landmark;
-        Quaterniond quat_camera,  quat_robot,  quat_landmark;
-        Vector4d    vquat_camera, vquat_robot, vquat_landmark; // quaternions as vectors
-        Vector7d    pose_camera,  pose_robot,  pose_landmark;
-
-        ProblemPtr     problem;
-        SolverCeresPtr solver;
-
-        SensorBasePtr   S;
-        FrameBasePtr    F1;
-        CapturePosePtr  c1;
-        FeaturePosePtr  f1;
-        LandmarkBasePtr lmk1;
-        FactorRelativePose3dWithExtrinsicsPtr fac;
-
-        Eigen::Matrix6d meas_cov;
-
-        void SetUp() override
+    //--------LICENSE_START--------
+    //
+    // Copyright (C) 2020,2021,2022 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
+    // Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+    // All rights reserved.
+    //
+    // This file is part of WOLF
+    // WOLF is free software: you can redistribute it and/or modify
+    // it under the terms of the GNU Lesser General Public License as published by
+    // the Free Software Foundation, either version 3 of the License, or
+    // at your option) any later version.
+    //
+    // This program is distributed in the hope that it will be useful,
+    // but WITHOUT ANY WARRANTY; without even the implied warranty of
+    // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    // GNU Lesser General Public License for more details.
+    //
+    // You should have received a copy of the GNU Lesser General Public License
+    // along with this program.  If not, see <http://www.gnu.org/licenses/>.
+    //
+    //--------LICENSE_END--------
+    #include "core/utils/utils_gtest.h"
+
+    #include "core/ceres_wrapper/solver_ceres.h"
+    #include "core/factor/factor_relative_pose_3d_with_extrinsics.h"
+    #include "core/capture/capture_odom_3d.h"
+    #include "core/sensor/sensor_odom_3d.h"
+    #include "core/math/rotations.h"
+    #include "core/state_block/state_block_derived.h"
+    #include "core/state_block/state_quaternion.h"
+
+
+    using namespace Eigen;
+    using namespace wolf;
+    using std::cout;
+    using std::endl;
+
+    std::string wolf_root = _WOLF_ROOT_DIR;
+
+    int N = 1e2;
+
+    // Vectors
+    Vector7d delta, x_0, x_1, x_f, x_l, x_s;
+
+    // Input odometry data and covariance
+    Matrix6d data_cov = (Vector6d() << 1e-3, 1e-3, 1e-3, 1e-6, 1e-6, 1e-6).finished().asDiagonal();
+
+    // Problem and solver
+    ProblemPtr problem_ptr = Problem::create("PO", 3);
+
+    SolverCeres solver(problem_ptr);
+
+    // Sensor
+    auto sensor_odom3d = problem_ptr->installSensor("SensorOdom3d", "odom", (Vector7d() << 0,0,0,0,0,0,1).finished(), wolf_root + "/test/yaml/sensor_odom_3d.yaml");
+
+    // Two frames
+    FrameBasePtr frm0 = problem_ptr->emplaceFrame(0, (Vector7d() << 0,0,0,0,0,0,1).finished() );
+    FrameBasePtr frm1 = problem_ptr->emplaceFrame(1, (Vector7d() << 0,0,0,0,0,0,1).finished() );
+
+    // Landmark
+    LandmarkBasePtr lmk = LandmarkBase::emplace<LandmarkBase>(problem_ptr->getMap(), 
+                                                            "LandmarkPose3d",
+                                                            std::make_shared<StatePoint3d>(Vector3d::Zero()),
+                                                            std::make_shared<StateQuaternion>(Quaterniond::Identity().coeffs()));
+
+    // Capture
+    auto cap1 = CaptureBase::emplace<CaptureOdom3d>(frm1, 1, sensor_odom3d, Vector7d::Zero(), data_cov);
+    // Feature
+    FeatureBasePtr fea = nullptr;
+    // Factor
+    FactorRelativePose3dWithExtrinsicsPtr fac = nullptr;
+
+    void generateRandomProblemFrame()
+    {
+        // Random delta
+        delta = Vector7d::Random() * 1e2;
+        delta.tail<4>().normalize();
+        auto q_delta = Quaterniond(delta.tail<4>());
+
+        // Random frame 0 pose
+        x_0 = Vector7d::Random() * 1e2;
+        x_0.tail<4>().normalize();
+        auto q_0 = Quaterniond(x_0.tail<4>());
+
+        // Random extrinsics
+        x_s = Vector7d::Random() * 1e2;
+        x_s.tail<4>().normalize();
+        auto q_s = Quaterniond(x_s.tail<4>());
+
+        // Random frame 1 pose
+        auto q_1 = q_0 * q_s * q_delta * q_s.conjugate();
+        x_1.head<3>() = x_0.head<3>() + q_0 * (x_s.head<3>() + q_s * delta.head<3>()) - q_1 *  x_s.head<3>();
+        x_1.tail<4>() = q_1.coeffs();
+
+        // WOLF_INFO("x_0:   ", x_0.transpose());
+        // WOLF_INFO("x_s:   ", x_s.transpose());
+        // WOLF_INFO("delta: ", delta.transpose());
+        // WOLF_INFO("x_1:   ", x_1.transpose());
+
+        // Set states
+        frm0->setState(x_0);
+        frm1->setState(x_1);
+        cap1->setData(delta);
+        sensor_odom3d->setState(x_s);
+
+        // feature & factor with delta measurement
+        fea = FeatureBase::emplace<FeatureBase>(cap1, "FeatureOdom3d", delta, data_cov);
+        fac = FactorBase::emplace<FactorRelativePose3dWithExtrinsics>(fea, fea, frm0, nullptr, false, TOP_MOTION); // create and add
+    }
+
+    void generateRandomProblemLandmark()
+    {
+        // Random delta
+        delta = Vector7d::Random() * 10;
+        delta.tail<4>().normalize();
+        auto q_delta = Quaterniond(delta.tail<4>());
+
+        // Random frame pose
+        x_f = Vector7d::Random() * 10;
+        x_f.tail<4>().normalize();
+        auto q_f = Quaterniond(x_f.tail<4>());
+
+        // Random extrinsics
+        x_s = Vector7d::Random() * 10;
+        x_s.tail<4>().normalize();
+        auto q_s = Quaterniond(x_s.tail<4>());
+
+        // landmark pose
+        x_l.head<3>() = x_f.head<3>() + q_f * (x_s.head<3>() + q_s * delta.head<3>());
+        x_l.tail<4>() = (q_f * q_s * q_delta).coeffs();
+
+        // Set states
+        frm1->setState(x_f);
+        lmk->setState(x_l);
+        cap1->setData(delta);
+        sensor_odom3d->setState(x_s);
+
+        // feature & factor with delta measurement
+        fea = FeatureBase::emplace<FeatureBase>(cap1, "FeatureLandmarkPose", delta, data_cov);
+        fac = FactorBase::emplace<FactorRelativePose3dWithExtrinsics>(fea, fea, lmk, nullptr, false, TOP_LMK); // create and add
+    }
+
+    TEST(FactorRelativePose3dWithExtrinsics, check_tree)
+    {
+        ASSERT_TRUE(problem_ptr->check(0));
+    }
+
+    // FRAME TO FRAME =========================================================================
+    TEST(FactorRelativePose3dWithExtrinsics, frame_solve_frame1)
+    {
+        for (int i = 0; i < N; i++)
         {
-            // configuration
-
-             /* We have three poses to take into account:
-             *  - pose of the sensor (extrinsincs)
-             *  - pose of the landmark
-             *  - pose of the robot (Keyframe)
-             *
-             * The measurement provides the pose of the landmark wrt sensor's current pose in the world.
-             * Camera's current pose in World is the composition of the robot pose with the sensor extrinsics.
-             *
-             * The robot has a sensor looking forward
-             *   S: pos = (0,0,0), ori = (0, 0, 0)
-             *
-             * There is a point at the origin
-             *   P: pos = (0,0,0)
-             *
-             * Therefore, P projects exactly at the origin of the sensor,
-             *   f: p = (0,0)
-             *
-             * We form a Wolf tree with 1 frames F1, 1 capture C1,
-             * 1 feature f1 (measurement), 1 landmark l and 1 relative kf landmark constraint c1:
-             *
-             *   Frame F1, Capture C1, feature f1, landmark l1, constraint c1
-             *
-             * The frame pose F1, and the sensor pose S
-             * in the robot frame are variables subject to optimization
-             *
-             * We perform a number of tests based on this configuration.*/
-
-
-            // sensor is at origin of robot and looking forward
-            // robot is at (0,0,0)
-            // landmark is right in front of sensor. Its orientation wrt sensor is identity.
-            pos_camera      << 0,0,0;
-            pos_robot       << 0,0,0; //robot is at origin
-            pos_landmark    << 0,1,0;
-            euler_camera    << 0,0,0;
-            //euler_camera    << -M_PI_2, 0, -M_PI_2;
-            euler_robot     << 0,0,0;
-            euler_landmark  << 0,0,0;
-            quat_camera     = e2q(euler_camera);
-            quat_robot      = e2q(euler_robot);
-            quat_landmark   = e2q(euler_landmark);
-            vquat_camera    = quat_camera.coeffs();
-            vquat_robot     = quat_robot.coeffs();
-            vquat_landmark  = quat_landmark.coeffs();
-            pose_camera     << pos_camera, vquat_camera;
-            pose_robot      << pos_robot, vquat_robot;
-            pose_landmark   << pos_landmark, vquat_landmark;
-
-            // Build problem
-            problem = Problem::create("PO", 3);
-            solver = std::make_shared<SolverCeres>(problem);
-
-            // Create sensor to be able to initialize (a camera for instance)
-            S = SensorBase::emplace<SensorBase>(problem->getHardware(), "SensorPose", 
-                                                std::make_shared<StatePoint3d>(pos_camera, true), 
-                                                std::make_shared<StateQuaternion>(vquat_camera, true), 
-                                                std::make_shared<StateParams4>(Vector4d::Zero(), false),  // fixed
-                                                Vector1d::Zero());
-
-            // ParamsSensorCameraPtr params_camera = std::make_shared<ParamsSensorCamera>();
-            // S      = problem->installSensor("SensorCamera", "canonical", pose_camera, params_camera);
-            // camera = std::static_pointer_cast<SensorCamera>(S);
-
-
-            // F1 is be origin KF
-            VectorComposite x0(pose_robot, "PO", {3,4});
-            VectorComposite s0("PO", {Vector3d(1,1,1), Vector3d(1,1,1)});
-            F1 = problem->setPriorFactor(x0, s0, 0.0);
-
-
-            // the sensor is at origin as well as the robot. The measurement matches with the pose of the tag wrt sensor (and also wrt robot and world)
-            meas_cov = Eigen::Matrix6d::Identity();
-            meas_cov.topLeftCorner(3,3)     *= 1e-2;
-            meas_cov.bottomRightCorner(3,3) *= 1e-3;
-
-            //emplace feature and landmark
-            c1 = std::static_pointer_cast<CapturePose>(CaptureBase::emplace<CapturePose>(F1, 0, S, pose_landmark, meas_cov));
-            f1 = std::static_pointer_cast<FeaturePose>(FeatureBase::emplace<FeaturePose>(c1, pose_landmark, meas_cov));
-            lmk1 = LandmarkBase::emplace<LandmarkBase>(problem->getMap(), "LandmarkPose",  
-                                                       std::make_shared<StatePoint3d>(pos_landmark), 
-                                                       std::make_shared<StateQuaternion>(Quaterniond(vquat_landmark)));
+            // random setup
+            generateRandomProblemFrame();
+
+            ASSERT_MATRIX_APPROX(fac->residual(), VectorXd::Zero(6), Constants::EPS);
+
+            // Perturb frm1, fix the rest
+            frm0->fix();
+            frm1->unfix();
+            sensor_odom3d->getP()->fix();
+            sensor_odom3d->getO()->fix();
+            frm1->perturb(1);
+
+            // solve for frm1
+            std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+            //WOLF_INFO(report);
+
+            ASSERT_POSE3d_APPROX(frm1->getStateVector(), x_1, 1e-3);
+
+            // remove feature (and factor) for the next loop
+            fea->remove();
+        }
+    }
+
+    TEST(FactorRelativePose3dWithExtrinsics, frame_solve_frame0)
+    {
+        for (int i = 0; i < N; i++)
+        {
+            // random setup
+            generateRandomProblemFrame();
+            
+            ASSERT_MATRIX_APPROX(fac->residual(), VectorXd::Zero(6), Constants::EPS);
+
+            // Perturb frm0, fix the rest
+            frm1->fix();
+            frm0->unfix();
+            sensor_odom3d->getP()->fix();
+            sensor_odom3d->getO()->fix();
+            frm0->perturb(1);
+
+            // solve for frm0
+            std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+            //WOLF_INFO(report);
+
+            ASSERT_POSE3d_APPROX(frm0->getStateVector(), x_0, 1e-3);
+
+            // remove feature (and factor) for the next loop
+            fea->remove();
+        }
+    }
+
+    // JV: The position extrinsics in case of pair of frames apears to be not very observable
+    TEST(FactorRelativePose3dWithExtrinsics, frame_solve_extrinsics_p)
+    {
+        for (int i = 0; i < N; i++)
+        {
+            // random setup
+            generateRandomProblemFrame();
+            
+            ASSERT_MATRIX_APPROX(fac->residual(), VectorXd::Zero(6), Constants::EPS);
+
+            // Perturb sensor P, fix the rest
+            frm1->fix();
+            frm0->fix();
+            sensor_odom3d->getP()->unfix();
+            sensor_odom3d->getO()->fix();
+            //WOLF_INFO("sensor P before perturbing: ", sensor_odom3d->getP()->getState().transpose());
+            sensor_odom3d->getP()->perturb(1);
+            //WOLF_INFO("sensor P after perturbing: ", sensor_odom3d->getP()->getState().transpose());
+
+            // solve for frm0
+            std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+            //WOLF_INFO("sensor P after solving: ", sensor_odom3d->getP()->getState().transpose());
+            //WOLF_INFO(report);
+
+            //ASSERT_POSE3d_APPROX(sensor_odom3d->getStateVector(), x_s, 1e-3);
+
+            // remove feature (and factor) for the next loop
+            fea->remove();
+        }
+    }
+
+    TEST(FactorRelativePose3dWithExtrinsics, frame_solve_extrinsics_o)
+    {
+        for (int i = 0; i < N; i++)
+        {
+            // random setup
+            generateRandomProblemFrame();
+            
+            ASSERT_MATRIX_APPROX(fac->residual(), VectorXd::Zero(6), Constants::EPS);
+
+            // Perturb sensor O, fix the rest
+            frm1->fix();
+            frm0->fix();
+            sensor_odom3d->getP()->fix();
+            sensor_odom3d->getO()->unfix();
+            sensor_odom3d->getO()->perturb(.2);
+
+            //std::cout << "Sensor O (perturbed): " << sensor_odom3d->getO()->getState().transpose() << std::endl;
+
+            // solve for frm0
+            std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+            //WOLF_INFO(report);
+
+            ASSERT_POSE3d_APPROX(sensor_odom3d->getStateVector(), x_s, 1e-3);
+
+            // remove feature (and factor) for the next loop
+            fea->remove();
         }
-};
-
-
-TEST_F(FactorRelativePose3dWithExtrinsics_class, Constructor)
-{
-    auto factor = std::make_shared<FactorRelativePose3dWithExtrinsics>(f1,
-                                                                       lmk1,
-                                                                       nullptr,
-                                                                       false);
-
-    ASSERT_TRUE(factor->getType() == "FactorRelativePose3dWithExtrinsics");
-}
-
-/////////////////////////////////////////////
-// Tree not ok with this gtest implementation
-/////////////////////////////////////////////
-TEST_F(FactorRelativePose3dWithExtrinsics_class, Check_tree)
-{
-    ASSERT_TRUE(problem->check(1));
-
-    auto factor = FactorBase::emplace<FactorRelativePose3dWithExtrinsics>(f1,
-                                                                          f1,
-                                                                          lmk1,
-                                                                          nullptr,
-                                                                          false);
-    ASSERT_TRUE(problem->check(1));
-}
-
-TEST_F(FactorRelativePose3dWithExtrinsics_class, solve_F1_P_perturbated)
-{
-    auto factor = FactorBase::emplace<FactorRelativePose3dWithExtrinsics>(f1,
-                                                                          f1,
-                                                                          lmk1,
-                                                                          nullptr,
-                                                                          false);
-
-    // unfix F1, perturbate state
-    F1->unfix();
-    F1->getP()->perturb();
-
-    std::string report = solver->solve(SolverManager::ReportVerbosity::QUIET); // 0: nothing, 1: BriefReport, 2: FullReport
-    ASSERT_MATRIX_APPROX(F1->getState().vector("PO"), pose_robot, 1e-6);
-}
-
-TEST_F(FactorRelativePose3dWithExtrinsics_class, solve_F1_O_perturbated)
-{
-    auto factor = FactorBase::emplace<FactorRelativePose3dWithExtrinsics>(f1,
-                                                                          f1,
-                                                                          lmk1,
-                                                                          nullptr,
-                                                                          false);
-
-    // unfix F1, perturbate state
-    F1->unfix();
-    F1->getO()->perturb();
-
-    std::string report = solver->solve(SolverManager::ReportVerbosity::QUIET); // 0: nothing, 1: BriefReport, 2: FullReport
-    ASSERT_MATRIX_APPROX(F1->getState().vector("PO"), pose_robot, 1e-6);
-}
-
-TEST_F(FactorRelativePose3dWithExtrinsics_class, Check_initialization)
-{
-    auto factor = FactorBase::emplace<FactorRelativePose3dWithExtrinsics>(f1,
-                                                                          f1,
-                                                                          lmk1,
-                                                                          nullptr,
-                                                                          false);
-
-    ASSERT_MATRIX_APPROX(F1->getState().vector("PO"), pose_robot, 1e-6);
-    ASSERT_MATRIX_APPROX(f1->getMeasurement(), pose_landmark, 1e-6);
-    ASSERT_MATRIX_APPROX(lmk1->getState().vector("PO"), pose_landmark, 1e-6);
-
-}
-
-TEST_F(FactorRelativePose3dWithExtrinsics_class, solve_L1_P_perturbated)
-{
-    auto factor = FactorBase::emplace<FactorRelativePose3dWithExtrinsics>(f1,
-                                                                          f1,
-                                                                          lmk1,
-                                                                          nullptr,
-                                                                          false);
-
-    // unfix lmk1, perturbate state
-    lmk1->unfix();
-    lmk1->getP()->perturb();
-
-    std::string report = solver->solve(SolverManager::ReportVerbosity::QUIET); // 0: nothing, 1: BriefReport, 2: FullReport
-    ASSERT_MATRIX_APPROX(F1->getState().vector("PO"), pose_robot, 1e-6);
-    ASSERT_MATRIX_APPROX(lmk1->getState().vector("PO"), pose_landmark, 1e-6);
-}
-
-TEST_F(FactorRelativePose3dWithExtrinsics_class, solve_L1_O_perturbated)
-{
-    auto factor = FactorBase::emplace<FactorRelativePose3dWithExtrinsics>(f1,
-                                                                          f1,
-                                                                          lmk1,
-                                                                          nullptr,
-                                                                          false);
-
-    // unfix F1, perturbate state
-    lmk1->unfix();
-    lmk1->getO()->perturb();
-
-    std::string report = solver->solve(SolverManager::ReportVerbosity::QUIET); // 0: nothing, 1: BriefReport, 2: FullReport
-    ASSERT_MATRIX_APPROX(F1->getState().vector("PO"), pose_robot, 1e-6);
-    ASSERT_MATRIX_APPROX(lmk1->getState().vector("PO"), pose_landmark, 1e-6);
-
-}
-
-TEST_F(FactorRelativePose3dWithExtrinsics_class, solve_L1_PO_perturbated)
-{
-    // Change setup
-    Vector3d p_w_r, p_r_c, p_c_l, p_w_l;
-    Quaterniond q_w_r, q_r_c, q_c_l, q_w_l;
-    p_w_r << 1, 2, 3;
-    p_r_c << 4, 5, 6;
-    p_c_l << 7, 8, 9;
-    q_w_r.coeffs() << 1, 2, 3, 4; q_w_r.normalize();
-    q_r_c.coeffs() << 4, 5, 6, 7; q_r_c.normalize();
-    q_c_l.coeffs() << 7, 8, 9, 0; q_c_l.normalize();
-
-    q_w_l = q_w_r * q_r_c * q_c_l;
-    p_w_l = p_w_r + q_w_r * (p_r_c + q_r_c * p_c_l);
-
-    // Change feature (remove and emplace)
-    Vector7d meas;
-    meas << p_c_l, q_c_l.coeffs();
-    f1->remove();
-    f1 = std::static_pointer_cast<FeaturePose>(FeatureBase::emplace<FeaturePose>(c1, meas, meas_cov));
-
-    // emplace factor
-    auto factor = FactorBase::emplace<FactorRelativePose3dWithExtrinsics>(f1,
-                                                                          f1,
-                                                                          lmk1,
-                                                                          nullptr,
-                                                                          false);
-
-    // Change Landmark
-    lmk1->getP()->setState(p_w_l);
-    lmk1->getO()->setState(q_w_l.coeffs());
-    ASSERT_TRUE(lmk1->getP()->stateUpdated());
-    ASSERT_TRUE(lmk1->getO()->stateUpdated());
-
-    // Change Frame
-    F1->getP()->setState(p_w_r);
-    F1->getO()->setState(q_w_r.coeffs());
-    F1->fix();
-    ASSERT_TRUE(F1->getP()->stateUpdated());
-    ASSERT_TRUE(F1->getO()->stateUpdated());
-
-    // Change sensor extrinsics
-    S->getP()->setState(p_r_c);
-    S->getO()->setState(q_r_c.coeffs());
-    ASSERT_TRUE(S->getP()->stateUpdated());
-    ASSERT_TRUE(S->getO()->stateUpdated());
-
-    Vector7d t_w_r, t_w_l;
-    t_w_r << p_w_r, q_w_r.coeffs();
-    t_w_l << p_w_l, q_w_l.coeffs();
-    ASSERT_MATRIX_APPROX(F1->getState().vector("PO"), t_w_r, 1e-6);
-    ASSERT_MATRIX_APPROX(lmk1->getState().vector("PO"), t_w_l, 1e-6);
-
-    // unfix LMK, perturbate state
-    lmk1->unfix();
-    lmk1->perturb();
-
-    std::string report = solver->solve(SolverManager::ReportVerbosity::QUIET); // 0: nothing, 1: BriefReport, 2: FullReport
-    ASSERT_MATRIX_APPROX(F1->getState().vector("PO").transpose(), t_w_r.transpose(), 1e-6);
-    ASSERT_MATRIX_APPROX(lmk1->getState().vector("PO").transpose(), t_w_l.transpose(), 1e-6);
-
-}
-
-int main(int argc, char **argv)
-{
+    }
+
+    // FRAME TO LANDMARK =========================================================================
+    TEST(FactorRelativePose3dWithExtrinsics, landmark_solve_lmk)
+    {
+        for (int i = 0; i < N; i++)
+        {
+            // random setup
+            generateRandomProblemLandmark();
+            
+            ASSERT_MATRIX_APPROX(fac->residual(), VectorXd::Zero(6), Constants::EPS);
+
+            // Perturb landmark, fix the rest
+            frm1->fix();
+            lmk->unfix();
+            sensor_odom3d->getP()->fix();
+            sensor_odom3d->getO()->fix();
+            lmk->perturb(1);
+
+            // solve for landmark
+            std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+            //WOLF_INFO(report);
+
+            ASSERT_POSE3d_APPROX(lmk->getStateVector(), x_l, 1e-3);
+
+            // remove feature (and factor) for the next loop
+            fea->remove();
+        }
+    }
+
+    TEST(FactorRelativePose3dWithExtrinsics, landmark_solve_frame)
+    {
+        for (int i = 0; i < N; i++)
+        {
+            // random setup
+            generateRandomProblemLandmark();
+            
+            ASSERT_MATRIX_APPROX(fac->residual(), VectorXd::Zero(6), Constants::EPS);
+
+            // Perturb frm0, fix the rest
+            frm1->unfix();
+            lmk->fix();
+            sensor_odom3d->getP()->fix();
+            sensor_odom3d->getO()->fix();
+            frm1->perturb(1);
+
+            // solve for frm0
+            std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+            //WOLF_INFO(report);
+
+            ASSERT_POSE3d_APPROX(frm1->getStateVector(), x_f, 1e-3);
+
+            // remove feature (and factor) for the next loop
+            fea->remove();
+        }
+    }
+
+    TEST(FactorRelativePose3dWithExtrinsics, landmark_extrinsics_p_solve)
+    {
+        for (int i = 0; i < N; i++)
+        {
+            // random setup
+            generateRandomProblemLandmark();
+            
+            ASSERT_MATRIX_APPROX(fac->residual(), VectorXd::Zero(6), Constants::EPS);
+
+            // Perturb sensor P, fix the rest
+            frm1->fix();
+            lmk->fix();
+            sensor_odom3d->getP()->unfix();
+            sensor_odom3d->getO()->fix();
+            sensor_odom3d->getP()->perturb(1);
+
+            // solve for frm0
+            std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+            //WOLF_INFO(report);
+
+            ASSERT_POSE3d_APPROX(sensor_odom3d->getStateVector(), x_s, 1e-3);
+
+            // remove feature (and factor) for the next loop
+            fea->remove();
+        }
+    }
+
+    TEST(FactorRelativePose3dWithExtrinsics, landmark_extrinsics_o_solve)
+    {
+        for (int i = 0; i < N; i++)
+        {
+            // random setup
+            generateRandomProblemLandmark();
+            
+            ASSERT_MATRIX_APPROX(fac->residual(), VectorXd::Zero(6), Constants::EPS);
+
+            // Perturb sensor O, fix the rest
+            frm1->fix();
+            lmk->fix();
+            sensor_odom3d->getP()->fix();
+            sensor_odom3d->getO()->unfix();
+            sensor_odom3d->getO()->perturb(.2);
+
+            //std::cout << "Sensor O (perturbed): " << sensor_odom3d->getO()->getState().transpose() << std::endl;
+
+            // solve for frm0
+            std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+            //WOLF_INFO(report);
+
+            ASSERT_POSE3d_APPROX(sensor_odom3d->getStateVector(), x_s, 1e-3);
+
+            // remove feature (and factor) for the next loop
+            fea->remove();
+        }
+    }
+
+    int main(int argc, char **argv)
+    {
     testing::InitGoogleTest(&argc, argv);
     return RUN_ALL_TESTS();
-}
-
+    }
diff --git a/test/gtest_factor_relative_pose_3d_with_extrinsics_new.cpp b/test/gtest_factor_relative_pose_3d_with_extrinsics_new.cpp
deleted file mode 100644
index 7b83aa4a8d014415fbd80405c47197e6eddaaba5..0000000000000000000000000000000000000000
--- a/test/gtest_factor_relative_pose_3d_with_extrinsics_new.cpp
+++ /dev/null
@@ -1,377 +0,0 @@
-    //--------LICENSE_START--------
-    //
-    // Copyright (C) 2020,2021,2022 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
-    // Authors: Joan Solà Ortega (jsola@iri.upc.edu)
-    // All rights reserved.
-    //
-    // This file is part of WOLF
-    // WOLF is free software: you can redistribute it and/or modify
-    // it under the terms of the GNU Lesser General Public License as published by
-    // the Free Software Foundation, either version 3 of the License, or
-    // at your option) any later version.
-    //
-    // This program is distributed in the hope that it will be useful,
-    // but WITHOUT ANY WARRANTY; without even the implied warranty of
-    // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
-    // GNU Lesser General Public License for more details.
-    //
-    // You should have received a copy of the GNU Lesser General Public License
-    // along with this program.  If not, see <http://www.gnu.org/licenses/>.
-    //
-    //--------LICENSE_END--------
-    #include "core/utils/utils_gtest.h"
-
-    #include "core/ceres_wrapper/solver_ceres.h"
-    #include "core/factor/factor_relative_pose_3d_with_extrinsics.h"
-    #include "core/capture/capture_odom_3d.h"
-    #include "core/sensor/sensor_odom_3d.h"
-    #include "core/math/rotations.h"
-    #include "core/state_block/state_block_derived.h"
-    #include "core/state_block/state_quaternion.h"
-
-
-    using namespace Eigen;
-    using namespace wolf;
-    using std::cout;
-    using std::endl;
-
-    std::string wolf_root = _WOLF_ROOT_DIR;
-
-    int N = 1;//1e2
-
-    // Vectors
-    Vector7d delta, x_0, x_1, x_f, x_l, x_s;
-
-    // Input odometry data and covariance
-    Matrix6d data_cov = (Vector6d() << 1e-3, 1e-3, 1e-3, 1e-6, 1e-6, 1e-6).finished().asDiagonal();
-
-    // Problem and solver
-    ProblemPtr problem_ptr = Problem::create("PO", 3);
-
-    SolverCeres solver(problem_ptr);
-
-    // Sensor
-    auto sensor_odom3d = problem_ptr->installSensor("SensorOdom3d", "odom", (Vector7d() << 0,0,0,0,0,0,1).finished(), wolf_root + "/test/yaml/sensor_odom_3d.yaml");
-
-    // Two frames
-    FrameBasePtr frm0 = problem_ptr->emplaceFrame(0, (Vector7d() << 0,0,0,0,0,0,1).finished() );
-    FrameBasePtr frm1 = problem_ptr->emplaceFrame(1, (Vector7d() << 0,0,0,0,0,0,1).finished() );
-
-    // Landmark
-    LandmarkBasePtr lmk = LandmarkBase::emplace<LandmarkBase>(problem_ptr->getMap(), 
-                                                            "LandmarkPose3d",
-                                                            std::make_shared<StatePoint3d>(Vector3d::Zero()),
-                                                            std::make_shared<StateQuaternion>(Quaterniond::Identity().coeffs()));
-
-    // Capture
-    auto cap1 = CaptureBase::emplace<CaptureOdom3d>(frm1, 1, sensor_odom3d, Vector7d::Zero(), data_cov);
-    // Feature
-    FeatureBasePtr fea = nullptr;
-    // Factor
-    FactorRelativePose3dWithExtrinsicsPtr fac = nullptr;
-
-    void generateRandomProblemFrame()
-    {
-        // Random delta
-        delta = Vector7d::Random() * 1e2;
-        delta.tail<4>().normalize();
-        auto q_delta = Quaterniond(delta.tail<4>());
-
-        // Random frame 0 pose
-        x_0 = Vector7d::Random() * 1e2;
-        x_0.tail<4>().normalize();
-        auto q_0 = Quaterniond(x_0.tail<4>());
-
-        // Random extrinsics
-        x_s = Vector7d::Random() * 1e2;
-        x_s.tail<4>().normalize();
-        auto q_s = Quaterniond(x_s.tail<4>());
-
-        // Random frame 1 pose
-        auto q_1 = q_0 * q_s * q_delta * q_s.conjugate();
-        x_1.head<3>() = x_0.head<3>() + q_0 * (x_s.head<3>() + q_s * delta.head<3>()) - q_1 *  x_s.head<3>();
-        x_1.tail<4>() = q_1.coeffs();
-
-        WOLF_INFO("x_0:   ", x_0.transpose());
-        WOLF_INFO("x_s:   ", x_s.transpose());
-        WOLF_INFO("delta: ", delta.transpose());
-        WOLF_INFO("x_1:   ", x_1.transpose());
-
-        // Set states
-        frm0->setState(x_0);
-        frm1->setState(x_1);
-        cap1->setData(delta);
-        sensor_odom3d->setState(x_s);
-
-        // feature & factor with delta measurement
-        fea = FeatureBase::emplace<FeatureBase>(cap1, "FeatureOdom3d", delta, data_cov);
-        fac = FactorBase::emplace<FactorRelativePose3dWithExtrinsics>(fea, fea, frm0, nullptr, false, TOP_MOTION); // create and add
-    }
-
-    void generateRandomProblemLandmark()
-    {
-        // Random delta
-        delta = Vector7d::Random() * 10;
-        delta.tail<4>().normalize();
-        auto q_delta = Quaterniond(delta.tail<4>());
-
-        // Random frame pose
-        x_f = Vector7d::Random() * 10;
-        x_f.tail<4>().normalize();
-        auto q_f = Quaterniond(x_f.tail<4>());
-
-        // Random extrinsics
-        x_s = Vector7d::Random() * 10;
-        x_s.tail<4>().normalize();
-        auto q_s = Quaterniond(x_s.tail<4>());
-
-        // landmark pose
-        x_l.head<3>() = x_f.head<3>() + q_f * (x_s.head<3>() + q_s * delta.head<3>());
-        x_l.tail<4>() = (q_f * q_s * q_delta).coeffs();
-
-        // Set states
-        frm1->setState(x_f);
-        lmk->setState(x_l);
-        cap1->setData(delta);
-        sensor_odom3d->setState(x_s);
-
-        // feature & factor with delta measurement
-        fea = FeatureBase::emplace<FeatureBase>(cap1, "FeatureLandmarkPose", delta, data_cov);
-        fac = FactorBase::emplace<FactorRelativePose3dWithExtrinsics>(fea, fea, lmk, nullptr, false, TOP_LMK); // create and add
-    }
-
-    TEST(FactorRelativePose3dWithExtrinsics, check_tree)
-    {
-        ASSERT_TRUE(problem_ptr->check(0));
-    }
-
-    // FRAME TO FRAME =========================================================================
-    TEST(FactorRelativePose3dWithExtrinsics, frame_solve_frame1)
-    {
-        for (int i = 0; i < N; i++)
-        {
-            // random setup
-            generateRandomProblemFrame();
-
-            ASSERT_MATRIX_APPROX(fac->residual(), VectorXd::Zero(6), Constants::EPS);
-
-            // Perturb frm1, fix the rest
-            frm0->fix();
-            frm1->unfix();
-            sensor_odom3d->getP()->fix();
-            sensor_odom3d->getO()->fix();
-            frm1->perturb(1);
-
-            // solve for frm1
-            std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
-            WOLF_INFO(report);
-
-            ASSERT_POSE3d_APPROX(frm1->getStateVector(), x_1, 1e-6);
-
-            // remove feature (and factor) for the next loop
-            fea->remove();
-        }
-    }
-
-    TEST(FactorRelativePose3dWithExtrinsics, frame_solve_frame0)
-    {
-        for (int i = 0; i < N; i++)
-        {
-            // random setup
-            generateRandomProblemFrame();
-            
-            ASSERT_MATRIX_APPROX(fac->residual(), VectorXd::Zero(6), Constants::EPS);
-
-            // Perturb frm0, fix the rest
-            frm1->fix();
-            frm0->unfix();
-            sensor_odom3d->getP()->fix();
-            sensor_odom3d->getO()->fix();
-            frm0->perturb(1);
-
-            // solve for frm0
-            std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
-            WOLF_INFO(report);
-
-            ASSERT_POSE3d_APPROX(frm0->getStateVector(), x_0, 1e-6);
-
-            // remove feature (and factor) for the next loop
-            fea->remove();
-        }
-    }
-
-    TEST(FactorRelativePose3dWithExtrinsics, frame_solve_extrinsics_p)
-    {
-        for (int i = 0; i < N; i++)
-        {
-            // random setup
-            generateRandomProblemFrame();
-            
-            ASSERT_MATRIX_APPROX(fac->residual(), VectorXd::Zero(6), Constants::EPS);
-
-            // Perturb sensor P, fix the rest
-            frm1->fix();
-            frm0->fix();
-            sensor_odom3d->getP()->unfix();
-            sensor_odom3d->getO()->fix();
-            WOLF_INFO("sensor P before perturbing: ", sensor_odom3d->getP()->getState().transpose());
-            sensor_odom3d->getP()->perturb(1);
-            WOLF_INFO("sensor P after perturbing: ", sensor_odom3d->getP()->getState().transpose());
-
-            // solve for frm0
-            std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
-            WOLF_INFO("sensor P after solving: ", sensor_odom3d->getP()->getState().transpose());
-            WOLF_INFO(report);
-
-            ASSERT_POSE3d_APPROX(sensor_odom3d->getStateVector(), x_s, 1e-6);
-
-            // remove feature (and factor) for the next loop
-            fea->remove();
-        }
-    }
-
-    TEST(FactorRelativePose3dWithExtrinsics, frame_solve_extrinsics_o)
-    {
-        for (int i = 0; i < N; i++)
-        {
-            // random setup
-            generateRandomProblemFrame();
-            
-            ASSERT_MATRIX_APPROX(fac->residual(), VectorXd::Zero(6), Constants::EPS);
-
-            // Perturb sensor O, fix the rest
-            frm1->fix();
-            frm0->fix();
-            sensor_odom3d->getP()->fix();
-            sensor_odom3d->getO()->unfix();
-            sensor_odom3d->getO()->perturb(.2);
-
-            //std::cout << "Sensor O (perturbed): " << sensor_odom3d->getO()->getState().transpose() << std::endl;
-
-            // solve for frm0
-            std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
-            WOLF_INFO(report);
-
-            ASSERT_POSE3d_APPROX(sensor_odom3d->getStateVector(), x_s, 1e-6);
-
-            // remove feature (and factor) for the next loop
-            fea->remove();
-        }
-    }
-
-    // FRAME TO LANDMARK =========================================================================
-    TEST(FactorRelativePose3dWithExtrinsics, landmark_solve_lmk)
-    {
-        for (int i = 0; i < N; i++)
-        {
-            // random setup
-            generateRandomProblemLandmark();
-            
-            ASSERT_MATRIX_APPROX(fac->residual(), VectorXd::Zero(6), Constants::EPS);
-
-            // Perturb landmark, fix the rest
-            frm1->fix();
-            lmk->unfix();
-            sensor_odom3d->getP()->fix();
-            sensor_odom3d->getO()->fix();
-            lmk->perturb(1);
-
-            // solve for landmark
-            std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
-            WOLF_INFO(report);
-
-            ASSERT_POSE3d_APPROX(lmk->getStateVector(), x_l, 1e-6);
-
-            // remove feature (and factor) for the next loop
-            fea->remove();
-        }
-    }
-
-    TEST(FactorRelativePose3dWithExtrinsics, landmark_solve_frame)
-    {
-        for (int i = 0; i < N; i++)
-        {
-            // random setup
-            generateRandomProblemLandmark();
-            
-            ASSERT_MATRIX_APPROX(fac->residual(), VectorXd::Zero(6), Constants::EPS);
-
-            // Perturb frm0, fix the rest
-            frm1->unfix();
-            lmk->fix();
-            sensor_odom3d->getP()->fix();
-            sensor_odom3d->getO()->fix();
-            frm1->perturb(1);
-
-            // solve for frm0
-            std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
-            WOLF_INFO(report);
-
-            ASSERT_POSE3d_APPROX(frm1->getStateVector(), x_f, 1e-6);
-
-            // remove feature (and factor) for the next loop
-            fea->remove();
-        }
-    }
-
-    TEST(FactorRelativePose3dWithExtrinsics, landmark_extrinsics_p_solve)
-    {
-        for (int i = 0; i < N; i++)
-        {
-            // random setup
-            generateRandomProblemLandmark();
-            
-            ASSERT_MATRIX_APPROX(fac->residual(), VectorXd::Zero(6), Constants::EPS);
-
-            // Perturb sensor P, fix the rest
-            frm1->fix();
-            lmk->fix();
-            sensor_odom3d->getP()->unfix();
-            sensor_odom3d->getO()->fix();
-            sensor_odom3d->getP()->perturb(1);
-
-            // solve for frm0
-            std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
-            WOLF_INFO(report);
-
-            ASSERT_POSE3d_APPROX(sensor_odom3d->getStateVector(), x_s, 1e-6);
-
-            // remove feature (and factor) for the next loop
-            fea->remove();
-        }
-    }
-
-    TEST(FactorRelativePose3dWithExtrinsics, landmark_extrinsics_o_solve)
-    {
-        for (int i = 0; i < N; i++)
-        {
-            // random setup
-            generateRandomProblemLandmark();
-            
-            ASSERT_MATRIX_APPROX(fac->residual(), VectorXd::Zero(6), Constants::EPS);
-
-            // Perturb sensor O, fix the rest
-            frm1->fix();
-            lmk->fix();
-            sensor_odom3d->getP()->fix();
-            sensor_odom3d->getO()->unfix();
-            sensor_odom3d->getO()->perturb(.2);
-
-            //std::cout << "Sensor O (perturbed): " << sensor_odom3d->getO()->getState().transpose() << std::endl;
-
-            // solve for frm0
-            std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
-            WOLF_INFO(report);
-
-            ASSERT_POSE3d_APPROX(sensor_odom3d->getStateVector(), x_s, 1e-6);
-
-            // remove feature (and factor) for the next loop
-            fea->remove();
-        }
-    }
-
-    int main(int argc, char **argv)
-    {
-    testing::InitGoogleTest(&argc, argv);
-    return RUN_ALL_TESTS();
-    }