diff --git a/CMakeLists.txt b/CMakeLists.txt
index f5f3ab9b4fdcd65fa8184b1a2db3cd6fc5cffe61..16e11c7d14548787af08849a911d5821df4c3e06 100644
--- a/CMakeLists.txt
+++ b/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
diff --git a/include/imu/capture/capture_compass.h b/include/imu/capture/capture_compass.h
new file mode 100644
index 0000000000000000000000000000000000000000..1a77f7ce5282fbaf6b615c8cea1950eaa7cbe274
--- /dev/null
+++ b/include/imu/capture/capture_compass.h
@@ -0,0 +1,63 @@
+#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
diff --git a/include/imu/factor/factor_compass_3d.h b/include/imu/factor/factor_compass_3d.h
new file mode 100644
index 0000000000000000000000000000000000000000..6bb877776b886236d74f69abb1ec8062dad0bec5
--- /dev/null
+++ b/include/imu/factor/factor_compass_3d.h
@@ -0,0 +1,62 @@
+#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_ */
diff --git a/include/imu/processor/processor_compass.h b/include/imu/processor/processor_compass.h
new file mode 100644
index 0000000000000000000000000000000000000000..690755716de4029d257ae0807bf947bc2b511acb
--- /dev/null
+++ b/include/imu/processor/processor_compass.h
@@ -0,0 +1,55 @@
+#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_ */
diff --git a/include/imu/sensor/sensor_compass.h b/include/imu/sensor/sensor_compass.h
new file mode 100644
index 0000000000000000000000000000000000000000..e21bb028d1daf45e662698e4e21b2055fa5c69df
--- /dev/null
+++ b/include/imu/sensor/sensor_compass.h
@@ -0,0 +1,80 @@
+#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_ */
diff --git a/include/imu/sensor/sensor_imu.h b/include/imu/sensor/sensor_imu.h
index 5ffd061841374ea942f8cdb44793a2a5ce223e9d..e875fde93d4d5ed446e7c433e142b2f8c917cdb7 100644
--- a/include/imu/sensor/sensor_imu.h
+++ b/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
diff --git a/src/processor/processor_compass.cpp b/src/processor/processor_compass.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..7abc25ff089774bc670a163dd41993566f0b3f2f
--- /dev/null
+++ b/src/processor/processor_compass.cpp
@@ -0,0 +1,75 @@
+#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 */
diff --git a/src/sensor/sensor_compass.cpp b/src/sensor/sensor_compass.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..d81f52926e3633cc31d248a88e8f119bc6f2012b
--- /dev/null
+++ b/src/sensor/sensor_compass.cpp
@@ -0,0 +1,85 @@
+#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
+
diff --git a/src/sensor/sensor_imu.cpp b/src/sensor/sensor_imu.cpp
index a0cec34a6166f872a14210cf1e55c32612006c71..488378b4f1f60b1baf670bcb80b22f17180d1f52 100644
--- a/src/sensor/sensor_imu.cpp
+++ b/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),
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index f05894d954d230b2168c35243162417df664b678..42bce1a6d1364c4ea6af8943a55efcbc88345aad 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -31,6 +31,9 @@ target_link_libraries(gtest_feature_imu ${PLUGIN_NAME} ${wolf_LIBRARY})
wolf_add_gtest(gtest_imu_mocap_fusion gtest_imu_mocap_fusion.cpp)
target_link_libraries(gtest_imu_mocap_fusion ${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)
@@ -38,3 +41,6 @@ target_link_libraries(gtest_imu_mocap_fusion ${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
diff --git a/test/gtest_factor_compass_3d.cpp b/test/gtest_factor_compass_3d.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..362e8b0748dfe724e6747377d19833a71c5c37cd
--- /dev/null
+++ b/test/gtest_factor_compass_3d.cpp
@@ -0,0 +1,214 @@
+#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();
+}
diff --git a/test/gtest_sensor_compass.cpp b/test/gtest_sensor_compass.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..8b404d2ad88ea63c0d6a6beac32f2a7b31377299
--- /dev/null
+++ b/test/gtest_sensor_compass.cpp
@@ -0,0 +1,244 @@
+#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();
+}
+
diff --git a/test/yaml/sensor_compass.yaml b/test/yaml/sensor_compass.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..5d069063e53589bdad76cc972696d00fb227db2d
--- /dev/null
+++ b/test/yaml/sensor_compass.yaml
@@ -0,0 +1,11 @@
+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"