From 250a150de9e9f395752f87a5b6c7fcfc30cb1fd9 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joan=20Vallv=C3=A9=20Navarro?= <jvallve@iri.upc.edu>
Date: Fri, 23 Sep 2022 12:21:00 +0200
Subject: [PATCH] added gtests and standarized geometric ones

---
 include/core/factor/factor_relative_pose_2d.h |  16 +-
 .../factor_relative_pose_2d_with_extrinsics.h |  32 +-
 include/core/factor/factor_relative_pose_3d.h | 144 ++--
 .../factor_relative_pose_3d_with_extrinsics.h |   8 +-
 .../core/factor/factor_relative_position_2d.h |   4 +-
 ...or_relative_position_2d_with_extrinsics.h} |   4 +-
 test/CMakeLists.txt                           |   6 +
 test/gtest_factor_relative_pose_2d.cpp        | 187 +++--
 ...actor_relative_pose_2d_with_extrinsics.cpp | 419 ++++-------
 test/gtest_factor_relative_pose_3d.cpp        | 199 ++++-
 ...actor_relative_pose_3d_with_extrinsics.cpp | 691 +++++++++---------
 test/gtest_factor_relative_position_2d.cpp    | 268 +++++++
 ...r_relative_position_2d_with_extrinsics.cpp | 239 ++++++
 13 files changed, 1436 insertions(+), 781 deletions(-)
 rename include/core/factor/{factor_relative_position_2d_with_extrinsics copy.h => factor_relative_position_2d_with_extrinsics.h} (99%)
 create mode 100644 test/gtest_factor_relative_position_2d.cpp
 create mode 100644 test/gtest_factor_relative_position_2d_with_extrinsics.cpp

diff --git a/include/core/factor/factor_relative_pose_2d.h b/include/core/factor/factor_relative_pose_2d.h
index 4a608feb0..d156213cd 100644
--- a/include/core/factor/factor_relative_pose_2d.h
+++ b/include/core/factor/factor_relative_pose_2d.h
@@ -142,11 +142,11 @@ class FactorRelativePose2d : public FactorAnalytic
          *
          **/
         void evaluateJacobians(const std::vector<Eigen::Map<const Eigen::VectorXd>>& _st_vector,
-                                       std::vector<Eigen::Map<Eigen::MatrixRowXd> >& jacobians,
-                                       const std::vector<bool>& _compute_jacobian) const override;
+                               std::vector<Eigen::Map<Eigen::MatrixRowXd> >& jacobians,
+                               const std::vector<bool>& _compute_jacobian) const override;
         void evaluateJacobians(const std::vector<Eigen::Map<const Eigen::VectorXd>>& _st_vector,
-                                       std::vector<Eigen::MatrixXd>& jacobians,
-                                       const std::vector<bool>& _compute_jacobian) const override;
+                               std::vector<Eigen::MatrixXd>& jacobians,
+                               const std::vector<bool>& _compute_jacobian) const override;
 
         /** \brief Returns the pure jacobians (without measurement noise) evaluated in the state blocks values
          * \param jacobians is an output vector of mapped eigen matrices that sould contain the jacobians w.r.t each state block
@@ -175,8 +175,8 @@ inline Eigen::VectorXd FactorRelativePose2d::evaluateResiduals(
 }
 
 inline void FactorRelativePose2d::evaluateJacobians(const std::vector<Eigen::Map<const Eigen::VectorXd> >& _st_vector,
-                                                        std::vector<Eigen::Map<Eigen::MatrixRowXd> >& jacobians,
-                                                        const std::vector<bool>& _compute_jacobian) const
+                                                    std::vector<Eigen::Map<Eigen::MatrixRowXd> >& jacobians,
+                                                    const std::vector<bool>& _compute_jacobian) const
 {
     assert(jacobians.size() == 4);
     assert(_st_vector.size() == 4);
@@ -211,8 +211,8 @@ inline void FactorRelativePose2d::evaluateJacobians(const std::vector<Eigen::Map
 }
 
 inline void FactorRelativePose2d::evaluateJacobians(const std::vector<Eigen::Map<const Eigen::VectorXd>>& _st_vector,
-                                                        std::vector<Eigen::MatrixXd>& jacobians,
-                                                        const std::vector<bool>& _compute_jacobian) const
+                                                    std::vector<Eigen::MatrixXd>& jacobians,
+                                                    const std::vector<bool>& _compute_jacobian) const
 {
     assert(jacobians.size() == 4);
     assert(_st_vector.size() == 4);
diff --git a/include/core/factor/factor_relative_pose_2d_with_extrinsics.h b/include/core/factor/factor_relative_pose_2d_with_extrinsics.h
index fb89f046e..4455b0960 100644
--- a/include/core/factor/factor_relative_pose_2d_with_extrinsics.h
+++ b/include/core/factor/factor_relative_pose_2d_with_extrinsics.h
@@ -65,6 +65,8 @@ class FactorRelativePose2dWithExtrinsics : public FactorAutodiff<FactorRelativeP
                          const T* const _p_sensor,
                          const T* const _o_sensor,
                          T* _residuals) const;
+
+        Eigen::Vector3d residual() const;
 };
 
 inline FactorRelativePose2dWithExtrinsics::FactorRelativePose2dWithExtrinsics(const FeatureBasePtr& _ftr_ptr,
@@ -171,7 +173,7 @@ inline bool FactorRelativePose2dWithExtrinsics::operator ()(const T* const _p_re
         // w_R_r2 = R(o_target)
         // ----------------------------------------
         // s1_p_s2 = s1_R_r1*(r1_R_w*(w_p_r2 - w_p_r1) - r1_p_s1)
-        
+
         expected_measurement.head(2) = Eigen::Rotation2D<T>(-o_sensor)*(-p_sensor + Eigen::Rotation2D<T>(-o_ref)*(-p_ref + p_target));
         expected_measurement(2) = o_target - o_ref;
     }
@@ -204,6 +206,34 @@ inline bool FactorRelativePose2dWithExtrinsics::operator ()(const T* const _p_re
     return true;
 }
 
+inline Eigen::Vector3d FactorRelativePose2dWithExtrinsics::residual() const
+{
+    Eigen::Vector3d res;
+    Eigen::VectorXd p_sensor, o_sensor, p_ref, o_ref, p_target, o_target;
+    p_sensor = getCapture()->getSensorP()->getState();
+    o_sensor = getCapture()->getSensorO()->getState();
+    // FRAME CASE
+    if (not getFrameOtherList().empty())
+    {
+        p_ref    = getFrameOther()->getP()->getState();
+        o_ref    = getFrameOther()->getO()->getState();
+        p_target = getCapture()->getFrame()->getP()->getState();
+        o_target = getCapture()->getFrame()->getO()->getState();
+    }
+    // LANDMARK CASE
+    else if (not getLandmarkOtherList().empty())
+    {
+        p_ref    = getCapture()->getFrame()->getP()->getState();
+        o_ref    = getCapture()->getFrame()->getO()->getState();
+        p_target = getLandmarkOther()->getP()->getState();
+        o_target = getLandmarkOther()->getO()->getState();
+    }
+
+    operator() (p_ref.data(), o_ref.data(), p_target.data(), o_target.data(), p_sensor.data(), o_sensor.data(), res.data());
+
+    return res;
+}
+
 } // namespace wolf
 
 #endif
diff --git a/include/core/factor/factor_relative_pose_3d.h b/include/core/factor/factor_relative_pose_3d.h
index 6003c4467..3e6ebceb5 100644
--- a/include/core/factor/factor_relative_pose_3d.h
+++ b/include/core/factor/factor_relative_pose_3d.h
@@ -19,12 +19,6 @@
 // along with this program.  If not, see <http://www.gnu.org/licenses/>.
 //
 //--------LICENSE_END--------
-/*
- * factor_relative_pose_3d.h
- *
- *  Created on: Oct 7, 2016
- *      Author: jsola
- */
 
 #ifndef FACTOR_RELATIVE_POSE_3D_H_
 #define FACTOR_RELATIVE_POSE_3D_H_
@@ -48,20 +42,27 @@ class FactorRelativePose3d : public FactorAutodiff<FactorRelativePose3d,6,3,4,3,
                              const FactorTopology& _top,
                              FactorStatus _status = FAC_ACTIVE);
 
+        FactorRelativePose3d(const FeatureBasePtr& _ftr_current_ptr,
+                             const LandmarkBasePtr& _landmark_ptr,
+                             const ProcessorBasePtr& _processor_ptr,
+                             bool _apply_loss_function,
+                             const FactorTopology& _top,
+                             FactorStatus _status = FAC_ACTIVE);
+
         ~FactorRelativePose3d() override = default;
 
         template<typename T>
-                bool operator ()(const T* const _p_current,
-                                 const T* const _q_current,
-                                 const T* const _p_past,
-                                 const T* const _q_past,
+                bool operator ()(const T* const _p_target,
+                                 const T* const _q_target,
+                                 const T* const _p_ref,
+                                 const T* const _q_ref,
                                  T* _residuals) const;
 
         template<typename T>
-                bool expectation(const T* const _p_current,
-                                 const T* const _q_current,
-                                 const T* const _p_past,
-                                 const T* const _q_past,
+                bool expectation(const T* const _p_target,
+                                 const T* const _q_target,
+                                 const T* const _p_ref,
+                                 const T* const _q_ref,
                                  T* _expectation_dp,
                                  T* _expectation_dq) const;
 
@@ -118,39 +119,64 @@ inline FactorRelativePose3d::FactorRelativePose3d(const FeatureBasePtr& _ftr_cur
     MatrixSizeCheck<6,6>::check(_ftr_current_ptr->getMeasurementSquareRootInformationUpper());
 }
 
+inline FactorRelativePose3d::FactorRelativePose3d(const FeatureBasePtr& _ftr_current_ptr,
+                                                  const LandmarkBasePtr& _lmk_ptr,
+                                                  const ProcessorBasePtr& _processor_ptr,
+                                                  bool _apply_loss_function,
+                                                  const FactorTopology& _top,
+                                                  FactorStatus _status) :
+        FactorAutodiff<FactorRelativePose3d, 6, 3, 4, 3, 4>("FactorRelativePose3d",     // type
+                                                            _top, // topology
+                                                            _ftr_current_ptr,   // feature
+                                                            nullptr,            // frame other
+                                                            nullptr,            // capture other
+                                                            nullptr,            // feature other
+                                                            _lmk_ptr,           // landmark other
+                                                            _processor_ptr,     // processor
+                                                            _apply_loss_function,
+                                                            _status,
+                                                            _lmk_ptr->getP(), // landmark P
+                                                            _lmk_ptr->getO(), // landmark Q
+                                                            _ftr_current_ptr->getFrame()->getP(), // current frame P
+                                                            _ftr_current_ptr->getFrame()->getO()) // current frame Q
+{
+    MatrixSizeCheck<7,1>::check(_ftr_current_ptr->getMeasurement());
+    MatrixSizeCheck<6,6>::check(_ftr_current_ptr->getMeasurementSquareRootInformationUpper());
+}
+
 template<typename T>
-inline bool FactorRelativePose3d::expectation(const T* const _p_current,
-                                      const T* const _q_current,
-                                      const T* const _p_past,
-                                      const T* const _q_past,
-                                      T* _expectation_dp,
-                                      T* _expectation_dq) const
+inline bool FactorRelativePose3d::expectation(const T* const _p_target,
+                                              const T* const _q_target,
+                                              const T* const _p_ref,
+                                              const T* const _q_ref,
+                                              T* _expectation_dp,
+                                              T* _expectation_dq) const
 {
-    Eigen::Map<const Eigen::Matrix<T,3,1> > p_current(_p_current);
-    Eigen::Map<const Eigen::Quaternion<T> > q_current(_q_current);
-    Eigen::Map<const Eigen::Matrix<T,3,1> > p_past(_p_past);
-    Eigen::Map<const Eigen::Quaternion<T> > q_past(_q_past);
+    Eigen::Map<const Eigen::Matrix<T,3,1> > p_target(_p_target);
+    Eigen::Map<const Eigen::Quaternion<T> > q_target(_q_target);
+    Eigen::Map<const Eigen::Matrix<T,3,1> > p_ref(_p_ref);
+    Eigen::Map<const Eigen::Quaternion<T> > q_ref(_q_ref);
     Eigen::Map<Eigen::Matrix<T,3,1> > expectation_dp(_expectation_dp);
     Eigen::Map<Eigen::Quaternion<T> > expectation_dq(_expectation_dq);
 
-//     std::cout << "p_current: " << p_current(0) << " "
-//                                << p_current(1) << " "
-//                                << p_current(2) << "\n";
-//     std::cout << "q_current: " << q_current.coeffs()(0) << " "
-//                                << q_current.coeffs()(1) << " "
-//                                << q_current.coeffs()(2) << " "
-//                                << q_current.coeffs()(3) << "\n";
-//     std::cout << "p_past: " << p_past(0) << " "
-//                             << p_past(1) << " "
-//                             << p_past(2) << "\n";
-//     std::cout << "q_past: " << q_past.coeffs()(0) << " "
-//                             << q_past.coeffs()(1) << " "
-//                             << q_past.coeffs()(2) << " "
-//                             << q_past.coeffs()(3) << "\n";
+//     std::cout << "p_target: " << p_target(0) << " "
+//                               << p_target(1) << " "
+//                               << p_target(2) << "\n";
+//     std::cout << "q_target: " << q_target.coeffs()(0) << " "
+//                               << q_target.coeffs()(1) << " "
+//                               << q_target.coeffs()(2) << " "
+//                               << q_target.coeffs()(3) << "\n";
+//     std::cout << "p_ref: " << p_ref(0) << " "
+//                            << p_ref(1) << " "
+//                            << p_ref(2) << "\n";
+//     std::cout << "q_ref: " << q_ref.coeffs()(0) << " "
+//                            << q_ref.coeffs()(1) << " "
+//                            << q_ref.coeffs()(2) << " "
+//                            << q_ref.coeffs()(3) << "\n";
 
     // estimate motion increment, dp, dq;
-    expectation_dp = q_past.conjugate() * (p_current - p_past);
-    expectation_dq = q_past.conjugate() * q_current;
+    expectation_dp = q_ref.conjugate() * (p_target - p_ref);
+    expectation_dq = q_ref.conjugate() * q_target;
 
 //    std::cout << "exp_p: " << expectation_dp(0) << " "
 //                           << expectation_dp(1) << " "
@@ -168,29 +194,31 @@ inline Eigen::VectorXd FactorRelativePose3d::expectation() const
     Eigen::VectorXd exp(7);
     auto frm_current = getFeature()->getFrame();
     auto frm_past = getFrameOther();
-    const Eigen::VectorXd frame_current_pos  = frm_current->getP()->getState();
-    const Eigen::VectorXd frame_current_ori  = frm_current->getO()->getState();
-    const Eigen::VectorXd frame_past_pos     = frm_past->getP()->getState();
-    const Eigen::VectorXd frame_past_ori     = frm_past->getO()->getState();
-
-//    std::cout << "frame_current_pos: " << frm_current->getP()->getState().transpose() << std::endl;
-//    std::cout << "frame_past_pos: " << frm_past->getP()->getState().transpose() << std::endl;
-
-    expectation(frame_current_pos.data(),
-                frame_current_ori.data(),
-                frame_past_pos.data(),
-                frame_past_ori.data(),
+    auto lmk = getLandmarkOther();
+
+    const Eigen::VectorXd target_pos  = ( frm_past ? frm_current->getP()->getState() : lmk->getP()->getState());
+    const Eigen::VectorXd target_ori  = ( frm_past ? frm_current->getO()->getState() : lmk->getO()->getState());
+    const Eigen::VectorXd ref_pos     = ( frm_past ? frm_past->getP()->getState() : frm_current->getP()->getState());
+    const Eigen::VectorXd ref_ori     = ( frm_past ? frm_past->getO()->getState() : frm_current->getO()->getState());
+
+//    std::cout << "target_pos: " << frm_current->getP()->getState().transpose() << std::endl;
+//    std::cout << "ref_pos: " << frm_past->getP()->getState().transpose() << std::endl;
+
+    expectation(target_pos.data(),
+                target_ori.data(),
+                ref_pos.data(),
+                ref_ori.data(),
                 exp.data(),
                 exp.data()+3);
     return exp;
 }
 
 template<typename T>
-inline bool FactorRelativePose3d::operator ()(const T* const _p_current,
-                                      const T* const _q_current,
-                                      const T* const _p_past,
-                                      const T* const _q_past,
-                                      T* _residuals) const
+inline bool FactorRelativePose3d::operator ()(const T* const _p_target,
+                                              const T* const _q_target,
+                                              const T* const _p_ref,
+                                              const T* const _q_ref,
+                                              T* _residuals) const
 {
 
     /* Residual expression
@@ -232,7 +260,7 @@ inline bool FactorRelativePose3d::operator ()(const T* const _p_current,
     Eigen::Map<Eigen::Matrix<T,6,1> > residuals(_residuals);
 
     Eigen::Matrix<T, Eigen::Dynamic, 1> expected(7) ;
-    expectation(_p_current, _q_current, _p_past, _q_past, expected.data(), expected.data()+3);
+    expectation(_p_target, _q_target, _p_ref, _q_ref, expected.data(), expected.data()+3);
 
     // measured motion increment, dp_m, dq_m
     // Eigen::Matrix<T,3,1> dp_m = getMeasurement().head<3>().cast<T>();
diff --git a/include/core/factor/factor_relative_pose_3d_with_extrinsics.h b/include/core/factor/factor_relative_pose_3d_with_extrinsics.h
index a72d9683a..0204203f3 100644
--- a/include/core/factor/factor_relative_pose_3d_with_extrinsics.h
+++ b/include/core/factor/factor_relative_pose_3d_with_extrinsics.h
@@ -211,9 +211,9 @@ inline bool FactorRelativePose3dWithExtrinsics::operator ()(const T* const _p_re
 inline Eigen::Vector6d FactorRelativePose3dWithExtrinsics::residual() const
 {
     Eigen::Vector6d res;
-    Eigen::VectorXd p_camera, o_camera, p_ref, o_ref, p_target, o_target;
-    p_camera = getCapture()->getSensorP()->getState();
-    o_camera = getCapture()->getSensorO()->getState();
+    Eigen::VectorXd p_sensor, o_sensor, p_ref, o_ref, p_target, o_target;
+    p_sensor = getCapture()->getSensorP()->getState();
+    o_sensor = getCapture()->getSensorO()->getState();
     // FRAME CASE
     if (not getFrameOtherList().empty())
     {
@@ -231,7 +231,7 @@ inline Eigen::Vector6d FactorRelativePose3dWithExtrinsics::residual() const
         o_target = getLandmarkOther()->getO()->getState();
     }
 
-    operator() (p_ref.data(), o_ref.data(), p_target.data(), o_target.data(), p_camera.data(), o_camera.data(), res.data());
+    operator() (p_ref.data(), o_ref.data(), p_target.data(), o_target.data(), p_sensor.data(), o_sensor.data(), res.data());
 
     return res;
 }
diff --git a/include/core/factor/factor_relative_position_2d.h b/include/core/factor/factor_relative_position_2d.h
index 8c52033c6..8a2c7a0e5 100644
--- a/include/core/factor/factor_relative_position_2d.h
+++ b/include/core/factor/factor_relative_position_2d.h
@@ -255,6 +255,4 @@ inline void FactorRelativePosition2d::evaluatePureJacobians(std::vector<Eigen::M
                      cos(getStateBlockPtrVector()[1]->getState()(0))).finished();
 }
 
-} // namespace wolf
-
-#endif
+} // namespace wolf
\ No newline at end of file
diff --git a/include/core/factor/factor_relative_position_2d_with_extrinsics copy.h b/include/core/factor/factor_relative_position_2d_with_extrinsics.h
similarity index 99%
rename from include/core/factor/factor_relative_position_2d_with_extrinsics copy.h
rename to include/core/factor/factor_relative_position_2d_with_extrinsics.h
index 5e62f6e00..fd36867f4 100644
--- a/include/core/factor/factor_relative_position_2d_with_extrinsics copy.h	
+++ b/include/core/factor/factor_relative_position_2d_with_extrinsics.h
@@ -125,6 +125,4 @@ inline bool FactorRelativePosition2dWithExtrinsics::operator ()(const T* const _
     return true;
 }
 
-} // namespace wolf
-
-#endif
+} // namespace wolf
\ No newline at end of file
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index 6fa50395a..1aaf3bc58 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -173,6 +173,12 @@ 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)
 
+# FactorRelativePosition2d class test
+wolf_add_gtest(gtest_factor_relative_position_2d gtest_factor_relative_position_2d.cpp)
+
+# FactorRelativePosition2dWithExtrinsics class test
+wolf_add_gtest(gtest_factor_relative_position_2d_with_extrinsics gtest_factor_relative_position_2d_with_extrinsics.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_2d.cpp b/test/gtest_factor_relative_pose_2d.cpp
index cc4030a9e..e31716188 100644
--- a/test/gtest_factor_relative_pose_2d.cpp
+++ b/test/gtest_factor_relative_pose_2d.cpp
@@ -25,6 +25,8 @@
 #include "core/factor/factor_relative_pose_2d.h"
 #include "core/capture/capture_odom_2d.h"
 #include "core/math/rotations.h"
+#include "core/state_block/state_block_derived.h"
+#include "core/state_block/state_angle.h"
 
 
 using namespace Eigen;
@@ -32,6 +34,12 @@ using namespace wolf;
 using std::cout;
 using std::endl;
 
+
+int N = 1e2;
+
+// Vectors
+Vector3d delta, x_0, x_1, x_f, x_l;
+
 // Input odometry data and covariance
 Matrix3d data_cov = 0.1*Matrix3d::Identity();
 
@@ -43,93 +51,164 @@ SolverCeres solver(problem_ptr);
 FrameBasePtr frm0 = problem_ptr->emplaceFrame(0.0, Vector3d::Zero());
 FrameBasePtr frm1 = problem_ptr->emplaceFrame(1.0, Vector3d::Zero());
 
+// Landmark
+LandmarkBasePtr lmk = LandmarkBase::emplace<LandmarkBase>(problem_ptr->getMap(), 
+                                                          "LandmarkPose2d",
+                                                          std::make_shared<StatePoint2d>(Vector2d::Zero()),
+                                                          std::make_shared<StateAngle>(Vector1d::Zero()));
+
 // Capture from frm1 to frm0
 auto cap1 = CaptureBase::emplace<CaptureOdom2d>(frm1, 1, nullptr, Vector3d::Zero(), data_cov);
+// Feature
+FeatureBasePtr fea = nullptr;
+// Factor
+FactorRelativePose2dPtr fac = nullptr;
 
+void generateRandomProblemFrame()
+{
+    // Random delta
+    delta = Vector3d::Random() * 1e2;
+    delta(2) = pi2pi(delta(2));
+
+    // Random frame 0 pose
+    x_0 = Vector3d::Random() * 1e2;
+    x_0(2) = pi2pi(x_0(2));
+
+    // Frame 1 pose
+    x_1(2) = pi2pi(x_0(2) + delta(2));
+    x_1.head<2>() = x_0.head<2>() + Rotation2Dd(x_0(2)) * delta.head<2>();
+
+    // Set states
+    frm0->setState(x_0);
+    frm1->setState(x_1);
+    cap1->setData(delta);
+
+    // feature & factor with delta measurement
+    fea = FeatureBase::emplace<FeatureBase>(cap1, "FeatureOdom2d", delta, data_cov);
+    fac = FactorBase::emplace<FactorRelativePose2d>(fea, fea, frm0, nullptr, false, TOP_MOTION); // create and add
+}
+
+void generateRandomProblemLandmark()
+{
+    // Random delta
+    delta = Vector3d::Random() * 1e2;
+    delta(2) = pi2pi(delta(2));
+
+    // Random frame 0 pose
+    x_f = Vector3d::Random() * 1e2;
+    x_f(2) = pi2pi(x_f(2));
+
+    // landmark pose
+    x_l(2) = pi2pi(x_f(2) + delta(2));
+    x_l.head<2>() = x_f.head<2>() + Rotation2Dd(x_f(2)) * delta.head<2>();
+
+    // Set states
+    frm1->setState(x_f);
+    lmk->setState(x_l);
+    cap1->setData(delta);
+
+    // feature & factor with delta measurement
+    fea = FeatureBase::emplace<FeatureBase>(cap1, "FeatureLandmarkPose2d", delta, data_cov);
+    fac = FactorBase::emplace<FactorRelativePose2d>(fea, fea, lmk, nullptr, false, TOP_LMK); // create and add
+}
+
+////////////// TESTS /////////////////////////////////////////////////////////////////////
 TEST(FactorRelativePose2d, check_tree)
 {
     ASSERT_TRUE(problem_ptr->check(0));
 }
 
-TEST(FactorRelativePose2d, fix_0_solve)
+// FRAME TO FRAME =========================================================================
+TEST(FactorRelativePose2d, frame_solve_frame1)
 {
-    for (int i = 0; i < 1e3; i++)
+    for (int i = 0; i < N; i++)
     {
-        // Random delta
-        Vector3d delta = Vector3d::Random() * 10;
-        pi2pi(delta(2));
-
-        // Random initial pose
-        Vector3d x0 = Vector3d::Random() * 10;
-        pi2pi(x0(2));
-
-        // x1 groundtruth
-        Vector3d x1;
-        x1.head<2>() = x0.head<2>() + Rotation2Dd(x0(2)) * delta.head<2>();
-        x1(2) = pi2pi(x0(2) + delta(2));
-
-        // Set states and measurement
-        frm0->setState(x0);
-        frm1->setState(x1);
-        cap1->setData(delta);
+        // random setup
+        generateRandomProblemFrame();
 
-        // feature & factor with delta measurement
-        auto fea1 = FeatureBase::emplace<FeatureBase>(cap1, "FeatureOdom2d", delta, data_cov);
-        FactorBase::emplace<FactorRelativePose2d>(fea1, fea1, frm0, nullptr, false, TOP_MOTION);
-
-        // Fix frm0, perturb frm1
+        // Perturb frm1, fix the rest
         frm0->fix();
         frm1->unfix();
-        frm1->perturb(5);
+        frm1->perturb(1);
 
         // solve for frm1
-        std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
+        std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+        //WOLF_INFO(report);
 
-        ASSERT_POSE2d_APPROX(frm1->getStateVector(), x1, 1e-6);
+        ASSERT_POSE2d_APPROX(frm1->getStateVector(), x_1, 1e-3);
 
         // remove feature (and factor) for the next loop
-        fea1->remove();
+        fea->remove();
     }
 }
 
-TEST(FactorRelativePose2d, fix_1_solve)
+TEST(FactorRelativePose2d, frame_solve_frame0)
 {
-    for (int i = 0; i < 1e3; i++)
+    for (int i = 0; i < N; i++)
     {
-        // Random delta
-        Vector3d delta = Vector3d::Random() * 10;
-        pi2pi(delta(2));
-
-        // Random initial pose
-        Vector3d x0 = Vector3d::Random() * 10;
-        pi2pi(x0(2));
+        // random setup
+        generateRandomProblemFrame();
+        
+        // Perturb frm0, fix the rest
+        frm1->fix();
+        frm0->unfix();
+        frm0->perturb(1);
 
-        // x1 groundtruth
-        Vector3d x1;
-        x1.head<2>() = x0.head<2>() + Rotation2Dd(x0(2)) * delta.head<2>();
-        x1(2) = pi2pi(x0(2) + delta(2));
+        // solve for frm0
+        std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+        //WOLF_INFO(report);
 
-        // Set states and measurement
-        frm0->setState(x0);
-        frm1->setState(x1);
-        cap1->setData(delta);
+        ASSERT_POSE2d_APPROX(frm0->getStateVector(), x_0, 1e-3);
 
-        // feature & factor with delta measurement
-        auto fea1 = FeatureBase::emplace<FeatureBase>(cap1, "FeatureOdom2d", delta, data_cov);
-        FactorBase::emplace<FactorRelativePose2d>(fea1, fea1, frm0, nullptr, false, TOP_MOTION);
+        // remove feature (and factor) for the next loop
+        fea->remove();
+    }
+}
 
-        // Fix frm1, perturb frm0
+// FRAME TO LANDMARK =========================================================================
+TEST(FactorRelativePose2d, landmark_solve_lmk)
+{
+    for (int i = 0; i < N; i++)
+    {
+        // random setup
+        generateRandomProblemLandmark();
+        
+        // Perturb landmark, fix the rest
         frm1->fix();
-        frm0->unfix();
-        frm0->perturb(5);
+        lmk->unfix();
+        lmk->perturb(1);
+
+        // solve for landmark
+        std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+        //WOLF_INFO(report);
+
+        ASSERT_POSE2d_APPROX(lmk->getStateVector(), x_l, 1e-3);
+
+        // remove feature (and factor) for the next loop
+        fea->remove();
+    }
+}
+
+TEST(FactorRelativePose2d, landmark_solve_frame)
+{
+    for (int i = 0; i < N; i++)
+    {
+        // random setup
+        generateRandomProblemLandmark();
+        
+        // Perturb frm0, fix the rest
+        frm1->unfix();
+        lmk->fix();
+        frm1->perturb(1);
 
         // solve for frm0
-        std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
+        std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+        //WOLF_INFO(report);
 
-        ASSERT_POSE2d_APPROX(frm0->getStateVector(), x0, 1e-6);
+        ASSERT_POSE2d_APPROX(frm1->getStateVector(), x_f, 1e-3);
 
         // remove feature (and factor) for the next loop
-        fea1->remove();
+        fea->remove();
     }
 }
 
diff --git a/test/gtest_factor_relative_pose_2d_with_extrinsics.cpp b/test/gtest_factor_relative_pose_2d_with_extrinsics.cpp
index 76744a478..a10190e0f 100644
--- a/test/gtest_factor_relative_pose_2d_with_extrinsics.cpp
+++ b/test/gtest_factor_relative_pose_2d_with_extrinsics.cpp
@@ -37,7 +37,10 @@ using std::endl;
 
 std::string wolf_root = _WOLF_ROOT_DIR;
 
-int N = 1;//1e2
+int N = 1e2;
+
+// Vectors
+Vector3d delta, x_0, x_1, x_f, x_l, x_s;
 
 // Input odometry data and covariance
 Matrix3d data_cov = 0.1*Matrix3d::Identity();
@@ -47,7 +50,7 @@ ProblemPtr problem_ptr = Problem::create("PO", 2);
 SolverCeres solver(problem_ptr);
 
 // Sensor
-auto sensor_odom2d = problem_ptr->installSensor("SensorOdom2d", "odom", Vector3d(0,0,0), wolf_root + "/test/yaml/sensor_odom_2d.yaml");
+auto sensor = problem_ptr->installSensor("SensorOdom2d", "odom", Vector3d(0,0,0), wolf_root + "/test/yaml/sensor_odom_2d.yaml");
 
 // Two frames
 FrameBasePtr frm0 = problem_ptr->emplaceFrame(0, Vector3d::Zero() );
@@ -60,7 +63,71 @@ LandmarkBasePtr lmk = LandmarkBase::emplace<LandmarkBase>(problem_ptr->getMap(),
                                                           std::make_shared<StateAngle>(Vector1d::Zero()));
 
 // Capture
-auto cap1 = CaptureBase::emplace<CaptureOdom2d>(frm1, 1, sensor_odom2d, Vector3d::Zero(), data_cov);
+auto cap1 = CaptureBase::emplace<CaptureOdom2d>(frm1, 1, sensor, Vector3d::Zero(), data_cov);
+// Feature
+FeatureBasePtr fea = nullptr;
+// Factor
+FactorRelativePose2dWithExtrinsicsPtr fac = nullptr;
+
+void generateRandomProblemFrame()
+{
+    // Random delta
+    delta = Vector3d::Random() * 1e2;
+    delta(2) = pi2pi(delta(2));
+
+    // Random frame 0 pose
+    x_0 = Vector3d::Random() * 1e2;
+    x_0(2) = pi2pi(x_0(2));
+
+    // Random extrinsics
+    x_s = Vector3d::Random() * 1e2;
+    x_s(2) = pi2pi(x_s(2));
+
+    // Frame 1 pose
+    x_1(2) = pi2pi(x_0(2) + delta(2));
+    x_1.head<2>() = x_0.head<2>() + Rotation2Dd(x_0(2)) * (x_s.head<2>() + Rotation2Dd(x_s(2)) * delta.head<2>()) - Rotation2Dd(x_1(2)) * x_s.head<2>();
+
+    // Set states
+    frm0->setState(x_0);
+    frm1->setState(x_1);
+    cap1->setData(delta);
+    sensor->setState(x_s);
+
+    // feature & factor with delta measurement
+    fea = FeatureBase::emplace<FeatureBase>(cap1, "FeatureOdom2d", delta, data_cov);
+    fac = FactorBase::emplace<FactorRelativePose2dWithExtrinsics>(fea, fea, frm0, nullptr, false, TOP_MOTION); // create and add
+}
+
+void generateRandomProblemLandmark()
+{
+    // Random delta
+    delta = Vector3d::Random() * 1e2;
+    delta(2) = pi2pi(delta(2));
+
+    // Random frame pose
+    x_f = Vector3d::Random() * 1e2;
+    x_f(2) = pi2pi(x_f(2));
+
+    // Random extrinsics
+    x_s = Vector3d::Random() * 1e2;
+    x_s(2) = pi2pi(x_s(2));
+
+    // landmark pose
+    x_l(2) = pi2pi(x_f(2) + delta(2));
+    x_l.head<2>() = x_f.head<2>() + Rotation2Dd(x_f(2)) * (x_s.head<2>() + Rotation2Dd(x_s(2)) * delta.head<2>());
+
+    // Set states
+    frm1->setState(x_f);
+    lmk->setState(x_l);
+    cap1->setData(delta);
+    sensor->setState(x_s);
+
+    // feature & factor with delta measurement
+    fea = FeatureBase::emplace<FeatureBase>(cap1, "FeatureLandmarkPose2d", delta, data_cov);
+    fac = FactorBase::emplace<FactorRelativePose2dWithExtrinsics>(fea, fea, lmk, nullptr, false, TOP_LMK); // create and add
+}
+
+////////////// TESTS /////////////////////////////////////////////////////////////////////
 
 TEST(FactorRelativePose2dWithExtrinsics, check_tree)
 {
@@ -72,47 +139,26 @@ TEST(FactorRelativePose2dWithExtrinsics, frame_solve_frame1)
 {
     for (int i = 0; i < N; i++)
     {
-        // Random delta
-        Vector3d delta = Vector3d::Random() * 10;
-        pi2pi(delta(2));
-
-        // Random initial pose
-        Vector3d x0 = Vector3d::Random() * 10;
-        pi2pi(x0(2));
-
-        // Random extrinsics
-        Vector3d xs = Vector3d::Random();
-        pi2pi(xs(2));
-
-        // x1 groundtruth
-        Vector3d x1;
-        x1(2) = pi2pi(x0(2) + delta(2));
-        x1.head<2>() = x0.head<2>() + Rotation2Dd(x0(2)) * (xs.head<2>() + Rotation2Dd(xs(2)) * delta.head<2>()) - Rotation2Dd(x1(2)) * xs.head<2>();
-
-        // Set states
-        frm0->setState(x0);
-        frm1->setState(x1);
-        cap1->setData(delta);
-        sensor_odom2d->setState(xs);
+        // random setup
+        generateRandomProblemFrame();
 
-        // feature & factor with delta measurement
-        auto fea1 = FeatureBase::emplace<FeatureBase>(cap1, "FeatureOdom2d", delta, data_cov);
-        FactorBase::emplace<FactorRelativePose2dWithExtrinsics>(fea1, fea1, frm0, nullptr, false, TOP_MOTION); // create and add
+        ASSERT_MATRIX_APPROX(fac->residual(), VectorXd::Zero(3), Constants::EPS);
 
         // Perturb frm1, fix the rest
         frm0->fix();
         frm1->unfix();
-        sensor_odom2d->getP()->fix();
-        sensor_odom2d->getO()->fix();
-        frm1->perturb(5);
+        sensor->getP()->fix();
+        sensor->getO()->fix();
+        frm1->perturb(1);
 
         // solve for frm1
-        std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
+        std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+        //WOLF_INFO(report);
 
-        ASSERT_POSE2d_APPROX(frm1->getStateVector(), x1, 1e-6);
+        ASSERT_POSE2d_APPROX(frm1->getStateVector(), x_1, 1e-3);
 
         // remove feature (and factor) for the next loop
-        fea1->remove();
+        fea->remove();
     }
 }
 
@@ -120,95 +166,53 @@ TEST(FactorRelativePose2dWithExtrinsics, frame_solve_frame0)
 {
     for (int i = 0; i < N; i++)
     {
-        // Random delta
-        Vector3d delta = Vector3d::Random() * 10;
-        pi2pi(delta(2));
-
-        // Random initial pose
-        Vector3d x0 = Vector3d::Random() * 10;
-        pi2pi(x0(2));
-
-        // Random extrinsics
-        Vector3d xs = Vector3d::Random();
-        pi2pi(xs(2));
-
-        // x1 groundtruth
-        Vector3d x1;
-        x1(2) = pi2pi(x0(2) + delta(2));
-        x1.head<2>() = x0.head<2>() + Rotation2Dd(x0(2)) * (xs.head<2>() + Rotation2Dd(xs(2)) * delta.head<2>()) - Rotation2Dd(x1(2)) * xs.head<2>();
-
-        // Set states
-        frm0->setState(x0);
-        frm1->setState(x1);
-        cap1->setData(delta);
-        sensor_odom2d->setState(xs);
-
-        // feature & factor with delta measurement
-        auto fea1 = FeatureBase::emplace<FeatureBase>(cap1, "FeatureOdom2d", delta, data_cov);
-        FactorBase::emplace<FactorRelativePose2dWithExtrinsics>(fea1, fea1, frm0, nullptr, false, TOP_MOTION); // create and add
+        // random setup
+        generateRandomProblemFrame();
+        
+        ASSERT_MATRIX_APPROX(fac->residual(), VectorXd::Zero(3), Constants::EPS);
 
         // Perturb frm0, fix the rest
         frm1->fix();
         frm0->unfix();
-        sensor_odom2d->getP()->fix();
-        sensor_odom2d->getO()->fix();
-        frm0->perturb(5);
+        sensor->getP()->fix();
+        sensor->getO()->fix();
+        frm0->perturb(1);
 
         // solve for frm0
-        std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
+        std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+        //WOLF_INFO(report);
 
-        ASSERT_POSE2d_APPROX(frm0->getStateVector(), x0, 1e-6);
+        ASSERT_POSE2d_APPROX(frm0->getStateVector(), x_0, 1e-3);
 
         // remove feature (and factor) for the next loop
-        fea1->remove();
+        fea->remove();
     }
 }
 
+// JV: The position extrinsics in case of pair of frames is not observable
 TEST(FactorRelativePose2dWithExtrinsics, frame_solve_extrinsics_p)
 {
     for (int i = 0; i < N; i++)
     {
-        // Random delta
-        Vector3d delta = Vector3d::Random() * 10;
-        pi2pi(delta(2));
-
-        // Random initial pose
-        Vector3d x0 = Vector3d::Random() * 10;
-        pi2pi(x0(2));
-
-        // Random extrinsics
-        Vector3d xs = Vector3d::Random();
-        pi2pi(xs(2));
-
-        // x1 groundtruth
-        Vector3d x1;
-        x1(2) = pi2pi(x0(2) + delta(2));
-        x1.head<2>() = x0.head<2>() + Rotation2Dd(x0(2)) * (xs.head<2>() + Rotation2Dd(xs(2)) * delta.head<2>()) - Rotation2Dd(x1(2)) * xs.head<2>();
-
-        // Set states
-        frm0->setState(x0);
-        frm1->setState(x1);
-        cap1->setData(delta);
-        sensor_odom2d->setState(xs);
-
-        // feature & factor with delta measurement
-        auto fea1 = FeatureBase::emplace<FeatureBase>(cap1, "FeatureOdom2d", delta, data_cov);
-        FactorBase::emplace<FactorRelativePose2dWithExtrinsics>(fea1, fea1, frm0, nullptr, false, TOP_MOTION); // create and add
+        // random setup
+        generateRandomProblemFrame();
+        
+        ASSERT_MATRIX_APPROX(fac->residual(), VectorXd::Zero(3), Constants::EPS);
 
         // Perturb sensor P, fix the rest
         frm1->fix();
         frm0->fix();
-        sensor_odom2d->getP()->unfix();
-        sensor_odom2d->getO()->fix();
-        sensor_odom2d->getP()->perturb(1);
+        sensor->getP()->unfix();
+        sensor->getO()->fix();
+        sensor->getP()->perturb(1);
 
         // solve for frm0
-        std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
+        std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
 
-        ASSERT_POSE2d_APPROX(sensor_odom2d->getStateVector(), xs, 1e-6);
+        //ASSERT_POSE2d_APPROX(sensor->getStateVector(), x_s, 1e-3);
 
         // remove feature (and factor) for the next loop
-        fea1->remove();
+        fea->remove();
     }
 }
 
@@ -216,49 +220,26 @@ TEST(FactorRelativePose2dWithExtrinsics, frame_solve_extrinsics_o)
 {
     for (int i = 0; i < N; i++)
     {
-        // Random delta
-        Vector3d delta = Vector3d::Random() * 10;
-        pi2pi(delta(2));
-
-        // Random initial pose
-        Vector3d x0 = Vector3d::Random() * 10;
-        pi2pi(x0(2));
-
-        // Random extrinsics
-        Vector3d xs = Vector3d::Random();
-        pi2pi(xs(2));
-
-        // x1 groundtruth
-        Vector3d x1;
-        x1(2) = pi2pi(x0(2) + delta(2));
-        x1.head<2>() = x0.head<2>() + Rotation2Dd(x0(2)) * (xs.head<2>() + Rotation2Dd(xs(2)) * delta.head<2>()) - Rotation2Dd(x1(2)) * xs.head<2>();
-
-        // Set states
-        frm0->setState(x0);
-        frm1->setState(x1);
-        cap1->setData(delta);
-        sensor_odom2d->setState(xs);
-
-        // feature & factor with delta measurement
-        auto fea1 = FeatureBase::emplace<FeatureBase>(cap1, "FeatureOdom2d", delta, data_cov);
-        FactorBase::emplace<FactorRelativePose2dWithExtrinsics>(fea1, fea1, frm0, nullptr, false, TOP_MOTION); // create and add
+        // random setup
+        generateRandomProblemFrame();
+        
+        ASSERT_MATRIX_APPROX(fac->residual(), VectorXd::Zero(3), Constants::EPS);
 
         // Perturb sensor O, fix the rest
         frm1->fix();
         frm0->fix();
-        sensor_odom2d->getP()->fix();
-        sensor_odom2d->getO()->unfix();
-        sensor_odom2d->getO()->perturb(.2);
-
-        //std::cout << "Sensor O (perturbed): " << sensor_odom2d->getO()->getState().transpose() << std::endl;
+        sensor->getP()->fix();
+        sensor->getO()->unfix();
+        sensor->getO()->perturb(.2);
 
         // solve for frm0
-        std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
+        std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+        //WOLF_INFO(report);
 
-        ASSERT_POSE2d_APPROX(sensor_odom2d->getStateVector(), xs, 1e-6);
+        ASSERT_POSE2d_APPROX(sensor->getStateVector(), x_s, 1e-3);
 
         // remove feature (and factor) for the next loop
-        fea1->remove();
+        fea->remove();
     }
 }
 
@@ -267,47 +248,26 @@ TEST(FactorRelativePose2dWithExtrinsics, landmark_solve_lmk)
 {
     for (int i = 0; i < N; i++)
     {
-        // Random delta
-        Vector3d delta = Vector3d::Random() * 10;
-        pi2pi(delta(2));
-
-        // Random frame pose
-        Vector3d x_f = Vector3d::Random() * 10;
-        pi2pi(x_f(2));
-
-        // Random extrinsics
-        Vector3d xs = Vector3d::Random();
-        pi2pi(xs(2));
-
-        // landmark pose
-        Vector3d x_l;
-        x_l(2) = pi2pi(x_f(2) + delta(2));
-        x_l.head<2>() = x_f.head<2>() + Rotation2Dd(x_f(2)) * (xs.head<2>() + Rotation2Dd(xs(2)) * delta.head<2>());
-
-        // Set states
-        frm1->setState(x_f);
-        lmk->setState(x_l);
-        cap1->setData(delta);
-        sensor_odom2d->setState(xs);
-
-        // feature & factor with delta measurement
-        auto fea1 = FeatureBase::emplace<FeatureBase>(cap1, "FeatureLandmarkPose", delta, data_cov);
-        FactorBase::emplace<FactorRelativePose2dWithExtrinsics>(fea1, fea1, lmk, nullptr, false, TOP_LMK); // create and add
+        // random setup
+        generateRandomProblemLandmark();
+        
+        ASSERT_MATRIX_APPROX(fac->residual(), VectorXd::Zero(3), Constants::EPS);
 
         // Perturb landmark, fix the rest
         frm1->fix();
         lmk->unfix();
-        sensor_odom2d->getP()->fix();
-        sensor_odom2d->getO()->fix();
-        lmk->perturb(5);
+        sensor->getP()->fix();
+        sensor->getO()->fix();
+        lmk->perturb(1);
 
         // solve for landmark
-        std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
+        std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+        //WOLF_INFO(report);
 
-        ASSERT_POSE2d_APPROX(lmk->getStateVector(), x_l, 1e-6);
+        ASSERT_POSE2d_APPROX(lmk->getStateVector(), x_l, 1e-3);
 
         // remove feature (and factor) for the next loop
-        fea1->remove();
+        fea->remove();
     }
 }
 
@@ -315,47 +275,26 @@ TEST(FactorRelativePose2dWithExtrinsics, landmark_solve_frame)
 {
     for (int i = 0; i < N; i++)
     {
-        // Random delta
-        Vector3d delta = Vector3d::Random() * 10;
-        pi2pi(delta(2));
-
-        // Random frame pose
-        Vector3d x_f = Vector3d::Random() * 10;
-        pi2pi(x_f(2));
-
-        // Random extrinsics
-        Vector3d xs = Vector3d::Random();
-        pi2pi(xs(2));
-
-        // landmark pose
-        Vector3d x_l;
-        x_l(2) = pi2pi(x_f(2) + delta(2));
-        x_l.head<2>() = x_f.head<2>() + Rotation2Dd(x_f(2)) * (xs.head<2>() + Rotation2Dd(xs(2)) * delta.head<2>());
-
-        // Set states
-        frm1->setState(x_f);
-        lmk->setState(x_l);
-        cap1->setData(delta);
-        sensor_odom2d->setState(xs);
-
-        // feature & factor with delta measurement
-        auto fea1 = FeatureBase::emplace<FeatureBase>(cap1, "FeatureLandmarkPose", delta, data_cov);
-        FactorBase::emplace<FactorRelativePose2dWithExtrinsics>(fea1, fea1, lmk, nullptr, false, TOP_LMK); // create and add
+        // random setup
+        generateRandomProblemLandmark();
+        
+        ASSERT_MATRIX_APPROX(fac->residual(), VectorXd::Zero(3), Constants::EPS);
 
         // Perturb frm0, fix the rest
         frm1->unfix();
         lmk->fix();
-        sensor_odom2d->getP()->fix();
-        sensor_odom2d->getO()->fix();
-        frm1->perturb(5);
+        sensor->getP()->fix();
+        sensor->getO()->fix();
+        frm1->perturb(1);
 
         // solve for frm0
-        std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
+        std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+        //WOLF_INFO(report);
 
-        ASSERT_POSE2d_APPROX(frm1->getStateVector(), x_f, 1e-6);
+        ASSERT_POSE2d_APPROX(frm1->getStateVector(), x_f, 1e-3);
 
         // remove feature (and factor) for the next loop
-        fea1->remove();
+        fea->remove();
     }
 }
 
@@ -363,47 +302,26 @@ TEST(FactorRelativePose2dWithExtrinsics, landmark_extrinsics_p_solve)
 {
     for (int i = 0; i < N; i++)
     {
-        // Random delta
-        Vector3d delta = Vector3d::Random() * 10;
-        pi2pi(delta(2));
-
-        // Random frame pose
-        Vector3d x_f = Vector3d::Random() * 10;
-        pi2pi(x_f(2));
-
-        // Random extrinsics
-        Vector3d xs = Vector3d::Random();
-        pi2pi(xs(2));
-
-        // landmark pose
-        Vector3d x_l;
-        x_l(2) = pi2pi(x_f(2) + delta(2));
-        x_l.head<2>() = x_f.head<2>() + Rotation2Dd(x_f(2)) * (xs.head<2>() + Rotation2Dd(xs(2)) * delta.head<2>());
-
-        // Set states
-        frm1->setState(x_f);
-        lmk->setState(x_l);
-        cap1->setData(delta);
-        sensor_odom2d->setState(xs);
-
-        // feature & factor with delta measurement
-        auto fea1 = FeatureBase::emplace<FeatureBase>(cap1, "FeatureLandmarkPose", delta, data_cov);
-        FactorBase::emplace<FactorRelativePose2dWithExtrinsics>(fea1, fea1, lmk, nullptr, false, TOP_LMK); // create and add
+        // random setup
+        generateRandomProblemLandmark();
+        
+        ASSERT_MATRIX_APPROX(fac->residual(), VectorXd::Zero(3), Constants::EPS);
 
         // Perturb sensor P, fix the rest
         frm1->fix();
         lmk->fix();
-        sensor_odom2d->getP()->unfix();
-        sensor_odom2d->getO()->fix();
-        sensor_odom2d->getP()->perturb(1);
+        sensor->getP()->unfix();
+        sensor->getO()->fix();
+        sensor->getP()->perturb(1);
 
         // solve for frm0
-        std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
+        std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+        //WOLF_INFO(report);
 
-        ASSERT_POSE2d_APPROX(sensor_odom2d->getStateVector(), xs, 1e-6);
+        ASSERT_POSE2d_APPROX(sensor->getStateVector(), x_s, 1e-3);
 
         // remove feature (and factor) for the next loop
-        fea1->remove();
+        fea->remove();
     }
 }
 
@@ -411,49 +329,28 @@ TEST(FactorRelativePose2dWithExtrinsics, landmark_extrinsics_o_solve)
 {
     for (int i = 0; i < N; i++)
     {
-        // Random delta
-        Vector3d delta = Vector3d::Random() * 10;
-        pi2pi(delta(2));
-
-        // Random frame pose
-        Vector3d x_f = Vector3d::Random() * 10;
-        pi2pi(x_f(2));
-
-        // Random extrinsics
-        Vector3d xs = Vector3d::Random();
-        pi2pi(xs(2));
-
-        // landmark pose
-        Vector3d x_l;
-        x_l(2) = pi2pi(x_f(2) + delta(2));
-        x_l.head<2>() = x_f.head<2>() + Rotation2Dd(x_f(2)) * (xs.head<2>() + Rotation2Dd(xs(2)) * delta.head<2>());
-
-        // Set states
-        frm1->setState(x_f);
-        lmk->setState(x_l);
-        cap1->setData(delta);
-        sensor_odom2d->setState(xs);
-
-        // feature & factor with delta measurement
-        auto fea1 = FeatureBase::emplace<FeatureBase>(cap1, "FeatureLandmarkPose", delta, data_cov);
-        FactorBase::emplace<FactorRelativePose2dWithExtrinsics>(fea1, fea1, lmk, nullptr, false, TOP_LMK); // create and add
+        // random setup
+        generateRandomProblemLandmark();
+        
+        ASSERT_MATRIX_APPROX(fac->residual(), VectorXd::Zero(3), Constants::EPS);
 
         // Perturb sensor O, fix the rest
         frm1->fix();
         lmk->fix();
-        sensor_odom2d->getP()->fix();
-        sensor_odom2d->getO()->unfix();
-        sensor_odom2d->getO()->perturb(.2);
+        sensor->getP()->fix();
+        sensor->getO()->unfix();
+        sensor->getO()->perturb(.2);
 
-        //std::cout << "Sensor O (perturbed): " << sensor_odom2d->getO()->getState().transpose() << std::endl;
+        //std::cout << "Sensor O (perturbed): " << sensor->getO()->getState().transpose() << std::endl;
 
         // solve for frm0
-        std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
+        std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+        //WOLF_INFO(report);
 
-        ASSERT_POSE2d_APPROX(sensor_odom2d->getStateVector(), xs, 1e-6);
+        ASSERT_POSE2d_APPROX(sensor->getStateVector(), x_s, 1e-3);
 
         // remove feature (and factor) for the next loop
-        fea1->remove();
+        fea->remove();
     }
 }
 
diff --git a/test/gtest_factor_relative_pose_3d.cpp b/test/gtest_factor_relative_pose_3d.cpp
index 9369c090e..00c75bec4 100644
--- a/test/gtest_factor_relative_pose_3d.cpp
+++ b/test/gtest_factor_relative_pose_3d.cpp
@@ -30,7 +30,9 @@
 
 #include "core/ceres_wrapper/solver_ceres.h"
 #include "core/factor/factor_relative_pose_3d.h"
-#include "core/capture/capture_motion.h"
+#include "core/capture/capture_odom_3d.h"
+#include "core/state_block/state_block_derived.h"
+#include "core/state_block/state_quaternion.h"
 
 
 using namespace Eigen;
@@ -38,72 +40,189 @@ using namespace wolf;
 using std::cout;
 using std::endl;
 
-Vector7d data2delta(Vector6d _data)
-{
-    return (Vector7d() << _data.head<3>() , v2q(_data.tail<3>()).coeffs()).finished();
-}
+std::string wolf_root = _WOLF_ROOT_DIR;
 
-// Input odometry data and covariance
-Vector6d data(Vector6d::Random());
-Vector7d delta = data2delta(data);
-Vector6d data_var((Vector6d() << 0.2,0.2,0.2,0.2,0.2,0.2).finished());
-Matrix6d data_cov = data_var.asDiagonal();
+int N = 1e2;
 
-// perturbated priors
-Vector7d x0 = data2delta(Vector6d::Random()*0.25);
-Vector7d x1 = data2delta(data + Vector6d::Random()*0.25);
+// Vectors
+Vector7d delta, x_0, x_1, x_f, x_l;
+
+// 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);
 
 // Two frames
-FrameBasePtr frm0 = problem_ptr->emplaceFrame(TimeStamp(0), problem_ptr->stateZero());
-FrameBasePtr frm1 = problem_ptr->emplaceFrame(TimeStamp(1), delta);
-
-// Capture, feature and factor from frm1 to frm0
-auto cap1 = CaptureBase::emplace<CaptureMotion>(frm1, "CaptureOdom3d", 1, nullptr, delta, nullptr);
-auto fea1 = FeatureBase::emplace<FeatureBase>(cap1, "FeatureOdom3d", delta, data_cov);
-FactorRelativePose3dPtr ctr1 = FactorBase::emplace<FactorRelativePose3d>(fea1, fea1, frm0, nullptr, false, TOP_MOTION); // create and add
+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, nullptr, Vector7d::Zero(), data_cov);
+// Feature
+FeatureBasePtr fea = nullptr;
+// Factor
+FactorRelativePose3dPtr fac = nullptr;
 
 ////////////////////////////////////////////////////////
 
-TEST(FactorRelativePose3d, check_tree)
+void generateRandomProblemFrame()
 {
-    ASSERT_TRUE(problem_ptr->check(0));
+    // 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>());
+
+    // Frame 1 pose
+    x_1.head<3>() = x_0.head<3>() + q_0 * delta.head<3>();
+    x_1.tail<4>() = (q_0 * q_delta).coeffs();
+
+    // Set states
+    frm0->setState(x_0);
+    frm1->setState(x_1);
+    cap1->setData(delta);
+
+    // feature & factor with delta measurement
+    fea = FeatureBase::emplace<FeatureBase>(cap1, "FeatureOdom3d", delta, data_cov);
+    fac = FactorBase::emplace<FactorRelativePose3d>(fea, fea, frm0, nullptr, false, TOP_MOTION); // create and add
 }
 
-TEST(FactorRelativePose3d, expectation)
+void generateRandomProblemLandmark()
 {
-    ASSERT_MATRIX_APPROX(ctr1->expectation() , delta, 1e-6);
+    // 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>());
+
+    // Landmark pose
+    x_l.head<3>() = x_f.head<3>() + q_f * delta.head<3>();
+    x_l.tail<4>() = (q_f * q_delta).coeffs();
+
+    // Set states
+    frm1->setState(x_f);
+    lmk->setState(x_l);
+    cap1->setData(delta);
+
+    // feature & factor with delta measurement
+    fea = FeatureBase::emplace<FeatureBase>(cap1, "FeatureLandmarkPose", delta, data_cov);
+    fac = FactorBase::emplace<FactorRelativePose3d>(fea, fea, lmk, nullptr, false, TOP_LMK); // create and add
 }
 
-TEST(FactorRelativePose3d, fix_0_solve)
+TEST(FactorRelativePose3d, check_tree)
+{
+    ASSERT_TRUE(problem_ptr->check(0));
+}
+
+// FRAME TO FRAME =========================================================================
+TEST(FactorRelativePose3d, frame_solve_frame1)
 {
+    for (int i = 0; i < N; i++)
+    {
+        // random setup
+        generateRandomProblemFrame();
 
-    // Fix frame 0, perturb frm1
-    frm0->fix();
-    frm1->unfix();
-    frm1->setState(x1);
+        // Perturb frm1, fix the rest
+        frm0->fix();
+        frm1->unfix();
+        frm1->perturb(1);
 
-    // solve for frm1
-    std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
+        // solve for frm1
+        std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+        //WOLF_INFO(report);
 
-    ASSERT_MATRIX_APPROX(frm1->getStateVector(), delta, 1e-6);
+        ASSERT_POSE3d_APPROX(frm1->getStateVector(), x_1, 1e-3);
 
+        // remove feature (and factor) for the next loop
+        fea->remove();
+    }
 }
 
-TEST(FactorRelativePose3d, fix_1_solve)
+TEST(FactorRelativePose3d, frame_solve_frame0)
 {
-    // Fix frame 1, perturb frm0
-    frm0->unfix();
-    frm1->fix();
-    frm0->setState(x0);
+    for (int i = 0; i < N; i++)
+    {
+        // random setup
+        generateRandomProblemFrame();
+        
+        // Perturb frm0, fix the rest
+        frm1->fix();
+        frm0->unfix();
+        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();
+    }
+}
 
-    // solve for frm0
-    std::string brief_report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
+// FRAME TO LANDMARK =========================================================================
+TEST(FactorRelativePose3d, landmark_solve_lmk)
+{
+    for (int i = 0; i < N; i++)
+    {
+        // random setup
+        generateRandomProblemLandmark();
+        
+        // Perturb landmark, fix the rest
+        frm1->fix();
+        lmk->unfix();
+        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();
+    }
+}
 
-    ASSERT_MATRIX_APPROX(frm0->getStateVector(), problem_ptr->stateZero().vector("PO"), 1e-6);
+TEST(FactorRelativePose3d, landmark_solve_frame)
+{
+    for (int i = 0; i < N; i++)
+    {
+        // random setup
+        generateRandomProblemLandmark();
+        
+        // Perturb frm0, fix the rest
+        frm1->unfix();
+        lmk->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();
+    }
 }
 
 int main(int argc, char **argv)
diff --git a/test/gtest_factor_relative_pose_3d_with_extrinsics.cpp b/test/gtest_factor_relative_pose_3d_with_extrinsics.cpp
index 54f4fc527..2e99b1d2b 100644
--- a/test/gtest_factor_relative_pose_3d_with_extrinsics.cpp
+++ b/test/gtest_factor_relative_pose_3d_with_extrinsics.cpp
@@ -1,378 +1,371 @@
-    //--------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()
+//--------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 = 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, 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>());
+
+    // 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->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->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 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
-    }
+        // random setup
+        generateRandomProblemFrame();
 
-    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
-    }
+        ASSERT_MATRIX_APPROX(fac->residual(), VectorXd::Zero(6), Constants::EPS);
 
-    TEST(FactorRelativePose3dWithExtrinsics, check_tree)
-    {
-        ASSERT_TRUE(problem_ptr->check(0));
-    }
+        // Perturb frm1, fix the rest
+        frm0->fix();
+        frm1->unfix();
+        sensor->getP()->fix();
+        sensor->getO()->fix();
+        frm1->perturb(1);
 
-    // FRAME TO FRAME =========================================================================
-    TEST(FactorRelativePose3dWithExtrinsics, frame_solve_frame1)
-    {
-        for (int i = 0; i < N; i++)
-        {
-            // random setup
-            generateRandomProblemFrame();
+        // solve for frm1
+        std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+        //WOLF_INFO(report);
 
-            ASSERT_MATRIX_APPROX(fac->residual(), VectorXd::Zero(6), Constants::EPS);
+        ASSERT_POSE3d_APPROX(frm1->getStateVector(), x_1, 1e-3);
 
-            // 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();
-        }
+        // remove feature (and factor) for the next loop
+        fea->remove();
     }
+}
 
-    TEST(FactorRelativePose3dWithExtrinsics, frame_solve_frame0)
+TEST(FactorRelativePose3dWithExtrinsics, frame_solve_frame0)
+{
+    for (int i = 0; i < N; i++)
     {
-        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();
-        }
+        // random setup
+        generateRandomProblemFrame();
+        
+        ASSERT_MATRIX_APPROX(fac->residual(), VectorXd::Zero(6), Constants::EPS);
+
+        // Perturb frm0, fix the rest
+        frm1->fix();
+        frm0->unfix();
+        sensor->getP()->fix();
+        sensor->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)
+// JV: The position extrinsics in case of pair of frames is not observable
+TEST(FactorRelativePose3dWithExtrinsics, frame_solve_extrinsics_p)
+{
+    for (int i = 0; i < N; i++)
     {
-        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();
-        }
+        // random setup
+        generateRandomProblemFrame();
+        
+        ASSERT_MATRIX_APPROX(fac->residual(), VectorXd::Zero(6), Constants::EPS);
+
+        // Perturb sensor P, fix the rest
+        frm1->fix();
+        frm0->fix();
+        sensor->getP()->unfix();
+        sensor->getO()->fix();
+        sensor->getP()->perturb(1);
+
+        // solve for frm0
+        std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+        //WOLF_INFO(report);
+
+        //ASSERT_POSE3d_APPROX(sensor->getStateVector(), x_s, 1e-3);
+
+        // remove feature (and factor) for the next loop
+        fea->remove();
     }
+}
 
-    TEST(FactorRelativePose3dWithExtrinsics, frame_solve_extrinsics_o)
+TEST(FactorRelativePose3dWithExtrinsics, frame_solve_extrinsics_o)
+{
+    for (int i = 0; i < N; i++)
     {
-        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();
-        }
+        // random setup
+        generateRandomProblemFrame();
+        
+        ASSERT_MATRIX_APPROX(fac->residual(), VectorXd::Zero(6), Constants::EPS);
+
+        // Perturb sensor O, fix the rest
+        frm1->fix();
+        frm0->fix();
+        sensor->getP()->fix();
+        sensor->getO()->unfix();
+        sensor->getO()->perturb(.2);
+
+        // solve for frm0
+        std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+        //WOLF_INFO(report);
+
+        ASSERT_POSE3d_APPROX(sensor->getStateVector(), x_s, 1e-3);
+
+        // remove feature (and factor) for the next loop
+        fea->remove();
     }
+}
 
-    // FRAME TO LANDMARK =========================================================================
-    TEST(FactorRelativePose3dWithExtrinsics, landmark_solve_lmk)
+// FRAME TO LANDMARK =========================================================================
+TEST(FactorRelativePose3dWithExtrinsics, landmark_solve_lmk)
+{
+    for (int i = 0; i < N; i++)
     {
-        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();
-        }
+        // random setup
+        generateRandomProblemLandmark();
+        
+        ASSERT_MATRIX_APPROX(fac->residual(), VectorXd::Zero(6), Constants::EPS);
+
+        // Perturb landmark, fix the rest
+        frm1->fix();
+        lmk->unfix();
+        sensor->getP()->fix();
+        sensor->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)
+TEST(FactorRelativePose3dWithExtrinsics, landmark_solve_frame)
+{
+    for (int i = 0; i < N; i++)
     {
-        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();
-        }
+        // random setup
+        generateRandomProblemLandmark();
+        
+        ASSERT_MATRIX_APPROX(fac->residual(), VectorXd::Zero(6), Constants::EPS);
+
+        // Perturb frm0, fix the rest
+        frm1->unfix();
+        lmk->fix();
+        sensor->getP()->fix();
+        sensor->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)
+TEST(FactorRelativePose3dWithExtrinsics, landmark_extrinsics_p_solve)
+{
+    for (int i = 0; i < N; i++)
     {
-        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();
-        }
+        // random setup
+        generateRandomProblemLandmark();
+        
+        ASSERT_MATRIX_APPROX(fac->residual(), VectorXd::Zero(6), Constants::EPS);
+
+        // Perturb sensor P, fix the rest
+        frm1->fix();
+        lmk->fix();
+        sensor->getP()->unfix();
+        sensor->getO()->fix();
+        sensor->getP()->perturb(1);
+
+        // solve for frm0
+        std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+        //WOLF_INFO(report);
+
+        ASSERT_POSE3d_APPROX(sensor->getStateVector(), x_s, 1e-3);
+
+        // remove feature (and factor) for the next loop
+        fea->remove();
     }
+}
 
-    TEST(FactorRelativePose3dWithExtrinsics, landmark_extrinsics_o_solve)
+TEST(FactorRelativePose3dWithExtrinsics, landmark_extrinsics_o_solve)
+{
+    for (int i = 0; i < N; i++)
     {
-        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();
-        }
+        // random setup
+        generateRandomProblemLandmark();
+        
+        ASSERT_MATRIX_APPROX(fac->residual(), VectorXd::Zero(6), Constants::EPS);
+
+        // Perturb sensor O, fix the rest
+        frm1->fix();
+        lmk->fix();
+        sensor->getP()->fix();
+        sensor->getO()->unfix();
+        sensor->getO()->perturb(.2);
+
+        // solve for frm0
+        std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+        //WOLF_INFO(report);
+
+        ASSERT_POSE3d_APPROX(sensor->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();
-    }
+int main(int argc, char **argv)
+{
+testing::InitGoogleTest(&argc, argv);
+return RUN_ALL_TESTS();
+}
diff --git a/test/gtest_factor_relative_position_2d.cpp b/test/gtest_factor_relative_position_2d.cpp
new file mode 100644
index 000000000..e91d0fb43
--- /dev/null
+++ b/test/gtest_factor_relative_position_2d.cpp
@@ -0,0 +1,268 @@
+//--------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_position_2d.h"
+#include "core/capture/capture_void.h"
+#include "core/sensor/sensor_odom_2d.h"
+#include "core/math/rotations.h"
+#include "core/state_block/state_block_derived.h"
+#include "core/state_block/state_angle.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
+Vector3d x_0, x_f, x_1;
+Vector2d delta, x_l;
+
+// Input odometry data and covariance
+Matrix2d data_cov = 0.1*Matrix2d::Identity();
+
+// Problem and solver
+ProblemPtr problem_ptr = Problem::create("PO", 2);
+SolverCeres solver(problem_ptr);
+
+// Frames
+FrameBasePtr frm0 = problem_ptr->emplaceFrame(0, Vector3d::Zero() );
+FrameBasePtr frm1 = problem_ptr->emplaceFrame(1, Vector3d::Zero() );
+
+// Landmark
+LandmarkBasePtr lmk = LandmarkBase::emplace<LandmarkBase>(problem_ptr->getMap(), 
+                                                          "LandmarkPosition2d",
+                                                          std::make_shared<StatePoint2d>(Vector2d::Zero()),
+                                                          nullptr);
+
+// Capture
+auto cap = CaptureBase::emplace<CaptureVoid>(frm1, 1, nullptr);
+// Feature
+FeatureBasePtr fea = nullptr;
+// Factor
+FactorRelativePosition2dPtr fac = nullptr;
+
+void generateRandomProblemFrame()
+{
+    // Random delta
+    delta = Vector2d::Random() * 1e2;
+
+    // Random frame 0 pose
+    x_0 = Vector3d::Random() * 1e2;
+    x_0(2) = pi2pi(x_0(2));
+
+    // Frame 1 position
+    x_1.head<2>() = x_0.head<2>() + Rotation2Dd(x_0(2)) * delta;
+
+    // Set states
+    frm0->setState(x_0);
+    frm1->setState(x_1);
+
+    // feature & factor with delta measurement
+    fea = FeatureBase::emplace<FeatureBase>(cap, "FeatureFramePosition2d", delta, data_cov);
+    fac = FactorBase::emplace<FactorRelativePosition2d>(fea, fea, frm0, nullptr, false, TOP_MOTION);
+}
+
+void generateRandomProblemLandmark()
+{
+    // Random delta
+    delta = Vector2d::Random() * 1e2;
+
+    // Random frame pose
+    x_f = Vector3d::Random() * 1e2;
+    x_f(2) = pi2pi(x_f(2));
+
+    // landmark position
+    x_l = x_f.head<2>() + Rotation2Dd(x_f(2)) * delta;
+
+    // Set states
+    frm1->setState(x_f);
+    lmk->setState(x_l);
+
+    // feature & factor with delta measurement
+    fea = FeatureBase::emplace<FeatureBase>(cap, "FeatureLandmarkPosition2d", delta, data_cov);
+    fac = FactorBase::emplace<FactorRelativePosition2d>(fea, fea, lmk, nullptr, false, TOP_LMK);
+}
+
+////////////// TESTS /////////////////////////////////////////////////////////////////////
+
+TEST(FactorRelativePosition2d, check_tree)
+{
+    ASSERT_TRUE(problem_ptr->check(0));
+}
+
+// FRAME TO FRAME =========================================================================
+TEST(FactorRelativePosition2d, frame_solve_frame1_p)
+{
+    for (int i = 0; i < N; i++)
+    {
+        // random setup
+        generateRandomProblemFrame();
+
+        // Perturb frm1, fix the rest
+        frm0->fix();
+        frm1->getP()->unfix();
+        frm1->getO()->fix();
+        frm1->getP()->perturb(1);
+
+        // solve for frm1
+        std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+        //WOLF_INFO(report);
+
+        ASSERT_POSE2d_APPROX(frm1->getStateVector(), x_1, 1e-3);
+
+        // remove feature (and factor) for the next loop
+        fea->remove();
+    }
+}
+
+TEST(FactorRelativePosition2d, frame_solve_frame0_p)
+{
+    for (int i = 0; i < N; i++)
+    {
+        // random setup
+        generateRandomProblemFrame();
+        
+        // Perturb frm0, fix the rest
+        frm1->fix();
+        frm0->getP()->unfix();
+        frm0->getO()->fix();
+        frm0->getP()->perturb(1);
+
+        // solve for frm0
+        std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+        //WOLF_INFO(report);
+
+        ASSERT_POSE2d_APPROX(frm0->getStateVector(), x_0, 1e-3);
+
+        // remove feature (and factor) for the next loop
+        fea->remove();
+    }
+}
+
+TEST(FactorRelativePosition2d, frame_solve_frame0_o)
+{
+    for (int i = 0; i < N; i++)
+    {
+        // random setup
+        generateRandomProblemFrame();
+        
+        // Perturb frm0, fix the rest
+        frm1->fix();
+        frm0->getP()->fix();
+        frm0->getO()->unfix();
+        frm0->getO()->perturb(1);
+
+        // solve for frm0
+        std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+        //WOLF_INFO(report);
+
+        ASSERT_POSE2d_APPROX(frm0->getStateVector(), x_0, 1e-3);
+
+        // remove feature (and factor) for the next loop
+        fea->remove();
+    }
+}
+
+// FRAME TO LANDMARK =========================================================================
+TEST(FactorRelativePosition2d, landmark_solve_lmk)
+{
+    for (int i = 0; i < N; i++)
+    {
+        // random setup
+        generateRandomProblemLandmark();
+        
+        // Perturb landmark, fix the rest
+        frm1->fix();
+        lmk->unfix();
+        lmk->perturb(1);
+
+        // solve for landmark
+        std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+        //WOLF_INFO(report);
+
+        ASSERT_MATRIX_APPROX(lmk->getStateVector(), x_l, 1e-3);
+
+        // remove feature (and factor) for the next loop
+        fea->remove();
+    }
+}
+
+TEST(FactorRelativePosition2d, landmark_solve_frame_p)
+{
+    for (int i = 0; i < N; i++)
+    {
+        // random setup
+        generateRandomProblemLandmark();
+        
+        // Perturb frm0, fix the rest
+        frm1->getP()->unfix();
+        frm1->getO()->fix();
+        lmk->fix();
+        frm1->getP()->perturb(1);
+
+        // solve for frm0
+        std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+        //WOLF_INFO(report);
+
+        ASSERT_POSE2d_APPROX(frm1->getStateVector(), x_f, 1e-3);
+
+        // remove feature (and factor) for the next loop
+        fea->remove();
+    }
+}
+
+TEST(FactorRelativePosition2d, landmark_solve_frame_o)
+{
+    for (int i = 0; i < N; i++)
+    {
+        // random setup
+        generateRandomProblemLandmark();
+        
+        // Perturb frm0, fix the rest
+        frm1->getP()->fix();
+        frm1->getO()->unfix();
+        lmk->fix();
+        frm1->getO()->perturb(1);
+
+        // solve for frm0
+        std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+        //WOLF_INFO(report);
+
+        ASSERT_POSE2d_APPROX(frm1->getStateVector(), x_f, 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_position_2d_with_extrinsics.cpp b/test/gtest_factor_relative_position_2d_with_extrinsics.cpp
new file mode 100644
index 000000000..87bb8f070
--- /dev/null
+++ b/test/gtest_factor_relative_position_2d_with_extrinsics.cpp
@@ -0,0 +1,239 @@
+//--------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_position_2d_with_extrinsics.h"
+#include "core/capture/capture_void.h"
+#include "core/sensor/sensor_odom_2d.h"
+#include "core/math/rotations.h"
+#include "core/state_block/state_block_derived.h"
+#include "core/state_block/state_angle.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
+Vector3d x_f, x_s;
+Vector2d delta, x_l;
+
+// Input odometry data and covariance
+Matrix2d data_cov = 0.1*Matrix2d::Identity();
+
+// Problem and solver
+ProblemPtr problem_ptr = Problem::create("PO", 2);
+SolverCeres solver(problem_ptr);
+
+// Sensor
+auto sensor = problem_ptr->installSensor("SensorOdom2d", "odom", Vector3d(0,0,0), wolf_root + "/test/yaml/sensor_odom_2d.yaml");
+
+// Frame
+FrameBasePtr frm = problem_ptr->emplaceFrame(0, Vector3d::Zero() );
+
+// Landmark
+LandmarkBasePtr lmk = LandmarkBase::emplace<LandmarkBase>(problem_ptr->getMap(), 
+                                                          "LandmarkPosition2d",
+                                                          std::make_shared<StatePoint2d>(Vector2d::Zero()),
+                                                          nullptr);
+
+// Capture
+auto cap = CaptureBase::emplace<CaptureVoid>(frm, 1, sensor);
+// Feature
+FeatureBasePtr fea = nullptr;
+// Factor
+FactorRelativePosition2dWithExtrinsicsPtr fac = nullptr;
+
+void generateRandomProblemLandmark()
+{
+    // Random delta
+    delta = Vector2d::Random() * 1e2;
+
+    // Random frame pose
+    x_f = Vector3d::Random() * 1e2;
+    x_f(2) = pi2pi(x_f(2));
+
+    // Random extrinsics
+    x_s = Vector3d::Random() * 1e2;
+    x_s(2) = pi2pi(x_s(2));
+
+    // landmark position
+    x_l = x_f.head<2>() + Rotation2Dd(x_f(2)) * (x_s.head<2>() + Rotation2Dd(x_s(2)) * delta);
+
+    // Set states
+    frm->setState(x_f);
+    lmk->setState(x_l);
+    sensor->setState(x_s);
+
+    // feature & factor with delta measurement
+    fea = FeatureBase::emplace<FeatureBase>(cap, "FeatureLandmarkPosition2d", delta, data_cov);
+    fac = FactorBase::emplace<FactorRelativePosition2dWithExtrinsics>(fea, fea, lmk, nullptr, false, TOP_LMK);
+}
+
+////////////// TESTS /////////////////////////////////////////////////////////////////////
+
+TEST(FactorRelativePosition2dWithExtrinsics, check_tree)
+{
+    ASSERT_TRUE(problem_ptr->check(0));
+}
+
+// FRAME TO LANDMARK =========================================================================
+TEST(FactorRelativePosition2dWithExtrinsics, landmark_solve_lmk)
+{
+    for (int i = 0; i < N; i++)
+    {
+        // random setup
+        generateRandomProblemLandmark();
+        
+        // Perturb landmark, fix the rest
+        frm->fix();
+        lmk->unfix();
+        sensor->getP()->fix();
+        sensor->getO()->fix();
+        lmk->perturb(1);
+
+        // solve for landmark
+        std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+        //WOLF_INFO(report);
+
+        ASSERT_MATRIX_APPROX(lmk->getStateVector(), x_l, 1e-3);
+
+        // remove feature (and factor) for the next loop
+        fea->remove();
+    }
+}
+
+TEST(FactorRelativePosition2dWithExtrinsics, landmark_solve_frame_p)
+{
+    for (int i = 0; i < N; i++)
+    {
+        // random setup
+        generateRandomProblemLandmark();
+        
+        // Perturb frm0, fix the rest
+        frm->getP()->unfix();
+        frm->getO()->fix();
+        lmk->fix();
+        sensor->getP()->fix();
+        sensor->getO()->fix();
+        frm->getP()->perturb(1);
+
+        // solve for frm0
+        std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+        //WOLF_INFO(report);
+
+        ASSERT_POSE2d_APPROX(frm->getStateVector(), x_f, 1e-3);
+
+        // remove feature (and factor) for the next loop
+        fea->remove();
+    }
+}
+
+TEST(FactorRelativePosition2dWithExtrinsics, landmark_solve_frame_o)
+{
+    for (int i = 0; i < N; i++)
+    {
+        // random setup
+        generateRandomProblemLandmark();
+        
+        // Perturb frm0, fix the rest
+        frm->getP()->fix();
+        frm->getO()->unfix();
+        lmk->fix();
+        sensor->getP()->fix();
+        sensor->getO()->fix();
+        frm->getO()->perturb(1);
+
+        // solve for frm0
+        std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+        //WOLF_INFO(report);
+
+        ASSERT_POSE2d_APPROX(frm->getStateVector(), x_f, 1e-3);
+
+        // remove feature (and factor) for the next loop
+        fea->remove();
+    }
+}
+
+TEST(FactorRelativePosition2dWithExtrinsics, landmark_extrinsics_p_solve)
+{
+    for (int i = 0; i < N; i++)
+    {
+        // random setup
+        generateRandomProblemLandmark();
+        
+        // Perturb sensor P, fix the rest
+        frm->fix();
+        lmk->fix();
+        sensor->getP()->unfix();
+        sensor->getO()->fix();
+        sensor->getP()->perturb(1);
+
+        // solve for frm0
+        std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+        //WOLF_INFO(report);
+
+        ASSERT_POSE2d_APPROX(sensor->getStateVector(), x_s, 1e-3);
+
+        // remove feature (and factor) for the next loop
+        fea->remove();
+    }
+}
+
+TEST(FactorRelativePosition2dWithExtrinsics, landmark_extrinsics_o_solve)
+{
+    for (int i = 0; i < N; i++)
+    {
+        // random setup
+        generateRandomProblemLandmark();
+        
+        // Perturb sensor O, fix the rest
+        frm->fix();
+        lmk->fix();
+        sensor->getP()->fix();
+        sensor->getO()->unfix();
+        sensor->getO()->perturb(.2);
+
+        //std::cout << "Sensor O (perturbed): " << sensor->getO()->getState().transpose() << std::endl;
+
+        // solve for frm0
+        std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+        //WOLF_INFO(report);
+
+        ASSERT_POSE2d_APPROX(sensor->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();
+}
-- 
GitLab