Implemented classes for a SensorPose with extrinsics, gtest sensor working,...

Implemented classes for a SensorPose with extrinsics, gtest sensor working, debugging factor pose3d with extrinsics

CMakeLists.txt

@@ -250,6 +250,7 @@ SET(HDRS_PROCESSOR
   include/core/processor/processor_motion.h
   include/core/processor/processor_odom_2d.h
   include/core/processor/processor_odom_3d.h
+  include/core/processor/processor_pose.h
   include/core/processor/processor_tracker.h
   include/core/processor/processor_tracker_feature.h
   include/core/processor/processor_tracker_landmark.h
@@ -261,6 +262,7 @@ SET(HDRS_SENSOR
   include/core/sensor/factory_sensor.h
   include/core/sensor/sensor_odom_2d.h
   include/core/sensor/sensor_odom_3d.h
+  include/core/sensor/sensor_pose.h
   )
 SET(HDRS_SOLVER
   include/core/solver/solver_manager.h
@@ -344,6 +346,7 @@ SET(SRCS_PROCESSOR
   src/processor/processor_motion.cpp
   src/processor/processor_odom_2d.cpp
   src/processor/processor_odom_3d.cpp
+  src/processor/processor_pose.cpp
   src/processor/processor_tracker.cpp
   src/processor/processor_tracker_feature.cpp
   src/processor/processor_tracker_landmark.cpp
@@ -354,6 +357,7 @@ SET(SRCS_SENSOR
   src/sensor/sensor_diff_drive.cpp
   src/sensor/sensor_odom_2d.cpp
   src/sensor/sensor_odom_3d.cpp
+  src/sensor/sensor_pose.cpp
   )
 SET(SRCS_SOLVER
   src/solver/solver_manager.cpp
@@ -367,6 +371,7 @@ SET(SRCS_YAML
   src/yaml/processor_odom_3d_yaml.cpp
   src/yaml/sensor_odom_2d_yaml.cpp
   src/yaml/sensor_odom_3d_yaml.cpp
+  src/yaml/sensor_pose_yaml.cpp
   )
 #OPTIONALS
 #optional HDRS and SRCS

include/core/capture/capture_pose.h

@@ -28,6 +28,8 @@ class CapturePose : public CaptureBase
         ~CapturePose() override;
 
         virtual void emplaceFeatureAndFactor();
+        Eigen::VectorXd getData(){    return data_;}
+        Eigen::MatrixXd getDataCovariance(){ return data_covariance_;}
 
 };
 

include/core/factor/factor_pose_3d_with_extrinsics.h

+#ifndef FACTOR_POSE_3D_WITH_EXTRINSICS_THETA_H_
+#define FACTOR_POSE_3D_WITH_EXTRINSICS_THETA_H_
+
+//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(FactorPose3dWithExtrinsics);
+
+//class
+class FactorPose3dWithExtrinsics : public FactorAutodiff<FactorPose3dWithExtrinsics, 6, 3, 4, 3, 4>
+{
+    public:
+        FactorPose3dWithExtrinsics(const FeatureBasePtr& _ftr_ptr,
+                                   const ProcessorBasePtr& _processor_ptr,
+                                   bool _apply_loss_function,
+                                   const FactorTopology& _top,
+                                   FactorStatus _status = FAC_ACTIVE) :
+             FactorAutodiff<FactorPose3dWithExtrinsics, 6, 3, 4, 3, 4>("FactorPose3dWithExtrinsics",
+                                                                       _top,
+                                                                       _ftr_ptr,
+                                                                       nullptr, nullptr, nullptr, nullptr,
+                                                                       _processor_ptr,
+                                                                       _apply_loss_function,
+                                                                       _status,
+                                                                       _ftr_ptr->getFrame()->getP(),
+                                                                       _ftr_ptr->getFrame()->getO(),
+                                                                       _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!");
+            //
+        }
+
+        ~FactorPose3dWithExtrinsics() override = default;
+
+        template<typename T>
+        bool operator ()(const T* const _p,
+                         const T* const _q,
+                         const T* const _sp,
+                         const T* const _sq,
+                         T* _residuals) const;
+};
+
+template<typename T>
+inline bool FactorPose3dWithExtrinsics::operator ()(const T* const _p,
+                                                    const T* const _q,
+                                                    const T* const _sp,
+                                                    const T* const _sq,
+                                                    T* _residuals) const
+{
+    // MAPS
+    Eigen::Map<const Eigen::Matrix<T,3,1> > w_p_wb(_p);
+    Eigen::Map<const Eigen::Quaternion<T> > w_q_b(_q);
+    Eigen::Map<const Eigen::Matrix<T,3,1> > b_p_bm(_sp);
+    Eigen::Map<const Eigen::Quaternion<T> > b_q_m(_sq);
+
+    // measurements
+    Eigen::Vector3d    w_p_wm(getMeasurement().data() + 0);  // measurements
+    Eigen::Quaterniond w_q_m (getMeasurement().data() + 3);  // measurements
+
+    // Transformation definitions:
+    // w_p_wm, w_q_m: world to measurement frame (robot attached mocap frame) 
+    // w_p_wb, w_q_b: world to robot base frame 
+    // b_p_bm, b_q_m: base to measurement frame (extrinsics)
+    // Error function:
+    // err = w_T_m |-| (w_T_b*b_T_m)
+
+    Eigen::Matrix<T, 6, 1> err; // error
+    err.head(3) = w_p_wm - (w_p_wb + w_q_b*b_p_bm);
+    err.tail(3) = q2v((w_q_b * b_q_m).conjugate() * w_q_m.cast<T>());
+
+    std::cout << "\n\n\n\nFAC" << std::endl;
+    std::cout << "w_p_wm: " <<  w_p_wm.transpose() << std::endl;
+    std::cout << "(w_p_wb + w_q_b*b_p_bm): " <<  (w_p_wb + w_q_b*b_p_bm).transpose() << std::endl;
+    std::cout << "b_p_bm: " <<  b_p_bm.transpose() << std::endl;
+    std::cout << "err: " <<  err.transpose() << std::endl;
+
+    // Residuals
+    Eigen::Map<Eigen::Matrix<T,6,1> > res(_residuals);
+    res = getMeasurementSquareRootInformationUpper().cast<T>() * err;
+
+    return true;
+}
+
+} // namespace wolf
+
+#endif

include/core/processor/processor_pose.h

+#ifndef PROCESSOR_POSE_NOMOVE_H
+#define PROCESSOR_POSE_NOMOVE_H
+
+// Wolf
+#include "core/sensor/sensor_base.h"
+#include "core/processor/processor_base.h"
+
+#include "core/capture/capture_pose.h"
+#include "core/sensor/sensor_pose.h"
+#include "core/factor/factor_pose_3d_with_extrinsics.h"
+
+namespace wolf {
+WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorPose);
+
+struct ParamsProcessorPose : public ParamsProcessorBase
+{
+    ParamsProcessorPose() = default;
+    ParamsProcessorPose(std::string _unique_name, const ParamsServer& _server):
+        ParamsProcessorBase(_unique_name, _server)
+    {
+    }
+    ~ParamsProcessorPose() override = default;
+    std::string print() const override
+    {
+        return "\n" + ParamsProcessorBase::print() + "\n";    
+    }
+};
+
+WOLF_PTR_TYPEDEFS(ProcessorPose);
+    
+//class
+class ProcessorPose : public ProcessorBase{
+    public:
+        ProcessorPose(ParamsProcessorPosePtr _params_pfnomove);
+        ~ProcessorPose() override = default;
+        WOLF_PROCESSOR_CREATE(ProcessorPose, ParamsProcessorPose);
+
+        void configure(SensorBasePtr _sensor) override;
+
+        void processCapture(CaptureBasePtr _incoming) override;
+        void processKeyFrame(FrameBasePtr _keyframe_ptr, const double& _time_tolerance) override;
+        bool triggerInCapture(CaptureBasePtr) const override;
+        bool triggerInKeyFrame(FrameBasePtr _keyframe_ptr, const double& _time_tolerance) const override;
+        bool storeKeyFrame(FrameBasePtr) override;
+        bool storeCapture(CaptureBasePtr) override;
+        bool voteForKeyFrame() const override;
+
+        void createFactorIfNecessary();
+
+    protected:
+        ParamsProcessorPosePtr params_pfnomove_;
+};
+
+
+
+inline bool ProcessorPose::triggerInKeyFrame(FrameBasePtr _keyframe_ptr,
+                                             const double& _time_tolerance) const
+{
+    return true;
+}
+
+inline bool ProcessorPose::triggerInCapture(CaptureBasePtr _capture) const
+{   
+    return true;
+}
+
+inline bool ProcessorPose::storeKeyFrame(FrameBasePtr)
+{
+    return true;
+}
+
+inline bool ProcessorPose::storeCapture(CaptureBasePtr)
+{
+    return true;
+}
+
+inline bool ProcessorPose::voteForKeyFrame() const
+{
+    return false;
+}
+
+} /* namespace wolf */
+
+/////////////////////////////////////////////////////////
+// IMPLEMENTATION. Put your implementation includes here
+/////////////////////////////////////////////////////////
+
+
+namespace wolf{
+
+} // namespace wolf
+
+#endif // PROCESSOR_POSE_NOMOVE_H

include/core/sensor/sensor_pose.h

+/**
+ * \file sensor_pose.h
+ *
+ *  Created on: Feb 18, 2020
+ *      \author: mfourmy
+ */
+
+#ifndef SRC_SENSOR_POSE_H_
+#define SRC_SENSOR_POSE_H_
+
+//wolf includes
+#include "core/sensor/sensor_base.h"
+#include "core/utils/params_server.h"
+
+namespace wolf {
+
+WOLF_STRUCT_PTR_TYPEDEFS(ParamsSensorPose);
+
+struct ParamsSensorPose : public ParamsSensorBase
+{
+    double std_p = 1;  // m
+    double std_o = 1;  // rad
+    ParamsSensorPose()
+    {
+        //DEFINED FOR COMPATIBILITY PURPOSES. TO BE REMOVED IN THE FUTURE.
+    }
+    ParamsSensorPose(std::string _unique_name, const ParamsServer& _server):
+        ParamsSensorBase(_unique_name, _server)
+    {
+        std_p = _server.getParam<double>(prefix + _unique_name + "/std_p");
+        std_o  = _server.getParam<double>(prefix + _unique_name + "/std_o");
+    }
+    std::string print() const override
+    {
+      return ParamsSensorBase::print()           + "\n"
+        + "std_p: "    + std::to_string(std_p)   + "\n"
+        + "std_o: "     + std::to_string(std_o)  + "\n";
+    }
+        ~ParamsSensorPose() override = default;
+};
+
+WOLF_PTR_TYPEDEFS(SensorPose);
+
+class SensorPose : public SensorBase
+{
+    protected:
+        double std_p_; // standard deviation of translation measurements
+        double std_o_; // standard deviation of orientation measurements
+
+    public:
+        SensorPose(const Eigen::VectorXd& _extrinsics_pq, const ParamsSensorPose& params);
+        SensorPose(const Eigen::VectorXd& _extrinsics_pq, ParamsSensorPosePtr params);
+        WOLF_SENSOR_CREATE(SensorPose, ParamsSensorPose, 7);
+
+        ~SensorPose() override;
+
+        double getStdP() const;
+        double getStdO() const;
+
+};
+
+inline double SensorPose::getStdP() const
+{
+    return std_p_;
+}
+
+inline double SensorPose::getStdO() const
+{
+    return std_o_;
+}
+
+// inline Matrix6d SensorPose::computeDataCov() const
+// {
+//     return (Vector6d() << std_p_*std_p_, std_p_*std_p_, std_p_*std_p_, std_o_*std_o_, std_o_*std_o_, std_o_*std_o_).finished().asDiagonal();
+// }
+
+} /* namespace wolf */
+
+#endif /* SRC_SENSOR_POSE_H_ */

src/processor/processor_pose.cpp

+/**
+ * \file processor_pose.cpp
+ *
+ *  Created on: Feb 19, 2020
+ *      \author: mfourmy
+ */
+
+#include "core/processor/processor_pose.h"
+
+
+namespace wolf{
+
+
+inline ProcessorPose::ProcessorPose(ParamsProcessorPosePtr _params_pfnomove) :
+        ProcessorBase("ProcessorPose", 3, _params_pfnomove),
+        params_pfnomove_(std::make_shared<ParamsProcessorPose>(*_params_pfnomove))
+{
+
+}
+
+void ProcessorPose::configure(SensorBasePtr _sensor)
+{
+}
+
+void ProcessorPose::createFactorIfNecessary(){
+    auto sensor_pose = std::static_pointer_cast<SensorPose>(getSensor());
+
+    while (buffer_pack_kf_.size() >= 1)
+    {
+        auto kf_it = buffer_pack_kf_.getContainer().begin();
+        TimeStamp t = kf_it->first;
+        auto cap_it = buffer_capture_.selectIterator(t, kf_it->second->time_tolerance);
+
+        // if capture with corresponding timestamp is not found, stop and assume you will get it later
+        if (cap_it == buffer_capture_.getContainer().end())
+        {
+            return;
+        }
+        else
+        {
+            // if a corresponding capture exists, link it to the KF and create a factor
+            auto cap = std::static_pointer_cast<CapturePose>(cap_it->second);
+            cap->link(kf_it->second->key_frame);
+            FeatureBasePtr feat = FeatureBase::emplace<FeatureBase>(cap, "Pose", cap->getData(), cap->getDataCovariance());
+            FactorPose3dWithExtrinsicsPtr fac = FactorBase::emplace<FactorPose3dWithExtrinsics>(feat, feat, nullptr, false, TOP_MOTION);
+
+            // erase removes range [first, last): it does not removes last
+            // so increment the iterator so that it points to the next element in the container
+            buffer_pack_kf_.getContainer().erase(buffer_pack_kf_.getContainer().begin(), std::next(kf_it));
+            buffer_capture_.getContainer().erase(buffer_capture_.getContainer().begin(), std::next(cap_it));
+        }
+    }
+}
+
+
+inline void ProcessorPose::processCapture(CaptureBasePtr _capture)
+{
+    if (!_capture)
+    {
+        WOLF_ERROR("Received capture is nullptr.");
+        return;
+    }
+    // nothing to do if any of the two buffer is empty
+    if(buffer_pack_kf_.empty()){
+        WOLF_DEBUG("PInertialKinematic: KF pack buffer empty, time ",  _capture->getTimeStamp());
+        return;
+    }
+    if(buffer_pack_kf_.empty()){
+        WOLF_DEBUG("PInertialKinematics: Capture buffer empty, time ",  _capture->getTimeStamp());
+        return;
+    }
+
+    createFactorIfNecessary();
+
+}
+
+inline void ProcessorPose::processKeyFrame(FrameBasePtr _keyframe_ptr, const double& _time_tolerance)
+{
+    if (!_keyframe_ptr)
+    {
+        WOLF_ERROR("Received KF is nullptr.");
+        return;
+    }
+    // nothing to do if any of the two buffer is empty
+    if(buffer_pack_kf_.empty()){
+        WOLF_DEBUG("ProcessorPose: KF pack buffer empty, time ",  _keyframe_ptr->getTimeStamp());
+        return;
+    }
+    if(buffer_pack_kf_.empty()){
+        WOLF_DEBUG("ProcessorPose: Capture buffer empty, time ",  _keyframe_ptr->getTimeStamp());
+        return;
+    }
+
+    createFactorIfNecessary();
+}
+
+
+
+
+
+} /* namespace wolf */
+
+// Register in the FactoryProcessor
+#include "core/processor/factory_processor.h"
+namespace wolf {
+WOLF_REGISTER_PROCESSOR(ProcessorPose);
+WOLF_REGISTER_PROCESSOR_AUTO(ProcessorPose);
+} // namespace wolf

src/sensor/sensor_pose.cpp

+/**
+ * \file sensor_pose.cpp
+ *
+ *  Created on: Feb 18, 2020
+ *      \author: mfourmy
+ */
+
+#include "core/sensor/sensor_pose.h"
+
+#include "core/state_block/state_block.h"
+#include "core/state_block/state_quaternion.h"
+#include "core/math/rotations.h"
+
+namespace wolf {
+
+SensorPose::SensorPose(const Eigen::VectorXd& _extrinsics_pq, const ParamsSensorPose& _intrinsics) :
+                        SensorBase("SensorPose", std::make_shared<StateBlock>(_extrinsics_pq.head(3), true), std::make_shared<StateQuaternion>(_extrinsics_pq.tail(4), true), nullptr, 6),
+                        std_p_(_intrinsics.std_p),
+                        std_o_(_intrinsics.std_o)
+{
+    assert(_extrinsics_pq.size() == 7 && "Bad extrinsics vector size! Should be 7 for 3d.");
+
+    noise_cov_ = Matrix6d::Identity();
+    // noise_cov_ = (Eigen::Vector6d() << std_p_*std_p_, std_p_*std_p_, std_p_*std_p_, std_o_*std_o_, std_o_*std_o_, std_o_*std_o_).finished().asDiagonal();
+    setNoiseCov(noise_cov_); // sets also noise_std_
+}
+
+SensorPose::SensorPose(const Eigen::VectorXd& _extrinsics_pq, ParamsSensorPosePtr _intrinsics) :
+        SensorPose(_extrinsics_pq, *_intrinsics)
+{
+    //
+}
+
+SensorPose::~SensorPose()
+{
+    //
+}
+
+} // namespace wolf
+
+// Register in the FactorySensor
+#include "core/sensor/factory_sensor.h"
+namespace wolf {
+WOLF_REGISTER_SENSOR(SensorPose);
+WOLF_REGISTER_SENSOR_AUTO(SensorPose);
+}

src/yaml/sensor_pose_yaml.cpp

+/**
+ * \file sensor_pose_yaml.cpp
+ *
+ *  Created on: Feb 18, 2020
+ *      \author: mfourmy
+ */
+
+// wolf yaml
+#include "core/yaml/yaml_conversion.h"
+
+// wolf
+#include "core/sensor/sensor_pose.h"
+#include "core/common/factory.h"
+
+// yaml-cpp library
+#include <yaml-cpp/yaml.h>
+
+namespace wolf
+{
+
+namespace
+{
+static ParamsSensorBasePtr createParamsSensorPose(const std::string & _filename_dot_yaml)
+{
+    YAML::Node config = YAML::LoadFile(_filename_dot_yaml);
+
+    if (config["type"].as<std::string>() == "SensorPose")
+    {
+        ParamsSensorPosePtr params = std::make_shared<ParamsSensorPose>();
+
+        params->std_p = config["std_p"].as<double>();
+        params->std_o = config["std_o"].as<double>();
+
+        return params;
+    }
+
+    std::cout << "Bad configuration file. No sensor type found." << std::endl;
+    return nullptr;
+}
+
+// Register in the FactorySensor
+const bool WOLF_UNUSED registered_odom_3d_intr = FactoryParamsSensor::registerCreator("SensorPose", createParamsSensorPose);
+
+} // namespace [unnamed]
+
+} // namespace wolf
+

test/CMakeLists.txt

@@ -209,6 +209,10 @@ target_link_libraries(gtest_factor_relative_pose_2d_with_extrinsics ${PLUGIN_NAM
 wolf_add_gtest(gtest_factor_relative_pose_3d gtest_factor_relative_pose_3d.cpp)
 target_link_libraries(gtest_factor_relative_pose_3d ${PLUGIN_NAME})
 
+# FactorPose3dWithExtrinsics class test
+wolf_add_gtest(gtest_factor_pose_3d_with_extrinsics gtest_factor_pose_3d_with_extrinsics.cpp)
+target_link_libraries(gtest_factor_pose_3d_with_extrinsics ${PLUGIN_NAME})
+
 # MakePosDef function test
 wolf_add_gtest(gtest_make_posdef gtest_make_posdef.cpp)
 target_link_libraries(gtest_make_posdef ${PLUGIN_NAME})
@@ -253,6 +257,10 @@ target_link_libraries(gtest_processor_tracker_landmark_dummy ${PLUGIN_NAME} dumm
 wolf_add_gtest(gtest_sensor_diff_drive gtest_sensor_diff_drive.cpp)
 target_link_libraries(gtest_sensor_diff_drive ${PLUGIN_NAME})
 
+# SensorDiffDriveSelfcalib class test
+wolf_add_gtest(gtest_sensor_pose gtest_sensor_pose.cpp)
+target_link_libraries(gtest_sensor_pose ${PLUGIN_NAME})
+
 IF (Ceres_FOUND)
 	# SolverCeres test
 	wolf_add_gtest(gtest_solver_ceres gtest_solver_ceres.cpp)

test/gtest_factor_pose_3d_with_extrinsics.cpp

+/**
+ * \file gtest_factor_pose_with_extrinsics.cpp
+ *
+ *  Created on: Feb a9, 2020
+ *      \author: mfourmy
+ */
+
+#include "core/utils/utils_gtest.h"
+#include "core/ceres_wrapper/solver_ceres.h"
+#include "core/factor/factor_relative_pose_3d.h"
+#include "core/sensor/sensor_pose.h"
+#include "core/capture/capture_odom_3d.h"
+#include "core/processor/processor_pose.h"
+#include "core/factor/factor_pose_3d_with_extrinsics.h"
+
+
+using namespace Eigen;
+using namespace wolf;
+using std::cout;
+using std::endl;
+
+const Vector3d zero3 = Vector3d::Zero();
+
+
+// 2 random 3d positions
+Vector3d w_p_wb1 = Vector3d::Random();
+Quaterniond w_q_b1 = Quaterniond::UnitRandom();
+Vector3d w_p_wb2 = Vector3d::Random();
+Quaterniond w_q_b2 = Quaterniond::UnitRandom();
+
+// Vector3d w_p_wb1 = Vector3d::Zero();
+// Quaterniond w_q_b1 = Quaterniond::Identity();
+// Vector3d w_p_wb2 = Vector3d::Ones();
+// Quaterniond w_q_b2 = Quaterniond::Identity();
+// Quaterniond w_q_b2(0,0,0,1);
+// Quaterniond w_q_b2 = Quaterniond::UnitRandom();
+
+// pose vectors
+Vector7d pose1((Vector7d() << w_p_wb1, w_q_b1.coeffs()).finished()); 
+Vector7d pose2((Vector7d() << w_p_wb2, w_q_b2.coeffs()).finished()); 
+// unitary covariance
+Matrix6d cov_odom = pow(0.01,2)*Matrix6d::Identity(); 
+Matrix6d cov_mocap = pow(0.0001,2)*Matrix6d::Identity(); 
+
+/////////////////////////////////////////
+// extrinsics ground truth and mocap data
+// Vector3d b_p_bm = Vector3d::Zero();
+// Vector3d b_p_bm = 0.1*Vector3d::Ones();
+Vector3d b_p_bm((Vector3d() << 0.1,0,0).finished());
+// Vector3d b_p_bm = Vector3d::Random();
+// Quaterniond b_q_m = Quaterniond::Identity();
+Quaterniond b_q_m(0,0,0,1);
+// Quaterniond b_q_m = Quaterniond::UnitRandom();
+
+Vector3d w_p_wb1_meas = w_p_wb1 + w_q_b1 * b_p_bm;
+Vector3d w_p_wb2_meas = w_p_wb2 + w_q_b2 * b_p_bm; 
+Quaterniond w_q_b1_meas = w_q_b1 * b_q_m;
+Quaterniond w_q_b2_meas = w_q_b2 * b_q_m;
+Vector7d pose1_meas((Vector7d() << w_p_wb1_meas, w_q_b1_meas.coeffs()).finished());
+Vector7d pose2_meas((Vector7d() << w_p_wb2_meas, w_q_b2_meas.coeffs()).finished());
+
+/////////////////////
+// relative odom data 
+Vector3d b1_p_b1b2 = w_q_b1.conjugate()*(w_p_wb2 - w_p_wb1);
+Quaterniond b1_q_b2 = w_q_b1.conjugate()*w_q_b2;
+Vector7d pose_12((Vector7d() << b1_p_b1b2, b1_q_b2.coeffs()).finished());
+
+// Problem and solver
+ProblemPtr problem = Problem::create("PO", 3);
+SolverCeres solver(problem);
+
+// pose sensor and proc (to get extrinsics in the prb)
+auto intr_sp = std::make_shared<ParamsSensorPose>();
+Vector7d extr = (Vector7d() << b_p_bm, b_q_m.coeffs()).finished();
+auto sensor_pose = problem->installSensor("SensorPose", "pose", extr, intr_sp);
+auto params_proc = std::make_shared<ParamsProcessorPose>();
+auto proc_pose = problem->installProcessor("ProcessorPose", "pose", sensor_pose, params_proc);
+
+// Two frames
+FrameBasePtr KF1 = problem->emplaceFrame(1, pose1);
+FrameBasePtr KF2 = problem->emplaceFrame(2, pose2);
+
+
+///////////////////
+// Create factor graph
+// Odometry capture, feature and factor
+auto cap_odom = CaptureBase::emplace<CapturePose>(KF2, 2, nullptr, pose_12, cov_odom);
+auto fea_odom = FeatureBase::emplace<FeatureBase>(cap_odom, "odom", pose_12, cov_odom);
+FactorRelativePose3dPtr fac_odom = FactorBase::emplace<FactorRelativePose3d>(fea_odom, fea_odom, KF1, nullptr, false, TOP_MOTION);
+
+// Captures mocap
+auto cap_mocap1 = CaptureBase::emplace<CapturePose>(KF1, 1, sensor_pose, pose1_meas, cov_mocap);
+auto fea_mocap1 = FeatureBase::emplace<FeatureBase>(cap_mocap1, "pose", pose1_meas, cov_mocap);
+FactorPose3dWithExtrinsicsPtr fac_mocap1 = FactorBase::emplace<FactorPose3dWithExtrinsics>(fea_mocap1, fea_mocap1, nullptr, false, TOP_MOTION);
+
+auto cap_mocap2 = CaptureBase::emplace<CapturePose>(KF2, 2, sensor_pose, pose2_meas, cov_mocap);
+auto fea_mocap2 = FeatureBase::emplace<FeatureBase>(cap_mocap2, "pose", pose2_meas, cov_mocap);
+FactorPose3dWithExtrinsicsPtr fac_mocap2 = FactorBase::emplace<FactorPose3dWithExtrinsics>(fea_mocap2, fea_mocap2, nullptr, false, TOP_MOTION);
+
+
+
+TEST(FactorPose3dWithExtrinsics, check_tree)
+{
+    ASSERT_TRUE(problem->check(0));
+}
+
+TEST(FactorPose3dWithExtrinsics, solve)
+{
+
+    // Fix frame 0, perturb frm1
+    sensor_pose->unfixExtrinsics();
+    // sensor_pose->fixExtrinsics();
+    // frm0->setState(x0);
+
+
+    problem->print(4, true, true, true);
+    problem->perturb();
+
+    std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+
+    std::cout << report << std::endl;
+
+    problem->print(4, true, true, true);
+
+    // ASSERT_MATRIX_APPROX(KF1->getStateVector(), pose1, 1e-6);
+    ASSERT_MATRIX_APPROX(KF2->getStateVector(), pose2, 1e-6);
+    ASSERT_MATRIX_APPROX(sensor_pose->getP()->getState(), b_p_bm, 1e-6);
+    ASSERT_MATRIX_APPROX(sensor_pose->getO()->getState(), b_q_m.coeffs(), 1e-6);
+
+}
+
+int main(int argc, char **argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}
\ No newline at end of file

test/gtest_sensor_pose.cpp

+/**
+ * \file gtest_sensor_pose.cpp
+ *
+ *  Created on: Feb 18, 2020
+ *      \author: mfourmy
+ */
+
+#include "core/sensor/sensor_pose.h"
+#include "core/utils/utils_gtest.h"
+
+#include <cstdio>
+
+using namespace wolf;
+using namespace Eigen;
+
+TEST(Pose, constructor)
+{
+
+    auto intr = std::make_shared<ParamsSensorPose>();
+    Vector7d extr; extr << 1,2,3,4,5,6,7;
+
+    auto sen = std::make_shared<SensorPose>(extr, intr);
+
+    ASSERT_NE(sen, nullptr);
+
+    ASSERT_MATRIX_APPROX(sen->getP()->getState(), extr.head<3>(), 1e-12);
+    ASSERT_MATRIX_APPROX(sen->getO()->getState(), extr.tail<4>(), 1e-12);
+}
+
+TEST(Pose, getParams)
+{
+    auto intr = std::make_shared<ParamsSensorPose>();
+    intr->std_p = 2;
+    intr->std_o = 3;
+
+    Vector7d extr; extr << 1,2,3,4,5,6,7;
+
+    auto sen = std::make_shared<SensorPose>(extr, intr);
+
+    ASSERT_EQ(sen->getStdP(), intr->std_p); 
+    ASSERT_EQ(sen->getStdO(), intr->std_o); 
+}
+
+TEST(Pose, create)
+{
+    auto intr = std::make_shared<ParamsSensorPose>();
+    intr->std_p = 2;
+    intr->std_o = 3;
+
+    Vector7d extr; extr << 1,2,3,4,5,6,7;
+
+    auto sen_base = SensorPose::create("name", extr, intr);
+
+    auto sen = std::static_pointer_cast<SensorPose>(sen_base);
+
+    ASSERT_EQ(sen->getStdP(), intr->std_p); 
+    ASSERT_EQ(sen->getStdO(), intr->std_o);
+}
+
+int main(int argc, char **argv)
+{
+    testing::InitGoogleTest(&argc, argv);
+    return RUN_ALL_TESTS();
+}
+