Resolve "Compass measurements"

CMakeLists.txt

@@ -132,9 +132,11 @@ SET(HDRS_MATH
 SET(HDRS_UTILS
   )
 SET(HDRS_CAPTURE
+  include/imu/capture/capture_compass.h
   include/imu/capture/capture_imu.h
   )
 SET(HDRS_FACTOR
+  include/imu/factor/factor_compass_3d.h
   include/imu/factor/factor_imu.h
   )
 SET(HDRS_FEATURE
@@ -143,9 +145,11 @@ SET(HDRS_FEATURE
 SET(HDRS_LANDMARK
   )
 SET(HDRS_PROCESSOR
+  include/imu/processor/processor_compass.h
   include/imu/processor/processor_imu.h
   )
 SET(HDRS_SENSOR
+  include/imu/sensor/sensor_compass.h
   include/imu/sensor/sensor_imu.h
   )
 SET(HDRS_SOLVER
@@ -174,10 +178,12 @@ SET(SRCS_FEATURE
 SET(SRCS_LANDMARK
   )
 SET(SRCS_PROCESSOR
+  src/processor/processor_compass.cpp
   src/processor/processor_imu.cpp
   test/processor_imu_UnitTester.cpp
 )
 SET(SRCS_SENSOR
+  src/sensor/sensor_compass.cpp
   src/sensor/sensor_imu.cpp
   )
 SET(SRCS_DTASSC

include/imu/capture/capture_compass.h

+#ifndef CAPTURE_COMPASS_H_
+#define CAPTURE_COMPASS_H_
+
+//Wolf includes
+#include "core/capture/capture_base.h"
+#include "imu/sensor/sensor_compass.h"
+
+//std includes
+//
+
+namespace wolf {
+
+WOLF_PTR_TYPEDEFS(CaptureCompass);
+
+//class CaptureCompass
+class CaptureCompass : public CaptureBase
+{
+    protected:
+        Eigen::Vector3d data_;              ///< Raw data (magnetic field).
+        Eigen::Matrix3d data_covariance_;   ///< Noise of the data.
+
+    public:
+
+        CaptureCompass(const TimeStamp& _ts,
+                       SensorBasePtr _sensor_ptr,
+                       const Eigen::Vector3d& _data) :
+            CaptureBase("CaptureCompass", _ts, _sensor_ptr),
+            data_(_data)
+        {
+            auto sensor_compass = std::dynamic_pointer_cast<SensorCompass>(_sensor_ptr);
+            assert(sensor_compass != nullptr && "CaptureCompass: Sensor should be of type SensorCompass");
+            data_covariance_ = sensor_compass->computeMeasurementCovariance(data_);
+        }
+
+        CaptureCompass(const TimeStamp& _ts,
+                       SensorBasePtr _sensor_ptr,
+                       const Eigen::Vector3d& _data,
+                       const Eigen::Matrix3d& _data_covariance) :
+            CaptureBase("CaptureCompass", _ts, _sensor_ptr),
+            data_(_data),
+            data_covariance_(_data_covariance)
+        {
+            assert((_sensor_ptr == nullptr or std::dynamic_pointer_cast<SensorCompass>(_sensor_ptr) != nullptr) && "CaptureCompass: Sensor should be of type SensorCompass");
+        }
+
+        ~CaptureCompass() override
+        {
+
+        }
+
+        const Eigen::Vector3d& getData() const
+        {
+            return data_;
+        }
+
+        const Eigen::Matrix3d& getDataCovariance() const
+        {
+            return data_covariance_;
+        }
+};
+
+} //namespace wolf
+#endif

include/imu/factor/factor_compass_3d.h

+#ifndef FACTOR_COMPASS_3D_H_
+#define FACTOR_COMPASS_3D_H_
+
+#include "core/factor/factor_autodiff.h"
+
+namespace wolf
+{
+
+WOLF_PTR_TYPEDEFS(FactorCompass3d);
+
+class FactorCompass3d : public FactorAutodiff<FactorCompass3d, 3, 4, 4, 3, 3>
+{
+    public:
+        FactorCompass3d(const FeatureBasePtr&   _feat,
+                        const ProcessorBasePtr& _processor_ptr,
+                        bool                    _apply_loss_function,
+                        FactorStatus            _status = FAC_ACTIVE) :
+            FactorAutodiff("FactorCompass3d",
+                           TOP_ABS,
+                           _feat,
+                           nullptr,
+                           nullptr,
+                           nullptr,
+                           nullptr,
+                           _processor_ptr,
+                           _apply_loss_function,
+                           _status,
+                           _feat->getFrame()->getO(),
+                           _feat->getCapture()->getSensor()->getO(),
+                           _feat->getCapture()->getSensor()->getIntrinsic(),
+                           _feat->getCapture()->getSensor()->getStateBlock('F'))
+        {
+        }
+
+        ~FactorCompass3d() override { /* nothing */ }
+
+        template<typename T>
+        bool operator () (const T* const _frm_q,
+                          const T* const _sen_q,
+                          const T* const _sen_bias,
+                          const T* const _field,
+                          T* _residual) const
+        {
+            using namespace Eigen;
+
+            Map<const Quaternion<T>> frm_q(_frm_q);
+            Map<const Quaternion<T>> sen_q(_sen_q);
+            Map<const Matrix<T,3,1>> sen_bias(_sen_bias);
+            Map<const Matrix<T,3,1>> field(_field);
+            Map<Matrix<T,3,1>> res(_residual);
+
+            Matrix<T,3,1> exp = sen_q.conjugate() * frm_q.conjugate() * field + sen_bias;
+
+            res  = getMeasurementSquareRootInformationUpper() * (getMeasurement() - exp);
+
+            return true;
+        }
+};
+
+} /* namespace wolf */
+
+#endif /* FACTOR_COMPASS_3D_H_ */

include/imu/processor/processor_compass.h

+#ifndef INCLUDE_IMU_PROCESSOR_PROCESSORCOMPASS_H_
+#define INCLUDE_IMU_PROCESSOR_PROCESSORCOMPASS_H_
+
+#include "core/processor/processor_base.h"
+
+namespace wolf {
+
+WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorCompass);
+
+struct ParamsProcessorCompass : public ParamsProcessorBase
+{
+    ParamsProcessorCompass() = default;
+    ParamsProcessorCompass(std::string _unique_name, const ParamsServer& _server):
+        ParamsProcessorBase(_unique_name, _server)
+    {
+        //
+    }
+    std::string print() const override
+    {
+        return "\n" + ParamsProcessorBase::print();
+    }
+};
+
+WOLF_PTR_TYPEDEFS(ProcessorCompass);
+
+//class
+class ProcessorCompass : public ProcessorBase
+{
+    protected:
+        ParamsProcessorCompassPtr params_compass_;
+
+    public:
+        ProcessorCompass(ParamsProcessorCompassPtr);
+        ~ProcessorCompass() override;
+        void configure(SensorBasePtr) override { };
+
+        WOLF_PROCESSOR_CREATE(ProcessorCompass, ParamsProcessorCompass);
+
+    protected:
+        void processCapture(CaptureBasePtr) override;
+        void processKeyFrame(FrameBasePtr, const double&) override;
+        void processMatch(FrameBasePtr, CaptureBasePtr);
+
+        bool triggerInCapture(CaptureBasePtr _cap) const override { return true;};
+        bool triggerInKeyFrame(FrameBasePtr _frm, const double& _time_tol) const override { return true;};
+
+        bool storeKeyFrame(FrameBasePtr _frm) override { return false;};
+        bool storeCapture(CaptureBasePtr _cap) override { return false;};
+
+        bool voteForKeyFrame() const override { return false;};
+};
+
+} /* namespace wolf */
+
+#endif /* INCLUDE_IMU_PROCESSOR_PROCESSORCOMPASS_H_ */

include/imu/sensor/sensor_compass.h

+#ifndef SENSOR_SENSOR_COMPASS_H_
+#define SENSOR_SENSOR_COMPASS_H_
+
+//wolf includes
+#include "core/sensor/sensor_base.h"
+#include "core/utils/params_server.h"
+#include <iostream>
+
+namespace wolf {
+
+WOLF_STRUCT_PTR_TYPEDEFS(ParamsSensorCompass);
+
+struct ParamsSensorCompass : public ParamsSensorBase
+{
+    double stdev_noise;
+
+    std::string     field_prior_mode;
+    Eigen::Vector3d field_prior_state;
+    Eigen::Vector3d field_prior_sigma;
+
+    std::string     bias_prior_mode;
+    Eigen::Vector3d bias_prior_state;
+    Eigen::Vector3d bias_prior_sigma;
+
+    ParamsSensorCompass() = default;
+    ParamsSensorCompass(std::string _unique_name, const wolf::ParamsServer& _server)
+      : ParamsSensorBase(_unique_name, _server)
+    {
+        stdev_noise         = _server.getParam<double>          (prefix + _unique_name + "/stdev_noise");
+
+        field_prior_mode    = _server.getParam<std::string>     (prefix + _unique_name + "/field_prior_mode");
+        field_prior_state   = _server.getParam<Eigen::Vector3d> (prefix + _unique_name + "/field_prior_state");
+        if (field_prior_mode == "factor")
+            field_prior_sigma = _server.getParam<Eigen::Vector3d> (prefix + _unique_name + "/field_prior_sigma");
+
+        bias_prior_mode    = _server.getParam<std::string>     (prefix + _unique_name + "/bias_prior_mode");
+        bias_prior_state   = _server.getParam<Eigen::Vector3d> (prefix + _unique_name + "/bias_prior_state");
+        if (bias_prior_mode == "factor")
+            bias_prior_sigma = _server.getParam<Eigen::Vector3d> (prefix + _unique_name + "/bias_prior_sigma");
+    }
+    ~ParamsSensorCompass() override = default;
+    std::string print() const override
+    {
+        return ParamsSensorBase::print()  + "\n"
+        + "stdev_noise: "       + std::to_string(stdev_noise)       + "\n"
+        + "field_prior_mode: "  + field_prior_mode                  + "\n"
+        + "field_prior_state: print not implemented. \n"
+        + "field_prior_sigma: print not implemented. \n"
+        + "bias_prior_mode: "   + bias_prior_mode                   + "\n"
+        + "bias_prior_state:  print not implemented. \n"
+        + "bias_prior_sigma:  print not implemented. \n";
+    }
+};
+
+WOLF_PTR_TYPEDEFS(SensorCompass);
+
+class SensorCompass : public SensorBase
+{
+    public:
+        SensorCompass(const Eigen::VectorXd& _extrinsics, ParamsSensorCompassPtr _params);
+        SensorCompass(const Eigen::VectorXd& _extrinsics, const ParamsSensorCompass& _params);
+        WOLF_SENSOR_CREATE(SensorCompass, ParamsSensorCompass, 7);
+
+        ~SensorCompass() override;
+        ParamsSensorCompassConstPtr getParams() const;
+
+        Eigen::Matrix3d computeMeasurementCovariance(const Eigen::Vector3d& _field) const;
+
+    protected:
+        ParamsSensorCompassPtr params_compass_;
+};
+
+inline ParamsSensorCompassConstPtr SensorCompass::getParams() const
+{
+    return params_compass_;
+}
+
+} /* namespace wolf */
+
+#endif /* SENSOR_SENSOR_DIFF_DRIVE_H_ */

include/imu/sensor/sensor_imu.h

@@ -116,4 +116,4 @@ inline double SensorImu::getAbRateStdev() const
 
 } // namespace wolf
 
-#endif // SENSOR_Imu_H
+#endif // SENSOR_IMU_H

src/processor/processor_compass.cpp

+#include "imu/processor/processor_compass.h"
+#include "imu/capture/capture_compass.h"
+#include "imu/factor/factor_compass_3d.h"
+
+namespace wolf {
+
+ProcessorCompass::ProcessorCompass(ParamsProcessorCompassPtr _params) :
+        ProcessorBase("ProcessorCompass", 3, _params)
+{
+    //
+}
+
+ProcessorCompass::~ProcessorCompass()
+{
+    //
+}
+
+void ProcessorCompass::processCapture(CaptureBasePtr _capture)
+{
+    // Search for any stored frame within time tolerance of capture
+    auto frame_pack = buffer_pack_kf_.select(_capture->getTimeStamp(), params_->time_tolerance);
+    if (frame_pack)
+    {
+        processMatch(frame_pack->key_frame, _capture);
+
+        // remove the frame and older frames
+        buffer_pack_kf_.removeUpTo(frame_pack->key_frame->getTimeStamp());
+    }
+    // Otherwise: store capture
+    // Note that more than one processor can be emplacing frames, so an older frame can arrive later than this one.
+    buffer_capture_.add(_capture->getTimeStamp(), _capture);
+}
+
+void ProcessorCompass::processKeyFrame(FrameBasePtr _frame, const double& _time_tolerance)
+{
+    // Search for any stored capture within time tolerance of frame
+    auto capture = buffer_capture_.select(_frame->getTimeStamp(), _time_tolerance);
+    if (capture)
+    {
+        processMatch(_frame, capture);
+
+        // remove (very) old captures
+        buffer_capture_.removeUpTo(_frame->getTimeStamp() - 10);
+    }
+    // Otherwise: If frame is more recent than any capture in buffer -> store frame to be processed later in processCapture
+    else if (buffer_capture_.selectLastAfter(_frame->getTimeStamp(), _time_tolerance) == nullptr)
+    {
+        // store frame
+        buffer_pack_kf_.add(_frame, _time_tolerance);
+    }
+    // Otherwise: There are captures more recent than the frame but none that match with it -> discard frame
+}
+
+void ProcessorCompass::processMatch(FrameBasePtr _frame, CaptureBasePtr _capture)
+{
+    auto cap_compass = std::dynamic_pointer_cast<CaptureCompass>(_capture);
+    assert(cap_compass && "ProcessorCompass::processMatch capture not of type CaptureCompass");
+
+    // link capture to the frame
+    _capture->link(_frame);
+
+    // emplace feature
+    auto feat = FeatureBase::emplace<FeatureBase>(cap_compass,
+                                                  "FeatureCompass",
+                                                  cap_compass->getData(),
+                                                  cap_compass->getDataCovariance());
+
+    // emplace factor
+    FactorBase::emplace<FactorCompass3d>(feat,
+                                         feat,
+                                         shared_from_this(),
+                                         params_->apply_loss_function);
+}
+
+} /* namespace wolf */

src/sensor/sensor_compass.cpp

+#include "imu/sensor/sensor_compass.h"
+#include "core/state_block/state_block.h"
+#include "core/state_block/state_quaternion.h"
+
+namespace wolf
+{
+
+SensorCompass::SensorCompass(const Eigen::VectorXd& _extrinsics,
+                             ParamsSensorCompassPtr _params) :
+        SensorBase("SensorCompass",
+                   std::make_shared<StateBlock>(_extrinsics.head(3), true),
+                   std::make_shared<StateQuaternion>(_extrinsics.tail(4), true),
+                   std::make_shared<StateBlock>(_params->bias_prior_state,
+                                                _params->bias_prior_mode == "fix"),
+                   3,
+                   false, // P static
+                   false, // O static
+                   false),// I static
+        params_compass_(_params)
+{
+    // Bias prior
+    if (params_compass_->bias_prior_mode == "factor")
+        addPriorIntrinsics(params_compass_->bias_prior_state,
+                           (params_compass_->bias_prior_sigma.array() * params_compass_->bias_prior_sigma.array()).matrix().asDiagonal());
+
+    // Global Magnetic Field 'F' state block
+    addStateBlock('F',
+                  std::make_shared<StateBlock>(params_compass_->field_prior_state,
+                                               params_compass_->field_prior_mode == "fix"));
+
+    if (params_compass_->field_prior_mode == "factor")
+        addPriorParameter('F',
+                          params_compass_->field_prior_state,
+                          params_compass_->field_prior_sigma.cwiseAbs2().asDiagonal());
+}
+
+SensorCompass::SensorCompass(const Eigen::VectorXd& _extrinsics, const ParamsSensorCompass& _params) :
+        SensorCompass(_extrinsics, std::make_shared<ParamsSensorCompass>(_params))
+{
+
+}
+
+SensorCompass::~SensorCompass()
+{
+    // Auto-generated destructor stub
+}
+
+Eigen::Matrix3d SensorCompass::computeMeasurementCovariance(const Eigen::Vector3d& _field) const
+{
+    Eigen::Vector3d v1(_field.normalized());
+
+    // Find auxiliar vector not "too parallel" to v1
+    Eigen::Vector3d aux(Eigen::Vector3d::Random().normalized());
+    while (v1.dot(aux) > 0.9)
+        aux = Eigen::Vector3d::Random().normalized();
+
+    // Find v2 and v3 orthogonals to v1
+    Eigen::Vector3d v2, v3;
+    v2 = v1.cross(aux).normalized();
+    v3 = v1.cross(v2).normalized();
+
+    // Take v1, v2 and v3 as covariance eigenvectors
+    Eigen::Matrix3d eigen_vectors;
+    eigen_vectors.col(0) = v1;
+    eigen_vectors.col(1) = v2;
+    eigen_vectors.col(2) = v3;
+
+    // Set the covariance eigenvalues
+    Eigen::Matrix3d eigen_values(Eigen::Matrix3d::Zero());
+    eigen_values(0,0) = 1e6; // High covariance along field measurement direction to avoid gain problems
+    eigen_values(1,1) = pow(params_compass_->stdev_noise,2);
+    eigen_values(2,2) = pow(params_compass_->stdev_noise,2);
+
+    return eigen_vectors * eigen_values * eigen_vectors.inverse();
+}
+
+} /* namespace wolf */
+
+// Register in the FactorySensor
+#include "core/sensor/factory_sensor.h"
+namespace wolf {
+WOLF_REGISTER_SENSOR(SensorCompass);
+WOLF_REGISTER_SENSOR_AUTO(SensorCompass);
+} // namespace wolf
+

src/sensor/sensor_imu.cpp

@@ -11,7 +11,12 @@ SensorImu::SensorImu(const Eigen::VectorXd& _extrinsics, ParamsSensorImuPtr _par
 }
 
 SensorImu::SensorImu(const Eigen::VectorXd& _extrinsics, const ParamsSensorImu& _params) :
-        SensorBase("SensorImu", std::make_shared<StateBlock>(_extrinsics.head(3), true), std::make_shared<StateQuaternion>(_extrinsics.tail(4), true), std::make_shared<StateBlock>(6, false, nullptr), (Eigen::Vector6d()<<_params.a_noise,_params.a_noise,_params.a_noise,_params.w_noise,_params.w_noise,_params.w_noise).finished(), false, false, true),
+        SensorBase("SensorImu",
+                   std::make_shared<StateBlock>(_extrinsics.head(3), true),
+                   std::make_shared<StateQuaternion>(_extrinsics.tail(4), true),
+                   std::make_shared<StateBlock>(6, false, nullptr),
+                   (Eigen::Vector6d()<<_params.a_noise,_params.a_noise,_params.a_noise,_params.w_noise,_params.w_noise,_params.w_noise).finished(),
+                   false, false, true),
         a_noise(_params.a_noise),
         w_noise(_params.w_noise),
         ab_initial_stdev(_params.ab_initial_stdev),

test/CMakeLists.txt

@@ -28,6 +28,9 @@ target_link_libraries(gtest_processor_imu_jacobians ${PLUGIN_NAME} ${wolf_LIBRAR
 wolf_add_gtest(gtest_feature_imu gtest_feature_imu.cpp)
 target_link_libraries(gtest_feature_imu ${PLUGIN_NAME} ${wolf_LIBRARY})
 
+wolf_add_gtest(gtest_factor_compass_3d gtest_factor_compass_3d.cpp)
+target_link_libraries(gtest_factor_compass_3d ${PLUGIN_NAME} ${wolf_LIBRARY})
+
 # Has been excluded from tests for god knows how long, so thing is broken.
 # Maybe call an archeologist to fix this thing?
 # wolf_add_gtest(gtest_factor_imu gtest_factor_imu.cpp)
@@ -35,3 +38,6 @@ target_link_libraries(gtest_feature_imu ${PLUGIN_NAME} ${wolf_LIBRARY})
 
 wolf_add_gtest(gtest_processor_motion_intrinsics_update gtest_processor_motion_intrinsics_update.cpp)
 target_link_libraries(gtest_processor_motion_intrinsics_update ${PLUGIN_NAME} ${wolf_LIBRARY})
+
+wolf_add_gtest(gtest_sensor_compass gtest_sensor_compass.cpp)
+target_link_libraries(gtest_sensor_compass ${PLUGIN_NAME} ${wolf_LIBRARY})
\ No newline at end of file

test/gtest_factor_compass_3d.cpp

+#include "core/utils/utils_gtest.h"
+
+#include "imu/internal/config.h"
+#include "imu/factor/factor_compass_3d.h"
+#include "imu/capture/capture_compass.h"
+#include "imu/sensor/sensor_compass.h"
+#include "imu/processor/processor_compass.h"
+
+#include "core/ceres_wrapper/solver_ceres.h"
+#include "core/math/rotations.h"
+
+using namespace Eigen;
+using namespace wolf;
+using std::cout;
+using std::endl;
+
+std::string wolf_root = _WOLF_IMU_ROOT_DIR;
+
+// Problem and solver
+ProblemPtr problem_ptr = Problem::create("PO", 3);
+SolverCeres solver(problem_ptr);
+
+// Sensor
+auto sen = std::static_pointer_cast<SensorCompass>(problem_ptr->installSensor("SensorCompass",
+                                                                              "magnetometer",
+                                                                              (Vector7d() << 0,0,0,0,0,0,1).finished(),
+                                                                              std::make_shared<ParamsSensorCompass>()));
+
+// Frame
+FrameBasePtr frm = problem_ptr->emplaceFrame(0, (Vector7d() << 0,0,0,0,0,0,1).finished() );
+
+// Pointers
+CaptureCompassPtr cap;
+
+// Auxliary states
+Vector3d frm_p, sen_p, sen_bias, field, meas;
+Quaterniond frm_q, sen_q;
+
+void generateRandomSetup()
+{
+    // remove capture (and feature and factor) just in case
+    if (cap)
+        cap->remove();
+
+    // Random states
+    frm_p = Vector3d::Random() * 10;
+    frm_q = Quaterniond(Vector4d::Random().normalized());
+    sen_p = Vector3d::Random() * 10;
+    sen_q = Quaterniond(Vector4d::Random().normalized());
+    sen_bias = Vector3d::Random();
+    field = Vector3d::Random().normalized() * 50;
+
+    // measurement
+    meas = sen_q.conjugate() * frm_q.conjugate() * field + sen_bias;
+
+    // Set states
+    frm->getP()->setState(frm_p);
+    frm->getO()->setState(frm_q.coeffs());
+    sen->getP()->setState(sen_p);
+    sen->getO()->setState(sen_q.coeffs());
+    sen->getIntrinsic()->setState(sen_bias);
+    sen->getStateBlock('F')->setState(field);
+
+    WOLF_DEBUG("Random setup:",
+               "\n\tfrm: P = ", frm_p.transpose(), " | O = ", frm_q.coeffs().transpose(),
+               "\n\tsen: P = ", sen_p.transpose(), " | O = ", sen_q.coeffs().transpose(), " | I = ", sen_bias.transpose(),
+               "\n\tfield: F = ", field.transpose(),
+               "\n\tmeasurement = ", meas.transpose());
+
+    // Capture
+    cap = CaptureBase::emplace<CaptureCompass>(frm,
+                                               1,
+                                               sen,
+                                               meas,
+                                               Matrix3d::Identity());
+
+    // feature & factor with delta measurement
+    auto fea = FeatureBase::emplace<FeatureBase>(cap,
+                                                 "FeatureCompass",
+                                                 meas,
+                                                 Eigen::Matrix3d::Identity());
+
+    FactorBase::emplace<FactorCompass3d>(fea,
+                                         fea,
+                                         nullptr,
+                                         false);
+
+    // Fix all
+    frm->getP()->fix();
+    frm->getO()->fix();
+    sen->getP()->fix();
+    sen->getO()->fix();
+    sen->getIntrinsic()->fix();
+    sen->getStateBlock('F')->fix();
+}
+
+TEST(FactorCompass3d, solve_frm_q)
+{
+    for (int i = 0; i < 1e2; i++)
+    {
+        // Random setup
+        generateRandomSetup();
+
+        // solve
+        std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
+
+        //WOLF_INFO(report);
+
+        ASSERT_MATRIX_APPROX(frm->getO()->getState(), frm_q.coeffs(), 1e-6);
+
+        // Unfix and perturb: frame orientation
+        frm->getO()->unfix();
+        frm->getO()->perturb();
+
+        // solve
+        report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
+
+        //WOLF_INFO(report);
+
+        // check measurement and field are aligned
+        Vector3d exp_meas = sen_q.conjugate() * frm_q.conjugate() * field + sen_bias;
+
+        ASSERT_MATRIX_APPROX(meas, exp_meas, 1e-6);
+
+        // remove capture (and feature and factor) for the next loop
+        cap->remove();
+    }
+}
+
+
+TEST(FactorCompass3d, solve_sen_q)
+{
+    for (int i = 0; i < 1e2; i++)
+    {
+        // Random setup
+        generateRandomSetup();
+
+        // solve
+        std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
+
+        //WOLF_INFO(report);
+
+        ASSERT_MATRIX_APPROX(frm->getO()->getState(), frm_q.coeffs(), 1e-6);
+
+        // Unfix and perturb: frame orientation
+        sen->getO()->unfix();
+        sen->getO()->perturb();
+
+        // solve
+        report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
+
+        //WOLF_INFO(report);
+
+        // check measurement and field are aligned
+        Vector3d exp_meas = sen_q.conjugate() * frm_q.conjugate() * field + sen_bias;
+
+        ASSERT_MATRIX_APPROX(meas, exp_meas, 1e-6);
+
+        // remove capture (and feature and factor) for the next loop
+        cap->remove();
+    }
+}
+
+TEST(FactorCompass3d, solve_bias)
+{
+    for (int i = 0; i < 1e2; i++)
+    {
+        // Random setup
+        generateRandomSetup();
+
+        // Unfix and perturb: bias
+        sen->getIntrinsic()->unfix();
+        sen->getIntrinsic()->perturb(1);
+
+        // solve
+        std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
+
+        //WOLF_INFO(report);
+
+        ASSERT_MATRIX_APPROX(sen->getIntrinsic()->getState(), sen_bias, 1e-6);
+
+        // remove capture (and feature and factor) for the next loop
+        cap->remove();
+    }
+}
+
+TEST(FactorCompass3d, solve_field)
+{
+    for (int i = 0; i < 1e2; i++)
+    {
+        // Random setup
+        generateRandomSetup();
+
+        // Unfix and perturb: bias
+        sen->getStateBlock('F')->unfix();
+        sen->getStateBlock('F')->perturb(10);
+
+        // solve
+        std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
+
+        //WOLF_INFO(report);
+
+        ASSERT_MATRIX_APPROX(sen->getStateBlock('F')->getState(), field, 1e-6);
+
+        // remove capture (and feature and factor) for the next loop
+        cap->remove();
+    }
+}
+
+int main(int argc, char **argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}

test/gtest_sensor_compass.cpp

+#include "core/utils/utils_gtest.h"
+
+#include "imu/internal/config.h"
+#include "imu/capture/capture_compass.h"
+#include "imu/sensor/sensor_compass.h"
+
+#include "core/math/covariance.h"
+
+#include <cstdio>
+
+using namespace wolf;
+using namespace Eigen;
+
+std::string wolf_root = _WOLF_IMU_ROOT_DIR;
+
+TEST(SensorCompass, constructor)
+{
+    auto params = std::make_shared<ParamsSensorCompass>();
+    Vector7d extr(Vector7d::Random());
+    extr.tail<4>().normalize();
+
+    ASSERT_NEAR(extr.tail<4>().norm(), 1, 1e-6);
+
+    auto sen = std::make_shared<SensorCompass>(extr, params);
+
+    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(SensorCompass, getParams)
+{
+    auto params = std::make_shared<ParamsSensorCompass>();
+    params->stdev_noise = 0.1;
+    params->field_prior_mode = "fix";
+    params->field_prior_state = Eigen::Vector3d(1, 2, 3);
+    params->field_prior_sigma = Eigen::Vector3d(4, 5, 6);
+
+    params->bias_prior_mode = "factor";
+    params->bias_prior_state = Eigen::Vector3d(3, 2, 1);
+    params->bias_prior_sigma = Eigen::Vector3d(6, 5, 4);
+
+    Vector7d extr(Vector7d::Random());
+    extr.tail<4>().normalize();
+
+    ASSERT_NEAR(extr.tail<4>().norm(), 1, 1e-6);
+
+    auto sen = std::make_shared<SensorCompass>(extr, params);
+
+    ASSERT_NE(sen->getParams(), nullptr);
+
+    ASSERT_NEAR(sen->getParams()->stdev_noise, 0.1, 1e-9);
+
+    ASSERT_EQ(sen->getParams()->field_prior_mode, "fix");
+    ASSERT_MATRIX_APPROX(sen->getParams()->field_prior_state, Eigen::Vector3d(1, 2, 3), 1e-9);
+    ASSERT_MATRIX_APPROX(sen->getParams()->field_prior_sigma, Eigen::Vector3d(4, 5, 6), 1e-9);
+
+    ASSERT_EQ(sen->getParams()->bias_prior_mode, "factor");
+    ASSERT_MATRIX_APPROX(sen->getParams()->bias_prior_state, Eigen::Vector3d(3, 2, 1), 1e-9);
+    ASSERT_MATRIX_APPROX(sen->getParams()->bias_prior_sigma, Eigen::Vector3d(6, 5, 4), 1e-9);
+
+    ASSERT_FALSE(sen->getIntrinsic()->isFixed());
+    ASSERT_TRUE(sen->getStateBlock('F')->isFixed());
+}
+
+TEST(SensorCompass, create)
+{
+    auto params = std::make_shared<ParamsSensorCompass>();
+    params->stdev_noise = 0.1;
+    params->field_prior_mode = "fix";
+    params->field_prior_state = Eigen::Vector3d(1, 2, 3);
+    params->field_prior_sigma = Eigen::Vector3d(4, 5, 6);
+
+    params->bias_prior_mode = "factor";
+    params->bias_prior_state = Eigen::Vector3d(3, 2, 1);
+    params->bias_prior_sigma = Eigen::Vector3d(6, 5, 4);
+
+    Vector7d extr(Vector7d::Random());
+    extr.tail<4>().normalize();
+
+    auto sen_base = SensorCompass::create("name", extr, params);
+
+    auto sen = std::static_pointer_cast<SensorCompass>(sen_base);
+
+    ASSERT_NE(sen->getParams(), nullptr);
+
+    ASSERT_NEAR(sen->getParams()->stdev_noise, 0.1, 1e-9);
+
+    ASSERT_EQ(sen->getParams()->field_prior_mode, "fix");
+    ASSERT_MATRIX_APPROX(sen->getParams()->field_prior_state, Eigen::Vector3d(1, 2, 3), 1e-9);
+    ASSERT_MATRIX_APPROX(sen->getParams()->field_prior_sigma, Eigen::Vector3d(4, 5, 6), 1e-9);
+
+    ASSERT_EQ(sen->getParams()->bias_prior_mode, "factor");
+    ASSERT_MATRIX_APPROX(sen->getParams()->bias_prior_state, Eigen::Vector3d(3, 2, 1), 1e-9);
+    ASSERT_MATRIX_APPROX(sen->getParams()->bias_prior_sigma, Eigen::Vector3d(6, 5, 4), 1e-9);
+
+    ASSERT_FALSE(sen->getIntrinsic()->isFixed());
+    ASSERT_TRUE(sen->getStateBlock('F')->isFixed());
+}
+
+TEST(SensorCompass, install)
+{
+    auto params = std::make_shared<ParamsSensorCompass>();
+    params->stdev_noise = 0.1;
+    params->field_prior_mode = "fix";
+    params->field_prior_state = Eigen::Vector3d(1, 2, 3);
+    params->field_prior_sigma = Eigen::Vector3d(4, 5, 6);
+
+    params->bias_prior_mode = "factor";
+    params->bias_prior_state = Eigen::Vector3d(3, 2, 1);
+    params->bias_prior_sigma = Eigen::Vector3d(6, 5, 4);
+
+    Vector7d extr(Vector7d::Random());
+    extr.tail<4>().normalize();
+
+    // Problem
+    ProblemPtr problem_ptr = Problem::create("PO", 3);
+
+    // Install sensor
+    auto sen_base = problem_ptr->installSensor("SensorCompass",
+                                               "magnetometer",
+                                               extr,
+                                               params);
+
+    auto sen = std::static_pointer_cast<SensorCompass>(sen_base);
+
+    ASSERT_NE(sen->getParams(), nullptr);
+
+    ASSERT_NEAR(sen->getParams()->stdev_noise, 0.1, 1e-9);
+
+    ASSERT_EQ(sen->getParams()->field_prior_mode, "fix");
+    ASSERT_MATRIX_APPROX(sen->getParams()->field_prior_state, Eigen::Vector3d(1, 2, 3), 1e-9);
+    ASSERT_MATRIX_APPROX(sen->getParams()->field_prior_sigma, Eigen::Vector3d(4, 5, 6), 1e-9);
+
+    ASSERT_EQ(sen->getParams()->bias_prior_mode, "factor");
+    ASSERT_MATRIX_APPROX(sen->getParams()->bias_prior_state, Eigen::Vector3d(3, 2, 1), 1e-9);
+    ASSERT_MATRIX_APPROX(sen->getParams()->bias_prior_sigma, Eigen::Vector3d(6, 5, 4), 1e-9);
+
+    ASSERT_FALSE(sen->getIntrinsic()->isFixed());
+    ASSERT_TRUE(sen->getStateBlock('F')->isFixed());
+}
+
+TEST(SensorCompass, computeMeasurementCovariance)
+{
+    auto params = std::make_shared<ParamsSensorCompass>();
+    params->stdev_noise = 0.1;
+    params->field_prior_mode = "fix";
+    params->field_prior_state = Eigen::Vector3d(1, 2, 3);
+    params->field_prior_sigma = Eigen::Vector3d(4, 5, 6);
+
+    params->bias_prior_mode = "factor";
+    params->bias_prior_state = Eigen::Vector3d(3, 2, 1);
+    params->bias_prior_sigma = Eigen::Vector3d(6, 5, 4);
+
+    Vector7d extr(Vector7d::Random());
+    extr.tail<4>().normalize();
+
+    auto sen_base = SensorCompass::create("name", extr, params);
+
+    auto sen = std::static_pointer_cast<SensorCompass>(sen_base);
+
+    for (auto i = 0; i < 3; i++)
+    {
+        Vector3d field(Vector3d::Zero());
+        field(i) = 10;
+        WOLF_INFO("field = ", field.transpose());
+
+        Matrix3d cov = sen->computeMeasurementCovariance(field);
+        WOLF_INFO("cov = \n", cov);
+
+        Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> eigensolver(cov, Eigen::ComputeEigenvectors);
+        EXPECT_TRUE(eigensolver.info() == Eigen::Success);
+        WOLF_INFO("eigenvalues: ", eigensolver.eigenvalues().transpose());
+        WOLF_INFO("eigenvectors: \n", eigensolver.eigenvectors());
+
+        // covariance is created from the direction of field:
+        // large covariance: 1e6 m² in the field direction
+        // small covariance: params->stdev_noise² in the other directions.
+
+        // field should be aligned with its weighted vector by the covariance
+        Vector3d field_w = cov * field;
+        WOLF_INFO("field = ", field.transpose());
+        WOLF_INFO("field_w = ", field_w.transpose());
+        EXPECT_NEAR(field.norm(), field_w.norm() * 1e-6, 1e-6);
+        EXPECT_LE(acos(field.dot(field_w) / (field.norm() * field_w.norm())), 0.1);
+    }
+}
+
+TEST(SensorCompass, computeMeasurementCovarianceRandom)
+{
+    auto params = std::make_shared<ParamsSensorCompass>();
+    params->stdev_noise = 0.1;
+    params->field_prior_mode = "fix";
+    params->field_prior_state = Eigen::Vector3d(1, 2, 3);
+    params->field_prior_sigma = Eigen::Vector3d(4, 5, 6);
+
+    params->bias_prior_mode = "factor";
+    params->bias_prior_state = Eigen::Vector3d(3, 2, 1);
+    params->bias_prior_sigma = Eigen::Vector3d(6, 5, 4);
+
+    Vector7d extr(Vector7d::Random());
+    extr.tail<4>().normalize();
+
+    auto sen_base = SensorCompass::create("name", extr, params);
+
+    auto sen = std::static_pointer_cast<SensorCompass>(sen_base);
+
+    for (auto i = 0; i < 1e2; i++)
+    {
+        Vector3d field = Vector3d::Random().normalized() * 10;
+        WOLF_INFO("field = ", field.transpose());
+
+        Matrix3d cov = sen->computeMeasurementCovariance(field);
+        WOLF_INFO("cov = \n", cov);
+
+        Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> eigensolver(cov, Eigen::ComputeEigenvectors);
+        EXPECT_TRUE(eigensolver.info() == Eigen::Success);
+        WOLF_INFO("eigenvalues: ", eigensolver.eigenvalues().transpose());
+        WOLF_INFO("eigenvectors: \n", eigensolver.eigenvectors());
+
+        // covariance is created from the direction of field:
+        // large covariance: 1e6 m² in the field direction
+        // small covariance: params->stdev_noise² in the other directions.
+
+        // field should be aligned with its weighted vector by the covariance
+        Vector3d field_w = cov * field;
+        WOLF_INFO("field = ", field.transpose());
+        WOLF_INFO("field_w = ", field_w.transpose());
+        WOLF_INFO("field.norm() * field_w.norm() = ", field.norm() * field_w.norm());
+        WOLF_INFO("field.dot(field_w) = ", field.dot(field_w));
+        WOLF_INFO("field.dot(field_w) / (field.norm() * field_w.norm()) = ", field.dot(field_w) / (field.norm() * field_w.norm()));
+        WOLF_INFO("acos(min(1.0,field.dot(field_w) / (field.norm() * field_w.norm()))) = ", acos(std::min(1.0,field.dot(field_w) / (field.norm() * field_w.norm()))));
+        //EXPECT_NEAR(field.norm(), field_w.norm() * 1e-6, 1e-6);
+        EXPECT_LE(acos(std::min(1.0,field.dot(field_w) / (field.norm() * field_w.norm()))), 0.1);
+    }
+}
+
+int main(int argc, char **argv)
+{
+    testing::InitGoogleTest(&argc, argv);
+    return RUN_ALL_TESTS();
+}
+

test/yaml/sensor_compass.yaml

+type: "SensorCompass"
+
+stdev_noise: 0.02 # m
+
+field_prior_mode: "fix" # "fix", "factor" or "initial_guess"
+field_prior_state: [1, 2, 3]
+#field_prior_sigma: [0.1, 0.1, 0.1] #only mandatory in mode "factor" 
+
+bias_prior_mode: "factor" # "fix", "factor" or "initial_guess"
+bias_prior_state: [3, 2, 1]
+bias_prior_sigma: [0.1, 0.1, 0.1] #only mandatory in mode "factor"