diff --git a/CMakeLists.txt b/CMakeLists.txt
index 4d5f0121057eda48060b18b4e625f3ed5fe56f10..01bb44f1c27e19e25777a4a6978c47a6c835bd04 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -134,11 +134,15 @@ SET(HDRS_FACTOR
   include/${PROJECT_NAME}/factor/factor_pose_2d.h
   include/${PROJECT_NAME}/factor/factor_pose_3d.h
   include/${PROJECT_NAME}/factor/factor_quaternion_absolute.h
+  include/${PROJECT_NAME}/factor/factor_pose_3d_with_extrinsics.h
   include/${PROJECT_NAME}/factor/factor_relative_pose_2d.h
   include/${PROJECT_NAME}/factor/factor_relative_pose_2d_with_extrinsics.h
   include/${PROJECT_NAME}/factor/factor_relative_pose_3d.h
-  include/${PROJECT_NAME}/factor/factor_pose_3d_with_extrinsics.h
   include/${PROJECT_NAME}/factor/factor_relative_pose_3d_with_extrinsics.h
+  include/${PROJECT_NAME}/factor/factor_relative_position_2d.h
+  include/${PROJECT_NAME}/factor/factor_relative_position_2d_with_extrinsics.h
+  include/${PROJECT_NAME}/factor/factor_relative_position_3d.h
+  include/${PROJECT_NAME}/factor/factor_relative_position_3d_with_extrinsics.h
   include/${PROJECT_NAME}/factor/factor_velocity_local_direction_3d.h
   )
 SET(HDRS_FEATURE
diff --git a/include/core/factor/factor_relative_pose_3d.h b/include/core/factor/factor_relative_pose_3d.h
index 3e6ebceb5354aed9f5ac67c0c5fb561ade536c89..4ea300d5c13d35aed9ff1c60ba57c822cd4386c6 100644
--- a/include/core/factor/factor_relative_pose_3d.h
+++ b/include/core/factor/factor_relative_pose_3d.h
@@ -66,7 +66,7 @@ class FactorRelativePose3d : public FactorAutodiff<FactorRelativePose3d,6,3,4,3,
                                  T* _expectation_dp,
                                  T* _expectation_dq) const;
 
-        Eigen::VectorXd expectation() const;
+        Eigen::Vector7d expectation() const;
 
     private:
         template<typename T>
@@ -189,9 +189,9 @@ inline bool FactorRelativePose3d::expectation(const T* const _p_target,
     return true;
 }
 
-inline Eigen::VectorXd FactorRelativePose3d::expectation() const
+inline Eigen::Vector7d FactorRelativePose3d::expectation() const
 {
-    Eigen::VectorXd exp(7);
+    Eigen::Vector7d exp;
     auto frm_current = getFeature()->getFrame();
     auto frm_past = getFrameOther();
     auto lmk = getLandmarkOther();
@@ -201,8 +201,10 @@ inline Eigen::VectorXd FactorRelativePose3d::expectation() const
     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;
+//    std::cout << "target_pos: " << target_pos.transpose() << std::endl;
+//    std::cout << "target_ori: " << target_ori.coeffs().transpose() << std::endl;
+//    std::cout << "ref_pos: " << ref_pos.transpose() << std::endl;
+//    std::cout << "ref_ori: " << ref_ori.coeffs().transpose() << std::endl;
 
     expectation(target_pos.data(),
                 target_ori.data(),
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 680c619c3622092e4e0631fb6c74c594794ce70d..66468bbcafb2b4480df5077ca6b2a16d6ff61839 100644
--- a/include/core/factor/factor_relative_pose_3d_with_extrinsics.h
+++ b/include/core/factor/factor_relative_pose_3d_with_extrinsics.h
@@ -58,7 +58,7 @@ class FactorRelativePose3dWithExtrinsics : public FactorAutodiff<FactorRelativeP
 
         /** \brief Class Destructor
          */
-        ~FactorRelativePose3dWithExtrinsics() override;
+        ~FactorRelativePose3dWithExtrinsics() override = default;
  
         /** brief : compute the residual from the state blocks being iterated by the solver.
          **/
@@ -116,6 +116,8 @@ inline FactorRelativePose3dWithExtrinsics::FactorRelativePose3dWithExtrinsics(co
                    _feature_ptr->getCapture()->getSensorP(),
                    _feature_ptr->getCapture()->getSensorO())
 {
+    assert(_feature_ptr->getCapture()->getSensorP() != nullptr && "No extrinsics found!");
+    assert(_feature_ptr->getCapture()->getSensorO() != nullptr && "No extrinsics found!");
 }
 
 inline FactorRelativePose3dWithExtrinsics::FactorRelativePose3dWithExtrinsics(const FeatureBasePtr& _feature_ptr,
@@ -143,11 +145,6 @@ inline FactorRelativePose3dWithExtrinsics::FactorRelativePose3dWithExtrinsics(co
 {
 }
 
-inline FactorRelativePose3dWithExtrinsics::~FactorRelativePose3dWithExtrinsics()
-{
-    //
-}
-
 template<typename T>
 inline bool FactorRelativePose3dWithExtrinsics::operator ()(const T* const _p_ref,
                                                             const T* const _o_ref,
diff --git a/include/core/factor/factor_relative_position_3d.h b/include/core/factor/factor_relative_position_3d.h
new file mode 100644
index 0000000000000000000000000000000000000000..8d76dcb3406059f37218d0ace894ec1b3f9bc474
--- /dev/null
+++ b/include/core/factor/factor_relative_position_3d.h
@@ -0,0 +1,154 @@
+//--------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--------
+
+#pragma once
+
+#include "core/factor/factor_autodiff.h"
+
+namespace wolf
+{
+
+WOLF_PTR_TYPEDEFS(FactorRelativePosition3d);
+    
+//class
+class FactorRelativePosition3d : public FactorAutodiff<FactorRelativePosition3d,3,3,4,3>
+{
+    public:
+        FactorRelativePosition3d(const FeatureBasePtr& _ftr_current_ptr,
+                                 const FrameBasePtr& _frame_past_ptr,
+                                 const ProcessorBasePtr& _processor_ptr,
+                                 bool _apply_loss_function,
+                                 const FactorTopology& _top,
+                                 FactorStatus _status = FAC_ACTIVE);
+
+        FactorRelativePosition3d(const FeatureBasePtr& _ftr_current_ptr,
+                                 const LandmarkBasePtr& _landmark_ptr,
+                                 const ProcessorBasePtr& _processor_ptr,
+                                 bool _apply_loss_function,
+                                 const FactorTopology& _top,
+                                 FactorStatus _status = FAC_ACTIVE);
+
+        ~FactorRelativePosition3d() override = default;
+
+        template<typename T>
+        bool operator ()(const T* const _p_ref,
+                         const T* const _q_ref,
+                         const T* const _p_target,
+                         T* _residuals) const;
+
+    private:
+        template<typename T>
+        void printRes(const Eigen::Matrix<T, 3, 1>& r) const;
+};
+
+template<typename T>
+inline void FactorRelativePosition3d::printRes(const Eigen::Matrix<T, 3, 1>& r) const
+{
+    std::cout << "Jet residual = " << std::endl;
+    std::cout << r(0) << std::endl;
+    std::cout << r(1) << std::endl;
+    std::cout << r(2) << std::endl;
+}
+
+template<>
+inline void FactorRelativePosition3d::printRes (const  Eigen::Matrix<double,3,1> & r) const
+{
+    std::cout << "Scalar residual = " << std::endl;
+    std::cout << r.transpose() << std::endl;
+}
+
+inline FactorRelativePosition3d::FactorRelativePosition3d(const FeatureBasePtr& _ftr_current_ptr,
+                                                          const FrameBasePtr& _frame_past_ptr,
+                                                          const ProcessorBasePtr& _processor_ptr,
+                                                          bool _apply_loss_function,
+                                                          const FactorTopology& _top,
+                                                          FactorStatus _status) :
+        FactorAutodiff("FactorRelativePosition3d",     // type
+                       _top, // topology
+                       _ftr_current_ptr,   // feature
+                       _frame_past_ptr,    // frame other
+                       nullptr,            // capture other
+                       nullptr,            // feature other
+                       nullptr,            // landmark other
+                       _processor_ptr,     // processor
+                       _apply_loss_function,
+                       _status,
+                       _frame_past_ptr->getP(), // past frame P
+                       _frame_past_ptr->getO(), // past frame Q
+                       _ftr_current_ptr->getFrame()->getP()) // current frame P
+{
+    MatrixSizeCheck<3,1>::check(_ftr_current_ptr->getMeasurement());
+    MatrixSizeCheck<3,3>::check(_ftr_current_ptr->getMeasurementSquareRootInformationUpper());
+}
+
+inline FactorRelativePosition3d::FactorRelativePosition3d(const FeatureBasePtr& _ftr_current_ptr,
+                                                  const LandmarkBasePtr& _lmk_ptr,
+                                                  const ProcessorBasePtr& _processor_ptr,
+                                                  bool _apply_loss_function,
+                                                  const FactorTopology& _top,
+                                                  FactorStatus _status) :
+        FactorAutodiff("FactorRelativePosition3d",     // 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,
+                       _ftr_current_ptr->getFrame()->getP(), // frame P
+                       _ftr_current_ptr->getFrame()->getO(), // frame Q
+                       _lmk_ptr->getP())                     // landmark P
+{
+    MatrixSizeCheck<3,1>::check(_ftr_current_ptr->getMeasurement());
+    MatrixSizeCheck<3,3>::check(_ftr_current_ptr->getMeasurementSquareRootInformationUpper());
+}
+
+template<typename T>
+inline bool FactorRelativePosition3d::operator ()(const T* const _p_ref,
+                                                  const T* const _q_ref,
+                                                  const T* const _p_target,
+                                                  T* _residuals) const
+{
+    Eigen::Map<const Eigen::Matrix<T,3,1> > p_target(_p_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> > residuals(_residuals);
+
+//     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";
+//     std::cout << "p_target: " << p_target(0) << " "
+//                               << p_target(1) << " "
+//                               << p_target(2) << "\n";
+
+    residuals = getMeasurementSquareRootInformationUpper() * (getMeasurement() - (q_ref.conjugate() * (p_target - p_ref)));
+
+    return true;
+}
+
+} /* namespace wolf */
\ No newline at end of file
diff --git a/include/core/factor/factor_relative_position_3d_with_extrinsics.h b/include/core/factor/factor_relative_position_3d_with_extrinsics.h
new file mode 100644
index 0000000000000000000000000000000000000000..f892ac25a4adcb6c31f43f76b4fc3022a62b56ae
--- /dev/null
+++ b/include/core/factor/factor_relative_position_3d_with_extrinsics.h
@@ -0,0 +1,149 @@
+//--------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--------
+#pragma once
+
+//Wolf includes
+#include "core/factor/factor_autodiff.h"
+#include "core/frame/frame_base.h"
+#include "core/math/rotations.h"
+
+//#include "ceres/jet.h"
+
+namespace wolf {
+
+WOLF_PTR_TYPEDEFS(FactorRelativePosition3dWithExtrinsics);
+
+//class
+class FactorRelativePosition3dWithExtrinsics : public FactorAutodiff<FactorRelativePosition3dWithExtrinsics, 3, 3, 4, 3, 3, 4>
+{
+    public:
+
+        /** \brief Class constructor Frame-Landmark
+         */
+        FactorRelativePosition3dWithExtrinsics(const FeatureBasePtr& _ftr_ptr,
+                                               const LandmarkBasePtr& _lmk_other_ptr,
+                                               const ProcessorBasePtr& _processor_ptr,
+                                               bool _apply_loss_function,
+                                               const FactorTopology& _top = TOP_LMK,
+                                               FactorStatus _status = FAC_ACTIVE);
+
+        ~FactorRelativePosition3dWithExtrinsics() override = default;
+
+        template<typename T>
+        bool operator ()(const T* const _p_ref,
+                         const T* const _o_ref,
+                         const T* const _p_target,
+                         const T* const _p_sensor,
+                         const T* const _o_sensor,
+                         T* _residuals) const;
+
+        Eigen::Vector3d residual() const;
+        double cost() const;
+};
+
+inline FactorRelativePosition3dWithExtrinsics::FactorRelativePosition3dWithExtrinsics(const FeatureBasePtr& _ftr_ptr,
+                                                                                      const LandmarkBasePtr& _lmk_other_ptr,
+                                                                                      const ProcessorBasePtr& _processor_ptr,
+                                                                                      bool _apply_loss_function,
+                                                                                      const FactorTopology& _top,
+                                                                                      FactorStatus _status) :
+     FactorAutodiff("FactorRelativePosition3dWithExtrinsics",
+                    _top,
+                    _ftr_ptr,
+                    nullptr, nullptr, nullptr, _lmk_other_ptr,
+                    _processor_ptr,
+                    _apply_loss_function,
+                    _status,
+                    _ftr_ptr->getFrame()->getP(),
+                    _ftr_ptr->getFrame()->getO(),
+                    _lmk_other_ptr->getP(),
+                    _ftr_ptr->getCapture()->getSensorP(),
+                    _ftr_ptr->getCapture()->getSensorO())
+{
+    assert(_ftr_ptr->getCapture()->getSensorP() != nullptr && "No extrinsics found!");
+    assert(_ftr_ptr->getCapture()->getSensorO() != nullptr && "No extrinsics found!");
+}
+
+template<typename T>
+inline bool FactorRelativePosition3dWithExtrinsics::operator ()(const T* const _p_ref,
+                                                                const T* const _o_ref,
+                                                                const T* const _p_target,
+                                                                const T* const _p_sensor,
+                                                                const T* const _o_sensor,
+                                                                T* _residuals) const
+{
+    // Maps
+    Eigen::Map<const Eigen::Matrix<T,3,1>> p_r_s(_p_sensor);
+    Eigen::Map<const Eigen::Quaternion<T>> q_r_s(_o_sensor);
+    Eigen::Map<const Eigen::Matrix<T,3,1>> p_w_r(_p_ref);
+    Eigen::Map<const Eigen::Quaternion<T>> q_w_r(_o_ref);
+    Eigen::Map<const Eigen::Matrix<T,3,1>> p_w_t(_p_target);
+    Eigen::Map<Eigen::Matrix<T,3,1>> residuals(_residuals);
+
+    // WOLF_INFO("p_sensor: ", p_r_s.transpose());
+    // WOLF_INFO("o_sensor: ", q_r_s.coeffs().transpose());
+    // WOLF_INFO("p_ref:    ", p_w_r.transpose());
+    // WOLF_INFO("o_ref:    ", q_w_r.coeffs().transpose());
+    // WOLF_INFO("p_target: ", p_w_t.transpose());
+
+    // Expected measurement
+    Eigen::Matrix<T,3,1> expected_p;
+    expected_p = q_r_s.conjugate() * (q_w_r.conjugate() * (p_w_t - p_w_r) - p_r_s);
+
+    // Residual
+    residuals = getMeasurementSquareRootInformationUpper().cast<T>() * (getMeasurement() - expected_p);
+
+    return true;
+}
+
+inline Eigen::Vector3d FactorRelativePosition3dWithExtrinsics::residual() const
+{
+    Eigen::Vector3d res;
+    Eigen::VectorXd p_sensor, o_sensor, p_ref, o_ref, p_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();
+    }
+    // LANDMARK CASE
+    else if (not getLandmarkOtherList().empty())
+    {
+        p_ref    = getCapture()->getFrame()->getP()->getState();
+        o_ref    = getCapture()->getFrame()->getO()->getState();
+        p_target = getLandmarkOther()->getP()->getState();
+    }
+
+    operator() (p_ref.data(), o_ref.data(), p_target.data(), p_sensor.data(), o_sensor.data(), res.data());
+
+    return res;
+}
+
+inline double FactorRelativePosition3dWithExtrinsics::cost() const
+{
+    return residual().squaredNorm();
+}
+
+} // namespace wolf
\ No newline at end of file
diff --git a/src/processor/processor_tracker_feature_landmark_external.cpp b/src/processor/processor_tracker_feature_landmark_external.cpp
index 5b47399e2445a33c3af4b8bde838fc9019ae05e9..50a1730c2d3bf08c7020022448913ca7c857cf3c 100644
--- a/src/processor/processor_tracker_feature_landmark_external.cpp
+++ b/src/processor/processor_tracker_feature_landmark_external.cpp
@@ -26,7 +26,7 @@
 #include "core/factor/factor_relative_pose_2d_with_extrinsics.h"
 #include "core/factor/factor_relative_position_2d_with_extrinsics.h"
 #include "core/factor/factor_relative_pose_3d_with_extrinsics.h"
-//#include "core/factor/factor_relative_position_3d_with_extrinsics.h"
+#include "core/factor/factor_relative_position_3d_with_extrinsics.h"
 #include "core/landmark/landmark_base.h"
 #include "core/state_block/state_block_derived.h"
 #include "core/state_block/state_quaternion.h"
@@ -182,16 +182,19 @@ bool ProcessorTrackerFeatureLandmarkExternal::voteForKeyFrame() const
         if (track_matrix_.track(match.first).size() >= params_tfle_->filter_track_length_th)
             n_tracks++;
     }
+    WOLF_INFO("Nbr. of active feature tracks: " , n_tracks );
     if (n_tracks == 0)
         return false;
 
-    WOLF_INFO("Nbr. of active feature tracks: " , n_tracks );
-
-    bool vote = n_tracks < params_tracker_feature_->min_features_for_keyframe;
+    bool vote = false;// = n_tracks < params_tracker_feature_->min_features_for_keyframe;
 
     if (origin_ptr_)
     {
-        WOLF_INFO("Time span since last frame: " , last_ptr_->getTimeStamp() - origin_ptr_->getTimeStamp() );
+        WOLF_INFO("Time span since last frame: " , 
+                  last_ptr_->getTimeStamp() - origin_ptr_->getTimeStamp() , 
+                  " (should be greater than ",
+                  params_tfle_->time_span,
+                  ")");
         vote = vote or last_ptr_->getTimeStamp() - origin_ptr_->getTimeStamp() > params_tfle_->time_span;
     }
 
@@ -213,6 +216,8 @@ FactorBasePtr ProcessorTrackerFeatureLandmarkExternal::emplaceFactor(FeatureBase
 {
     assert(_feature_ptr);
     assert(_feature_other_ptr);
+    assert(_feature_ptr->getCapture());
+    assert(_feature_ptr->getCapture()->getFrame());
 
     assert(getProblem());
     assert(getProblem()->getMap());
@@ -241,9 +246,9 @@ FactorBasePtr ProcessorTrackerFeatureLandmarkExternal::emplaceFactor(FeatureBase
         if (not lmk)
         {
             Vector2d frm_p = _feature_ptr->getCapture()->getFrame()->getP()->getState();
-            Vector2d sen_p = _feature_ptr->getCapture()->getP()->getState();
+            Vector2d sen_p = _feature_ptr->getCapture()->getSensorP()->getState();
             double frm_o   = _feature_ptr->getCapture()->getFrame()->getO()->getState()(0);
-            double sen_o   = _feature_ptr->getCapture()->getO()->getState()(0);
+            double sen_o   = _feature_ptr->getCapture()->getSensorO()->getState()(0);
             
             Vector2d lmk_p = frm_p + Rotation2Dd(frm_o) * (sen_p + Rotation2Dd(sen_o) * _feature_ptr->getMeasurement());
 
@@ -268,9 +273,9 @@ FactorBasePtr ProcessorTrackerFeatureLandmarkExternal::emplaceFactor(FeatureBase
         if (not lmk)
         {
             Vector2d frm_p = _feature_ptr->getCapture()->getFrame()->getP()->getState();
-            Vector2d sen_p = _feature_ptr->getCapture()->getP()->getState();
+            Vector2d sen_p = _feature_ptr->getCapture()->getSensorP()->getState();
             double frm_o   = _feature_ptr->getCapture()->getFrame()->getO()->getState()(0);
-            double sen_o   = _feature_ptr->getCapture()->getO()->getState()(0);
+            double sen_o   = _feature_ptr->getCapture()->getSensorO()->getState()(0);
 
             Vector2d lmk_p = frm_p + Rotation2Dd(frm_o) * (sen_p + Rotation2Dd(sen_o) * _feature_ptr->getMeasurement().head<2>());
             double lmk_o   = frm_o + sen_o + _feature_ptr->getMeasurement()(2);
@@ -285,7 +290,7 @@ FactorBasePtr ProcessorTrackerFeatureLandmarkExternal::emplaceFactor(FeatureBase
         else if (not lmk->getO())
         {
             double frm_o = _feature_ptr->getCapture()->getFrame()->getO()->getState()(0);
-            double sen_o = _feature_ptr->getCapture()->getO()->getState()(0);
+            double sen_o = _feature_ptr->getCapture()->getSensorO()->getState()(0);
 
             double lmk_o = frm_o + sen_o + _feature_ptr->getMeasurement()(2);
 
@@ -302,15 +307,13 @@ FactorBasePtr ProcessorTrackerFeatureLandmarkExternal::emplaceFactor(FeatureBase
     // 3D - Relative Position
     else if (getProblem()->getDim() == 3 and _feature_ptr->getMeasurement().size() == 3)
     {
-        throw std::runtime_error("FactorRelativePosition3dWithExtrinsics is not implemented yet");
-
         // no landmark, create it
         if (not lmk)
         {
             Vector3d frm_p    = _feature_ptr->getCapture()->getFrame()->getP()->getState();
-            Vector3d sen_p    = _feature_ptr->getCapture()->getP()->getState();
+            Vector3d sen_p    = _feature_ptr->getCapture()->getSensorP()->getState();
             Quaterniond frm_o = Quaterniond(Vector4d(_feature_ptr->getCapture()->getFrame()->getO()->getState()));
-            Quaterniond sen_o = Quaterniond(Vector4d(_feature_ptr->getCapture()->getO()->getState()));
+            Quaterniond sen_o = Quaterniond(Vector4d(_feature_ptr->getCapture()->getSensorO()->getState()));
 
             Vector3d lmk_p = frm_p + frm_o * (sen_p + sen_o * _feature_ptr->getMeasurement());
 
@@ -322,11 +325,11 @@ FactorBasePtr ProcessorTrackerFeatureLandmarkExternal::emplaceFactor(FeatureBase
         }
 
         // emplace factor
-        // return FactorBase::emplace<FactorRelativePosition3dWithExtrinsics>(_feature_ptr, 
-        //                                                                    _feature_ptr,
-        //                                                                    lmk,
-        //                                                                    shared_from_this(),
-        //                                                                    params_tfle_->apply_loss_function);
+        return FactorBase::emplace<FactorRelativePosition3dWithExtrinsics>(_feature_ptr, 
+                                                                           _feature_ptr,
+                                                                           lmk,
+                                                                           shared_from_this(),
+                                                                           params_tfle_->apply_loss_function);
     }
     // 3D - Relative Pose
     else if (getProblem()->getDim() == 3 and _feature_ptr->getMeasurement().size() == 7)
@@ -335,9 +338,9 @@ FactorBasePtr ProcessorTrackerFeatureLandmarkExternal::emplaceFactor(FeatureBase
         if (not lmk)
         {
             Vector3d frm_p    = _feature_ptr->getCapture()->getFrame()->getP()->getState();
-            Vector3d sen_p    = _feature_ptr->getCapture()->getP()->getState();
+            Vector3d sen_p    = _feature_ptr->getCapture()->getSensorP()->getState();
             Quaterniond frm_o = Quaterniond(Vector4d(_feature_ptr->getCapture()->getFrame()->getO()->getState()));
-            Quaterniond sen_o = Quaterniond(Vector4d(_feature_ptr->getCapture()->getO()->getState()));
+            Quaterniond sen_o = Quaterniond(Vector4d(_feature_ptr->getCapture()->getSensorO()->getState()));
 
             Vector3d lmk_p    = frm_p + frm_o * (sen_p + sen_o * _feature_ptr->getMeasurement().head<3>());
             Quaterniond lmk_o = frm_o * sen_o * Quaterniond(_feature_ptr->getMeasurement().tail<4>());
@@ -352,7 +355,7 @@ FactorBasePtr ProcessorTrackerFeatureLandmarkExternal::emplaceFactor(FeatureBase
         else if (not lmk->getO())
         {
             Quaterniond frm_o = Quaterniond(Vector4d(_feature_ptr->getCapture()->getFrame()->getO()->getState()));
-            Quaterniond sen_o = Quaterniond(Vector4d(_feature_ptr->getCapture()->getO()->getState()));
+            Quaterniond sen_o = Quaterniond(Vector4d(_feature_ptr->getCapture()->getSensorO()->getState()));
 
             Quaterniond lmk_o = frm_o * sen_o * Quaterniond(_feature_ptr->getMeasurement().tail<4>());
 
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index 73af9e2ed1d3f9fbbdc32240eb2ef83d9b07ff46..261438924ee686d0799f738278b5f1186630a48e 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -179,6 +179,12 @@ wolf_add_gtest(gtest_factor_relative_position_2d gtest_factor_relative_position_
 # FactorRelativePosition2dWithExtrinsics class test
 wolf_add_gtest(gtest_factor_relative_position_2d_with_extrinsics gtest_factor_relative_position_2d_with_extrinsics.cpp)
 
+# FactorRelativePosition3d class test
+wolf_add_gtest(gtest_factor_relative_position_3d gtest_factor_relative_position_3d.cpp)
+
+# FactorRelativePosition3dWithExtrinsics class test
+wolf_add_gtest(gtest_factor_relative_position_3d_with_extrinsics gtest_factor_relative_position_3d_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_3d.cpp b/test/gtest_factor_relative_pose_3d.cpp
index 00c75bec4aeec1df11a17fbd39eea2f06ca8577e..3b7f25c8fba43f0d56f5146fd2d5168e83733631 100644
--- a/test/gtest_factor_relative_pose_3d.cpp
+++ b/test/gtest_factor_relative_pose_3d.cpp
@@ -19,12 +19,6 @@
 // along with this program.  If not, see <http://www.gnu.org/licenses/>.
 //
 //--------LICENSE_END--------
-/**
- * \file gtest_factor_relative_pose_3d.cpp
- *
- *  Created on: Mar 30, 2017
- *      \author: jsola
- */
 
 #include "core/utils/utils_gtest.h"
 
diff --git a/test/gtest_factor_relative_position_3d.cpp b/test/gtest_factor_relative_position_3d.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..e928a49baf2f61b7c23eb663830a7a0d63c78a47
--- /dev/null
+++ b/test/gtest_factor_relative_position_3d.cpp
@@ -0,0 +1,280 @@
+//--------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_3d.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;
+using namespace wolf;
+using std::cout;
+using std::endl;
+
+std::string wolf_root = _WOLF_ROOT_DIR;
+
+int N = 1e2;
+
+// Vectors
+Vector7d x_f;
+Vector3d delta1, delta2, delta3, x_1, x_2, x_3;
+
+// Input odometry data and covariance
+Matrix3d data_cov = (Vector3d() << 1e-3, 1e-3, 1e-3).finished().asDiagonal();
+
+// Problem and solver
+ProblemPtr problem_ptr = Problem::create("PO", 3);
+
+SolverCeres solver(problem_ptr);
+
+// Two frames
+FrameBasePtr frm  = 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() );
+FrameBasePtr frm2 = problem_ptr->emplaceFrame(2, (Vector7d() << 0,0,0,0,0,0,1).finished() );
+FrameBasePtr frm3 = problem_ptr->emplaceFrame(3, (Vector7d() << 0,0,0,0,0,0,1).finished() );
+
+// Landmarks
+LandmarkBasePtr lmk1 = LandmarkBase::emplace<LandmarkBase>(problem_ptr->getMap(), 
+                                                           "LandmarkPosition3d",
+                                                           std::make_shared<StatePoint3d>(Vector3d::Zero()),
+                                                           nullptr);
+
+LandmarkBasePtr lmk2 = LandmarkBase::emplace<LandmarkBase>(problem_ptr->getMap(), 
+                                                           "LandmarkPosition3d",
+                                                           std::make_shared<StatePoint3d>(Vector3d::Zero()),
+                                                           nullptr);
+
+LandmarkBasePtr lmk3 = LandmarkBase::emplace<LandmarkBase>(problem_ptr->getMap(), 
+                                                           "LandmarkPosition3d",
+                                                           std::make_shared<StatePoint3d>(Vector3d::Zero()),
+                                                           nullptr);
+
+// Captures
+CaptureBasePtr cap1, cap2, cap3;
+// Features
+FeatureBasePtr fea1, fea2, fea3;
+// Factors
+FactorRelativePosition3dPtr fac1, fac2, fac3;
+
+////////////////////////////////////////////////////////
+void generateRandomProblemFrame()
+{
+    // Random delta
+    delta1 = Vector3d::Random() * 1e2;
+    delta2 = Vector3d::Random() * 1e2;
+    delta3 = Vector3d::Random() * 1e2;
+
+    // Random frame pose
+    x_f = Vector7d::Random() * 1e2;
+    x_f.tail<4>().normalize();
+    auto q_f = Quaterniond(x_f.tail<4>());
+
+    // Frames position
+    x_1 = x_f.head<3>() + q_f * delta1;
+    x_2 = x_f.head<3>() + q_f * delta2;
+    x_3 = x_f.head<3>() + q_f * delta3;
+
+    // Set states
+    frm->setState(x_f);
+    frm1->getP()->setState(x_1);
+    frm2->getP()->setState(x_2);
+    frm3->getP()->setState(x_3);
+
+    // capture & feature & factor with delta measurementCaptureBase(const std::string& _type,
+    cap1 = CaptureBase::emplace<CaptureBase>(frm1, "CaptureFramePosition3d", 1);
+    cap2 = CaptureBase::emplace<CaptureBase>(frm2, "CaptureFramePosition3d", 2);
+    cap3 = CaptureBase::emplace<CaptureBase>(frm3, "CaptureFramePosition3d", 3);
+    fea1 = FeatureBase::emplace<FeatureBase>(cap1, "FeatureFramePosition3d", delta1, data_cov);
+    fea2 = FeatureBase::emplace<FeatureBase>(cap2, "FeatureFramePosition3d", delta2, data_cov);
+    fea3 = FeatureBase::emplace<FeatureBase>(cap3, "FeatureFramePosition3d", delta3, data_cov);
+    fac1 = FactorBase::emplace<FactorRelativePosition3d>(fea1, fea1, frm, nullptr, false, TOP_MOTION);
+    fac2 = FactorBase::emplace<FactorRelativePosition3d>(fea2, fea2, frm, nullptr, false, TOP_MOTION);
+    fac3 = FactorBase::emplace<FactorRelativePosition3d>(fea3, fea3, frm, nullptr, false, TOP_MOTION);
+}
+
+void generateRandomProblemLandmark()
+{
+    // Random delta
+    delta1 = Vector3d::Random() * 1e2;
+    delta2 = Vector3d::Random() * 1e2;
+    delta3 = Vector3d::Random() * 1e2;
+
+    // Random frame pose
+    x_f = Vector7d::Random() * 1e2;
+    x_f.tail<4>().normalize();
+    auto q_f = Quaterniond(x_f.tail<4>());
+
+    // Landmarks positions
+    x_1 = x_f.head<3>() + q_f * delta1;
+    x_2 = x_f.head<3>() + q_f * delta2;
+    x_3 = x_f.head<3>() + q_f * delta3;
+
+    // Set states
+    frm->setState(x_f);
+    lmk1->setState(x_1);
+    lmk2->setState(x_2);
+    lmk3->setState(x_3);
+
+    // feature & factor with delta measurement
+    cap1 = CaptureBase::emplace<CaptureBase>(frm, "CaptureLandmarkPosition3d", 1);
+    cap2 = CaptureBase::emplace<CaptureBase>(frm, "CaptureLandmarkPosition3d", 1);
+    cap3 = CaptureBase::emplace<CaptureBase>(frm, "CaptureLandmarkPosition3d", 1);
+    fea1 = FeatureBase::emplace<FeatureBase>(cap1, "FeatureLandmarkPosition3d", delta1, data_cov);
+    fea2 = FeatureBase::emplace<FeatureBase>(cap2, "FeatureLandmarkPosition3d", delta2, data_cov);
+    fea3 = FeatureBase::emplace<FeatureBase>(cap3, "FeatureLandmarkPosition3d", delta3, data_cov);
+    fac1 = FactorBase::emplace<FactorRelativePosition3d>(fea1, fea1, lmk1, nullptr, false, TOP_LMK);
+    fac2 = FactorBase::emplace<FactorRelativePosition3d>(fea2, fea2, lmk2, nullptr, false, TOP_LMK);
+    fac3 = FactorBase::emplace<FactorRelativePosition3d>(fea3, fea3, lmk3, nullptr, false, TOP_LMK);
+}
+
+TEST(FactorRelativePosition3d, check_tree)
+{
+    ASSERT_TRUE(problem_ptr->check(0));
+}
+
+// FRAME TO FRAME =========================================================================
+TEST(FactorRelativePosition3d, frame_solve_frame)
+{
+    for (int i = 0; i < N; i++)
+    {
+        // random setup
+        generateRandomProblemFrame();
+
+        // Perturb frm, fix the rest
+        frm->unfix();
+        frm1->fix();
+        frm2->fix();
+        frm3->fix();
+        frm->perturb(1);
+
+        // solve for frm1
+        std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+        //WOLF_INFO(report);
+
+        ASSERT_POSE3d_APPROX(frm->getStateVector(), x_f, 1e-3);
+
+        // remove captures (and features and factors) for the next loop
+        cap1->remove();
+        cap2->remove();
+        cap3->remove();
+    }
+}
+
+TEST(FactorRelativePosition3d, frame_solve_frames)
+{
+    for (int i = 0; i < N; i++)
+    {
+        // random setup
+        generateRandomProblemFrame();
+        
+        // Perturb frm0, fix the rest
+        frm->fix();
+        frm1->unfix();
+        frm2->unfix();
+        frm3->unfix();
+        frm1->getP()->perturb(1);
+        frm2->getP()->perturb(1);
+        frm3->getP()->perturb(1);
+
+        // solve for frm0
+        std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+        //WOLF_INFO(report);
+
+        ASSERT_MATRIX_APPROX(frm1->getStateVector("P"), x_1, 1e-3);
+        ASSERT_MATRIX_APPROX(frm2->getStateVector("P"), x_2, 1e-3);
+        ASSERT_MATRIX_APPROX(frm3->getStateVector("P"), x_3, 1e-3);
+
+        // remove captures (and features and factors) for the next loop
+        cap1->remove();
+        cap2->remove();
+        cap3->remove();
+    }
+}
+
+// FRAME TO LANDMARK =========================================================================
+TEST(FactorRelativePosition3d, landmark_solve_frame)
+{
+    for (int i = 0; i < N; i++)
+    {
+        // random setup
+        generateRandomProblemLandmark();
+        
+        // Perturb frm0, fix the rest
+        frm->unfix();
+        lmk1->fix();
+        lmk2->fix();
+        lmk3->fix();
+        frm->perturb(1);
+
+        // solve for frm0
+        std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+        //WOLF_INFO(report);
+
+        ASSERT_POSE3d_APPROX(frm->getStateVector(), x_f, 1e-3);
+
+        // remove captures (and features and factors) for the next loop
+        cap1->remove();
+        cap2->remove();
+        cap3->remove();
+    }
+}
+
+TEST(FactorRelativePosition3d, landmark_solve_lmks)
+{
+    for (int i = 0; i < N; i++)
+    {
+        // random setup
+        generateRandomProblemLandmark();
+        
+        // Perturb landmark, fix the rest
+        frm->fix();
+        lmk1->unfix();
+        lmk2->unfix();
+        lmk3->unfix();
+        lmk1->perturb(1);
+        lmk2->perturb(1);
+        lmk3->perturb(1);
+
+        // solve for landmark
+        std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+        //WOLF_INFO(report);
+
+        ASSERT_MATRIX_APPROX(lmk1->getStateVector("P"), x_1, 1e-3);
+        ASSERT_MATRIX_APPROX(lmk2->getStateVector("P"), x_2, 1e-3);
+        ASSERT_MATRIX_APPROX(lmk3->getStateVector("P"), x_3, 1e-3);
+
+        // remove captures (and features and factors) for the next loop
+        cap1->remove();
+        cap2->remove();
+        cap3->remove();
+    }
+}
+
+int main(int argc, char **argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}
diff --git a/test/gtest_factor_relative_position_3d_with_extrinsics.cpp b/test/gtest_factor_relative_position_3d_with_extrinsics.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..3917e5d85f021c0f950b18fa7ab4d191fb835379
--- /dev/null
+++ b/test/gtest_factor_relative_position_3d_with_extrinsics.cpp
@@ -0,0 +1,267 @@
+//--------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_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 x_f, x_s;
+Vector3d delta1, delta2, delta3, x_l1, x_l2, x_l3;
+
+// Input odometry data and covariance
+Matrix3d data_cov = (Vector3d() << 1e-3, 1e-3, 1e-3).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");
+
+// Frame
+FrameBasePtr frm = problem_ptr->emplaceFrame(0, (Vector7d() << 0,0,0,0,0,0,1).finished() );
+
+// Landmarks
+LandmarkBasePtr lmk1 = LandmarkBase::emplace<LandmarkBase>(problem_ptr->getMap(), 
+                                                           "LandmarkPosition3d",
+                                                           std::make_shared<StatePoint3d>(Vector3d::Zero()),
+                                                           nullptr);
+
+LandmarkBasePtr lmk2 = LandmarkBase::emplace<LandmarkBase>(problem_ptr->getMap(), 
+                                                           "LandmarkPosition3d",
+                                                           std::make_shared<StatePoint3d>(Vector3d::Zero()),
+                                                           nullptr);
+
+LandmarkBasePtr lmk3 = LandmarkBase::emplace<LandmarkBase>(problem_ptr->getMap(), 
+                                                           "LandmarkPosition3d",
+                                                           std::make_shared<StatePoint3d>(Vector3d::Zero()),
+                                                           nullptr);
+
+// Capture
+auto cap = CaptureBase::emplace<CaptureOdom3d>(frm, 1, sensor, Vector3d::Zero(), data_cov);
+// Features
+FeatureBasePtr fea1, fea2, fea3;
+// Factors
+FactorRelativePosition3dWithExtrinsicsPtr fac1, fac2, fac3;
+
+void generateRandomProblemLandmark()
+{
+    // Random deltas
+    delta1 = Vector3d::Random() * 10;
+    delta2 = Vector3d::Random() * 10;
+    delta3 = Vector3d::Random() * 10;
+
+    // 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>());
+
+    // Landmarks poses
+    x_l1 = x_f.head<3>() + q_f * (x_s.head<3>() + q_s * delta1);
+    x_l2 = x_f.head<3>() + q_f * (x_s.head<3>() + q_s * delta2);
+    x_l3 = x_f.head<3>() + q_f * (x_s.head<3>() + q_s * delta3);
+
+    // Set states
+    frm->setState(x_f);
+    lmk1->setState(x_l1);
+    lmk2->setState(x_l2);
+    lmk3->setState(x_l3);
+    sensor->setState(x_s);
+
+    // features & factors with deltas measurements
+    fea1 = FeatureBase::emplace<FeatureBase>(cap, "FeatureLandmarkPose", delta1, data_cov);
+    fea2 = FeatureBase::emplace<FeatureBase>(cap, "FeatureLandmarkPose", delta2, data_cov);
+    fea3 = FeatureBase::emplace<FeatureBase>(cap, "FeatureLandmarkPose", delta3, data_cov);
+    fac1 = FactorBase::emplace<FactorRelativePosition3dWithExtrinsics>(fea1, fea1, lmk1, nullptr, false, TOP_LMK);
+    fac2 = FactorBase::emplace<FactorRelativePosition3dWithExtrinsics>(fea2, fea2, lmk2, nullptr, false, TOP_LMK);
+    fac3 = FactorBase::emplace<FactorRelativePosition3dWithExtrinsics>(fea3, fea3, lmk3, nullptr, false, TOP_LMK);
+}
+
+TEST(FactorRelativePosition3dWithExtrinsics, check_tree)
+{
+    ASSERT_TRUE(problem_ptr->check(0));
+}
+
+// FRAME TO LANDMARK =========================================================================
+TEST(FactorRelativePosition3dWithExtrinsics, landmark_solve_lmks)
+{
+    for (int i = 0; i < N; i++)
+    {
+        // random setup
+        generateRandomProblemLandmark();
+        
+        ASSERT_MATRIX_APPROX(fac1->residual(), VectorXd::Zero(3), Constants::EPS);
+        ASSERT_MATRIX_APPROX(fac2->residual(), VectorXd::Zero(3), Constants::EPS);
+        ASSERT_MATRIX_APPROX(fac3->residual(), VectorXd::Zero(3), Constants::EPS);
+
+        // Perturb landmark, fix the rest
+        frm->fix();
+        lmk1->unfix();
+        lmk2->unfix();
+        lmk3->unfix();
+        sensor->getP()->fix();
+        sensor->getO()->fix();
+        lmk1->perturb(1);
+        lmk2->perturb(1);
+        lmk3->perturb(1);
+
+        // solve for landmark
+        std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+        //WOLF_INFO(report);
+
+        ASSERT_MATRIX_APPROX(lmk1->getStateVector(), x_l1, 1e-3);
+        ASSERT_MATRIX_APPROX(lmk2->getStateVector(), x_l2, 1e-3);
+        ASSERT_MATRIX_APPROX(lmk3->getStateVector(), x_l3, 1e-3);
+
+        // remove features (and factors) for the next loop
+        fea1->remove();
+        fea2->remove();
+        fea3->remove();
+    }
+}
+
+TEST(FactorRelativePosition3dWithExtrinsics, landmark_solve_frame)
+{
+    for (int i = 0; i < N; i++)
+    {
+        // random setup
+        generateRandomProblemLandmark();
+        
+        ASSERT_MATRIX_APPROX(fac1->residual(), VectorXd::Zero(3), Constants::EPS);
+        ASSERT_MATRIX_APPROX(fac2->residual(), VectorXd::Zero(3), Constants::EPS);
+        ASSERT_MATRIX_APPROX(fac3->residual(), VectorXd::Zero(3), Constants::EPS);
+
+        // Perturb frm0, fix the rest
+        frm->unfix();
+        lmk1->fix();
+        lmk2->fix();
+        lmk3->fix();
+        sensor->getP()->fix();
+        sensor->getO()->fix();
+        frm->perturb(1);
+
+        // solve for frm0
+        std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+        //WOLF_INFO(report);
+
+        ASSERT_POSE3d_APPROX(frm->getStateVector(), x_f, 1e-3);
+
+        // remove feature (and factor) for the next loop
+        fea1->remove();
+        fea2->remove();
+        fea3->remove();
+    }
+}
+
+TEST(FactorRelativePosition3dWithExtrinsics, landmark_extrinsics_p_solve)
+{
+    for (int i = 0; i < N; i++)
+    {
+        // random setup
+        generateRandomProblemLandmark();
+        
+        ASSERT_MATRIX_APPROX(fac1->residual(), VectorXd::Zero(3), Constants::EPS);
+        ASSERT_MATRIX_APPROX(fac2->residual(), VectorXd::Zero(3), Constants::EPS);
+        ASSERT_MATRIX_APPROX(fac3->residual(), VectorXd::Zero(3), Constants::EPS);
+
+        // Perturb sensor P, fix the rest
+        frm->fix();
+        lmk1->fix();
+        lmk2->fix();
+        lmk3->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
+        fea1->remove();
+        fea2->remove();
+        fea3->remove();
+    }
+}
+
+TEST(FactorRelativePosition3dWithExtrinsics, landmark_extrinsics_o_solve)
+{
+    for (int i = 0; i < N; i++)
+    {
+        // random setup
+        generateRandomProblemLandmark();
+        
+        ASSERT_MATRIX_APPROX(fac1->residual(), VectorXd::Zero(3), Constants::EPS);
+        ASSERT_MATRIX_APPROX(fac2->residual(), VectorXd::Zero(3), Constants::EPS);
+        ASSERT_MATRIX_APPROX(fac3->residual(), VectorXd::Zero(3), Constants::EPS);
+
+        // Perturb sensor O, fix the rest
+        frm->fix();
+        lmk1->fix();
+        lmk2->fix();
+        lmk3->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
+        fea1->remove();
+        fea2->remove();
+        fea3->remove();
+    }
+}
+
+int main(int argc, char **argv)
+{
+testing::InitGoogleTest(&argc, argv);
+return RUN_ALL_TESTS();
+}
diff --git a/test/gtest_processor_tracker_feature_landmark_external.cpp b/test/gtest_processor_tracker_feature_landmark_external.cpp
index 5ec2e7998ec1dd40259db5e3bb16ffc257b868e4..5010597787c5d715592c5f5a81338460af79877e 100644
--- a/test/gtest_processor_tracker_feature_landmark_external.cpp
+++ b/test/gtest_processor_tracker_feature_landmark_external.cpp
@@ -31,6 +31,7 @@
 #include "core/state_block/state_block_derived.h"
 #include "core/state_block/state_angle.h"
 #include "core/state_block/state_quaternion.h"
+#include "core/ceres_wrapper/solver_ceres.h"
 
 #include "core/processor/processor_tracker_feature_landmark_external.h"
 
@@ -50,6 +51,7 @@ class ProcessorTrackerFeatureLandmarkExternalTest : public testing::Test
         double dt;
 
         ProblemPtr problem;
+        SolverManagerPtr solver;
         SensorBasePtr sensor, sensor_odom;
         ProcessorTrackerFeatureLandmarkExternalPtr processor;
         ProcessorMotionPtr processor_motion;
@@ -64,10 +66,17 @@ class ProcessorTrackerFeatureLandmarkExternalTest : public testing::Test
                          double _quality_th, 
                          double _dist_th, 
                          unsigned int _track_length_th,
-                         double _time_span);
+                         double _time_span,
+                         bool _add_landmarks);
         void randomStep();
         CaptureLandmarksExternalPtr computeCaptureLandmarks() const;
-        void addRandomDetection(CaptureLandmarksExternalPtr _cap, int _id, double _quality) const;
+        void testConfiguration(int dim, 
+                               bool orientation, 
+                               double quality_th, 
+                               double dist_th, 
+                               int track_length, 
+                               double time_span, 
+                               bool add_landmarks);
 };
 
 void ProcessorTrackerFeatureLandmarkExternalTest::initProblem(int _dim, 
@@ -75,7 +84,8 @@ void ProcessorTrackerFeatureLandmarkExternalTest::initProblem(int _dim,
                                                               double _quality_th, 
                                                               double _dist_th, 
                                                               unsigned int _track_length_th,
-                                                              double _time_span)
+                                                              double _time_span,
+                                                              bool _add_landmarks)
 {
     dim = _dim;
     orientation = _orientation;
@@ -83,6 +93,7 @@ void ProcessorTrackerFeatureLandmarkExternalTest::initProblem(int _dim,
     dt = 0.1;
 
     problem = Problem::create("PO", dim);
+    solver = std::make_shared<SolverCeres>(problem);
 
     // Sensors
     if (dim == 2)
@@ -152,7 +163,7 @@ void ProcessorTrackerFeatureLandmarkExternalTest::initProblem(int _dim,
     {
         LandmarkBasePtr lmk;
         if (dim == 2)
-            lmk = LandmarkBase::emplace<LandmarkBase>(problem->getMap(), 
+            lmk = LandmarkBase::emplace<LandmarkBase>(_add_landmarks ? problem->getMap() : nullptr, 
                                                       "LandmarkExternal",
                                                       std::make_shared<StatePoint2d>(Vector2d::Random() * 10),
                                                       (orientation ? 
@@ -160,7 +171,7 @@ void ProcessorTrackerFeatureLandmarkExternalTest::initProblem(int _dim,
                                                         nullptr));
     
         else 
-            lmk = LandmarkBase::emplace<LandmarkBase>(problem->getMap(), 
+            lmk = LandmarkBase::emplace<LandmarkBase>(_add_landmarks ? problem->getMap() : nullptr, 
                                                       "LandmarkExternal",
                                                       std::make_shared<StatePoint3d>(Vector3d::Random() * 10),
                                                       (orientation ? 
@@ -181,18 +192,21 @@ void ProcessorTrackerFeatureLandmarkExternalTest::randomStep()
 {
     // compute delta
     VectorXd delta;
+    MatrixXd delta_cov;
     if (dim == 2)
     {
         delta = Vector2d::Random() * 0.1;
+        delta_cov = Matrix2d::Identity() * 0.1;
     }
     else
     {
         delta = Vector7d::Random() * 0.1;
         delta.tail<4>().normalize();
+        delta_cov = Matrix6d::Identity() * 0.1;
     }
 
     // Process odometry
-    auto cap_odom = std::make_shared<CaptureMotion>("CaptureOdom", t, sensor_odom, delta, nullptr);
+    auto cap_odom = std::make_shared<CaptureMotion>("CaptureOdom", t, sensor_odom, delta, delta_cov, nullptr);
     cap_odom->process();
 
     // update groundtruth pose with random movement
@@ -218,7 +232,7 @@ CaptureLandmarksExternalPtr ProcessorTrackerFeatureLandmarkExternalTest::compute
         {
             meas.head(2) = Rotation2Dd(-o_sensor(0))*(Rotation2Dd(-o_robot(0))*(p_lmk - p_robot) - p_sensor);
             if (orientation)
-                meas(2) = o_lmk(0) - p_robot(0);
+                meas(2) = o_lmk(0) - o_robot(0) - o_sensor(0);
         }
         else
         {
@@ -229,69 +243,30 @@ CaptureLandmarksExternalPtr ProcessorTrackerFeatureLandmarkExternalTest::compute
             if (orientation)
                 meas.tail(4) = (q_sensor.conjugate() * q_robot.conjugate() * Quaterniond(Vector4d(o_lmk))).coeffs();
         }
+        // cov
         MatrixXd cov = MatrixXd::Identity(meas.size(), meas.size());
         if (orientation and dim != 2)
             cov = MatrixXd::Identity(6, 6);
 
+        // quality
+        double quality = (double) i / (double) (landmarks.size()-1); // increasing from 0 to 1
+
         // add detection
-        cap->addDetection(landmarks.at(i)->id(), meas, cov, (double) i / (double) landmarks.size());
+        cap->addDetection(landmarks.at(i)->id(), meas, cov, quality );
     }
 
     return cap;
 }
 
-void ProcessorTrackerFeatureLandmarkExternalTest::addRandomDetection(CaptureLandmarksExternalPtr _cap, int _id, double _quality) const
-{
-    VectorXd detection;
-    MatrixXd cov;
-    if (orientation)
-    {
-        if (dim == 2)
-        {
-            detection = Vector3d::Random()*5;
-            detection(2) = pi2pi(detection(2));
-            cov = Matrix3d::Identity()*0.1;
-        }
-        else
-        {
-            detection = Vector7d::Random()*5;
-            detection.tail<4>().normalize();
-            cov = Matrix6d::Identity()*0.1;
-        }
-    } 
-    else
-    {
-        if (dim == 2)
-        {
-            detection = Vector2d::Random()*5;
-            cov = Matrix2d::Identity()*0.1;
-        }
-        else
-        {
-            detection = Vector3d::Random()*5;
-            cov = Matrix3d::Identity()*0.1;
-        }
-    }
-    _cap->addDetection(_id, detection, cov, _quality);
-}
-
-// TESTS /////////////////////////////////////////////////////////////////////////////////
-
-TEST_F(ProcessorTrackerFeatureLandmarkExternalTest, check_p_2d)
+void ProcessorTrackerFeatureLandmarkExternalTest::testConfiguration(int dim, 
+                                                                    bool orientation, 
+                                                                    double quality_th, 
+                                                                    double dist_th, 
+                                                                    int track_length, 
+                                                                    double time_span, 
+                                                                    bool add_landmarks)
 {
-    int dim = 2;
-    bool orientation = false;
-    double quality_th = 0;
-    double dist_th = 1e6;
-    int track_length = 5;
-    double time_span = 3*dt;
-    bool remove_landmarks = true;
-
-    initProblem(dim, orientation, quality_th, dist_th, track_length, time_span);  
-
-    if (remove_landmarks)
-        for (auto lmk : landmarks)
-            problem->getMap()->removeLanlmk->remove();
+    initProblem(dim, orientation, quality_th, dist_th, track_length, time_span, add_landmarks);  
 
     ASSERT_TRUE(problem->check());
 
@@ -306,12 +281,16 @@ TEST_F(ProcessorTrackerFeatureLandmarkExternalTest, check_p_2d)
         // process detections
         cap->process();
         ASSERT_TRUE(problem->check());
-        //problem->print(4,1,1,1);
+        problem->print(4,1,1,1);
 
         // CHECKS
         FactorBasePtrList fac_list;
         problem->getTrajectory()->getFactorList(fac_list);
-        bool should_emplace_KF = t.get() >= time_span and i >= track_length-1;
+        // should emplace KF in last?
+        bool any_active_track = quality_th <=1 and i >= track_length-1;
+        bool should_emplace_KF = t-dt > time_span and any_active_track;
+        WOLF_INFO("should emplace KF: ", should_emplace_KF, " t-dt = ", t-dt, " time_span = ", time_span, " track_length-1 = ", track_length-1);
+
         if (should_emplace_KF)
         {   
             // voted for keyframe
@@ -327,34 +306,37 @@ TEST_F(ProcessorTrackerFeatureLandmarkExternalTest, check_p_2d)
                     if (fac->getProcessor() == processor)
                     {
                         ASSERT_EQ(fac->getType(), std::string("FactorRelative") + 
-                                                    (orientation ? "Pose" : "Position") + 
-                                                    (dim == 2 ? "2d" : "3d") + 
-                                                    "WithExtrinsiscs");
+                                                  (orientation ? "Pose" : "Position") + 
+                                                  (dim == 2 ? "2d" : "3d") + 
+                                                  "WithExtrinsics");
                     }
                 }
-            // landmarks
-            auto landmarks_map = problem->getMap()->getLandmarkList();
-            ASSERT_EQ(landmarks_map.size(), landmarks.size());
-            for (auto lmk_map : landmarks_map)
+            // landmarks created by processor
+            if (not add_landmarks)
             {
-                ASSERT_TRUE(lmk_map->id() < landmarks.size());
-                auto lmk_gt = landmarks.at(lmk_map->id());
-                ASSERT_EQ(lmk_map->id(), lmk_gt->id());
-                if (dim == 2 and orientation)
-                {
-                    ASSERT_POSE2d_APPROX(lmk_map->getState().vector("PO"), lmk_gt->getState().vector("PO"), Constants::EPS);
-                }
-                else if (dim == 2 and not orientation)
-                {
-                    ASSERT_MATRIX_APPROX(lmk_map->getState().vector("P"), lmk_gt->getState().vector("P"), Constants::EPS);
-                }
-                else if (dim == 3 and orientation)
-                {
-                    ASSERT_POSE3d_APPROX(lmk_map->getState().vector("PO"), lmk_gt->getState().vector("PO"), Constants::EPS);
-                }
-                else if (dim == 3 and not orientation)
+                auto landmarks_map = problem->getMap()->getLandmarkList();
+                // amount of landmarks due to quality filtering of detections
+                auto n_landmarks = (quality_th <= 0 ? 3 : (quality_th <= 0.5 ? 2 : (quality_th <= 1 ? 1 : 0)));
+                ASSERT_EQ(landmarks_map.size(), n_landmarks);
+
+                // check state correctly initialized
+                for (auto lmk_map : landmarks_map)
                 {
-                    ASSERT_MATRIX_APPROX(lmk_map->getState().vector("P"), lmk_gt->getState().vector("P"), Constants::EPS);
+                    ASSERT_TRUE(lmk_map->id() < landmarks.size());
+                    auto lmk_gt = landmarks.at(lmk_map->id());
+                    ASSERT_EQ(lmk_map->id(), lmk_gt->id());
+                    if (not orientation)
+                    {
+                        ASSERT_MATRIX_APPROX(lmk_map->getState().vector("P"), lmk_gt->getState().vector("P"), Constants::EPS);
+                    }
+                    else if (dim == 2)
+                    {
+                        ASSERT_POSE2d_APPROX(lmk_map->getState().vector("PO"), lmk_gt->getState().vector("PO"), Constants::EPS);
+                    }
+                    else if (dim == 3)
+                    {
+                        ASSERT_POSE3d_APPROX(lmk_map->getState().vector("PO"), lmk_gt->getState().vector("PO"), Constants::EPS);
+                    }
                 }
             }
         }
@@ -364,10 +346,23 @@ TEST_F(ProcessorTrackerFeatureLandmarkExternalTest, check_p_2d)
             ASSERT_FALSE(problem->getTrajectory()->size() > 1);
             // no factors emplaced
             ASSERT_TRUE(fac_list.empty());
-            // landmarks
-            ASSERT_EQ(problem->getMap()->getLandmarkList().empty(), remove_landmarks);
+            // no landmarks created yet (if not added by the test)
+            ASSERT_EQ(problem->getMap()->getLandmarkList().empty(), not add_landmarks);
         }
 
+        // solve
+        if (not problem->getMap()->getLandmarkList().empty())
+        {
+            // TODO store gt values
+            
+            // TODO perturb
+
+            // solve
+            auto report = solver->solve(SolverManager::ReportVerbosity::FULL);
+            WOLF_INFO(report);
+
+            // TODO check values
+        }
 
         // step with random movement
         t+=dt;
@@ -375,42 +370,138 @@ TEST_F(ProcessorTrackerFeatureLandmarkExternalTest, check_p_2d)
     }
 }
 
-// TEST_F(ProcessorTrackerFeatureLandmarkExternalTest, check_po_2d)
-// {
-//     initProblem(2, true);
-//     ASSERT_TRUE(problem->check());
+// / TESTS //////////////////////////////////////////
+TEST_F(ProcessorTrackerFeatureLandmarkExternalTest, P_2d_existing_lmks)
+{
+    testConfiguration(2,      //int dim
+                      false,  //bool orientation
+                      0,      //double quality_th
+                      1e6,    //double dist_th
+                      6,      //int track_length
+                      4.5*dt, //double time_span
+                      true);  //bool add_landmarks
+}
+
+TEST_F(ProcessorTrackerFeatureLandmarkExternalTest, P_2d_no_lmks)
+{
+    testConfiguration(2,      //int dim
+                      false,  //bool orientation
+                      0,      //double quality_th
+                      1e6,    //double dist_th
+                      2,      //int track_length
+                      4.5*dt, //double time_span
+                      false); //bool add_landmarks
+}
 
-//     auto cap = randomStep();
-//     ASSERT_TRUE(problem->check());
+TEST_F(ProcessorTrackerFeatureLandmarkExternalTest, P_2d_no_lmks_quality)
+{
+    testConfiguration(2,      //int dim
+                      false,  //bool orientation
+                      0.3,      //double quality_th
+                      1e6,    //double dist_th
+                      6,      //int track_length
+                      4.5*dt,   //double time_span
+                      false); //bool add_landmarks
+}
+
+TEST_F(ProcessorTrackerFeatureLandmarkExternalTest, PO_2d_existing_lmks)
+{
+    testConfiguration(2,      //int dim
+                      true,   //bool orientation
+                      0,      //double quality_th
+                      1e6,    //double dist_th
+                      3,      //int track_length
+                      4.5*dt,   //double time_span
+                      true);  //bool add_landmarks
+}
 
-//     cap->process();
-//     ASSERT_TRUE(problem->check());
-// }
+TEST_F(ProcessorTrackerFeatureLandmarkExternalTest, PO_2d_no_lmks)
+{
+    testConfiguration(2,      //int dim
+                      true,   //bool orientation
+                      0,      //double quality_th
+                      1e6,    //double dist_th
+                      1,      //int track_length
+                      4.5*dt,   //double time_span
+                      false); //bool add_landmarks
+}
 
-// TEST_F(ProcessorTrackerFeatureLandmarkExternalTest, check_p_3d)
-// {
-//     initProblem(3, false);
-//     ASSERT_TRUE(problem->check());
+TEST_F(ProcessorTrackerFeatureLandmarkExternalTest, PO_2d_no_lmks_quality)
+{
+    testConfiguration(2,      //int dim
+                      true,   //bool orientation
+                      0.3,      //double quality_th
+                      1e6,    //double dist_th
+                      0,      //int track_length
+                      4.5*dt,   //double time_span
+                      false); //bool add_landmarks
+}
 
-//     auto cap = randomStep();
-//     ASSERT_TRUE(problem->check());
+TEST_F(ProcessorTrackerFeatureLandmarkExternalTest, P_3d_existing_lmks)
+{
+    testConfiguration(3,      //int dim
+                      false,  //bool orientation
+                      0,      //double quality_th
+                      1e6,    //double dist_th
+                      7,      //int track_length
+                      4.5*dt,   //double time_span
+                      true);  //bool add_landmarks
+}
 
-//     cap->process();
-//     ASSERT_TRUE(problem->check());
-// }
+TEST_F(ProcessorTrackerFeatureLandmarkExternalTest, P_3d_no_lmks)
+{
+    testConfiguration(3,      //int dim
+                      false,  //bool orientation
+                      0,      //double quality_th
+                      1e6,    //double dist_th
+                      53,      //int track_length
+                      4.5*dt,   //double time_span
+                      false); //bool add_landmarks
+}
 
-// TEST_F(ProcessorTrackerFeatureLandmarkExternalTest, check_po_3d)
-// {
-//     initProblem(3, true);
-//     ASSERT_TRUE(problem->check());
+TEST_F(ProcessorTrackerFeatureLandmarkExternalTest, P_3d_no_lmks_quality)
+{
+    testConfiguration(3,      //int dim
+                      false,  //bool orientation
+                      0.3,      //double quality_th
+                      1e6,    //double dist_th
+                      2,      //int track_length
+                      4.5*dt,   //double time_span
+                      false); //bool add_landmarks
+}
 
-//     auto cap = randomStep();
-//     ASSERT_TRUE(problem->check());
+TEST_F(ProcessorTrackerFeatureLandmarkExternalTest, PO_3d_existing_lmks)
+{
+    testConfiguration(3,      //int dim
+                      true,   //bool orientation
+                      0,      //double quality_th
+                      1e6,    //double dist_th
+                      1,      //int track_length
+                      4.5*dt,   //double time_span
+                      true);  //bool add_landmarks
+}
 
-//     cap->process();
-//     ASSERT_TRUE(problem->check());
-// }
+TEST_F(ProcessorTrackerFeatureLandmarkExternalTest, PO_3d_no_lmks)
+{
+    testConfiguration(3,      //int dim
+                      true,   //bool orientation
+                      0,      //double quality_th
+                      1e6,    //double dist_th
+                      4,      //int track_length
+                      4.5*dt,   //double time_span
+                      false); //bool add_landmarks
+}
 
+TEST_F(ProcessorTrackerFeatureLandmarkExternalTest, PO_3d_no_lmks_quality)
+{
+    testConfiguration(3,      //int dim
+                      true,   //bool orientation
+                      0.2,      //double quality_th
+                      1e6,    //double dist_th
+                      5,      //int track_length
+                      4.5*dt,   //double time_span
+                      false); //bool add_landmarks
+}
 
 
 int main(int argc, char **argv)