diff --git a/CMakeLists.txt b/CMakeLists.txt
index 27580e0d52c0c02c288e9f434b88ca6c1d5f148f..58ca2e3ad9f835249db3f7711d72cf78c544245a 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -127,6 +127,9 @@ SET(HDRS_SENSOR
include/${PROJECT_NAME}/sensor/sensor_compass.h
include/${PROJECT_NAME}/sensor/sensor_imu.h
)
+SET(HDRS_UTILS
+ include/${PROJECT_NAME}/utils/load_imu.h
+ )
# ============ SOURCES ============
SET(SRCS_CAPTURE
@@ -140,11 +143,15 @@ SET(SRCS_PROCESSOR
src/processor/processor_compass.cpp
src/processor/processor_imu.cpp
src/processor/processor_imu2d.cpp
- test/processor_imu_UnitTester.cpp
- test/processor_imu2d_UnitTester.cpp
-)
+ test/processor_imu_tester.cpp
+ test/processor_imu2d_tester.cpp
+ )
SET(SRCS_SENSOR
src/sensor/sensor_compass.cpp
+ src/sensor/sensor_imu.cpp
+ )
+SET(SRCS_UTILS
+ src/utils/load_imu.cpp
)
# create the shared library
@@ -154,6 +161,7 @@ ADD_LIBRARY(${PLUGIN_NAME}
${SRCS_FEATURE}
${SRCS_PROCESSOR}
${SRCS_SENSOR}
+ ${SRCS_UTILS}
)
# Set compiler options
@@ -222,6 +230,8 @@ INSTALL(FILES ${HDRS_PROCESSOR}
DESTINATION ${INCLUDE_INSTALL_DIR}/${PROJECT_NAME}/processor)
INSTALL(FILES ${HDRS_SENSOR}
DESTINATION ${INCLUDE_INSTALL_DIR}/${PROJECT_NAME}/sensor)
+INSTALL(FILES ${HDRS_UTILS}
+ DESTINATION ${INCLUDE_INSTALL_DIR}/${PROJECT_NAME}/utils)
INSTALL(FILES "${WOLF_CONFIG_DIR}/config.h"
DESTINATION ${INCLUDE_INSTALL_DIR}/${PROJECT_NAME}/internal)
diff --git a/demos/demo_imuDock.cpp b/demos/demo_imuDock.cpp
index 42f16c70889e7bae1ed253f3129da2734c436c5e..0e9411ba5db7f28b81c501bcd874b2f8ce8f3216 100644
--- a/demos/demo_imuDock.cpp
+++ b/demos/demo_imuDock.cpp
@@ -17,12 +17,6 @@
//
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
-/**
- * \file test_imuDock.cpp
- *
- * Created on: July 18, 2017
- * \author: Dinesh Atchuthan
- */
#include <core/ceres_wrapper/solver_ceres.h>
diff --git a/demos/demo_imuDock_autoKFs.cpp b/demos/demo_imuDock_autoKFs.cpp
index fbfc3d06f37d4de7432f0d3998700adb891eb2fb..72da12854aaa4de36e727d5ce0fd12c9d719fdb1 100644
--- a/demos/demo_imuDock_autoKFs.cpp
+++ b/demos/demo_imuDock_autoKFs.cpp
@@ -17,12 +17,6 @@
//
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
-/**
- * \file test_imuDock_autoKFs.cpp
- *
- * Created on: July 22, 2017
- * \author: Dinesh Atchuthan
- */
#include <core/ceres_wrapper/solver_ceres.h>
diff --git a/demos/demo_processor_imu.cpp b/demos/demo_processor_imu.cpp
index 0f2acd7d0cb2c2a65d6e4d2fd6749612a94ce865..67d202cb9e6a9d884edee97a7f0cd705f3468193 100644
--- a/demos/demo_processor_imu.cpp
+++ b/demos/demo_processor_imu.cpp
@@ -17,12 +17,6 @@
//
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
-/**
- * \file test_processor_imu.cpp
- *
- * Created on: Apr 12, 2016
- * \author: dtsbourg
- */
//Wolf
#include "core/capture/capture_imu.h"
diff --git a/demos/demo_processor_imu_jacobians.cpp b/demos/demo_processor_imu_jacobians.cpp
index a83a8b41fa7626147a21b7236f1403537f57d549..faacf2b3f8d7aa71e5d98be05cfbcc76fcf154df 100644
--- a/demos/demo_processor_imu_jacobians.cpp
+++ b/demos/demo_processor_imu_jacobians.cpp
@@ -17,17 +17,11 @@
//
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
-/**
- * \file test_processor_imu_jacobians.cpp
- *
- * Created on: Sep 26, 2016
- * \author: AtDinesh
- */
//Wolf
#include "core/capture/capture_imu.h"
#include "core/sensor/sensor_imu.h"
-#include <test/processor_imu_UnitTester.h>
+#include <test/processor_imu_tester.h>
#include "core/common/wolf.h"
#include "core/problem/problem.h"
#include "core/state_block/state_block.h"
@@ -72,7 +66,7 @@ int main(int argc, char** argv)
//CaptureIMU* imu_ptr;
- ProcessorIMU_UnitTester processor_imu;
+ ProcessorImuTester processor_imu;
//processor_imu.setOrigin(x0, t);
double ddelta_bias = 0.00000001;
double dt = 0.001;
diff --git a/include/imu/common/imu.h b/include/imu/common/imu.h
index 5f4463b4d094d1542e98448e1edd3566a2b27e7e..c39ffab348bb1322b89b101d0a4db15e822b3c8e 100644
--- a/include/imu/common/imu.h
+++ b/include/imu/common/imu.h
@@ -28,6 +28,6 @@ namespace wolf
{
// Folder schema Registry
-WOLF_REGISTER_FOLDER(_WOLF_IMU_SCHEMA_DIR);
+WOLF_REGISTER_FOLDER("imu", _WOLF_IMU_SCHEMA_DIR);
}
diff --git a/include/imu/processor/processor_compass.h b/include/imu/processor/processor_compass.h
index 9913a6bab8ad747182adee1b8ee693b56318ccab..85806f1af7ad9b45d80c42ab9730df0be5e861b1 100644
--- a/include/imu/processor/processor_compass.h
+++ b/include/imu/processor/processor_compass.h
@@ -25,36 +25,17 @@
namespace wolf {
-WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorCompass);
-
-struct ParamsProcessorCompass : public ParamsProcessorBase
-{
- ParamsProcessorCompass() = default;
- ParamsProcessorCompass(const YAML::Node& _n):
- ParamsProcessorBase(_n)
- {
- //
- }
- 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(const YAML::Node& _params);
~ProcessorCompass() override;
void configure(SensorBasePtr) override { };
- WOLF_PROCESSOR_CREATE(ProcessorCompass, ParamsProcessorCompass);
+ WOLF_PROCESSOR_CREATE(ProcessorCompass);
protected:
void processCapture(CaptureBasePtr) override;
diff --git a/include/imu/processor/processor_imu.h b/include/imu/processor/processor_imu.h
index aeba38a34d3de68be8a055a2c6bbcf77eb03dbab..f565acb9af4f8437f067fd5fa212c239e0f6103e 100644
--- a/include/imu/processor/processor_imu.h
+++ b/include/imu/processor/processor_imu.h
@@ -25,64 +25,29 @@
#include "imu/sensor/sensor_imu.h"
#include "imu/capture/capture_imu.h"
#include "imu/feature/feature_imu.h"
-// Wolf core
+// Wolf core
#include <core/processor/processor_motion.h>
namespace wolf
{
-WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorImu);
-struct ParamsProcessorImu : public ParamsProcessorMotion
+WOLF_PTR_TYPEDEFS(ProcessorImu);
+
+enum BootstrapMethod
{
- bool bootstrap_enable;
- enum BootstrapMethod
- {
- BOOTSTRAP_STATIC,
- BOOTSTRAP_G,
- BOOTSTRAP_V0_G
- } bootstrap_method;
- double bootstrap_averaging_length;
-
- ParamsProcessorImu() = default;
- ParamsProcessorImu(const YAML::Node& _n) : ParamsProcessorMotion(_n)
- {
- bootstrap_enable = _n["bootstrap"]["enable"].as<bool>();
- if (bootstrap_enable)
- {
- std::string str = _n["bootstrap"]["method"].as<std::string>();
- std::transform(str.begin(), str.end(), str.begin(), ::toupper);
- if (str == "STATIC" /**/)
- {
- bootstrap_method = BOOTSTRAP_STATIC;
- bootstrap_averaging_length = _n["bootstrap"]["averaging_length"].as<double>();
- }
- if (str == "G" /* */)
- {
- bootstrap_method = BOOTSTRAP_G;
- bootstrap_averaging_length = _n["bootstrap"]["averaging_length"].as<double>();
- }
- if (str == "V0_G" /* */)
- {
- bootstrap_method = BOOTSTRAP_V0_G;
- }
- }
- }
- std::string print() const override
- {
- return ParamsProcessorMotion::print();
- }
+ BOOTSTRAP_STATIC,
+ BOOTSTRAP_G,
+ BOOTSTRAP_V0_G
};
-WOLF_PTR_TYPEDEFS(ProcessorImu);
-
// class
class ProcessorImu : public ProcessorMotion
{
public:
- ProcessorImu(ParamsProcessorImuPtr _params_motion_Imu);
+ ProcessorImu(const YAML::Node& _params);
~ProcessorImu() override;
- WOLF_PROCESSOR_CREATE(ProcessorImu, ParamsProcessorImu);
+ WOLF_PROCESSOR_CREATE(ProcessorImu);
void configure(SensorBasePtr _sensor) override;
void preProcess() override;
@@ -148,10 +113,13 @@ class ProcessorImu : public ProcessorMotion
bool recomputeStates() const;
protected:
- ParamsProcessorImuPtr params_motion_Imu_;
std::list<FactorBasePtr> bootstrap_factor_list_; ///< List of all IMU factors created while IMU is bootstrapping
SensorImu3dPtr sensor_imu_;
Matrix6d imu_drift_cov_;
+
+ // parameters
+ BootstrapMethod bootstrap_method_;
+ double bootstrap_averaging_length_;
};
} // namespace wolf
diff --git a/include/imu/processor/processor_imu2d.h b/include/imu/processor/processor_imu2d.h
index 472f4800c19ea7c969e3b4798159cd3eec9b35d5..3f466dce71f34073598446426a0845bc7d3e8a2c 100644
--- a/include/imu/processor/processor_imu2d.h
+++ b/include/imu/processor/processor_imu2d.h
@@ -29,20 +29,6 @@
namespace wolf
{
-WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorImu2d);
-
-struct ParamsProcessorImu2d : public ParamsProcessorMotion
-{
- ParamsProcessorImu2d() = default;
- ParamsProcessorImu2d(const YAML::Node& _n) : ParamsProcessorMotion(_n)
- {
- //
- }
- std::string print() const override
- {
- return ParamsProcessorMotion::print();
- }
-};
WOLF_PTR_TYPEDEFS(ProcessorImu2d);
@@ -50,11 +36,11 @@ WOLF_PTR_TYPEDEFS(ProcessorImu2d);
class ProcessorImu2d : public ProcessorMotion
{
public:
- ProcessorImu2d(ParamsProcessorImu2dPtr _params_motion_Imu);
+ ProcessorImu2d(const YAML::Node& _params);
~ProcessorImu2d() override;
void configure(SensorBasePtr _sensor) override;
- WOLF_PROCESSOR_CREATE(ProcessorImu2d, ParamsProcessorImu2d);
+ WOLF_PROCESSOR_CREATE(ProcessorImu2d);
void preProcess() override;
protected:
@@ -96,7 +82,6 @@ class ProcessorImu2d : public ProcessorMotion
virtual void emplaceFeaturesAndFactors(CaptureBasePtr _capture_origin, CaptureMotionPtr _capture_own) override;
protected:
- ParamsProcessorImu2dPtr params_motion_Imu_;
SensorImu2dPtr sensor_imu2d_;
Matrix3d imu_drift_cov_;
};
diff --git a/include/imu/sensor/sensor_compass.h b/include/imu/sensor/sensor_compass.h
index ac8b90f130928c6647dd5eed481175dc2b25820f..2475df34a91e9358eff40906efbb6e78f9a6eb00 100644
--- a/include/imu/sensor/sensor_compass.h
+++ b/include/imu/sensor/sensor_compass.h
@@ -28,51 +28,22 @@
namespace wolf
{
-WOLF_STRUCT_PTR_TYPEDEFS(ParamsSensorCompass);
-
-struct ParamsSensorCompass : public ParamsSensorBase
-{
- double stdev_noise;
-
- ParamsSensorCompass() = default;
- ParamsSensorCompass(const YAML::Node& _input_node) : ParamsSensorBase(_input_node)
- {
- stdev_noise = _input_node["stdev_noise"].as<double>();
- }
- ~ParamsSensorCompass() override = default;
-
- std::string getStatesKeys() const override
- {
- return "POIF";
- }
-
- std::string print() const override
- {
- return ParamsSensorBase::print() + "\n" + "stdev_noise: " + toString(stdev_noise) + "\n";
- }
-};
WOLF_PTR_TYPEDEFS(SensorCompass);
class SensorCompass : public SensorBase
{
protected:
- ParamsSensorCompassPtr params_compass_;
+ double stdev_noise_;
public:
- SensorCompass(ParamsSensorCompassPtr _params, const SpecStateSensorComposite& _priors);
+ SensorCompass(const YAML::Node& _params);
- WOLF_SENSOR_CREATE(SensorCompass, ParamsSensorCompass);
+ WOLF_SENSOR_CREATE(SensorCompass);
~SensorCompass() override;
- ParamsSensorCompassConstPtr getParams() const;
Eigen::MatrixXd computeNoiseCov(const Eigen::VectorXd& _data) const override;
};
-inline ParamsSensorCompassConstPtr SensorCompass::getParams() const
-{
- return params_compass_;
-}
-
} /* namespace wolf */
\ No newline at end of file
diff --git a/include/imu/sensor/sensor_imu.h b/include/imu/sensor/sensor_imu.h
index 15dfcd91c749c1cd590b4dcad05b7ea1b15a67e0..0f864ae0e840eeb2e806cf52da1fbfa5a095fec0 100644
--- a/include/imu/sensor/sensor_imu.h
+++ b/include/imu/sensor/sensor_imu.h
@@ -27,51 +27,26 @@
namespace wolf
{
-WOLF_STRUCT_PTR_TYPEDEFS(ParamsSensorImu);
-
-struct ParamsSensorImu : public ParamsSensorBase
-{
- // noise std dev
- double w_noise = 0.001; // standard deviation of Gyroscope noise (same for all the axis) in rad/sec/ sqrt(s)
- double a_noise = 0.04; // standard deviation of Acceleration noise (same for all the axis) in m/s2/sqrt(s)
-
- // OPTIONAL Is the 2D plane orthogonal to gravity? (only for 2D cases)
- bool orthogonal_gravity = true;
-
- ParamsSensorImu() = default;
- ParamsSensorImu(const YAML::Node& _input_node) : ParamsSensorBase(_input_node)
- {
- w_noise = _input_node["w_noise"].as<double>();
- a_noise = _input_node["a_noise"].as<double>();
-
- if (_input_node["orthogonal_gravity"]) orthogonal_gravity = _input_node["orthogonal_gravity"].as<bool>();
- }
- ~ParamsSensorImu() override = default;
-
- std::string getStatesKeys() const override
- {
- return "POI";
- }
-
- std::string print() const override
- {
- return ParamsSensorBase::print() + "\n" + "w_noise: " + toString(w_noise) + "\n" +
- "a_noise: " + toString(a_noise) + "\n" +
- "orthogonal_gravity: " + toString(orthogonal_gravity) + "\n";
- }
-};
-
template <unsigned int DIM>
class SensorImu : public SensorBase
{
protected:
- ParamsSensorImuPtr params_imu_;
+ double w_noise_; ///< standard deviation of Gyroscope noise (same for all the axis) in rad/sec/ sqrt(s)
+ double a_noise_; ///< standard deviation of Acceleration noise (same for all the axis) in m/s2/sqrt(s)
+
+ // Is the 2D plane orthogonal to gravity? (only for 2D cases)
+ bool orthogonal_gravity_ = true;
public:
- SensorImu(ParamsSensorImuPtr _params, const SpecStateSensorComposite& _priors)
- : SensorBase("SensorImu", DIM, _params, _priors("POI")), params_imu_(_params){};
+ SensorImu(const YAML::Node& _params) : SensorBase("SensorImu" + toString(DIM) + "d", DIM, _params)
+ {
+ w_noise_ = _params["w_noise"].as<double>();
+ a_noise_ = _params["a_noise"].as<double>();
- WOLF_SENSOR_TEMPLATE_DIM_CREATE(SensorImu, DIM, ParamsSensorImu);
+ if (_params["orthogonal_gravity"]) orthogonal_gravity_ = _params["orthogonal_gravity"].as<bool>();
+ };
+
+ WOLF_SENSOR_TEMPLATE_DIM_CREATE(SensorImu, DIM);
double getGyroNoise() const;
double getAccelNoise() const;
@@ -85,39 +60,31 @@ class SensorImu : public SensorBase
template <unsigned int DIM>
inline double SensorImu<DIM>::getGyroNoise() const
{
- return params_imu_->w_noise;
+ return w_noise_;
}
template <unsigned int DIM>
inline double SensorImu<DIM>::getAccelNoise() const
{
- return params_imu_->a_noise;
+ return a_noise_;
}
template <unsigned int DIM>
inline bool SensorImu<DIM>::isGravityOrthogonal() const
{
- return DIM == 2 and params_imu_->orthogonal_gravity;
+ return DIM == 2 and orthogonal_gravity_;
}
template <>
inline Eigen::MatrixXd SensorImu<2>::computeNoiseCov(const Eigen::VectorXd& _data) const
{
- return (Eigen::Vector3d() << params_imu_->a_noise, params_imu_->a_noise, params_imu_->w_noise)
- .finished()
- .cwiseAbs2()
- .asDiagonal();
+ return (Eigen::Vector3d() << a_noise_, a_noise_, w_noise_).finished().cwiseAbs2().asDiagonal();
}
template <>
inline Eigen::MatrixXd SensorImu<3>::computeNoiseCov(const Eigen::VectorXd& _data) const
{
- return (Eigen::Vector6d() << params_imu_->a_noise,
- params_imu_->a_noise,
- params_imu_->a_noise,
- params_imu_->w_noise,
- params_imu_->w_noise,
- params_imu_->w_noise)
+ return (Eigen::Vector6d() << a_noise_, a_noise_, a_noise_, w_noise_, w_noise_, w_noise_)
.finished()
.cwiseAbs2()
.asDiagonal();
@@ -128,12 +95,4 @@ typedef SensorImu<3> SensorImu3d;
WOLF_DECLARED_PTR_TYPEDEFS(SensorImu2d);
WOLF_DECLARED_PTR_TYPEDEFS(SensorImu3d);
-} // namespace wolf
-
-// Register in the FactorySensor
-#include "core/sensor/factory_sensor.h"
-namespace wolf
-{
-WOLF_REGISTER_SENSOR(SensorImu2d);
-WOLF_REGISTER_SENSOR(SensorImu3d);
-} // namespace wolf
+} // namespace wolf
\ No newline at end of file
diff --git a/include/imu/utils/load_imu.h b/include/imu/utils/load_imu.h
new file mode 100644
index 0000000000000000000000000000000000000000..3d82098cd257ef45ec8ceec5d4b81b818328a92c
--- /dev/null
+++ b/include/imu/utils/load_imu.h
@@ -0,0 +1,51 @@
+// WOLF - Copyright (C) 2020,2021,2022,2023
+// Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu) and
+// Joan Vallvé Navarro (jvallve@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF: http://www.iri.upc.edu/wolf
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+
+#pragma once
+
+namespace wolf
+{
+// This class is just to force the .so to be easily loaded in tests
+class LoadImu
+{
+ public:
+ static bool aux_var;
+};
+
+#ifdef __GNUC__
+#define WOLF_UNUSED __attribute__((used))
+#elif defined _MSC_VER
+#pragma warning(disable : Cxxxxx)
+#define WOLF_UNUSED
+#elif defined(__LCLINT__)
+#define WOLF_UNUSED /*@unused@*/
+#elif defined(__cplusplus)
+#define WOLF_UNUSED
+#else
+#define UNUSED(x) x
+#endif
+
+#define WOLF_LOAD_IMU \
+ namespace \
+ { \
+ const bool WOLF_UNUSED aux_var_imu = wolf::LoadImu::aux_var; \
+ }
+
+} // namespace wolf
diff --git a/schema/processor/ProcessorImu.schema b/schema/processor/ProcessorImu.schema
index ee5f2cd9d433ba713f59c492fbb060928484db30..e2a89a2fbb6818beaf97f0a7859d3a68742b09e0 100644
--- a/schema/processor/ProcessorImu.schema
+++ b/schema/processor/ProcessorImu.schema
@@ -5,12 +5,12 @@ bootstrap:
_type: bool
_doc: If any boostrap mechanism is enabled for the processor.
method:
- _mandatory: false
+ _mandatory: $enable
_type: string
_options: ["STATIC", "G", "V0_G"]
- _doc: (only if enable==true) Boostrap method.
+ _doc: "Boostrap method (only if enable==true)"
averaging_length:
- _mandatory: false
+ _mandatory: "$enable and (method=='STATIC' or method=='G')"
_type: double
_doc: Averaging length used by the bootstrap mechanism.
diff --git a/schema/sensor/SensorCompass.schema b/schema/sensor/SensorCompass.schema
index 3b9f8916bd885617f6735ecd2ecca9b4aca0aa2b..1cad477cd3f9ff30e7372c868d397793d9a1f961 100644
--- a/schema/sensor/SensorCompass.schema
+++ b/schema/sensor/SensorCompass.schema
@@ -6,6 +6,11 @@ stdev_noise:
_doc: standard deviation of the compass measurements (square root of the covariance matrix diagonal).
states:
+ keys:
+ _mandatory: false
+ _type: string
+ _value: "POIF"
+ _doc: "state keys: PO extrinsics and I for biases and F for field"
P:
follow: SpecStateSensorP3d.schema
O:
diff --git a/schema/sensor/SensorImu2d.schema b/schema/sensor/SensorImu2d.schema
index 5687b1694f07b47c18ebc3660c32074c5b7a1bb9..111c038d5f37df848cd2a1a635d6afa8b8612016 100644
--- a/schema/sensor/SensorImu2d.schema
+++ b/schema/sensor/SensorImu2d.schema
@@ -11,6 +11,11 @@ a_noise:
_doc: standard deviation of the accelerometer measurements (square root of the covariance matrix diagonal).
states:
+ keys:
+ _mandatory: false
+ _type: string
+ _value: POI
+ _doc: "State keys: PO extrinsics, I for biases"
P:
follow: SpecStateSensorP2d.schema
state:
diff --git a/schema/sensor/SensorImu3d.schema b/schema/sensor/SensorImu3d.schema
index bc3bbd6d9852b0ad446b8f667d8201c4d79d384e..2599ac37541b21bb0fa21c80af64302e8f962dda 100644
--- a/schema/sensor/SensorImu3d.schema
+++ b/schema/sensor/SensorImu3d.schema
@@ -11,6 +11,11 @@ a_noise:
_doc: standard deviation of the accelerometer measurements (square root of the covariance matrix diagonal).
states:
+ keys:
+ _mandatory: false
+ _type: string
+ _value: POI
+ _doc: "State keys: PO extrinsics, I for biases"
P:
follow: SpecStateSensorP3d.schema
state:
diff --git a/src/processor/processor_compass.cpp b/src/processor/processor_compass.cpp
index e6d96c6686652d0918da0551517c7aa4da0bb9d1..a6022a3faf6e266d58bb27dedf9d10f35f5889e5 100644
--- a/src/processor/processor_compass.cpp
+++ b/src/processor/processor_compass.cpp
@@ -24,7 +24,7 @@
namespace wolf {
-ProcessorCompass::ProcessorCompass(ParamsProcessorCompassPtr _params) :
+ProcessorCompass::ProcessorCompass(const YAML::Node& _params) :
ProcessorBase("ProcessorCompass", 0, _params)
{
//
@@ -38,7 +38,7 @@ ProcessorCompass::~ProcessorCompass()
void ProcessorCompass::processCapture(CaptureBasePtr _capture)
{
// Search for any stored frame within time tolerance of capture
- auto keyframe = buffer_frame_.select(_capture->getTimeStamp(), params_->time_tolerance);
+ auto keyframe = buffer_frame_.select(_capture->getTimeStamp(), getTimeTolerance());
if (keyframe)
{
processMatch(keyframe, _capture);
@@ -90,15 +90,8 @@ void ProcessorCompass::processMatch(FrameBasePtr _frame, CaptureBasePtr _capture
FactorBase::emplace<FactorCompass3d>(feat,
feat,
shared_from_this(),
- params_->apply_loss_function);
+ applyLossFunction());
}
-} /* namespace wolf */
-
-// Register in the FactoryProcessor
-#include "core/processor/factory_processor.h"
-
-namespace wolf
-{
WOLF_REGISTER_PROCESSOR(ProcessorCompass)
}
diff --git a/src/processor/processor_imu.cpp b/src/processor/processor_imu.cpp
index b0546defe0901eb56aa930e152038ea2844093e2..86d54557d36a17687cc716d26f5429c114a8d6a9 100644
--- a/src/processor/processor_imu.cpp
+++ b/src/processor/processor_imu.cpp
@@ -27,16 +27,42 @@
#include <core/composite/vector_composite.h>
#include <core/factor/factor_block_difference.h>
-namespace wolf {
+namespace wolf
+{
-ProcessorImu::ProcessorImu(ParamsProcessorImuPtr _params_motion_imu) :
- ProcessorMotion("ProcessorImu",
- {{'P',"StatePoint3d"},{'O',"StateQuaternion"},{'V',"StateVector3d"}},
- 3, 10, 10, 9, 6, 6, _params_motion_imu),
- params_motion_Imu_(_params_motion_imu)
+ProcessorImu::ProcessorImu(const YAML::Node& _params)
+ : ProcessorMotion("ProcessorImu",
+ {{'P', "StatePoint3d"}, {'O', "StateQuaternion"}, {'V', "StateVector3d"}},
+ 3,
+ 10,
+ 10,
+ 9,
+ 6,
+ 6,
+ _params)
{
- bootstrapping_ = params_motion_Imu_->bootstrap_enable;
+ bootstrapping_ = _params["bootstrap"]["enable"].as<bool>();
bootstrap_factor_list_.clear();
+
+ if (bootstrapping_)
+ {
+ std::string str = _params["bootstrap"]["method"].as<std::string>();
+ std::transform(str.begin(), str.end(), str.begin(), ::toupper);
+ if (str == "STATIC" /**/)
+ {
+ bootstrap_method_ = BOOTSTRAP_STATIC;
+ bootstrap_averaging_length_ = _params["bootstrap"]["averaging_length"].as<double>();
+ }
+ if (str == "G" /* */)
+ {
+ bootstrap_method_ = BOOTSTRAP_G;
+ bootstrap_averaging_length_ = _params["bootstrap"]["averaging_length"].as<double>();
+ }
+ if (str == "V0_G" /* */)
+ {
+ bootstrap_method_ = BOOTSTRAP_V0_G;
+ }
+ }
}
ProcessorImu::~ProcessorImu()
@@ -46,56 +72,53 @@ ProcessorImu::~ProcessorImu()
void ProcessorImu::preProcess()
{
auto cap_ptr = std::dynamic_pointer_cast<CaptureImu>(incoming_ptr_);
- assert(cap_ptr != nullptr && ("Capture type mismatch. Processor " + getName() + " can only process captures of type CaptureImu").c_str());
+ assert(
+ cap_ptr != nullptr &&
+ ("Capture type mismatch. Processor " + getName() + " can only process captures of type CaptureImu").c_str());
}
bool ProcessorImu::voteForKeyFrame() const
{
// time span
- if (getBuffer().back().ts_ - getBuffer().front().ts_ >= params_motion_Imu_->max_time_span - Constants::EPS)
+ if (getBuffer().back().ts_ - getBuffer().front().ts_ >= getMaxTimeSpan() - Constants::EPS)
{
- WOLF_DEBUG( "PM: vote: time span" );
+ WOLF_DEBUG("PM: vote: time span");
return true;
}
// buffer length
- if (getBuffer().size() -1 > params_motion_Imu_->max_buff_length)
+ if (getBuffer().size() - 1 > getMaxBuffLength())
{
- WOLF_DEBUG( "PM: vote: buffer length" );
+ WOLF_DEBUG("PM: vote: buffer length");
return true;
}
// angle turned
- double angle = 2.0 * asin(delta_integrated_.segment(3,3).norm());
- if (angle > params_motion_Imu_->angle_turned)
+ double angle = 2.0 * asin(delta_integrated_.segment(3, 3).norm());
+ if (angle > getMaxAngleTurned())
{
- WOLF_DEBUG( "PM: vote: angle turned" );
+ WOLF_DEBUG("PM: vote: angle turned");
return true;
}
- //WOLF_DEBUG( "PM: do not vote" );
+ // WOLF_DEBUG( "PM: do not vote" );
return false;
}
-
-CaptureMotionPtr ProcessorImu::emplaceCapture(const FrameBasePtr& _frame_own,
- const SensorBasePtr& _sensor,
- const TimeStamp& _ts,
- const VectorXd& _data,
- const MatrixXd& _data_cov,
- const VectorXd& _calib,
- const VectorXd& _calib_preint,
+CaptureMotionPtr ProcessorImu::emplaceCapture(const FrameBasePtr& _frame_own,
+ const SensorBasePtr& _sensor,
+ const TimeStamp& _ts,
+ const VectorXd& _data,
+ const MatrixXd& _data_cov,
+ const VectorXd& _calib,
+ const VectorXd& _calib_preint,
const CaptureBasePtr& _capture_origin)
{
- auto cap_motion = std::static_pointer_cast<CaptureMotion>(CaptureBase::emplace<CaptureImu>(_frame_own,
- _ts,
- _sensor,
- _data,
- _data_cov,
- _capture_origin));
+ auto cap_motion = std::static_pointer_cast<CaptureMotion>(
+ CaptureBase::emplace<CaptureImu>(_frame_own, _ts, _sensor, _data, _data_cov, _capture_origin));
setCalibration(cap_motion, _calib);
cap_motion->setCalibrationPreint(_calib_preint);
return cap_motion;
}
-VectorXd ProcessorImu::getCalibration (const CaptureBaseConstPtr _capture) const
+VectorXd ProcessorImu::getCalibration(const CaptureBaseConstPtr _capture) const
{
if (_capture)
return _capture->getStateBlock('I')->getState();
@@ -103,12 +126,12 @@ VectorXd ProcessorImu::getCalibration (const CaptureBaseConstPtr _capture) const
return getSensor()->getStateBlockDynamic('I')->getState();
}
-void ProcessorImu::setCalibration (const CaptureBasePtr _capture, const VectorXd& _calibration)
+void ProcessorImu::setCalibration(const CaptureBasePtr _capture, const VectorXd& _calibration)
{
_capture->getSensorIntrinsic()->setState(_calibration);
}
-void ProcessorImu::configure(SensorBasePtr _sensor)
+void ProcessorImu::configure(SensorBasePtr _sensor)
{
imu_drift_cov_ = _sensor->getDriftCov('I');
}
@@ -119,14 +142,15 @@ void ProcessorImu::emplaceFeaturesAndFactors(CaptureBasePtr _capture_origin, Cap
//---------------------------
auto ftr_imu = FeatureBase::emplace<FeatureImu>(
- _capture_own, _capture_own->getBuffer().back().delta_integr_,
+ _capture_own,
+ _capture_own->getBuffer().back().delta_integr_,
_capture_own->getBuffer().back().delta_integr_cov_ + unmeasured_perturbation_cov_,
- _capture_own->getCalibrationPreint(), _capture_own->getBuffer().back().jacobian_calib_);
+ _capture_own->getCalibrationPreint(),
+ _capture_own->getBuffer().back().jacobian_calib_);
CaptureImuPtr cap_imu = std::static_pointer_cast<CaptureImu>(_capture_origin);
- auto fac_imu =
- FactorBase::emplace<FactorImu>(ftr_imu, ftr_imu, cap_imu, shared_from_this(), params_->apply_loss_function);
+ auto fac_imu = FactorBase::emplace<FactorImu>(ftr_imu, ftr_imu, cap_imu, shared_from_this(), applyLossFunction());
if (bootstrapping_)
{
@@ -141,29 +165,25 @@ void ProcessorImu::emplaceFeaturesAndFactors(CaptureBasePtr _capture_origin, Cap
// - factor relates bias(last capture) and bias(origin capture)
if (getSensor()->isStateBlockDynamic('I'))
{
- const auto& sb_imubias_own = _capture_own->getStateBlock('I');
- const auto& sb_imubias_origin = _capture_origin->getStateBlock('I');
- if (sb_imubias_own != sb_imubias_origin) // make sure it's two different state blocks! -- just in case
+ if (_capture_own->getStateBlock('I') !=
+ _capture_origin->getStateBlock('I')) // make sure it's two different captures! -- just in case
{
auto dt = _capture_own->getTimeStamp() - _capture_origin->getTimeStamp();
auto ftr_bias =
- FeatureBase::emplace<FeatureBase>(_capture_own, "FeatureBase",
+ FeatureBase::emplace<FeatureBase>(_capture_own,
+ "FeatureBase",
Vector6d::Zero(), // mean IMU drift is zero
imu_drift_cov_ * dt); // IMU drift cov specified in continuous time
auto fac_bias =
- FactorBase::emplace<FactorBlockDifference>(ftr_bias, ftr_bias,
- sb_imubias_own, // IMU bias block at t=own
- sb_imubias_origin, // IMU bias block at t=origin
- nullptr, // frame other
- _capture_origin, // origin capture
- nullptr, // feature other
- nullptr, // landmark other
- 0, // take all of first state block
- -1, // take all of first state block
- 0, // take all of first second block
- -1, // take all of first second block
- shared_from_this(), // this processor
- params_->apply_loss_function); // loss function
+ FactorBase::emplace<FactorBlockDifference>(ftr_bias,
+ ftr_bias->getMeasurement(),
+ ftr_bias->getMeasurementSquareRootInformationUpper(),
+ _capture_origin, // take all of first state block
+ _capture_own, // take all of first state block
+ 'I', // key of the 1st state block
+ 'I', // key of the 2nd state block
+ shared_from_this(), // this processor
+ applyLossFunction()); // loss function
if (bootstrapping_)
{
@@ -177,10 +197,10 @@ void ProcessorImu::emplaceFeaturesAndFactors(CaptureBasePtr _capture_origin, Cap
void ProcessorImu::computeCurrentDelta(const Eigen::VectorXd& _data,
const Eigen::MatrixXd& _data_cov,
const Eigen::VectorXd& _calib,
- const double _dt,
- Eigen::VectorXd& _delta,
- Eigen::MatrixXd& _delta_cov,
- Eigen::MatrixXd& _jac_delta_calib) const
+ const double _dt,
+ Eigen::VectorXd& _delta,
+ Eigen::MatrixXd& _delta_cov,
+ Eigen::MatrixXd& _jac_delta_calib) const
{
assert(_data.size() == data_size_ && "Wrong data size!");
@@ -206,20 +226,19 @@ void ProcessorImu::computeCurrentDelta(const Eigen::VectorXd& _data,
*/
// create delta
- imu::body2delta(_data - _calib, _dt, _delta, jac_delta_data); // Jacobians tested in imu_tools
+ imu::body2delta(_data - _calib, _dt, _delta, jac_delta_data); // Jacobians tested in imu_tools
// compute delta_cov
_delta_cov = jac_delta_data * _data_cov * jac_delta_data.transpose();
// compute jacobian_calib
- _jac_delta_calib = - jac_delta_data;
-
+ _jac_delta_calib = -jac_delta_data;
}
void ProcessorImu::deltaPlusDelta(const Eigen::VectorXd& _delta_preint,
const Eigen::VectorXd& _delta,
- const double _dt,
- Eigen::VectorXd& _delta_preint_plus_delta) const
+ const double _dt,
+ Eigen::VectorXd& _delta_preint_plus_delta) const
{
/* MATHS according to Sola-16
* Dp' = Dp + Dv*dt + 1/2*Dq*(a-a_b)*dt^2 = Dp + Dv*dt + Dq*dp if dp = 1/2*(a-a_b)*dt^2
@@ -233,13 +252,13 @@ void ProcessorImu::deltaPlusDelta(const Eigen::VectorXd& _delta_preint,
void ProcessorImu::statePlusDelta(const VectorComposite& _x,
const Eigen::VectorXd& _delta,
- const double _dt,
- VectorComposite& _x_plus_delta) const
+ const double _dt,
+ VectorComposite& _x_plus_delta) const
{
assert(_delta.size() == 10 && "Wrong _delta vector size");
assert(_dt >= 0 && "Time interval _dt is negative!");
- const auto& delta = VectorComposite("POV", _delta, {3,4,3});
+ const auto& delta = VectorComposite("POV", _delta, {3, 4, 3});
/* MATH according to Sola-16
*
@@ -252,10 +271,10 @@ void ProcessorImu::statePlusDelta(const VectorComposite& _x,
void ProcessorImu::deltaPlusDelta(const Eigen::VectorXd& _delta_preint,
const Eigen::VectorXd& _delta,
- const double _dt,
- Eigen::VectorXd& _delta_preint_plus_delta,
- Eigen::MatrixXd& _jacobian_delta_preint,
- Eigen::MatrixXd& _jacobian_delta) const
+ const double _dt,
+ Eigen::VectorXd& _delta_preint_plus_delta,
+ Eigen::MatrixXd& _jacobian_delta_preint,
+ Eigen::MatrixXd& _jacobian_delta) const
{
/*
* Expression of the delta integration step, D' = D (+) d:
@@ -280,11 +299,16 @@ void ProcessorImu::deltaPlusDelta(const Eigen::VectorXd& _delta_preint,
*
* Note: covariance propagation, i.e., P+ = D_D * P * D_D' + D_d * M * D_d', is done in ProcessorMotion.
*/
- imu::compose(_delta_preint, _delta, _dt, _delta_preint_plus_delta, _jacobian_delta_preint, _jacobian_delta); // jacobians tested in imu_tools
+ imu::compose(_delta_preint,
+ _delta,
+ _dt,
+ _delta_preint_plus_delta,
+ _jacobian_delta_preint,
+ _jacobian_delta); // jacobians tested in imu_tools
}
-Eigen::VectorXd ProcessorImu::correctDelta (const Eigen::VectorXd& delta_preint,
- const Eigen::VectorXd& delta_step) const
+Eigen::VectorXd ProcessorImu::correctDelta(const Eigen::VectorXd& delta_preint,
+ const Eigen::VectorXd& delta_step) const
{
return imu::plus(delta_preint, delta_step);
}
@@ -302,16 +326,18 @@ void ProcessorImu::bootstrap()
CaptureBasePtr first_capture = bootstrapOrigin();
TimeStamp t_current = last_ptr_->getBuffer().back().ts_;
VectorComposite transfo_w_l;
- switch (params_motion_Imu_->bootstrap_method)
+ switch (bootstrap_method_)
{
- case ParamsProcessorImu::BootstrapMethod::BOOTSTRAP_STATIC:
-
+ case BootstrapMethod::BOOTSTRAP_STATIC:
+
// Implementation of static strategy
- if (t_current - first_capture->getTimeStamp() >= params_motion_Imu_->bootstrap_averaging_length)
+ if (t_current - first_capture->getTimeStamp() >= bootstrap_averaging_length_)
{
// get initial IMU frame 's' expressed in local world frame 'l'
- Quaterniond q_l_r; q_l_r.coeffs() = first_capture->getFrame()->getO()->getState();
- Quaterniond q_r_s; q_r_s.coeffs() = first_capture->getSensor()->getO()->getState();
+ Quaterniond q_l_r;
+ q_l_r.coeffs() = first_capture->getFrame()->getO()->getState();
+ Quaterniond q_r_s;
+ q_r_s.coeffs() = first_capture->getSensor()->getO()->getState();
const auto& q_l_s = q_l_r * q_r_s;
// Compute total integrated delta during bootstrap period
@@ -353,14 +379,16 @@ void ProcessorImu::bootstrap()
}
break;
- case ParamsProcessorImu::BootstrapMethod::BOOTSTRAP_G:
-
+ case BootstrapMethod::BOOTSTRAP_G:
+
// Implementation of G strategy.
- if (t_current - first_capture->getTimeStamp() >= params_motion_Imu_->bootstrap_averaging_length)
+ if (t_current - first_capture->getTimeStamp() >= bootstrap_averaging_length_)
{
// get initial IMU frame 's' expressed in local world frame 'l'
- Quaterniond q_l_r; q_l_r.coeffs() = first_capture->getFrame()->getO()->getState();
- Quaterniond q_r_s; q_r_s.coeffs() = first_capture->getSensor()->getO()->getState();
+ Quaterniond q_l_r;
+ q_l_r.coeffs() = first_capture->getFrame()->getO()->getState();
+ Quaterniond q_r_s;
+ q_r_s.coeffs() = first_capture->getSensor()->getO()->getState();
const auto& q_l_s = q_l_r * q_r_s;
// Compute total integrated delta during bootstrap period
@@ -398,8 +426,8 @@ void ProcessorImu::bootstrap()
}
break;
- case ParamsProcessorImu::BootstrapMethod::BOOTSTRAP_V0_G:
-
+ case BootstrapMethod::BOOTSTRAP_V0_G:
+
// TODO implement v0-g strategy
WOLF_WARN("Bootstrapping strategy BOOTSTRAP_V0_G not yet implemented. Disabling bootstrap!");
bootstrapping_ = false;
@@ -416,8 +444,7 @@ void ProcessorImu::bootstrap()
void ProcessorImu::bootstrapEnable(bool _bootstrap_enable)
{
- params_motion_Imu_->bootstrap_enable = _bootstrap_enable;
- bootstrapping_ = _bootstrap_enable;
+ bootstrapping_ = _bootstrap_enable;
};
CaptureBasePtr ProcessorImu::bootstrapOrigin() const
@@ -440,7 +467,7 @@ VectorXd ProcessorImu::bootstrapDelta() const
{
if (std::dynamic_pointer_cast<FactorImu>(fac) != nullptr)
{
- dt = fac->getCapture()->getTimeStamp() - fac->getCapturesFactored().front().lock()->getTimeStamp();
+ dt = fac->getCapture()->getTimeStamp() - fac->getCapturesFactored().front().lock()->getTimeStamp();
const auto& delta = fac->getFeature()->getMeasurement(); // In FeatImu, delta = measurement
delta_int = imu::compose(delta_int, delta, dt);
}
diff --git a/src/processor/processor_imu2d.cpp b/src/processor/processor_imu2d.cpp
index 0fbba76f16429bf439671594f7cef38606d6db50..883889f1ba5524878484a4a98c331742833c492a 100644
--- a/src/processor/processor_imu2d.cpp
+++ b/src/processor/processor_imu2d.cpp
@@ -32,7 +32,7 @@
namespace wolf
{
-ProcessorImu2d::ProcessorImu2d(ParamsProcessorImu2dPtr _params_motion_imu)
+ProcessorImu2d::ProcessorImu2d(const YAML::Node& _params)
: ProcessorMotion("ProcessorImu2d",
{{'P', "StatePoint2d"}, {'O', "StateAngle"}, {'V', "StateVector2d"}},
2,
@@ -41,8 +41,7 @@ ProcessorImu2d::ProcessorImu2d(ParamsProcessorImu2dPtr _params_motion_imu)
5,
6,
3,
- _params_motion_imu),
- params_motion_Imu_(std::make_shared<ParamsProcessorImu2d>(*_params_motion_imu))
+ _params)
{
//
}
@@ -63,20 +62,20 @@ void ProcessorImu2d::preProcess()
bool ProcessorImu2d::voteForKeyFrame() const
{
// time span
- if (getBuffer().back().ts_ - getBuffer().front().ts_ > params_motion_Imu_->max_time_span)
+ if (getBuffer().back().ts_ - getBuffer().front().ts_ > getMaxTimeSpan())
{
WOLF_DEBUG("PM: vote: time span");
return true;
}
// buffer length
- if (getBuffer().size() > params_motion_Imu_->max_buff_length)
+ if (getBuffer().size() > getMaxBuffLength())
{
WOLF_DEBUG("PM: vote: buffer length");
return true;
}
// angle turned
double angle = std::abs(delta_integrated_(2));
- if (angle > params_motion_Imu_->angle_turned)
+ if (angle > getMaxAngleTurned())
{
WOLF_DEBUG("PM: vote: angle turned");
return true;
@@ -133,10 +132,10 @@ void ProcessorImu2d::emplaceFeaturesAndFactors(CaptureBasePtr _capture_origin, C
FeatureImu2dPtr ftr_imu = std::static_pointer_cast<FeatureImu2d>(feature);
if (std::static_pointer_cast<SensorImu2d>(getSensor())->isGravityOrthogonal())
- FactorBase::emplace<FactorImu2d>(ftr_imu, ftr_imu, cap_imu, shared_from_this(), params_->apply_loss_function);
+ FactorBase::emplace<FactorImu2d>(ftr_imu, ftr_imu, cap_imu, shared_from_this(), applyLossFunction());
else
FactorBase::emplace<FactorImu2dWithGravity>(
- ftr_imu, ftr_imu, cap_imu, shared_from_this(), params_->apply_loss_function);
+ ftr_imu, ftr_imu, cap_imu, shared_from_this(), applyLossFunction());
if (getSensor()->isStateBlockDynamic('I'))
{
@@ -157,14 +156,14 @@ void ProcessorImu2d::emplaceFeaturesAndFactors(CaptureBasePtr _capture_origin, C
'I',
'I',
shared_from_this(), // this processor
- params_->apply_loss_function); // loss function
+ applyLossFunction()); // loss function
if (std::static_pointer_cast<SensorImu2d>(getSensor())->isGravityOrthogonal())
FactorBase::emplace<FactorImu2d>(
- ftr_imu, ftr_imu, cap_imu, shared_from_this(), params_->apply_loss_function);
+ ftr_imu, ftr_imu, cap_imu, shared_from_this(), applyLossFunction());
else
FactorBase::emplace<FactorImu2dWithGravity>(
- ftr_imu, ftr_imu, cap_imu, shared_from_this(), params_->apply_loss_function);
+ ftr_imu, ftr_imu, cap_imu, shared_from_this(), applyLossFunction());
}
}
}
diff --git a/src/sensor/sensor_compass.cpp b/src/sensor/sensor_compass.cpp
index dd30a3c1b52e293223a870ba112d63ca6f5b9416..389cc01b50743165bbfed6cd6467c33e2ad8b8a6 100644
--- a/src/sensor/sensor_compass.cpp
+++ b/src/sensor/sensor_compass.cpp
@@ -25,23 +25,17 @@
namespace wolf
{
-SensorCompass::SensorCompass(ParamsSensorCompassPtr _params, const SpecStateSensorComposite& _priors)
- : SensorBase("SensorCompass", 3, _params, _priors), params_compass_(_params)
+SensorCompass::SensorCompass(const YAML::Node& _params) : SensorBase("SensorCompass", 0, _params)
{
+ stdev_noise_ = _params["stdev_noise"].as<double>();
}
SensorCompass::~SensorCompass() {}
Eigen::MatrixXd SensorCompass::computeNoiseCov(const Eigen::VectorXd& _data) const
{
- return Eigen::Vector3d::Constant(pow(params_compass_->stdev_noise, 2)).asDiagonal();
+ return Eigen::Vector3d::Constant(pow(stdev_noise_, 2)).asDiagonal();
}
-} /* namespace wolf */
-
-// Register in the FactorySensor
-#include "core/sensor/factory_sensor.h"
-namespace wolf
-{
WOLF_REGISTER_SENSOR(SensorCompass);
} // namespace wolf
diff --git a/src/sensor/sensor_imu.cpp b/src/sensor/sensor_imu.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..3edd6210369a9f793773f91c05deecf6bcee6bfc
--- /dev/null
+++ b/src/sensor/sensor_imu.cpp
@@ -0,0 +1,29 @@
+// WOLF - Copyright (C) 2020,2021,2022,2023
+// Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu) and
+// Joan Vallvé Navarro (jvallve@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF: http://www.iri.upc.edu/wolf
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+
+#include "imu/sensor/sensor_imu.h"
+
+namespace wolf
+{
+
+WOLF_REGISTER_SENSOR(SensorImu2d);
+WOLF_REGISTER_SENSOR(SensorImu3d);
+
+} // namespace wolf
diff --git a/src/utils/load_imu.cpp b/src/utils/load_imu.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..90bfd8f5ddfd92cd6640c1cfec9d8632b92ed349
--- /dev/null
+++ b/src/utils/load_imu.cpp
@@ -0,0 +1,26 @@
+// WOLF - Copyright (C) 2020,2021,2022,2023
+// Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu) and
+// Joan Vallvé Navarro (jvallve@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF: http://www.iri.upc.edu/wolf
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+
+#include "imu/utils/load_imu.h"
+
+namespace wolf
+{
+bool LoadImu::aux_var = true;
+} // namespace wolf
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index 3318eba7174a462947ffbf26f33497389dc75400..ad1848367592e3d19cec69be50e90b1910ee15d2 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -9,38 +9,40 @@ wolf_add_gtest(gtest_example gtest_example.cpp) #
# #
###########################################################
-wolf_add_gtest(gtest_processor_imu gtest_processor_imu.cpp)
+wolf_add_gtest(gtest_factor_compass_3d gtest_factor_compass_3d.cpp)
-wolf_add_gtest(gtest_processor_imu2d gtest_processor_imu2d.cpp)
+# 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)
-wolf_add_gtest(gtest_processor_imu2d_with_gravity gtest_processor_imu2d_with_gravity.cpp)
+wolf_add_gtest(gtest_factor_imu2d gtest_factor_imu2d.cpp)
-wolf_add_gtest(gtest_imu gtest_imu.cpp)
+# wolf_add_gtest(gtest_factor_imu2d_with_gravity gtest_factor_imu2d_with_gravity.cpp)
-wolf_add_gtest(gtest_imu_tools gtest_imu_tools.cpp)
+# wolf_add_gtest(gtest_feature_imu gtest_feature_imu.cpp)
-wolf_add_gtest(gtest_imu2d_tools gtest_imu2d_tools.cpp)
+# wolf_add_gtest(gtest_imu_mocap_fusion gtest_imu_mocap_fusion.cpp)
-wolf_add_gtest(gtest_processor_imu_jacobians gtest_processor_imu_jacobians.cpp)
+# wolf_add_gtest(gtest_imu_static_init gtest_imu_static_init.cpp)
-wolf_add_gtest(gtest_feature_imu gtest_feature_imu.cpp)
+# wolf_add_gtest(gtest_imu_tools gtest_imu_tools.cpp)
-wolf_add_gtest(gtest_factor_imu2d gtest_factor_imu2d.cpp)
+# wolf_add_gtest(gtest_imu gtest_imu.cpp)
-wolf_add_gtest(gtest_imu_static_init gtest_imu_static_init.cpp)
+# wolf_add_gtest(gtest_imu2d_static_init gtest_imu2d_static_init.cpp)
-wolf_add_gtest(gtest_imu2d_static_init gtest_imu2d_static_init.cpp)
+# wolf_add_gtest(gtest_imu2d_tools gtest_imu2d_tools.cpp)
-wolf_add_gtest(gtest_factor_imu2d_with_gravity gtest_factor_imu2d_with_gravity.cpp)
+# wolf_add_gtest(gtest_processor_imu_jacobians gtest_processor_imu_jacobians.cpp)
-wolf_add_gtest(gtest_imu_mocap_fusion gtest_imu_mocap_fusion.cpp)
+# wolf_add_gtest(gtest_processor_imu gtest_processor_imu.cpp)
-wolf_add_gtest(gtest_factor_compass_3d gtest_factor_compass_3d.cpp)
+# wolf_add_gtest(gtest_processor_imu2d_with_gravity gtest_processor_imu2d_with_gravity.cpp)
-# 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)
+# wolf_add_gtest(gtest_processor_imu2d gtest_processor_imu2d.cpp)
+
+# wolf_add_gtest(gtest_processor_motion_intrinsics_update gtest_processor_motion_intrinsics_update.cpp)
-wolf_add_gtest(gtest_processor_motion_intrinsics_update gtest_processor_motion_intrinsics_update.cpp)
+# wolf_add_gtest(gtest_schema gtest_schema.cpp)
-wolf_add_gtest(gtest_sensor_compass gtest_sensor_compass.cpp)
+# wolf_add_gtest(gtest_sensor_compass gtest_sensor_compass.cpp)
diff --git a/test/gtest_factor_compass_3d.cpp b/test/gtest_factor_compass_3d.cpp
index 8d419a60bb90908d2b87cd3b8e320b6096aea465..85001276b5487dc025899815e35123874578b392 100644
--- a/test/gtest_factor_compass_3d.cpp
+++ b/test/gtest_factor_compass_3d.cpp
@@ -26,48 +26,48 @@
#include "imu/sensor/sensor_compass.h"
#include "imu/processor/processor_compass.h"
-#include "core/ceres_wrapper/solver_ceres.h"
-#include "core/math/rotations.h"
+#include <core/ceres_wrapper/solver_ceres.h>
+#include <core/math/rotations.h>
using namespace Eigen;
using namespace wolf;
-std::string wolf_root = _WOLF_IMU_CODE_DIR;
+std::string imu_dir = _WOLF_IMU_CODE_DIR;
+std::string wolf_schema_dir = _WOLF_SCHEMA_DIR;
int N_TESTS = 10;
// Problem and solver
-ProblemPtr problem = Problem::create("PO", 3);
-SolverCeres solver(problem);
+ProblemPtr problem = Problem::create("PO", 3);
+SolverManagerPtr solver =
+ FactorySolverFile::create("SolverCeres", problem, imu_dir + "/test/yaml/solver_ceres.yaml", {wolf_schema_dir});
// Sensor
-auto sen = std::static_pointer_cast<SensorCompass>(problem->installSensor("SensorCompass",
- wolf_root + "/test/yaml/sensor_compass_3d.yaml",
- {wolf_root}));
+auto sen = std::static_pointer_cast<SensorCompass>(
+ problem->installSensor(imu_dir + "/test/yaml/sensor_compass_3d.yaml", {imu_dir}));
// Frame
-FrameBasePtr frm = problem->emplaceFrame(0, "PO", (Vector7d() << 0,0,0,0,0,0,1).finished() );
+FrameBasePtr frm = problem->emplaceFrame(0, "PO", (Vector7d() << 0, 0, 0, 0, 0, 0, 1).finished());
// Pointers
CaptureCompassPtr cap;
// Auxliary states
-Vector3d frm_p, sen_p, sen_bias, field, meas;
+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();
+ if (cap) cap->remove();
// Random states
- frm_p = Vector3d::Random() * 10;
- frm_q = Quaterniond::UnitRandom();
- sen_p = Vector3d::Random() * 10;
- sen_q = Quaterniond::UnitRandom();
+ frm_p = Vector3d::Random() * 10;
+ frm_q = Quaterniond::UnitRandom();
+ sen_p = Vector3d::Random() * 10;
+ sen_q = Quaterniond::UnitRandom();
sen_bias = Vector3d::Random();
- field = Vector3d::Random().normalized() * 50;
+ field = Vector3d::Random().normalized() * 50;
// measurement
meas = sen_q.conjugate() * frm_q.conjugate() * field + sen_bias;
@@ -81,28 +81,28 @@ void generateRandomSetup()
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());
+ "\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());
+ 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());
+ auto fea = FeatureBase::emplace<FeatureBase>(cap, "FeatureCompass", meas, Eigen::Matrix3d::Identity());
- FactorBase::emplace<FactorCompass3d>(fea,
- fea,
- nullptr,
- false);
+ FactorBase::emplace<FactorCompass3d>(fea, fea, nullptr, false);
// Fix all
frm->getP()->fix();
@@ -121,9 +121,9 @@ TEST(FactorCompass3d, solve_frm_q)
generateRandomSetup();
// solve
- std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
+ std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
- //WOLF_INFO(report);
+ // WOLF_INFO(report);
ASSERT_QUATERNION_VECTOR_APPROX(frm->getO()->getState(), frm_q.coeffs(), 1e-6);
@@ -132,9 +132,9 @@ TEST(FactorCompass3d, solve_frm_q)
frm->getO()->perturb();
// solve
- report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
+ report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
- //WOLF_INFO(report);
+ // WOLF_INFO(report);
// check measurement and field are aligned
Vector3d exp_meas = sen_q.conjugate() * frm_q.conjugate() * field + sen_bias;
@@ -146,7 +146,6 @@ TEST(FactorCompass3d, solve_frm_q)
}
}
-
TEST(FactorCompass3d, solve_sen_q)
{
for (int i = 0; i < N_TESTS; i++)
@@ -155,9 +154,9 @@ TEST(FactorCompass3d, solve_sen_q)
generateRandomSetup();
// solve
- std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
+ std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
- //WOLF_INFO(report);
+ // WOLF_INFO(report);
ASSERT_QUATERNION_VECTOR_APPROX(frm->getO()->getState(), frm_q.coeffs(), 1e-6);
@@ -166,9 +165,9 @@ TEST(FactorCompass3d, solve_sen_q)
sen->getO()->perturb();
// solve
- report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
+ report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
- //WOLF_INFO(report);
+ // WOLF_INFO(report);
// check measurement and field are aligned
Vector3d exp_meas = sen_q.conjugate() * frm_q.conjugate() * field + sen_bias;
@@ -192,9 +191,9 @@ TEST(FactorCompass3d, solve_bias)
sen->getIntrinsic()->perturb(1);
// solve
- std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
+ std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
- //WOLF_INFO(report);
+ // WOLF_INFO(report);
ASSERT_MATRIX_APPROX(sen->getIntrinsic()->getState(), sen_bias, 1e-6);
@@ -215,9 +214,9 @@ TEST(FactorCompass3d, solve_field)
sen->getStateBlock('F')->perturb(10);
// solve
- std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ std::string report = solver->solve(SolverManager::ReportVerbosity::FULL);
- //WOLF_INFO(report);
+ // WOLF_INFO(report);
ASSERT_MATRIX_APPROX(sen->getStateBlock('F')->getState(), field, 1e-6);
@@ -228,6 +227,6 @@ TEST(FactorCompass3d, solve_field)
int main(int argc, char **argv)
{
- testing::InitGoogleTest(&argc, argv);
- return RUN_ALL_TESTS();
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
}
diff --git a/test/gtest_factor_imu.cpp b/test/gtest_factor_imu.cpp
index 86331b8136a2ef7c625b1ac9c7ead478e23a5437..63ae694c045eef311d02126415958fd3c6367b7c 100644
--- a/test/gtest_factor_imu.cpp
+++ b/test/gtest_factor_imu.cpp
@@ -41,7 +41,7 @@ using namespace wolf;
using std::make_shared;
using std::static_pointer_cast;
-std::string wolf_root = _WOLF_IMU_CODE_DIR;
+std::string imu_dir = _WOLF_IMU_CODE_DIR;
/*
* This test is designed to test Imu biases in a particular case : perfect Imu, not moving.
@@ -80,8 +80,8 @@ class FactorImu_biasTest_Static_NullBias : public testing::Test
solver->getSolverOptions().max_num_iterations = 1e4;
// SENSOR + PROCESSOR Imu
- SensorBasePtr sen0_ptr = problem->installSensor("SensorImu3d", wolf_root + "/test/yaml/sensor_imu.yaml", {wolf_root});
- processor_ = problem->installProcessor("ProcessorImu", sen0_ptr, wolf_root + "/test/yaml/processor_imu.yaml", {wolf_root});
+ SensorBasePtr sen0_ptr = problem->installSensor("SensorImu3d", imu_dir + "/test/yaml/sensor_imu.yaml", {imu_dir});
+ processor_ = problem->installProcessor("ProcessorImu", sen0_ptr, imu_dir + "/test/yaml/processor_imu.yaml", {imu_dir});
sen_imu = static_pointer_cast<SensorImu>(sen0_ptr);
processor_imu = static_pointer_cast<ProcessorImu>(processor_);
@@ -161,8 +161,8 @@ class FactorImu_biasTest_Static_NonNullAccBias : public testing::Test
solver->getSolverOptions().max_num_iterations = 1e4;
// SENSOR + PROCESSOR Imu
- SensorBasePtr sen0_ptr = problem->installSensor("SensorImu3d", (Vector7d()<<0,0,0,0,0,0,1).finished(), wolf_root + "/test/yaml/sensor_imu.yaml", {wolf_root});
- processor_ = problem->installProcessor("ProcessorImu", sen0_ptr, wolf_root + "/test/yaml/processor_imu.yaml", {wolf_root});
+ SensorBasePtr sen0_ptr = problem->installSensor("SensorImu3d", (Vector7d()<<0,0,0,0,0,0,1).finished(), imu_dir + "/test/yaml/sensor_imu.yaml", {imu_dir});
+ processor_ = problem->installProcessor("ProcessorImu", sen0_ptr, imu_dir + "/test/yaml/processor_imu.yaml", {imu_dir});
sen_imu = static_pointer_cast<SensorImu>(sen0_ptr);
processor_imu = static_pointer_cast<ProcessorImu>(processor_);
@@ -238,8 +238,8 @@ class FactorImu_biasTest_Static_NonNullGyroBias : public testing::Test
//solver->getSolverOptions().max_num_iterations = 1e4;
// SENSOR + PROCESSOR Imu
- SensorBasePtr sen0_ptr = problem->installSensor("SensorImu3d", (Vector7d()<<0,0,0,0,0,0,1).finished(), wolf_root + "/test/yaml/sensor_imu.yaml", {wolf_root});
- processor_ = problem->installProcessor("ProcessorImu", sen0_ptr, wolf_root + "/test/yaml/processor_imu.yaml", {wolf_root});
+ SensorBasePtr sen0_ptr = problem->installSensor("SensorImu3d", (Vector7d()<<0,0,0,0,0,0,1).finished(), imu_dir + "/test/yaml/sensor_imu.yaml", {imu_dir});
+ processor_ = problem->installProcessor("ProcessorImu", sen0_ptr, imu_dir + "/test/yaml/processor_imu.yaml", {imu_dir});
sen_imu = static_pointer_cast<SensorImu>(sen0_ptr);
processor_imu = static_pointer_cast<ProcessorImu>(processor_);
@@ -315,8 +315,8 @@ class FactorImu_biasTest_Static_NonNullBias : public testing::Test
solver->getSolverOptions().max_num_iterations = 1e4;
// SENSOR + PROCESSOR Imu
- SensorBasePtr sen0_ptr = problem->installSensor("SensorImu3d", (Vector7d()<<0,0,0,0,0,0,1).finished(), wolf_root + "/test/yaml/sensor_imu.yaml", {wolf_root});
- processor_ = problem->installProcessor("ProcessorImu", sen0_ptr, wolf_root + "/test/yaml/processor_imu.yaml", {wolf_root});
+ SensorBasePtr sen0_ptr = problem->installSensor("SensorImu3d", (Vector7d()<<0,0,0,0,0,0,1).finished(), imu_dir + "/test/yaml/sensor_imu.yaml", {imu_dir});
+ processor_ = problem->installProcessor("ProcessorImu", sen0_ptr, imu_dir + "/test/yaml/processor_imu.yaml", {imu_dir});
sen_imu = static_pointer_cast<SensorImu>(sen0_ptr);
processor_imu = static_pointer_cast<ProcessorImu>(processor_);
@@ -394,8 +394,8 @@ class FactorImu_biasTest_Move_NullBias : public testing::Test
solver->getSolverOptions().max_num_iterations = 1e4;
// SENSOR + PROCESSOR Imu
- SensorBasePtr sen0_ptr = problem->installSensor("SensorImu3d", (Vector7d()<<0,0,0,0,0,0,1).finished(), wolf_root + "/test/yaml/sensor_imu.yaml", {wolf_root});
- processor_ = problem->installProcessor("ProcessorImu", sen0_ptr, wolf_root + "/test/yaml/processor_imu.yaml", {wolf_root});
+ SensorBasePtr sen0_ptr = problem->installSensor("SensorImu3d", (Vector7d()<<0,0,0,0,0,0,1).finished(), imu_dir + "/test/yaml/sensor_imu.yaml", {imu_dir});
+ processor_ = problem->installProcessor("ProcessorImu", sen0_ptr, imu_dir + "/test/yaml/processor_imu.yaml", {imu_dir});
sen_imu = static_pointer_cast<SensorImu>(sen0_ptr);
processor_imu = static_pointer_cast<ProcessorImu>(processor_);
@@ -474,8 +474,8 @@ class FactorImu_biasTest_Move_NonNullBias : public testing::Test
solver->getSolverOptions().max_num_iterations = 1e4;
// SENSOR + PROCESSOR Imu
- SensorBasePtr sen0_ptr = problem->installSensor("SensorImu3d", (Vector7d()<<0,0,0,0,0,0,1).finished(), wolf_root + "/test/yaml/sensor_imu.yaml", {wolf_root});
- processor_ = problem->installProcessor("ProcessorImu", sen0_ptr, wolf_root + "/test/yaml/processor_imu.yaml", {wolf_root});
+ SensorBasePtr sen0_ptr = problem->installSensor("SensorImu3d", (Vector7d()<<0,0,0,0,0,0,1).finished(), imu_dir + "/test/yaml/sensor_imu.yaml", {imu_dir});
+ processor_ = problem->installProcessor("ProcessorImu", sen0_ptr, imu_dir + "/test/yaml/processor_imu.yaml", {imu_dir});
sen_imu = static_pointer_cast<SensorImu>(sen0_ptr);
processor_imu = static_pointer_cast<ProcessorImu>(processor_);
@@ -550,8 +550,8 @@ class FactorImu_biasTest_Move_NonNullBiasRotCst : public testing::Test
solver->getSolverOptions().max_num_iterations = 1e4;
// SENSOR + PROCESSOR Imu
- SensorBasePtr sen0_ptr = problem->installSensor("SensorImu3d", (Vector7d()<<0,0,0,0,0,0,1).finished(), wolf_root + "/test/yaml/sensor_imu.yaml", {wolf_root});
- processor_ = problem->installProcessor("ProcessorImu", sen0_ptr, wolf_root + "/test/yaml/processor_imu.yaml", {wolf_root});
+ SensorBasePtr sen0_ptr = problem->installSensor("SensorImu3d", (Vector7d()<<0,0,0,0,0,0,1).finished(), imu_dir + "/test/yaml/sensor_imu.yaml", {imu_dir});
+ processor_ = problem->installProcessor("ProcessorImu", sen0_ptr, imu_dir + "/test/yaml/processor_imu.yaml", {imu_dir});
sen_imu = static_pointer_cast<SensorImu>(sen0_ptr);
processor_imu = static_pointer_cast<ProcessorImu>(processor_);
@@ -636,8 +636,8 @@ class FactorImu_biasTest_Move_NonNullBiasRotAndVCst : public testing::Test
solver->getSolverOptions().max_num_iterations = 1e4;
// SENSOR + PROCESSOR Imu
- SensorBasePtr sen0_ptr = problem->installSensor("SensorImu3d", (Vector7d()<<0,0,0,0,0,0,1).finished(), wolf_root + "/test/yaml/sensor_imu.yaml", {wolf_root});
- processor_ = problem->installProcessor("ProcessorImu", sen0_ptr, wolf_root + "/test/yaml/processor_imu.yaml", {wolf_root});
+ SensorBasePtr sen0_ptr = problem->installSensor("SensorImu3d", (Vector7d()<<0,0,0,0,0,0,1).finished(), imu_dir + "/test/yaml/sensor_imu.yaml", {imu_dir});
+ processor_ = problem->installProcessor("ProcessorImu", sen0_ptr, imu_dir + "/test/yaml/processor_imu.yaml", {imu_dir});
sen_imu = static_pointer_cast<SensorImu>(sen0_ptr);
processor_imu = static_pointer_cast<ProcessorImu>(processor_);
@@ -730,8 +730,8 @@ class FactorImu_biasTest_Move_NonNullBiasRot : public testing::Test
solver->getSolverOptions().max_num_iterations = 1e4;
// SENSOR + PROCESSOR Imu
- SensorBasePtr sen0_ptr = problem->installSensor("SensorImu3d", (Vector7d()<<0,0,0,0,0,0,1).finished(), wolf_root + "/test/yaml/sensor_imu.yaml", {wolf_root});
- processor_ = problem->installProcessor("ProcessorImu", sen0_ptr, wolf_root + "/test/yaml/processor_imu.yaml", {wolf_root});
+ SensorBasePtr sen0_ptr = problem->installSensor("SensorImu3d", (Vector7d()<<0,0,0,0,0,0,1).finished(), imu_dir + "/test/yaml/sensor_imu.yaml", {imu_dir});
+ processor_ = problem->installProcessor("ProcessorImu", sen0_ptr, imu_dir + "/test/yaml/processor_imu.yaml", {imu_dir});
sen_imu = static_pointer_cast<SensorImu>(sen0_ptr);
processor_imu = static_pointer_cast<ProcessorImu>(processor_);
@@ -823,13 +823,13 @@ class FactorImu_ODOM_biasTest_Move_NonNullBiasRot : public testing::Test
ceres_manager->getSolverOptions().minimizer_type = ceres::TRUST_REGION; //ceres::TRUST_REGION;ceres::LINE_SEARCH
// SENSOR + PROCESSOR Imu
- sensor = problem->installSensor("SensorImu3d", (Vector7d()<<0,0,0,0,0,0,1).finished(), wolf_root + "/test/yaml/sensor_imu.yaml", {wolf_root});
+ sensor = problem->installSensor("SensorImu3d", (Vector7d()<<0,0,0,0,0,0,1).finished(), imu_dir + "/test/yaml/sensor_imu.yaml", {imu_dir});
sensor_imu = static_pointer_cast<SensorImu>(sensor);
- processor = problem->installProcessor("ProcessorImu", sen0_ptr, wolf_root + "/test/yaml/processor_imu.yaml", {wolf_root});
+ processor = problem->installProcessor("ProcessorImu", sen0_ptr, imu_dir + "/test/yaml/processor_imu.yaml", {imu_dir});
processor_imu = static_pointer_cast<ProcessorImu>(processor);
// SENSOR + PROCESSOR ODOM 3d
- sensor = problem->installSensor("ODOM 3d", "odom", (Vector7d()<<0,0,0,0,0,0,1).finished(), wolf_root + "/test/yaml/sensor_odom_3d.yaml");
+ sensor = problem->installSensor("ODOM 3d", "odom", (Vector7d()<<0,0,0,0,0,0,1).finished(), imu_dir + "/test/yaml/sensor_odom_3d.yaml");
sensor_odo = static_pointer_cast<SensorOdom3d>(sensor);
sensor_imu->setNoiseStd((sensor_imu->getNoiseStd()/10.0).eval());
@@ -999,13 +999,13 @@ class FactorImu_ODOM_biasTest_Move_NonNullBiasRotY : public testing::Test
solver->getSolverOptions().max_num_iterations = 1e4;
// SENSOR + PROCESSOR Imu
- SensorBasePtr sen0_ptr = problem->installSensor("SensorImu3d", (Vector7d()<<0,0,0,0,0,0,1).finished(), wolf_root + "/test/yaml/sensor_imu.yaml", {wolf_root});
- processor_ = problem->installProcessor("ProcessorImu", sen0_ptr, wolf_root + "/test/yaml/processor_imu.yaml", {wolf_root});
+ SensorBasePtr sen0_ptr = problem->installSensor("SensorImu3d", (Vector7d()<<0,0,0,0,0,0,1).finished(), imu_dir + "/test/yaml/sensor_imu.yaml", {imu_dir});
+ processor_ = problem->installProcessor("ProcessorImu", sen0_ptr, imu_dir + "/test/yaml/processor_imu.yaml", {imu_dir});
sen_imu = static_pointer_cast<SensorImu>(sen0_ptr);
processor_imu = static_pointer_cast<ProcessorImu>(processor_);
// SENSOR + PROCESSOR ODOM 3d
- SensorBasePtr sen1_ptr = problem->installSensor("ODOM 3d", "odom", (Vector7d()<<0,0,0,0,0,0,1).finished(), wolf_root + "/test/yaml/sensor_odom_3d.yaml");
+ SensorBasePtr sen1_ptr = problem->installSensor("ODOM 3d", "odom", (Vector7d()<<0,0,0,0,0,0,1).finished(), imu_dir + "/test/yaml/sensor_odom_3d.yaml");
ParamsProcessorOdom3dPtr prc_odom3d_params = make_shared<ParamsProcessorOdom3d>();
prc_odom3d_params->max_time_span = 0.9999;
prc_odom3d_params->max_buff_length = 1000000000; //make it very high so that this condition will not pass
@@ -1134,13 +1134,13 @@ class FactorImu_ODOM_biasTest_Move_NonNullBiasRotXY : public testing::Test
solver->getSolverOptions().max_num_iterations = 1e4;
// SENSOR + PROCESSOR Imu
- SensorBasePtr sen0_ptr = problem->installSensor("SensorImu3d", (Vector7d()<<0,0,0,0,0,0,1).finished(), wolf_root + "/test/yaml/sensor_imu.yaml", {wolf_root});
- processor = problem->installProcessor("ProcessorImu", sen0_ptr, wolf_root + "/test/yaml/processor_imu.yaml", {wolf_root});
+ SensorBasePtr sen0_ptr = problem->installSensor("SensorImu3d", (Vector7d()<<0,0,0,0,0,0,1).finished(), imu_dir + "/test/yaml/sensor_imu.yaml", {imu_dir});
+ processor = problem->installProcessor("ProcessorImu", sen0_ptr, imu_dir + "/test/yaml/processor_imu.yaml", {imu_dir});
sen_imu = static_pointer_cast<SensorImu>(sen0_ptr);
processor_imu = static_pointer_cast<ProcessorImu>(processor);
// SENSOR + PROCESSOR ODOM 3d
- SensorBasePtr sen1_ptr = problem->installSensor("ODOM 3d", "odom", (Vector7d()<<0,0,0,0,0,0,1).finished(), wolf_root + "/test/yaml/sensor_odom_3d.yaml");
+ SensorBasePtr sen1_ptr = problem->installSensor("ODOM 3d", "odom", (Vector7d()<<0,0,0,0,0,0,1).finished(), imu_dir + "/test/yaml/sensor_odom_3d.yaml");
ParamsProcessorOdom3dPtr prc_odom3d_params = make_shared<ParamsProcessorOdom3d>();
prc_odom3d_params->max_time_span = 0.9999;
prc_odom3d_params->max_buff_length = 1000000000; //make it very high so that this condition will not pass
diff --git a/test/gtest_factor_imu2d.cpp b/test/gtest_factor_imu2d.cpp
index a6ebea4ea6046c211bd67274da19482c5f24a023..eda9d97a5bb0684031c60f3925ec185e7c669812 100644
--- a/test/gtest_factor_imu2d.cpp
+++ b/test/gtest_factor_imu2d.cpp
@@ -29,21 +29,22 @@
using namespace Eigen;
using namespace wolf;
-string wolf_root = _WOLF_IMU_CODE_DIR;
-int N_TESTS = 10;
+std::string imu_dir = _WOLF_IMU_CODE_DIR;
+std::string wolf_schema_dir = _WOLF_SCHEMA_DIR;
+int N_TESTS = 10;
class FactorImu2d_test : public testing::Test
{
public:
- Matrix6d data_cov;
- Matrix5d delta_cov;
- MatrixXd jacobian_bias = MatrixXd(5, 3);
- Vector3d b0_preint;
- ProblemPtr problem_ptr;
- SolverCeresPtr solver;
- FrameBasePtr frm0, frm1;
- SensorBasePtr sensor;
- CaptureImuPtr cap0, cap1;
+ Matrix6d data_cov;
+ Matrix5d delta_cov;
+ MatrixXd jacobian_bias = MatrixXd(5, 3);
+ Vector3d b0_preint;
+ ProblemPtr problem;
+ SolverManagerPtr solver;
+ FrameBasePtr frm0, frm1;
+ SensorBasePtr sensor;
+ CaptureImuPtr cap0, cap1;
virtual void SetUp()
{
@@ -57,17 +58,17 @@ class FactorImu2d_test : public testing::Test
b0_preint = Vector3d::Zero();
// Problem and solver
- problem_ptr = Problem::create("POV", 2);
- solver = std::make_shared<SolverCeres>(problem_ptr);
- solver->getSolverOptions().function_tolerance = 1e-8;
- solver->getSolverOptions().gradient_tolerance = 1e-8;
+ problem = Problem::create("POV", 2);
+
+ solver = FactorySolverFile::create(
+ "SolverCeres", problem, imu_dir + "/test/yaml/solver_ceres.yaml", {wolf_schema_dir});
// Two frames
- frm0 = problem_ptr->emplaceFrame(TimeStamp(0), "POV", Vector5d::Zero());
- frm1 = problem_ptr->emplaceFrame(TimeStamp(1), "POV", Vector5d::Zero());
+ frm0 = problem->emplaceFrame(TimeStamp(0), "POV", Vector5d::Zero());
+ frm1 = problem->emplaceFrame(TimeStamp(1), "POV", Vector5d::Zero());
// Imu2d sensor
- sensor = problem_ptr->installSensor("SensorImu", wolf_root + "/test/yaml/sensor_imu2d.yaml", {wolf_root});
+ sensor = problem->installSensor(imu_dir + "/test/yaml/sensor_imu2d.yaml", {imu_dir});
// capture from frm1 to frm0
cap0 =
@@ -80,7 +81,7 @@ class FactorImu2d_test : public testing::Test
TEST_F(FactorImu2d_test, check_tree)
{
- ASSERT_TRUE(problem_ptr->check(0));
+ ASSERT_TRUE(problem->check(0));
}
TEST_F(FactorImu2d_test, bias_zero_solve_f1)
@@ -115,9 +116,9 @@ TEST_F(FactorImu2d_test, bias_zero_solve_f1)
frm1->perturb(0.01);
// solve
- problem_ptr->print(4, 1, 1, 1);
+ problem->print(4, 1, 1, 1);
std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
- problem_ptr->print(4, 1, 1, 1);
+ problem->print(4, 1, 1, 1);
ASSERT_POSE2d_APPROX(frm1->getStateVector("PO"), x1.head(3), 1e-6);
ASSERT_MATRIX_APPROX(frm1->getStateVector("V"), x1.tail(2), 1e-6);
@@ -160,9 +161,9 @@ TEST_F(FactorImu2d_test, bias_zero_solve_f0)
frm0->perturb(0.01);
// solve
- problem_ptr->print(4, 1, 1, 1);
+ problem->print(4, 1, 1, 1);
std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
- problem_ptr->print(4, 1, 1, 1);
+ problem->print(4, 1, 1, 1);
ASSERT_POSE2d_APPROX(frm1->getStateVector("PO"), x1.head(3), 1e-6);
ASSERT_MATRIX_APPROX(frm1->getStateVector("V"), x1.tail(2), 1e-6);
@@ -210,9 +211,9 @@ TEST_F(FactorImu2d_test, bias_zero_solve_b1)
cap1->perturb(0.01);
// solve to estimate bias at cap1
- problem_ptr->print(4, 1, 1, 1);
+ problem->print(4, 1, 1, 1);
std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
- problem_ptr->print(4, 1, 1, 1);
+ problem->print(4, 1, 1, 1);
EXPECT_POSE2d_APPROX(cap1->getStateVector("I"), b0, 1e-6);
// WOLF_INFO(cap1->getStateVector("I"));
@@ -263,9 +264,9 @@ TEST_F(FactorImu2d_test, solve_b0)
cap0->perturb(0.1);
// solve
- problem_ptr->print(4, 1, 1, 1);
+ problem->print(4, 1, 1, 1);
std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
- problem_ptr->print(4, 1, 1, 1);
+ problem->print(4, 1, 1, 1);
EXPECT_POSE2d_APPROX(cap0->getStateVector("I"), b0, 1e-6);
// WOLF_INFO(cap0->getStateVector("I"));
@@ -312,9 +313,9 @@ TEST_F(FactorImu2d_test, solve_b1)
cap1->perturb(0.01);
// solve to estimate bias at cap1
- problem_ptr->print(4, 1, 1, 1);
+ problem->print(4, 1, 1, 1);
std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
- problem_ptr->print(4, 1, 1, 1);
+ problem->print(4, 1, 1, 1);
EXPECT_POSE2d_APPROX(cap1->getStateVector("I"), b0, 1e-6);
// WOLF_INFO(cap1->getStateVector("I"));
@@ -365,9 +366,9 @@ TEST_F(FactorImu2d_test, solve_f0)
frm0->perturb(0.1);
// solve
- problem_ptr->print(4, 1, 1, 1);
+ problem->print(4, 1, 1, 1);
std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
- problem_ptr->print(4, 1, 1, 1);
+ problem->print(4, 1, 1, 1);
ASSERT_POSE2d_APPROX(frm1->getStateVector("PO"), x1.head(3), 1e-6);
ASSERT_MATRIX_APPROX(frm1->getStateVector("V"), x1.tail(2), 1e-6);
@@ -419,9 +420,9 @@ TEST_F(FactorImu2d_test, solve_f1)
frm1->perturb(0.1);
// solve
- problem_ptr->print(4, 1, 1, 1);
+ problem->print(4, 1, 1, 1);
std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
- problem_ptr->print(4, 1, 1, 1);
+ problem->print(4, 1, 1, 1);
ASSERT_POSE2d_APPROX(frm1->getStateVector("PO"), x1.head(3), 1e-6);
ASSERT_MATRIX_APPROX(frm1->getStateVector("V"), x1.tail(2), 1e-6);
@@ -474,9 +475,9 @@ TEST_F(FactorImu2d_test, solve_f1_b1)
cap1->perturb(0.1);
// solve
- problem_ptr->print(4, 1, 1, 1);
+ problem->print(4, 1, 1, 1);
std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
- problem_ptr->print(4, 1, 1, 1);
+ problem->print(4, 1, 1, 1);
ASSERT_POSE2d_APPROX(frm1->getStateVector("PO"), x1.head(3), 1e-6);
ASSERT_MATRIX_APPROX(frm1->getStateVector("V"), x1.tail(2), 1e-6);
diff --git a/test/gtest_factor_imu2d_with_gravity.cpp b/test/gtest_factor_imu2d_with_gravity.cpp
index f6e30fa5594fb941b75bd358d437a3ace7f410a5..637e14d110c501542163e0d86fbc61f2684b56c5 100644
--- a/test/gtest_factor_imu2d_with_gravity.cpp
+++ b/test/gtest_factor_imu2d_with_gravity.cpp
@@ -28,8 +28,9 @@
using namespace Eigen;
using namespace wolf;
-string wolf_root = _WOLF_IMU_CODE_DIR;
-int N_TESTS = 10;
+std::string imu_dir = _WOLF_IMU_CODE_DIR;
+std::string wolf_schema_dir = _WOLF_SCHEMA_DIR;
+int N_TESTS = 10;
class FactorImu2dwithGravity_test : public testing::Test
{
@@ -66,7 +67,7 @@ class FactorImu2dwithGravity_test : public testing::Test
frm1 = problem_ptr->emplaceFrame(TimeStamp(1), "POV", Vector5d::Zero());
// Imu2d sensor
- sensor = problem_ptr->installSensor(wolf_root + "/test/yaml/sensor_imu2d_with_gravity.yaml", {wolf_root});
+ sensor = problem_ptr->installSensor(imu_dir + "/test/yaml/sensor_imu2d_with_gravity.yaml", {imu_dir});
// capture from frm1 to frm0
cap0 =
diff --git a/test/gtest_processor_imu_jacobians.cpp b/test/gtest_processor_imu_jacobians.cpp
index 810e1e9a3418a0db4514d3df667f4f926ef103aa..f9bda2fc33585e5ee3740099559968cad02246d0 100644
--- a/test/gtest_processor_imu_jacobians.cpp
+++ b/test/gtest_processor_imu_jacobians.cpp
@@ -17,17 +17,11 @@
//
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
-/**
- * \file gtest_imu_preintegration_jacobians.cpp
- *
- * Created on: Nov 29, 2016
- * \author: AtDinesh
- */
//Wolf
#include "imu/capture/capture_imu.h"
#include "imu/sensor/sensor_imu.h"
-#include "processor_imu_UnitTester.h"
+#include "processor_imu_tester.h"
#include "core/common/wolf.h"
#include "core/problem/problem.h"
#include "core/state_block/state_block.h"
@@ -47,7 +41,7 @@
using namespace wolf;
// A new one of these is created for each test
-class ProcessorImu_jacobians : public testing::Test
+class ProcessorImuJacobians : public testing::Test
{
public:
TimeStamp t;
@@ -84,7 +78,7 @@ class ProcessorImu_jacobians : public testing::Test
Eigen::VectorXd Imu_extrinsics(7);
Imu_extrinsics << 0,0,0, 0,0,0,1; // Imu pose in the robot
- ProcessorImu_UnitTester processor_imu;
+ ProcessorImuTester processor_imu;
ddelta_bias = 0.00000001;
dt = 0.001;
@@ -129,7 +123,7 @@ class ProcessorImu_jacobians : public testing::Test
}
};
-class ProcessorImu_jacobians_Dq : public testing::Test
+class ProcessorImuJacobians_Dq : public testing::Test
{
public:
TimeStamp t;
@@ -163,7 +157,7 @@ class ProcessorImu_jacobians_Dq : public testing::Test
Eigen::VectorXd Imu_extrinsics(7);
Imu_extrinsics << 0,0,0, 0,0,0,1; // Imu pose in the robot
- ProcessorImu_UnitTester processor_imu;
+ ProcessorImuTester processor_imu;
ddelta_bias2 = 0.0001;
dt = 0.001;
@@ -234,7 +228,7 @@ class ProcessorImu_jacobians_Dq : public testing::Test
Then at first step, dR.tr() = Id, dDq_dwb = 0_{3x3}, which boils down to dDq_dwb_ = Jr(wdt)*dt
*/
-TEST_F(ProcessorImu_jacobians, dDp_dab)
+TEST_F(ProcessorImuJacobians, dDp_dab)
{
using namespace wolf;
Eigen::Matrix3d dDp_dab;
@@ -246,7 +240,7 @@ TEST_F(ProcessorImu_jacobians, dDp_dab)
"\ndDp_dab_a - dDp_dab_ : \n" << bias_jac.dDp_dab_ - dDp_dab << std::endl;
}
-TEST_F(ProcessorImu_jacobians, dDv_dab)
+TEST_F(ProcessorImuJacobians, dDv_dab)
{
using namespace wolf;
Eigen::Matrix3d dDv_dab;
@@ -258,7 +252,7 @@ TEST_F(ProcessorImu_jacobians, dDv_dab)
"\ndDv_dab_a - dDv_dab_ : \n" << bias_jac.dDv_dab_ - dDv_dab << std::endl;
}
-TEST_F(ProcessorImu_jacobians, dDp_dwb)
+TEST_F(ProcessorImuJacobians, dDp_dwb)
{
using namespace wolf;
Eigen::Matrix3d dDp_dwb;
@@ -270,7 +264,7 @@ TEST_F(ProcessorImu_jacobians, dDp_dwb)
"\ndDp_dwb_a - dDv_dab_ : \n" << bias_jac.dDp_dwb_ - dDp_dwb << std::endl;
}
-TEST_F(ProcessorImu_jacobians, dDv_dwb_)
+TEST_F(ProcessorImuJacobians, dDv_dwb_)
{
using namespace wolf;
Eigen::Matrix3d dDv_dwb;
@@ -282,7 +276,7 @@ TEST_F(ProcessorImu_jacobians, dDv_dwb_)
"\ndDv_dwb_a - dDv_dwb_ : \n" << bias_jac.dDv_dwb_ - dDv_dwb << std::endl;
}
-TEST_F(ProcessorImu_jacobians, dDq_dab)
+TEST_F(ProcessorImuJacobians, dDq_dab)
{
using namespace wolf;
Eigen::Map<Eigen::Quaterniond> q_in_1(NULL), q_in_2(NULL);
@@ -297,7 +291,7 @@ TEST_F(ProcessorImu_jacobians, dDq_dab)
ASSERT_TRUE(dDq_dab.isZero(wolf::Constants::EPS)) << "\t\tdDq_dab_ jacobian is not Zero :" << dDq_dab << std::endl;
}
-TEST_F(ProcessorImu_jacobians, dDq_dwb_noise_1Em8_)
+TEST_F(ProcessorImuJacobians, dDq_dwb_noise_1Em8_)
{
using namespace wolf;
Eigen::Map<Eigen::Quaterniond> q_in_1(NULL), q_in_2(NULL);
@@ -319,7 +313,7 @@ TEST_F(ProcessorImu_jacobians, dDq_dwb_noise_1Em8_)
<< std::endl;
}
-TEST_F(ProcessorImu_jacobians_Dq, dDq_dwb_noise_1Em4_precision_1Em3_)
+TEST_F(ProcessorImuJacobians_Dq, dDq_dwb_noise_1Em4_precision_1Em3_)
{
using namespace wolf;
Eigen::Map<Eigen::Quaterniond> q_in_1(NULL), q_in_2(NULL);
@@ -413,7 +407,7 @@ TEST_F(ProcessorImu_jacobians_Dq, dDq_dwb_noise_1Em4_precision_1Em3_)
dDo_doz = log( (dR(Theta) * dr(theta)).transpose() * dR(Theta) * (dr(theta)*exp(0,0,dthetaz)) )/dthetaz
*/
-TEST_F(ProcessorImu_jacobians, dDp_dP)
+TEST_F(ProcessorImuJacobians, dDp_dP)
{
using namespace wolf;
Eigen::Matrix3d dDp_dP;
@@ -426,7 +420,7 @@ TEST_F(ProcessorImu_jacobians, dDp_dP)
"\ndDp_dP_a - dDp_dP_ : \n" << deltas_jac.jacobian_delta_preint_.block(0,0,3,3) - dDp_dP << std::endl;
}
-TEST_F(ProcessorImu_jacobians, dDp_dV)
+TEST_F(ProcessorImuJacobians, dDp_dV)
{
using namespace wolf;
Eigen::Matrix3d dDp_dV;
@@ -439,7 +433,7 @@ TEST_F(ProcessorImu_jacobians, dDp_dV)
"\ndDp_dV_a - dDp_dV_ : \n" << deltas_jac.jacobian_delta_preint_.block(0,6,3,3) - dDp_dV << std::endl;
}
-TEST_F(ProcessorImu_jacobians, dDp_dO)
+TEST_F(ProcessorImuJacobians, dDp_dO)
{
using namespace wolf;
Eigen::Map<Eigen::Quaterniond> Dq0(NULL), dq0(NULL), Dq_noisy(NULL), dq_noisy(NULL);
@@ -456,7 +450,7 @@ TEST_F(ProcessorImu_jacobians, dDp_dO)
"\ndDp_dO_a - dDp_dO_ : \n" << deltas_jac.jacobian_delta_preint_.block(0,3,3,3) - dDp_dO << std::endl;
}
-TEST_F(ProcessorImu_jacobians, dDv_dV)
+TEST_F(ProcessorImuJacobians, dDv_dV)
{
using namespace wolf;
Eigen::Matrix3d dDv_dV;
@@ -469,7 +463,7 @@ TEST_F(ProcessorImu_jacobians, dDv_dV)
"\ndDv_dV_a - dDv_dV_ : \n" << deltas_jac.jacobian_delta_preint_.block(6,6,3,3) - dDv_dV << std::endl;
}
-TEST_F(ProcessorImu_jacobians, dDv_dO)
+TEST_F(ProcessorImuJacobians, dDv_dO)
{
using namespace wolf;
Eigen::Map<Eigen::Quaterniond> Dq0(NULL), dq0(NULL), Dq_noisy(NULL), dq_noisy(NULL);
@@ -488,7 +482,7 @@ TEST_F(ProcessorImu_jacobians, dDv_dO)
//dDo_dP = dDo_dV = [0, 0, 0]
-TEST_F(ProcessorImu_jacobians, dDo_dO)
+TEST_F(ProcessorImuJacobians, dDo_dO)
{
using namespace wolf;
Eigen::Map<Eigen::Quaterniond> Dq0(NULL), dq0(NULL), Dq_noisy(NULL), dq_noisy(NULL);
@@ -505,7 +499,7 @@ TEST_F(ProcessorImu_jacobians, dDo_dO)
"\ndDo_dO_a - dDo_dO_ : \n" << deltas_jac.jacobian_delta_preint_.block(3,3,3,3) - dDo_dO << std::endl;
}
-TEST_F(ProcessorImu_jacobians, dDp_dp)
+TEST_F(ProcessorImuJacobians, dDp_dp)
{
using namespace wolf;
Eigen::Map<Eigen::Quaterniond> Dq0(NULL), dq0(NULL);
@@ -523,7 +517,7 @@ TEST_F(ProcessorImu_jacobians, dDp_dp)
//dDv_dp = [0, 0, 0]
-TEST_F(ProcessorImu_jacobians, dDv_dv)
+TEST_F(ProcessorImuJacobians, dDv_dv)
{
using namespace wolf;
Eigen::Map<Eigen::Quaterniond> Dq0(NULL), dq0(NULL);
@@ -541,7 +535,7 @@ TEST_F(ProcessorImu_jacobians, dDv_dv)
//dDo_dp = dDo_dv = [0, 0, 0]
-TEST_F(ProcessorImu_jacobians, dDo_do)
+TEST_F(ProcessorImuJacobians, dDo_do)
{
using namespace wolf;
Eigen::Map<Eigen::Quaterniond> Dq0(NULL), dq0(NULL), Dq_noisy(NULL), dq_noisy(NULL);
diff --git a/test/gtest_schema.cpp b/test/gtest_schema.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..6e95b908004dffada91c3c87b2af7912531cacfa
--- /dev/null
+++ b/test/gtest_schema.cpp
@@ -0,0 +1,128 @@
+// WOLF - Copyright (C) 2020,2021,2022,2023
+// Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu) and
+// Joan Vallvé Navarro (jvallve@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF: http://www.iri.upc.edu/wolf
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+
+#include "gnss/common/gnss.h"
+#include "core/utils/utils_gtest.h"
+#include "gnss/capture/capture_gnss_fix.h"
+#include "gnss/sensor/sensor_gnss.h"
+#include "core/map/map_base.h"
+#include "core/sensor/sensor_base.h"
+#include "core/solver/solver_manager.h"
+#include "core/processor/processor_base.h"
+#include "core/tree_manager/tree_manager_base.h"
+
+#include "yaml-schema-cpp/yaml_schema.hpp"
+
+using namespace wolf;
+using namespace yaml_schema_cpp;
+using namespace Eigen;
+
+std::string wolf_schema_dir = _WOLF_SCHEMA_DIR;
+std::string imu_dir = _WOLF_IMU_CODE_DIR;
+
+// just force the plugin .so library to be loaded
+WOLF_LOAD_CORE;
+WOLF_LOAD_IMU;
+
+bool existsSchema(std::string name_schema)
+{
+ // Check extension
+ if (filesystem::extension(name_schema).empty())
+ {
+ name_schema += SCHEMA_EXTENSION;
+ }
+ else if (filesystem::extension(name_schema) != SCHEMA_EXTENSION)
+ {
+ WOLF_ERROR("Wrong schema file extension ", name_schema, ", it should be '", SCHEMA_EXTENSION, "'");
+ return false;
+ }
+
+ // Find schema file
+ try
+ {
+ filesystem::path path_schema = findFileRecursive(name_schema, {imu_dir + "/schema", wolf_schema_dir});
+ }
+ catch (const std::exception& e)
+ {
+ WOLF_ERROR(name_schema, " was NOT found.");
+ return false;
+ }
+ WOLF_INFO(name_schema, " found!");
+ return true;
+}
+
+TEST(Schema, check_schema_existence)
+{
+ // Check that there is an schema file for each of the registered creators of all Factories from yaml nodes/files
+ // (all except FactoryStateBlock)
+
+ // FactoryLandmark
+ auto registered_landmarks = FactoryLandmark::getRegisteredKeys();
+ for (auto key : registered_landmarks)
+ {
+ EXPECT_TRUE(existsSchema(key));
+ }
+
+ // FactoryMapNode
+ auto registered_maps = FactoryMapNode::getRegisteredKeys();
+ for (auto key : registered_maps)
+ {
+ EXPECT_TRUE(existsSchema(key));
+ }
+
+ // FactoryProcessorNode
+ auto registered_processors = FactoryProcessorNode::getRegisteredKeys();
+ for (auto key : registered_processors)
+ {
+ EXPECT_TRUE(existsSchema(key));
+ }
+
+ // FactorySensorNode
+ auto registered_sensors = FactorySensorNode::getRegisteredKeys();
+ for (auto key : registered_sensors)
+ {
+ EXPECT_TRUE(existsSchema(key));
+ }
+
+ // FactorySolverNode
+ auto registered_solvers = FactorySolverNode::getRegisteredKeys();
+ for (auto key : registered_solvers)
+ {
+ EXPECT_TRUE(existsSchema(key));
+ }
+
+ // FactoryTreeManagerNode
+ auto registered_tree_managers = FactoryTreeManagerNode::getRegisteredKeys();
+ for (auto key : registered_tree_managers)
+ {
+ EXPECT_TRUE(existsSchema(key));
+ }
+}
+
+TEST(Schema, validate_all_schemas)
+{
+ ASSERT_TRUE(validateAllSchemas({imu_dir + "/schema", wolf_schema_dir}, true));
+}
+
+int main(int argc, char** argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
diff --git a/test/processor_imu_UnitTester.cpp b/test/processor_imu2d_tester.cpp
similarity index 83%
rename from test/processor_imu_UnitTester.cpp
rename to test/processor_imu2d_tester.cpp
index d9c40a3c9f179839b6503f727a731a6eddfa43d1..eb006080f1b496d892fe11193adddd5a3763f98f 100644
--- a/test/processor_imu_UnitTester.cpp
+++ b/test/processor_imu2d_tester.cpp
@@ -18,16 +18,16 @@
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
-#include "processor_imu_UnitTester.h"
+#include "processor_imu2d_tester.h"
namespace wolf {
-ProcessorImu_UnitTester::ProcessorImu_UnitTester() :
- ProcessorImu(std::make_shared<ParamsProcessorImu>()),
+ProcessorImu2dTester::ProcessorImu2dTester(const YAML::Node& _params) :
+ ProcessorImu2d(_params),
Dq_out_(nullptr)
{}
-ProcessorImu_UnitTester::~ProcessorImu_UnitTester(){}
+ProcessorImu2dTester::~ProcessorImu2dTester(){}
} // namespace wolf
diff --git a/test/processor_imu2d_UnitTester.h b/test/processor_imu2d_tester.h
similarity index 95%
rename from test/processor_imu2d_UnitTester.h
rename to test/processor_imu2d_tester.h
index a54c1dc5d691fd52e3782492a744dafcf8354cce..8f9fb23b9944a7a6caf10b28f16ced52066ddaff 100644
--- a/test/processor_imu2d_UnitTester.h
+++ b/test/processor_imu2d_tester.h
@@ -165,12 +165,12 @@ namespace wolf {
}
};
- class ProcessorImu2d_UnitTester : public ProcessorImu2d{
+ class ProcessorImu2dTester : public ProcessorImu2d{
public:
- ProcessorImu2d_UnitTester();
- ~ProcessorImu2d_UnitTester() override;
+ ProcessorImu2dTester(const YAML::Node& _params);
+ ~ProcessorImu2dTester() override;
//Functions to test jacobians with finite difference method
@@ -196,10 +196,10 @@ namespace wolf {
const Eigen::Matrix<double,9,1>& _delta_noise,
const Eigen::Matrix<double,10,1>& _Delta0);
- public:
- static ProcessorBasePtr create(const std::string& _unique_name,
- const ParamsProcessorBasePtr _params,
- const SensorBasePtr = nullptr);
+ // public:
+ // static ProcessorBasePtr create(const std::string& _unique_name,
+ // const ParamsProcessorBasePtr _params,
+ // const SensorBasePtr = nullptr);
public:
// Maps quat, to be used as temporary
@@ -220,7 +220,7 @@ namespace wolf {
namespace wolf{
//Functions to test jacobians with finite difference method
-inline Imu_jac_bias ProcessorImu2d_UnitTester::finite_diff_ab(const double _dt,
+inline Imu_jac_bias ProcessorImu2dTester::finite_diff_ab(const double _dt,
Eigen::Vector6d& _data,
const double& da_b,
const Eigen::Matrix<double,10,1>& _delta_preint0)
@@ -287,7 +287,7 @@ inline Imu_jac_bias ProcessorImu2d_UnitTester::finite_diff_ab(const double _dt,
return bias_jacobians;
}
-inline Imu_jac_deltas ProcessorImu2d_UnitTester::finite_diff_noise(const double& _dt, Eigen::Vector6d& _data, const Eigen::Matrix<double,9,1>& _Delta_noise, const Eigen::Matrix<double,9,1>& _delta_noise, const Eigen::Matrix<double,10,1>& _Delta0)
+inline Imu_jac_deltas ProcessorImu2dTester::finite_diff_noise(const double& _dt, Eigen::Vector6d& _data, const Eigen::Matrix<double,9,1>& _Delta_noise, const Eigen::Matrix<double,9,1>& _delta_noise, const Eigen::Matrix<double,10,1>& _Delta0)
{
//we do not propagate any noise from data vector
Eigen::VectorXd Delta_; //will contain the preintegrated Delta affected by added noise
diff --git a/test/processor_imu2d_UnitTester.cpp b/test/processor_imu_tester.cpp
similarity index 82%
rename from test/processor_imu2d_UnitTester.cpp
rename to test/processor_imu_tester.cpp
index bf908e077d83b7f6e919860e15354ed1324c9e9d..35d0bc21e6842035204a8ad5d700899fa3fa546e 100644
--- a/test/processor_imu2d_UnitTester.cpp
+++ b/test/processor_imu_tester.cpp
@@ -18,16 +18,16 @@
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
-#include "processor_imu2d_UnitTester.h"
+#include "processor_imu_tester.h"
namespace wolf {
-ProcessorImu2d_UnitTester::ProcessorImu2d_UnitTester() :
- ProcessorImu2d(std::make_shared<ParamsProcessorImu2d>()),
+ProcessorImuTester::ProcessorImuTester(const YAML::Node& _params) :
+ ProcessorImu(_params),
Dq_out_(nullptr)
{}
-ProcessorImu2d_UnitTester::~ProcessorImu2d_UnitTester(){}
+ProcessorImuTester::~ProcessorImuTester(){}
} // namespace wolf
diff --git a/test/processor_imu_UnitTester.h b/test/processor_imu_tester.h
similarity index 96%
rename from test/processor_imu_UnitTester.h
rename to test/processor_imu_tester.h
index 7aa7c95fcaa6d4e7eb5215ec2d753b5b6f026697..ea941f55d1f0bc603092707e959fec5111972907 100644
--- a/test/processor_imu_UnitTester.h
+++ b/test/processor_imu_tester.h
@@ -165,12 +165,12 @@ namespace wolf {
}
};
- class ProcessorImu_UnitTester : public ProcessorImu{
+ class ProcessorImuTester : public ProcessorImu{
public:
- ProcessorImu_UnitTester();
- ~ProcessorImu_UnitTester() override;
+ ProcessorImuTester(const YAML::Node& _params);
+ ~ProcessorImuTester() override;
//Functions to test jacobians with finite difference method
@@ -197,9 +197,9 @@ namespace wolf {
const Eigen::Matrix<double,10,1>& _Delta0);
public:
- static ProcessorBasePtr create(const std::string& _unique_name,
- const ParamsProcessorBasePtr _params,
- const SensorBasePtr = nullptr);
+ // static ProcessorBasePtr create(const std::string& _unique_name,
+ // const ParamsProcessorBasePtr _params,
+ // const SensorBasePtr = nullptr);
public:
// Maps quat, to be used as temporary
@@ -220,7 +220,7 @@ namespace wolf {
namespace wolf{
//Functions to test jacobians with finite difference method
-inline Imu_jac_bias ProcessorImu_UnitTester::finite_diff_ab(const double _dt,
+inline Imu_jac_bias ProcessorImuTester::finite_diff_ab(const double _dt,
Eigen::Vector6d& _data,
const double& da_b,
const Eigen::Matrix<double,10,1>& _delta_preint0)
@@ -287,7 +287,7 @@ inline Imu_jac_bias ProcessorImu_UnitTester::finite_diff_ab(const double _dt,
return bias_jacobians;
}
-inline Imu_jac_deltas ProcessorImu_UnitTester::finite_diff_noise(const double& _dt, Eigen::Vector6d& _data, const Eigen::Matrix<double,9,1>& _Delta_noise, const Eigen::Matrix<double,9,1>& _delta_noise, const Eigen::Matrix<double,10,1>& _Delta0)
+inline Imu_jac_deltas ProcessorImuTester::finite_diff_noise(const double& _dt, Eigen::Vector6d& _data, const Eigen::Matrix<double,9,1>& _Delta_noise, const Eigen::Matrix<double,9,1>& _delta_noise, const Eigen::Matrix<double,10,1>& _Delta0)
{
//we do not propagate any noise from data vector
Eigen::VectorXd Delta_; //will contain the preintegrated Delta affected by added noise
diff --git a/test/yaml/imu_mocap_fusion/processor_imu.yaml b/test/yaml/imu_mocap_fusion/processor_imu.yaml
index 9a769ef09adb46988151ed4a40c3132618551560..bc011b0aff7d61c9f0a569ac7b45fbcdca972b5b 100644
--- a/test/yaml/imu_mocap_fusion/processor_imu.yaml
+++ b/test/yaml/imu_mocap_fusion/processor_imu.yaml
@@ -1,3 +1,7 @@
+name: "cool processor imu"
+plugin: imu
+type: ProcessorImu
+
time_tolerance: 0.0025 # Time tolerance for joining KFs
unmeasured_perturbation_std: 0.00001
apply_loss_function: false
diff --git a/test/yaml/imu_mocap_fusion/processor_pose_3d.yaml b/test/yaml/imu_mocap_fusion/processor_pose_3d.yaml
index ebef1457a86f0023bf60d35876955769efc965ce..5d2b7c3e1414275ddd82ce810c376ed0da568f0b 100644
--- a/test/yaml/imu_mocap_fusion/processor_pose_3d.yaml
+++ b/test/yaml/imu_mocap_fusion/processor_pose_3d.yaml
@@ -1,3 +1,7 @@
+name: "cool processor pose"
+plugin: core
+type: ProcessorPose3d
+
time_tolerance: 0.0005 # seconds
apply_loss_function: false
diff --git a/test/yaml/imu_mocap_fusion/sensor_imu.yaml b/test/yaml/imu_mocap_fusion/sensor_imu.yaml
index a0d1923abdb37e400e3df3affbad4f604441bc25..ac53d7de4e9205ee53f6da758fa1795265d7dff6 100644
--- a/test/yaml/imu_mocap_fusion/sensor_imu.yaml
+++ b/test/yaml/imu_mocap_fusion/sensor_imu.yaml
@@ -1,3 +1,7 @@
+name: "cool sensor imu"
+plugin: imu
+type: SensorImu
+
states:
P:
state: [0, 0, 0]
diff --git a/test/yaml/imu_mocap_fusion/sensor_pose_3d.yaml b/test/yaml/imu_mocap_fusion/sensor_pose_3d.yaml
index 328f368d476c97c3dcc84b82900785f5ca98ffe0..39acfcd702e3de7966441d9ab3be47c7ab755dab 100644
--- a/test/yaml/imu_mocap_fusion/sensor_pose_3d.yaml
+++ b/test/yaml/imu_mocap_fusion/sensor_pose_3d.yaml
@@ -1,3 +1,7 @@
+name: "cool sensor pose"
+plugin: core
+type: SensorPose3d
+
states:
P:
mode: initial_guess
diff --git a/test/yaml/processor_imu.yaml b/test/yaml/processor_imu.yaml
index 16a2edafa7c0e449d4f5870e951b144b7c550082..7de284af7f8c8680abeb11dd4afaa233e8e61426 100644
--- a/test/yaml/processor_imu.yaml
+++ b/test/yaml/processor_imu.yaml
@@ -1,3 +1,7 @@
+name: "cool processor imu"
+plugin: imu
+type: ProcessorImu
+
time_tolerance: 0.0025 # Time tolerance for joining KFs
unmeasured_perturbation_std: 0.00001
apply_loss_function: false
@@ -6,8 +10,8 @@ keyframe_vote:
voting_active: false
max_time_span: 2.0 # seconds
max_buff_length: 20000 # motion deltas
- dist_traveled: 2.0 # meters
- angle_turned: 0.2 # radians (1 rad approx 57 deg, approx 60 deg)
+ max_dist_traveled: 2.0 # meters
+ max_angle_turned: 0.2 # radians (1 rad approx 57 deg, approx 60 deg)
state_provider: true
state_provider_order: 1
diff --git a/test/yaml/processor_imu2d.yaml b/test/yaml/processor_imu2d.yaml
index 16a2edafa7c0e449d4f5870e951b144b7c550082..7705ed4c0d53c111f70e879bd339623cab81fd3c 100644
--- a/test/yaml/processor_imu2d.yaml
+++ b/test/yaml/processor_imu2d.yaml
@@ -1,3 +1,7 @@
+name: "cool processor odom"
+plugin: imu
+type: ProcessorImu2d
+
time_tolerance: 0.0025 # Time tolerance for joining KFs
unmeasured_perturbation_std: 0.00001
apply_loss_function: false
@@ -6,8 +10,8 @@ keyframe_vote:
voting_active: false
max_time_span: 2.0 # seconds
max_buff_length: 20000 # motion deltas
- dist_traveled: 2.0 # meters
- angle_turned: 0.2 # radians (1 rad approx 57 deg, approx 60 deg)
+ max_dist_traveled: 2.0 # meters
+ max_angle_turned: 0.2 # radians (1 rad approx 57 deg, approx 60 deg)
state_provider: true
state_provider_order: 1
diff --git a/test/yaml/processor_imu2d_static_init.yaml b/test/yaml/processor_imu2d_static_init.yaml
index 9174c8e5aa4a62c5c1fddc911786cbf5e85dd5c0..10aa1163e0b7dcfe0326d2b15fc3ee74e208d24d 100644
--- a/test/yaml/processor_imu2d_static_init.yaml
+++ b/test/yaml/processor_imu2d_static_init.yaml
@@ -1,3 +1,7 @@
+name: "cool processor odom"
+plugin: imu
+type: ProcessorImu2d
+
time_tolerance: 0.0025 # Time tolerance for joining KFs
unmeasured_perturbation_std: 0.0001
apply_loss_function: false
@@ -6,8 +10,8 @@ keyframe_vote:
voting_active: true
max_time_span: 0.9999 # seconds
max_buff_length: 1000000000 # motion deltas
- dist_traveled: 100000000000 # meters
- angle_turned: 10000000000 # radians (1 rad approx 57 deg, approx 60 deg)
+ max_dist_traveled: 100000000000 # meters
+ max_angle_turned: 10000000000 # radians (1 rad approx 57 deg, approx 60 deg)
bootstrap:
enable: false
diff --git a/test/yaml/processor_imu_static_init.yaml b/test/yaml/processor_imu_static_init.yaml
index 3776c8720e581fada284e5581238e5c89f49c09f..7cf7350a1d5de35cc41402e00124721a5505a455 100644
--- a/test/yaml/processor_imu_static_init.yaml
+++ b/test/yaml/processor_imu_static_init.yaml
@@ -1,3 +1,7 @@
+name: "cool processor imu"
+plugin: imu
+type: ProcessorImu
+
time_tolerance: 0.0025 # Time tolerance for joining KFs
unmeasured_perturbation_std: 0.0001
apply_loss_function: false
@@ -6,8 +10,8 @@ keyframe_vote:
voting_active: true
max_time_span: 0.9999 # seconds
max_buff_length: 1000000000 # motion deltas
- dist_traveled: 100000000000 # meters
- angle_turned: 10000000000 # radians (1 rad approx 57 deg, approx 60 deg)
+ max_dist_traveled: 100000000000 # meters
+ max_angle_turned: 10000000000 # radians (1 rad approx 57 deg, approx 60 deg)
state_provider: true
state_provider_order: 1
diff --git a/test/yaml/processor_odom_3d.yaml b/test/yaml/processor_odom_3d.yaml
index 835d9388c33b80cf0310c964d8de6b2a5dfa3eb6..5fa752180d1e36a0f5eb0f633f0ee7d62d54b86c 100644
--- a/test/yaml/processor_odom_3d.yaml
+++ b/test/yaml/processor_odom_3d.yaml
@@ -1,11 +1,15 @@
+name: "cool processor odom"
+plugin: core
+type: ProcessorOdom3d
+
time_tolerance: 0.01 # seconds
keyframe_vote:
voting_active: false
max_time_span: 0.2 # seconds
max_buff_length: 10 # motion deltas
- dist_traveled: 0.5 # meters
- angle_turned: 0.1 # radians (1 rad approx 57 deg, approx 60 deg)
+ max_dist_traveled: 0.5 # meters
+ max_angle_turned: 0.1 # radians (1 rad approx 57 deg, approx 60 deg)
unmeasured_perturbation_std: 0.001
apply_loss_function: false
diff --git a/test/yaml/sensor_compass_2d.yaml b/test/yaml/sensor_compass_2d.yaml
index 5a7610735596724de5130e558c7093623fb19635..c50ff2992eff2ab93bfa88bbfdf1ee9fbd3bff1f 100644
--- a/test/yaml/sensor_compass_2d.yaml
+++ b/test/yaml/sensor_compass_2d.yaml
@@ -1,3 +1,7 @@
+name: "cool sensor compass"
+plugin: imu
+type: SensorCompass
+
stdev_noise: 0.02 # m
states:
P:
diff --git a/test/yaml/sensor_compass_3d.yaml b/test/yaml/sensor_compass_3d.yaml
index 596293311587f3d603f24f61de23358b85678a9b..cc7ee5394f3849ea0b0c291412204cae1496fdf9 100644
--- a/test/yaml/sensor_compass_3d.yaml
+++ b/test/yaml/sensor_compass_3d.yaml
@@ -1,3 +1,7 @@
+name: "cool sensor compass"
+plugin: imu
+type: SensorCompass
+
states:
P:
state: [0, 0, 0]
diff --git a/test/yaml/sensor_imu.yaml b/test/yaml/sensor_imu.yaml
index 935986c9eb0494aa2dd75461484e09d9f804b240..072a77156a456f6fb765b78ee1d084dc9c754cda 100644
--- a/test/yaml/sensor_imu.yaml
+++ b/test/yaml/sensor_imu.yaml
@@ -1,3 +1,7 @@
+name: "cool sensor imu"
+plugin: imu
+type: SensorImu
+
states:
P:
state: [0, 0, 0]
diff --git a/test/yaml/sensor_imu2d.yaml b/test/yaml/sensor_imu2d.yaml
index 94d08f7d2ec8a86b97ab836c648f4beb60219448..f1e85064e06c8744f38e8e1f40ca48b758bda065 100644
--- a/test/yaml/sensor_imu2d.yaml
+++ b/test/yaml/sensor_imu2d.yaml
@@ -1,3 +1,7 @@
+name: "cool sensor imu"
+plugin: imu
+type: SensorImu2d
+
states:
P:
state: [0, 0]
diff --git a/test/yaml/sensor_imu2d_with_gravity.yaml b/test/yaml/sensor_imu2d_with_gravity.yaml
index 97666a123d039599a3c1da24b02d82ad2f66f0ab..3706c6e43c9f1a2d3b32263a87a7775cdae6f360 100644
--- a/test/yaml/sensor_imu2d_with_gravity.yaml
+++ b/test/yaml/sensor_imu2d_with_gravity.yaml
@@ -1,3 +1,7 @@
+name: "cool sensor imu"
+plugin: imu
+type: SensorImu2d
+
states:
P:
state: [0, 0]
diff --git a/test/yaml/sensor_odom_3d.yaml b/test/yaml/sensor_odom_3d.yaml
index edb79bc8203ced2bbc5e4b819a60960e49ac696e..63675654b6ddba29e67a27c1196a8fed6059267f 100644
--- a/test/yaml/sensor_odom_3d.yaml
+++ b/test/yaml/sensor_odom_3d.yaml
@@ -1,3 +1,7 @@
+name: "cool sensor odom"
+plugin: core
+type: SensorOdom3d
+
states:
P:
state: [0, 0, 0]
diff --git a/test/yaml/solver_ceres.yaml b/test/yaml/solver_ceres.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..0bc74798825c7884edb203ea20e87b11e84e62be
--- /dev/null
+++ b/test/yaml/solver_ceres.yaml
@@ -0,0 +1,16 @@
+type: SolverCeres
+plugin: core
+
+period: 1
+verbose: 2
+interrupt_on_problem_change: false
+max_num_iterations: 100
+n_threads: 2
+function_tolerance: 0.0000000001
+gradient_tolerance: 0.0000000001
+minimizer: "LEVENBERG_MARQUARDT"
+use_nonmonotonic_steps: false # only for LEVENBERG_MARQUARDT and DOGLEG
+max_consecutive_nonmonotonic_steps: 5 # only if use_nonmonotonic_steps = true
+compute_cov: true
+cov_period: 1 # only if compute_cov
+cov_enum: 2 # only if compute_cov
\ No newline at end of file