diff --git a/CMakeLists.txt b/CMakeLists.txt
index e2b959b0b2b8481385c135a610d42d972c3af820..80a9f2404d851bb255d3754777ed6c491e44052c 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -215,7 +215,7 @@ SET(HDRS_SENSOR
include/core/sensor/sensor_base.h
# include/core/sensor/sensor_diff_drive.h
# include/core/sensor/sensor_motion_model.h
- # include/core/sensor/sensor_odom_2d.h
+ include/core/sensor/sensor_odom_2d.h
# include/core/sensor/sensor_odom_3d.h
# include/core/sensor/sensor_pose.h
)
@@ -323,7 +323,7 @@ SET(SRCS_SENSOR
src/sensor/sensor_base.cpp
# src/sensor/sensor_diff_drive.cpp
# src/sensor/sensor_motion_model.cpp
- # src/sensor/sensor_odom_2d.cpp
+ src/sensor/sensor_odom_2d.cpp
# src/sensor/sensor_odom_3d.cpp
# src/sensor/sensor_pose.cpp
)
diff --git a/include/core/sensor/sensor_base.h b/include/core/sensor/sensor_base.h
index c43d61d1553d74b3eae74cf7f95fa73ca05d1f71..5f55b72dded65866629a550f8307910d9f50d755 100644
--- a/include/core/sensor/sensor_base.h
+++ b/include/core/sensor/sensor_base.h
@@ -53,9 +53,9 @@ namespace wolf {
* must have a constructor available with the API:
*
* SensorClass(const std::string& _unique_name,
- * SizeEigen _dim,
- * const ParamsServer& _server,
- * ParamsSensorClassPtr _params)
+ * SizeEigen _dim,
+ * ParamsSensorClassPtr _params,
+ * const ParamsServer& _server)
*
*/
#define WOLF_SENSOR_CREATE(SensorClass, ParamsSensorClass) \
@@ -100,7 +100,7 @@ struct ParamsSensorBase: public ParamsBase
std::string print() const override
{
- return "noise_std: " + to_string(noise_std) + "\n";
+ return "noise_std: " + toString(noise_std) + "\n";
}
};
diff --git a/include/core/sensor/sensor_odom_2d.h b/include/core/sensor/sensor_odom_2d.h
index c06fc2ba6c430232b0bd34ee727e1fffc44faed6..ede10ff4b781e289701734f48880dd125614743e 100644
--- a/include/core/sensor/sensor_odom_2d.h
+++ b/include/core/sensor/sensor_odom_2d.h
@@ -32,17 +32,19 @@ WOLF_STRUCT_PTR_TYPEDEFS(ParamsSensorOdom2d);
struct ParamsSensorOdom2d : public ParamsSensorBase
{
- ~ParamsSensorOdom2d() override = default;
-
double k_disp_to_disp; ///< ratio of displacement variance to displacement, for odometry noise calculation
double k_rot_to_rot; ///< ratio of rotation variance to rotation, for odometry noise calculation
+ ParamsSensorOdom2d() = default;
ParamsSensorOdom2d(std::string _unique_name, const ParamsServer& _server):
ParamsSensorBase(_unique_name, _server)
{
k_disp_to_disp = _server.getParam<double>(prefix + _unique_name + "/k_disp_to_disp");
k_rot_to_rot = _server.getParam<double>(prefix + _unique_name + "/k_rot_to_rot");
}
+
+ ~ParamsSensorOdom2d() override = default;
+
std::string print() const override
{
return ParamsSensorBase::print() + "\n"
@@ -56,18 +58,19 @@ WOLF_PTR_TYPEDEFS(SensorOdom2d);
class SensorOdom2d : public SensorBase
{
protected:
- double k_disp_to_disp_; ///< ratio of displacement variance to displacement, for odometry noise calculation
- double k_rot_to_rot_; ///< ratio of rotation variance to rotation, for odometry noise calculation
-
+ ParamsSensorOdom2dPtr params_odom2d_;
+
public:
SensorOdom2d(const std::string& _unique_name,
const SizeEigen& _dim,
- const Priors& _priors,
- ParamsSensorOdom2d _params);
+ ParamsSensorOdom2dPtr _params,
+ const Priors& _priors);
+
SensorOdom2d(const std::string& _unique_name,
const SizeEigen& _dim,
- const ParamsServer& _server,
- ParamsSensorOdom2d _params);
+ ParamsSensorOdom2dPtr _params,
+ const ParamsServer& _server);
+
WOLF_SENSOR_CREATE(SensorOdom2d, ParamsSensorOdom2d);
~SensorOdom2d() override;
@@ -104,7 +107,6 @@ class SensorOdom2d : public SensorBase
*/
Matrix2d computeCovFromMotion (const VectorXd& _data) const;
-
};
} // namespace wolf
diff --git a/include/core/utils/params_server.h b/include/core/utils/params_server.h
index a94a0320418eb8cc8c4125d3cd3536d50689db73..b1562a90678b17dfab4b1ea3f3839f8f09c93144 100644
--- a/include/core/utils/params_server.h
+++ b/include/core/utils/params_server.h
@@ -70,7 +70,7 @@ class ParamsServer
};
template<typename Derived>
-std::string to_string(const Eigen::DenseBase<Derived>& mat)
+std::string toString(const Eigen::DenseBase<Derived>& mat)
{
std::stringstream ss;
if (mat.cols() == 1)
@@ -80,7 +80,16 @@ std::string to_string(const Eigen::DenseBase<Derived>& mat)
return ss.str();
}
-std::string to_string(bool _arg);
+std::string toString(bool _arg);
+std::string toString(int _arg);
+std::string toString(long _arg);
+std::string toString(long long _arg);
+std::string toString(unsigned _arg);
+std::string toString(unsigned long _arg);
+std::string toString(unsigned long long _arg);
+std::string toString(float _arg);
+std::string toString(double _arg);
+std::string toString(long double _arg);
inline bool ParamsServer::hasParam(std::string _key) const
{
diff --git a/src/sensor/sensor_base.cpp b/src/sensor/sensor_base.cpp
index 47fc91f406d062fd3da00df7322d79f76a36fb51..17bb36595df46e9c48cff4293d5060f614395fb4 100644
--- a/src/sensor/sensor_base.cpp
+++ b/src/sensor/sensor_base.cpp
@@ -112,9 +112,9 @@ void SensorBase::loadPriors(const Priors& _priors, SizeEigen _dim)
// Checks state size (P, O, V)
if ((key == 'P' or key == 'V') and prior.getState().size() != _dim)
- throw std::runtime_error("Prior state for P or V has wrong size: " + to_string(prior.getState().size()) + " in " + (_dim == 2 ? "2D" : "3D"));
+ throw std::runtime_error("Prior state for P or V has wrong size: " + toString(prior.getState().size()) + " in " + (_dim == 2 ? "2D" : "3D"));
if (key == '0' and prior.getState().size() != (_dim == 2 ? 1 : 4))
- throw std::runtime_error("Prior state for O has wrong size: " + to_string(prior.getState().size()) + " in " + (_dim == 2 ? "2D" : "3D"));
+ throw std::runtime_error("Prior state for O has wrong size: " + toString(prior.getState().size()) + " in " + (_dim == 2 ? "2D" : "3D"));
// create state block
auto sb = FactoryStateBlock::create(prior.getType(), prior.getState(), prior.isFixed());
@@ -702,11 +702,11 @@ CheckLog SensorBase::localCheck(bool _verbose, SensorBaseConstPtr _sen_ptr, std:
{
auto last_capture_found = findLastCaptureBefore(getProblem()->getTimeStamp());
inconsistency_explanation << "SensorBase::last_capture_: "
- << (last_capture_ ? std::to_string(last_capture_->id()) : "-")
+ << (last_capture_ ? toString(last_capture_->id()) : "-")
<< " @ " << last_capture_
<< " is not the actual last capture: "
<< (last_capture_found ?
- std::to_string(last_capture_found->id()) :
+ toString(last_capture_found->id()) :
"-")
<< " @ " << last_capture_found << std::endl;
log.assertTrue(last_capture_ == last_capture_found, inconsistency_explanation);
diff --git a/src/sensor/sensor_odom_2d.cpp b/src/sensor/sensor_odom_2d.cpp
index 711545f66b48b99fcfaf891954db3ddba7b56d7a..b5d0946059c44d191e4e13cf244e1b443ac5f950 100644
--- a/src/sensor/sensor_odom_2d.cpp
+++ b/src/sensor/sensor_odom_2d.cpp
@@ -24,20 +24,34 @@
#include "core/state_block/state_angle.h"
namespace wolf {
-
-SensorOdom2d::SensorOdom2d(Eigen::VectorXd _extrinsics, const ParamsSensorOdom2d& _intrinsics) :
- SensorBase("SensorOdom2d", std::make_shared<StateBlock>(_extrinsics.head(2), true), std::make_shared<StateAngle>(_extrinsics(2), true), nullptr, 2),
- k_disp_to_disp_(_intrinsics.k_disp_to_disp),
- k_rot_to_rot_(_intrinsics.k_rot_to_rot)
+SensorOdom2d::SensorOdom2d(const std::string& _unique_name,
+ const SizeEigen& _dim,
+ ParamsSensorOdom2dPtr _params,
+ const Priors& _priors) :
+ SensorBase("SensorOdom2d",
+ _unique_name,
+ _dim,
+ _params,
+ _priors),
+ params_odom2d_(_params)
{
- assert(_extrinsics.size() == 3 && "Wrong extrinsics vector size! Should be 3 for 2d.");
- //
+ if (_dim != 2)
+ throw std::runtime_error("SensorOdom2d only works with 2D problems");
}
-SensorOdom2d::SensorOdom2d(Eigen::VectorXd _extrinsics, ParamsSensorOdom2dPtr _intrinsics) :
- SensorOdom2d(_extrinsics, *_intrinsics)
+SensorOdom2d::SensorOdom2d(const std::string& _unique_name,
+ const SizeEigen& _dim,
+ ParamsSensorOdom2dPtr _params,
+ const ParamsServer& _server) :
+ SensorBase("SensorOdom2d",
+ _unique_name,
+ _dim,
+ _params,
+ _server),
+ params_odom2d_(_params)
{
- //
+ if (_dim != 2)
+ throw std::runtime_error("SensorOdom2d only works with 2D problems");
}
SensorOdom2d::~SensorOdom2d()
@@ -47,12 +61,12 @@ SensorOdom2d::~SensorOdom2d()
double SensorOdom2d::getDispVarToDispNoiseFactor() const
{
- return k_disp_to_disp_;
+ return params_odom2d_->k_disp_to_disp;
}
double SensorOdom2d::getRotVarToRotNoiseFactor() const
{
- return k_rot_to_rot_;
+ return params_odom2d_->k_rot_to_rot;
}
Eigen::Matrix2d SensorOdom2d::computeCovFromMotion (const VectorXd& _data) const
@@ -61,8 +75,8 @@ Eigen::Matrix2d SensorOdom2d::computeCovFromMotion (const VectorXd& _data) const
double d = fabs(_data(0));
double r = fabs(_data(1));
- double dvar = k_disp_to_disp_ * d;
- double rvar = k_rot_to_rot_ * r;
+ double dvar = params_odom2d_->k_disp_to_disp * d;
+ double rvar = params_odom2d_->k_rot_to_rot * r;
return (Vector2d() << dvar, rvar).finished().asDiagonal();
}
@@ -73,5 +87,5 @@ Eigen::Matrix2d SensorOdom2d::computeCovFromMotion (const VectorXd& _data) const
#include "core/sensor/factory_sensor.h"
namespace wolf {
WOLF_REGISTER_SENSOR(SensorOdom2d);
-WOLF_REGISTER_SENSOR_AUTO(SensorOdom2d);
+WOLF_REGISTER_SENSOR_YAML(SensorOdom2d);
} // namespace wolf
diff --git a/src/state_block/prior.cpp b/src/state_block/prior.cpp
index a58824ece6d2e3fb0a986d1a1fa690d734cbd97c..9e31d09a037cad680a2c3581dff24912849fa944 100644
--- a/src/state_block/prior.cpp
+++ b/src/state_block/prior.cpp
@@ -101,9 +101,9 @@ std::string Prior::print() const
{
return "Prior " + type_ + "\n" +
"mode: " + mode_ + "\n" +
- "state: " + to_string(state_) + "\n" +
- (mode_ == "factor" ? "noise_std: " + to_string(noise_std_) + "\n" : "") + "dynamic: " + to_string(dynamic_) +
- "\n" + (dynamic_ ? "drift_std: " + to_string(drift_std_) + "\n" : "");
+ "state: " + toString(state_) + "\n" +
+ (mode_ == "factor" ? "noise_std: " + toString(noise_std_) + "\n" : "") + "dynamic: " + toString(dynamic_) +
+ "\n" + (dynamic_ ? "drift_std: " + toString(drift_std_) + "\n" : "");
}
} // namespace wolf
\ No newline at end of file
diff --git a/src/utils/params_server.cpp b/src/utils/params_server.cpp
index ac02963700c178cf021cce4a3f487ef6e41a18a6..b8f2475bde3fcbbf4ceadf246814d96310a883fd 100644
--- a/src/utils/params_server.cpp
+++ b/src/utils/params_server.cpp
@@ -49,9 +49,55 @@ void ParamsServer::addParams(std::map<std::string, std::string> _params)
params_.insert(_params.begin(), _params.end());
}
-std::string to_string(bool _arg)
+std::string toString(bool _arg)
{
return (_arg ? "true" : "false");
}
+std::string toString(int _arg)
+{
+ return std::to_string(_arg);
+}
+
+std::string toString(long _arg)
+{
+ return std::to_string(_arg);
+}
+
+std::string toString(long long _arg)
+{
+ return std::to_string(_arg);
+}
+
+std::string toString(unsigned _arg)
+{
+ return std::to_string(_arg);
+}
+
+std::string toString(unsigned long _arg)
+{
+ return std::to_string(_arg);
+}
+
+std::string toString(unsigned long long _arg)
+{
+ return std::to_string(_arg);
+}
+
+std::string toString(float _arg)
+{
+ return std::to_string(_arg);
+}
+
+std::string toString(double _arg)
+{
+ return std::to_string(_arg);
+}
+
+std::string toString(long double _arg)
+{
+ return std::to_string(_arg);
+}
+
+
}
\ No newline at end of file
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index bac2b0d4c0c403e1a33fdf2224a035cae2172dfc..c2bb2e5306e46e51c28cca201b8031669b362d47 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -288,7 +288,11 @@ target_link_libraries(gtest_processor_tracker_landmark_dummy ${PLUGIN_NAME} dumm
# wolf_add_gtest(gtest_sensor_diff_drive gtest_sensor_diff_drive.cpp)
# target_link_libraries(gtest_sensor_diff_drive ${PLUGIN_NAME})
-# SensorDiffDriveSelfcalib class test
+# SensorOdom2d class test
+wolf_add_gtest(gtest_sensor_odom_2d gtest_sensor_odom_2d.cpp)
+target_link_libraries(gtest_sensor_odom_2d ${PLUGIN_NAME})
+
+# SensorPose class test
# wolf_add_gtest(gtest_sensor_pose gtest_sensor_pose.cpp)
# target_link_libraries(gtest_sensor_pose ${PLUGIN_NAME})
diff --git a/test/gtest_sensor_base.cpp b/test/gtest_sensor_base.cpp
index 6d7e6bcab14701fcc3669cfb9b62ff0baaff2189..76a9a0d424357f763819a8d24ded2034bb3a6f10 100644
--- a/test/gtest_sensor_base.cpp
+++ b/test/gtest_sensor_base.cpp
@@ -53,7 +53,7 @@ MatrixXd o_cov_3D = o_std_3D.cwiseAbs2().asDiagonal();
VectorXd noise_std = Vector2d::Constant(0.1);
MatrixXd noise_cov = noise_std.cwiseAbs2().asDiagonal();
-void testSensor(SensorBasePtr S,
+void checkSensor(SensorBasePtr S,
char _key,
const VectorXd& _state,
bool _fixed,
@@ -113,8 +113,8 @@ TEST(SensorBase, POfix2D)
ASSERT_MATRIX_APPROX(S->getNoiseCov(), noise_cov, Constants::EPS);
// checks
- testSensor(S, 'P', p_state_2D, true, vector0, false, vector0);
- testSensor(S, 'O', o_state_2D, true, vector0, false, vector0);
+ checkSensor(S, 'P', p_state_2D, true, vector0, false, vector0);
+ checkSensor(S, 'O', o_state_2D, true, vector0, false, vector0);
}
// 3D Fix
@@ -135,8 +135,8 @@ TEST(SensorBase, POfix3D)
ASSERT_EQ(S->getPriorFeatures().size(), 0);
// check
- testSensor(S, 'P', p_state_3D, true, vector0, false, vector0);
- testSensor(S, 'O', o_state_3D, true, vector0, false, vector0);
+ checkSensor(S, 'P', p_state_3D, true, vector0, false, vector0);
+ checkSensor(S, 'O', o_state_3D, true, vector0, false, vector0);
}
// 2D Initial guess
@@ -157,8 +157,8 @@ TEST(SensorBase, POinitial_guess2D)
ASSERT_EQ(S->getPriorFeatures().size(), 0);
// check
- testSensor(S, 'P', p_state_2D, false, vector0, false, vector0);
- testSensor(S, 'O', o_state_2D, false, vector0, false, vector0);
+ checkSensor(S, 'P', p_state_2D, false, vector0, false, vector0);
+ checkSensor(S, 'O', o_state_2D, false, vector0, false, vector0);
}
// 3D Initial guess
@@ -179,8 +179,8 @@ TEST(SensorBase, POinitial_guess3D)
ASSERT_EQ(S->getPriorFeatures().size(), 0);
// check
- testSensor(S, 'P', p_state_3D, false, vector0, false, vector0);
- testSensor(S, 'O', o_state_3D, false, vector0, false, vector0);
+ checkSensor(S, 'P', p_state_3D, false, vector0, false, vector0);
+ checkSensor(S, 'O', o_state_3D, false, vector0, false, vector0);
}
// 2D Factor
@@ -201,8 +201,8 @@ TEST(SensorBase, POfactor2D)
ASSERT_EQ(S->getPriorFeatures().size(), 2);
// check
- testSensor(S, 'P', p_state_2D, false, p_std_2D, false, vector0);
- testSensor(S, 'O', o_state_2D, false, o_std_2D, false, vector0);
+ checkSensor(S, 'P', p_state_2D, false, p_std_2D, false, vector0);
+ checkSensor(S, 'O', o_state_2D, false, o_std_2D, false, vector0);
}
// 3D Factor
@@ -223,8 +223,8 @@ TEST(SensorBase, POfactor3D)
ASSERT_EQ(S->getPriorFeatures().size(), 2);
// check
- testSensor(S, 'P', p_state_3D, false, p_std_3D, false, vector0);
- testSensor(S, 'O', o_state_3D, false, o_std_3D, false, vector0);
+ checkSensor(S, 'P', p_state_3D, false, p_std_3D, false, vector0);
+ checkSensor(S, 'O', o_state_3D, false, o_std_3D, false, vector0);
}
// 2D Initial guess dynamic
@@ -245,8 +245,8 @@ TEST(SensorBase, POinitial_guess_dynamic2D)
ASSERT_EQ(S->getPriorFeatures().size(), 0);
// check
- testSensor(S, 'P', p_state_2D, false, vector0, true, vector0);
- testSensor(S, 'O', o_state_2D, false, vector0, true, vector0);
+ checkSensor(S, 'P', p_state_2D, false, vector0, true, vector0);
+ checkSensor(S, 'O', o_state_2D, false, vector0, true, vector0);
}
// 3D Initial guess dynamic
@@ -267,8 +267,8 @@ TEST(SensorBase, POinitial_guess_dynamic3D)
ASSERT_EQ(S->getPriorFeatures().size(), 0);
// check
- testSensor(S, 'P', p_state_3D, false, vector0, true, vector0);
- testSensor(S, 'O', o_state_3D, false, vector0, true, vector0);
+ checkSensor(S, 'P', p_state_3D, false, vector0, true, vector0);
+ checkSensor(S, 'O', o_state_3D, false, vector0, true, vector0);
}
// 2D Initial guess dynamic drift
@@ -289,8 +289,8 @@ TEST(SensorBase, POinitial_guess_dynamic2D_drift)
ASSERT_EQ(S->getPriorFeatures().size(), 0);
// check
- testSensor(S, 'P', p_state_2D, false, vector0, true, p_std_2D);
- testSensor(S, 'O', o_state_2D, false, vector0, true, o_std_2D);
+ checkSensor(S, 'P', p_state_2D, false, vector0, true, p_std_2D);
+ checkSensor(S, 'O', o_state_2D, false, vector0, true, o_std_2D);
}
// 3D Initial guess dynamic drift
@@ -311,8 +311,8 @@ TEST(SensorBase, POinitial_guess_dynamic3D_drift)
ASSERT_EQ(S->getPriorFeatures().size(), 0);
// check
- testSensor(S, 'P', p_state_3D, false, vector0, true, p_std_3D);
- testSensor(S, 'O', o_state_3D, false, vector0, true, o_std_3D);
+ checkSensor(S, 'P', p_state_3D, false, vector0, true, p_std_3D);
+ checkSensor(S, 'O', o_state_3D, false, vector0, true, o_std_3D);
}
// 3D POI mixed
@@ -337,15 +337,14 @@ TEST(SensorBase, POI_mixed)
ASSERT_EQ(S->getPriorFeatures().size(), 2);
// check
- testSensor(S, 'P', p_state_3D, true, vector0, true, vector0);
- testSensor(S, 'O', o_state_3D, false, o_std_3D, true, o_std_3D);
- testSensor(S, 'I', i_state_3D, false, vector0, false, vector0);
- testSensor(S, 'A', o_state_3D, false, o_std_3D, false, vector0);
+ checkSensor(S, 'P', p_state_3D, true, vector0, true, vector0);
+ checkSensor(S, 'O', o_state_3D, false, o_std_3D, true, o_std_3D);
+ checkSensor(S, 'I', i_state_3D, false, vector0, false, vector0);
+ checkSensor(S, 'A', o_state_3D, false, o_std_3D, false, vector0);
}
// CONSTRUCTOR WITH PARAM SERVER and KEY_TYPES
-// 2D Fix
-TEST(SensorBase, POfix2D_server)
+TEST(SensorBase, server)
{
ParserYaml parser = ParserYaml("test/yaml/sensor_base.yaml", wolf_root, true);
ParamsServer server = ParamsServer(parser.getParams());
@@ -392,8 +391,8 @@ TEST(SensorBase, POfix2D_server)
ASSERT_EQ(S->getPriorFeatures().size(), mode == "factor" ? 2 : 0);
// check
- testSensor(S, 'P', p_state.head(p_size), mode == "fix", po_std.head(p_size_std), dynamic, po_std.head(p_size_std_drift));
- testSensor(S, 'O', o_state.head(o_size), mode == "fix", po_std.head(o_size_std), dynamic, po_std.head(o_size_std_drift));
+ checkSensor(S, 'P', p_state.head(p_size), mode == "fix", po_std.head(p_size_std), dynamic, po_std.head(p_size_std_drift));
+ checkSensor(S, 'O', o_state.head(o_size), mode == "fix", po_std.head(o_size_std), dynamic, po_std.head(o_size_std_drift));
// INCORRECT YAML
@@ -419,10 +418,10 @@ TEST(SensorBase, POfix2D_server)
ASSERT_EQ(S->getPriorFeatures().size(), 2);
// check
- testSensor(S, 'P', p_state.head(3), false, po_std.head(3), true, vector0);
- testSensor(S, 'O', o_state.head(4), true, vector0, false, vector0);
- testSensor(S, 'I', p_state.head(4), false, vector0, true, po_std.head(4));
- testSensor(S, 'A', o_state.head(4), false, po_std.head(3), true, po_std.head(3));
+ checkSensor(S, 'P', p_state.head(3), false, po_std.head(3), true, vector0);
+ checkSensor(S, 'O', o_state.head(4), true, vector0, false, vector0);
+ checkSensor(S, 'I', p_state.head(4), false, vector0, true, po_std.head(4));
+ checkSensor(S, 'A', o_state.head(4), false, po_std.head(3), true, po_std.head(3));
// POIA - 3D - INCORRECT YAML
WOLF_INFO("Creating sensor from name sensor_POIA_3D_wrong");
@@ -460,10 +459,10 @@ TEST(SensorBase, dummy_make_shared)
ASSERT_EQ(S->getPriorFeatures().size(), 2);
// check
- testSensor(S, 'P', p_state.head(3), false, po_std.head(3), true, vector0);
- testSensor(S, 'O', o_state.head(4), true, vector0, false, vector0);
- testSensor(S, 'I', p_state.head(4), false, vector0, true, po_std.head(4));
- testSensor(S, 'A', o_state.head(4), false, po_std.head(3), true, po_std.head(3));
+ checkSensor(S, 'P', p_state.head(3), false, po_std.head(3), true, vector0);
+ checkSensor(S, 'O', o_state.head(4), true, vector0, false, vector0);
+ checkSensor(S, 'I', p_state.head(4), false, vector0, true, po_std.head(4));
+ checkSensor(S, 'A', o_state.head(4), false, po_std.head(3), true, po_std.head(3));
// POIA - 3D - INCORRECT YAML
@@ -498,10 +497,10 @@ TEST(SensorBase, dummy_factory)
ASSERT_EQ(S->getPriorFeatures().size(), 2);
// check
- testSensor(S, 'P', p_state.head(3), false, po_std.head(3), true, vector0);
- testSensor(S, 'O', o_state.head(4), true, vector0, false, vector0);
- testSensor(S, 'I', p_state.head(4), false, vector0, true, po_std.head(4));
- testSensor(S, 'A', o_state.head(4), false, po_std.head(3), true, po_std.head(3));
+ checkSensor(S, 'P', p_state.head(3), false, po_std.head(3), true, vector0);
+ checkSensor(S, 'O', o_state.head(4), true, vector0, false, vector0);
+ checkSensor(S, 'I', p_state.head(4), false, vector0, true, po_std.head(4));
+ checkSensor(S, 'A', o_state.head(4), false, po_std.head(3), true, po_std.head(3));
// POIA - 3D - INCORRECT YAML
@@ -531,11 +530,10 @@ TEST(SensorBase, dummy_factory_yaml)
ASSERT_EQ(S->getPriorFeatures().size(), 2);
// check
- testSensor(S, 'P', p_state.head(3), false, po_std.head(3), true, vector0);
- testSensor(S, 'O', o_state.head(4), true, vector0, false, vector0);
- testSensor(S, 'I', p_state.head(4), false, vector0, true, po_std.head(4));
- testSensor(S, 'A', o_state.head(4), false, po_std.head(3), true, po_std.head(3));
-
+ checkSensor(S, 'P', p_state.head(3), false, po_std.head(3), true, vector0);
+ checkSensor(S, 'O', o_state.head(4), true, vector0, false, vector0);
+ checkSensor(S, 'I', p_state.head(4), false, vector0, true, po_std.head(4));
+ checkSensor(S, 'A', o_state.head(4), false, po_std.head(3), true, po_std.head(3));
// POIA - 3D - INCORRECT YAML
WOLF_INFO("Creating sensor from name sensor_dummy_1_wrong");
diff --git a/test/gtest_sensor_odom_2d.cpp b/test/gtest_sensor_odom_2d.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..c8a072fc1fb927bd7c582c76c89c0a165d01bdd5
--- /dev/null
+++ b/test/gtest_sensor_odom_2d.cpp
@@ -0,0 +1,344 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+
+#include "core/sensor/sensor_odom_2d.h"
+#include "core/utils/utils_gtest.h"
+#include "core/yaml/parser_yaml.h"
+#include "core/utils/params_server.h"
+#include "dummy/sensor_dummy.h"
+
+using namespace wolf;
+using namespace Eigen;
+
+std::string wolf_root = _WOLF_ROOT_DIR;
+
+VectorXd vector0 = VectorXd(0);
+Vector2d p_state = (Vector2d() << 1, 2).finished();
+Vector1d o_state = (Vector1d() << 3).finished();
+Vector2d p_std = (Vector2d() << 0.1, 0.2).finished();
+Vector1d o_std = (Vector1d() << 0.3).finished();
+Matrix2d p_cov_2D = p_std.cwiseAbs2().asDiagonal();
+Matrix1d o_cov = o_std.cwiseAbs2().asDiagonal();
+Vector2d noise_std = Vector2d::Constant(0.1);
+Matrix2d noise_cov = noise_std.cwiseAbs2().asDiagonal();
+double k_disp_to_disp = 0.5;
+double k_rot_to_rot = 0.8;
+
+void checkSensorOdom2d(SensorOdom2dPtr S,
+ const Vector2d& _p_state,
+ const Vector1d& _o_state,
+ bool _p_fixed,
+ bool _o_fixed,
+ const VectorXd& _p_noise_std,
+ const VectorXd& _o_noise_std,
+ bool _p_dynamic,
+ bool _o_dynamic,
+ const VectorXd& _p_drift_std,
+ const VectorXd& _o_drift_std)
+{
+ MatrixXd p_noise_cov = _p_noise_std.cwiseAbs2().asDiagonal();
+ MatrixXd o_noise_cov = _o_noise_std.cwiseAbs2().asDiagonal();
+ MatrixXd p_drift_cov = _p_drift_std.cwiseAbs2().asDiagonal();
+ MatrixXd o_drift_cov = _o_drift_std.cwiseAbs2().asDiagonal();
+
+ // params
+ ASSERT_NEAR(S->getDispVarToDispNoiseFactor(), k_disp_to_disp, Constants::EPS);
+ ASSERT_NEAR(S->getRotVarToRotNoiseFactor(), k_rot_to_rot, Constants::EPS);
+ // states
+ ASSERT_MATRIX_APPROX(_p_state, S->getStateBlockDynamic('P')->getState(), Constants::EPS);
+ ASSERT_MATRIX_APPROX(_o_state, S->getStateBlockDynamic('O')->getState(), Constants::EPS);
+ // fixed
+ ASSERT_EQ(S->getStateBlockDynamic('P')->isFixed(), _p_fixed);
+ ASSERT_EQ(S->getStateBlockDynamic('O')->isFixed(), _o_fixed);
+ // dynamic
+ ASSERT_EQ(S->isStateBlockDynamic('P'), _p_dynamic);
+ ASSERT_EQ(S->isStateBlockDynamic('O'), _o_dynamic);
+ // drift
+ ASSERT_EQ(_p_drift_std.size(), S->getDriftStd('P').size());
+ ASSERT_EQ(_o_drift_std.size(), S->getDriftStd('O').size());
+ ASSERT_EQ(p_drift_cov.size(), S->getDriftCov('P').size());
+ ASSERT_EQ(o_drift_cov.size(), S->getDriftCov('O').size());
+ if (_p_drift_std.size() != 0)
+ {
+ ASSERT_MATRIX_APPROX(_p_drift_std, S->getDriftStd('P'), Constants::EPS);
+ ASSERT_MATRIX_APPROX(p_drift_cov, S->getDriftCov('P'), Constants::EPS);
+ }
+ if (_o_drift_std.size() != 0)
+ {
+ ASSERT_MATRIX_APPROX(_o_drift_std, S->getDriftStd('O'), Constants::EPS);
+ ASSERT_MATRIX_APPROX(o_drift_cov, S->getDriftCov('O'), Constants::EPS);
+ }
+ // factors
+ ASSERT_EQ(S->getPriorFeatures().size(), (_p_noise_std.size() == 0 ? 0 : 1) + (_o_noise_std.size() == 0 ? 0 : 1) );
+ if (_p_noise_std.size() != 0)
+ {
+ ASSERT_TRUE(S->getPriorFeature('P') != nullptr);
+ ASSERT_EQ(_p_state.size(), S->getPriorFeature('P')->getMeasurement().size());
+ ASSERT_MATRIX_APPROX(_p_state,
+ S->getPriorFeature('P')->getMeasurement(),
+ Constants::EPS);
+ ASSERT_EQ(p_noise_cov.size(), S->getPriorFeature('P')->getMeasurementCovariance().size());
+ ASSERT_MATRIX_APPROX(p_noise_cov,
+ S->getPriorFeature('P')->getMeasurementCovariance(),
+ Constants::EPS);
+ }
+ else
+ {
+ ASSERT_TRUE(S->getPriorFeature('P') == nullptr);
+ }
+ if (_o_noise_std.size() != 0)
+ {
+ ASSERT_TRUE(S->getPriorFeature('O') != nullptr);
+ ASSERT_EQ(_o_state.size(), S->getPriorFeature('O')->getMeasurement().size());
+ ASSERT_MATRIX_APPROX(_o_state,
+ S->getPriorFeature('O')->getMeasurement(),
+ Constants::EPS);
+ ASSERT_EQ(o_noise_cov.size(), S->getPriorFeature('O')->getMeasurementCovariance().size());
+ ASSERT_MATRIX_APPROX(o_noise_cov,
+ S->getPriorFeature('O')->getMeasurementCovariance(),
+ Constants::EPS);
+ }
+ else
+ {
+ ASSERT_TRUE(S->getPriorFeature('O') == nullptr);
+ }
+}
+
+// CONSTRUCTOR WITH PRIORS AND PARAMS
+// Fix
+TEST(SensorOdom2d, fix)
+{
+ auto params = std::make_shared<ParamsSensorOdom2d>();
+ params->noise_std = noise_std;
+ params->k_disp_to_disp = k_disp_to_disp;
+ params->k_rot_to_rot = k_rot_to_rot;
+
+ auto S = std::make_shared<SensorOdom2d>("sensor1", 2, params,
+ Priors({{'P',Prior("P", p_state)}, //default "fix", not dynamic
+ {'O',Prior("O", o_state)}}));
+ // checks
+ checkSensorOdom2d(S, p_state, o_state, true, true, vector0, vector0, false, false, vector0, vector0);
+}
+
+// Initial guess
+TEST(SensorOdom2d, initial_guess)
+{
+ auto params = std::make_shared<ParamsSensorOdom2d>();
+ params->noise_std = noise_std;
+ params->k_disp_to_disp = k_disp_to_disp;
+ params->k_rot_to_rot = k_rot_to_rot;
+
+ auto S = std::make_shared<SensorOdom2d>("sensor1", 2, params,
+ Priors({{'P',Prior("P", p_state, "initial_guess")},
+ {'O',Prior("O", o_state, "initial_guess")}}));
+ // check
+ checkSensorOdom2d(S, p_state, o_state, false, false, vector0, vector0, false, false, vector0, vector0);
+}
+
+// Factor
+TEST(SensorOdom2d, factor)
+{
+ auto params = std::make_shared<ParamsSensorOdom2d>();
+ params->noise_std = noise_std;
+ params->k_disp_to_disp = k_disp_to_disp;
+ params->k_rot_to_rot = k_rot_to_rot;
+
+ auto S = std::make_shared<SensorOdom2d>("sensor1", 2, params,
+ Priors({{'P',Prior("P", p_state, "factor", p_std)},
+ {'O',Prior("O", o_state, "factor", o_std)}}));
+ // check
+ checkSensorOdom2d(S, p_state, o_state, false, false, p_std, o_std, false, false, vector0, vector0);
+}
+
+// Initial guess dynamic
+TEST(SensorOdom2d, initial_guess_dynamic)
+{
+ auto params = std::make_shared<ParamsSensorOdom2d>();
+ params->noise_std = noise_std;
+ params->k_disp_to_disp = k_disp_to_disp;
+ params->k_rot_to_rot = k_rot_to_rot;
+
+ auto S = std::make_shared<SensorOdom2d>("sensor1", 2, params,
+ Priors({{'P',Prior("P", p_state, "initial_guess", vector0, true)},
+ {'O',Prior("O", o_state, "initial_guess", vector0, true)}}));
+ // check
+ checkSensorOdom2d(S, p_state, o_state, false, false, vector0, vector0, true, true, vector0, vector0);
+}
+
+// Initial guess dynamic drift
+TEST(SensorOdom2d, initial_guess_dynamic_drift)
+{
+ auto params = std::make_shared<ParamsSensorOdom2d>();
+ params->noise_std = noise_std;
+ params->k_disp_to_disp = k_disp_to_disp;
+ params->k_rot_to_rot = k_rot_to_rot;
+
+ auto S = std::make_shared<SensorOdom2d>("sensor1", 2, params,
+ Priors({{'P',Prior("P", p_state, "initial_guess", vector0, true, p_std)},
+ {'O',Prior("O", o_state, "initial_guess", vector0, true, o_std)}}));
+ // check
+ checkSensorOdom2d(S, p_state, o_state, false, false, vector0, vector0, true, true, p_std, o_std);
+}
+
+// mixed
+TEST(SensorOdom2d, POI_mixed)
+{
+ auto params = std::make_shared<ParamsSensorOdom2d>();
+ params->noise_std = noise_std;
+ params->k_disp_to_disp = k_disp_to_disp;
+ params->k_rot_to_rot = k_rot_to_rot;
+
+ auto S = std::make_shared<SensorOdom2d>("sensor1", 2, params,
+ Priors({{'P',Prior("P", p_state, "fix", vector0, false)},
+ {'O',Prior("O", o_state, "factor", o_std, true, o_std)}}));
+ // check
+ checkSensorOdom2d(S, p_state, o_state, true, false, vector0, o_std, false, true, vector0, o_std);
+}
+
+// CONSTRUCTOR WITH PARAM SERVER and KEY_TYPES
+TEST(SensorOdom2d, server)
+{
+ ParserYaml parser = ParserYaml("test/yaml/sensor_odom_2d.yaml", wolf_root, true);
+ ParamsServer server = ParamsServer(parser.getParams());
+
+ std::vector<std::string> modes({"fix", "initial_guess", "factor"});
+ std::vector<bool> dynamics({false, true});
+ std::vector<bool> drifts({false, true});
+
+ // P & O
+ for (auto mode : modes)
+ for (auto dynamic : dynamics)
+ for (auto drift : drifts)
+ {
+ // nonsense combination
+ if (not dynamic and drift)
+ continue;
+
+ // CORRECT YAML
+ std::string name = "sensor_" + mode + (dynamic ? "_dynamic" : "") + (drift ? "_drift" : "");
+ WOLF_INFO("Creating sensor from name ", name);
+
+ auto params = std::make_shared<ParamsSensorOdom2d>(name, server);
+ auto S = std::make_shared<SensorOdom2d>(name, 2, params, server);
+
+ auto p_size_std = mode == "factor" ? 2 : 0;
+ auto o_size_std = mode == "factor" ? 1 : 0;
+ auto p_size_std_drift = drift ? 2 : 0;
+ auto o_size_std_drift = drift ? 1 : 0;
+
+ // check
+ checkSensorOdom2d(S, p_state, o_state,
+ mode == "fix", mode == "fix",
+ p_std.head(p_size_std), o_std.head(o_size_std),
+ dynamic, dynamic,
+ p_std.head(p_size_std_drift), o_std.head(o_size_std_drift));
+
+ // INCORRECT YAML
+ WOLF_INFO("Creating sensor from name ", name + "_wrong");
+ ASSERT_THROW(std::make_shared<SensorOdom2d>(name + "_wrong", 2,
+ std::make_shared<ParamsSensorOdom2d>(name + "_wrong", server),
+ server),
+ std::runtime_error);
+ }
+
+ // MIXED - CORRECT YAML
+ WOLF_INFO("Creating sensor from name sensor_mixed");
+
+ auto params = std::make_shared<ParamsSensorOdom2d>("sensor_mixed", server);
+ auto S = std::make_shared<SensorOdom2d>("sensor_mixed", 2, params, server);
+
+ // check
+ checkSensorOdom2d(S,
+ p_state, o_state,
+ false, true,
+ p_std, vector0,
+ true, false,
+ p_std, vector0);
+
+ // MIXED - INCORRECT YAML
+ WOLF_INFO("Creating sensor from name sensor_mixed_wrong");
+
+ ASSERT_THROW(std::make_shared<SensorOdom2d>("sensor_mixed_wrong", 2,
+ std::make_shared<ParamsSensorOdom2d>("sensor_mixed_wrong", server),
+ server),
+ std::runtime_error);
+}
+
+TEST(SensorOdom2d, factory)
+{
+ ParserYaml parser = ParserYaml("test/yaml/sensor_odom_2d.yaml", wolf_root, true);
+ ParamsServer server = ParamsServer(parser.getParams());
+
+ // CORRECT YAML
+ WOLF_INFO("Creating sensor from name sensor_mixed");
+
+ auto S = FactorySensor::create("SensorOdom2d", "sensor_mixed", 2, server);
+
+ SensorOdom2dPtr S_odom = std::dynamic_pointer_cast<SensorOdom2d>(S);
+
+ // checks
+ ASSERT_TRUE(S_odom != nullptr);
+ checkSensorOdom2d(S_odom,
+ p_state, o_state,
+ false, true,
+ p_std, vector0,
+ true, false,
+ p_std, vector0);
+
+ // INCORRECT YAML
+ WOLF_INFO("Creating sensor from name sensor_mixed_wrong");
+
+ ASSERT_THROW(FactorySensor::create("SensorOdom2d", "sensor_mixed_wrong", 2, server),
+ std::runtime_error);
+}
+
+TEST(SensorOdom2d, factory_yaml)
+{
+ // CORRECT YAML
+ WOLF_INFO("Creating sensor from name sensor_mixed");
+
+ auto S = FactorySensorYaml::create("SensorOdom2d", "sensor_mixed", 2, wolf_root + "/test/yaml/sensor_odom_2d.yaml");
+
+ SensorOdom2dPtr S_odom = std::dynamic_pointer_cast<SensorOdom2d>(S);
+
+ // checks
+ ASSERT_TRUE(S_odom != nullptr);
+ checkSensorOdom2d(S_odom,
+ p_state, o_state,
+ false, true,
+ p_std, vector0,
+ true, false,
+ p_std, vector0);
+
+ // INCORRECT YAML
+ WOLF_INFO("Creating sensor from name sensor_mixed_wrong");
+
+ ASSERT_THROW(FactorySensorYaml::create("SensorOdom2d", "sensor_mixed_wrong", 2, wolf_root + "/test/yaml/sensor_odom_2d.yaml"),
+ std::runtime_error);
+}
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
+
diff --git a/test/yaml/sensor_odom_2d.yaml b/test/yaml/sensor_odom_2d.yaml
index 2d26af901956a324988ac86a67a78dacb5ae8a67..079c3022ccae53c510fea589e9c6827f27174155 100644
--- a/test/yaml/sensor_odom_2d.yaml
+++ b/test/yaml/sensor_odom_2d.yaml
@@ -1,4 +1,316 @@
-type: "SensorOdom2d" # This must match the KEY used in the FactorySensor. Otherwise it is an error.
+sensor:
-k_disp_to_disp: 0.02 # m^2 / m
-k_rot_to_rot: 0.01 # rad^2 / rad
+ #############################################################################################
+ ########################################## CORRECT ##########################################
+ #############################################################################################
+
+ sensor_fix:
+ states:
+ P:
+ mode: fix
+ state: [1, 2]
+ dynamic: false
+ O:
+ mode: fix
+ state: [3]
+ dynamic: false
+ noise_std: [0.1, 0.2]
+ k_disp_to_disp: 0.5
+ k_rot_to_rot: 0.8
+
+ sensor_initial_guess:
+ states:
+ P:
+ mode: initial_guess
+ state: [1, 2]
+ dynamic: false
+ O:
+ mode: initial_guess
+ state: [3]
+ dynamic: false
+ noise_std: [0.1, 0.2]
+ k_disp_to_disp: 0.5
+ k_rot_to_rot: 0.8
+
+ sensor_factor:
+ states:
+ P:
+ mode: factor
+ state: [1, 2]
+ noise_std: [0.1, 0.2]
+ dynamic: false
+ O:
+ mode: factor
+ state: [3]
+ noise_std: [0.3]
+ dynamic: false
+ noise_std: [0.1, 0.2]
+ k_disp_to_disp: 0.5
+ k_rot_to_rot: 0.8
+
+ sensor_fix_dynamic:
+ states:
+ P:
+ mode: fix
+ state: [1, 2]
+ dynamic: true
+ O:
+ mode: fix
+ state: [3]
+ dynamic: true
+ noise_std: [0.1, 0.2]
+ k_disp_to_disp: 0.5
+ k_rot_to_rot: 0.8
+
+ sensor_initial_guess_dynamic:
+ states:
+ P:
+ mode: initial_guess
+ state: [1, 2]
+ dynamic: true
+ O:
+ mode: initial_guess
+ state: [3]
+ dynamic: true
+ noise_std: [0.1, 0.2]
+ k_disp_to_disp: 0.5
+ k_rot_to_rot: 0.8
+
+ sensor_factor_dynamic:
+ states:
+ P:
+ mode: factor
+ state: [1, 2]
+ noise_std: [0.1, 0.2]
+ dynamic: true
+ O:
+ mode: factor
+ state: [3]
+ noise_std: [0.3]
+ dynamic: true
+ noise_std: [0.1, 0.2]
+ k_disp_to_disp: 0.5
+ k_rot_to_rot: 0.8
+
+ sensor_fix_dynamic_drift:
+ states:
+ P:
+ mode: fix
+ state: [1, 2]
+ dynamic: true
+ drift_std: [0.1, 0.2]
+ O:
+ mode: fix
+ state: [3]
+ dynamic: true
+ drift_std: [0.3]
+ noise_std: [0.1, 0.2]
+ k_disp_to_disp: 0.5
+ k_rot_to_rot: 0.8
+
+ sensor_initial_guess_dynamic_drift:
+ states:
+ P:
+ mode: initial_guess
+ state: [1, 2]
+ dynamic: true
+ drift_std: [0.1, 0.2]
+ O:
+ mode: initial_guess
+ state: [3]
+ dynamic: true
+ drift_std: [0.3]
+ noise_std: [0.1, 0.2]
+ k_disp_to_disp: 0.5
+ k_rot_to_rot: 0.8
+
+ sensor_factor_dynamic_drift:
+ states:
+ P:
+ mode: factor
+ state: [1, 2]
+ noise_std: [0.1, 0.2]
+ dynamic: true
+ drift_std: [0.1, 0.2]
+ O:
+ mode: factor
+ state: [3]
+ noise_std: [0.3]
+ dynamic: true
+ drift_std: [0.3]
+ noise_std: [0.1, 0.2]
+ k_disp_to_disp: 0.5
+ k_rot_to_rot: 0.8
+
+ sensor_mixed:
+ states:
+ P:
+ mode: factor
+ state: [1, 2]
+ noise_std: [0.1, 0.2]
+ dynamic: true
+ drift_std: [0.1, 0.2]
+ O:
+ mode: fix
+ state: [3]
+ dynamic: false
+ noise_std: [0.1, 0.2]
+ k_disp_to_disp: 0.5
+ k_rot_to_rot: 0.8
+
+ #############################################################################################
+ ######################################### INCORRECT #########################################
+ #############################################################################################
+
+ sensor_fix_wrong:
+ states:
+ P:
+ mode: fix
+ state: [1, 2]
+ #dynamic: false #missing
+ O:
+ mode: fix
+ state: [3]
+ dynamic: false
+ noise_std: [0.1, 0.2]
+ k_disp_to_disp: 0.5
+ k_rot_to_rot: 0.8
+
+ sensor_initial_guess_wrong:
+ states:
+ P:
+ mode: initial_guess
+ state: [1, 2]
+ dynamic: false
+ O:
+ mode: initial_guess
+ state: [3]
+ dynamic: false
+ noise_std: [0.1, 0.2]
+ #k_disp_to_disp: 0.5 #missing
+ k_rot_to_rot: 0.8
+
+ sensor_factor_wrong:
+ states:
+ P:
+ mode: factor
+ state: [1, 2]
+ noise_std: [0.1, 0.2]
+ dynamic: false
+ O:
+ mode: factor
+ #state: [3] #missing
+ noise_std: [0.3]
+ dynamic: false
+ noise_std: [0.1, 0.2]
+ k_disp_to_disp: 0.5
+ k_rot_to_rot: 0.8
+
+ sensor_fix_dynamic_wrong:
+ states:
+ P:
+ mode: fix
+ state: [1, 2, 3] # wrong size
+ dynamic: true
+ O:
+ mode: fix
+ state: [3]
+ dynamic: true
+ noise_std: [0.1, 0.2]
+ k_disp_to_disp: 0.5
+ k_rot_to_rot: 0.8
+
+ sensor_initial_guess_dynamic_wrong:
+ states:
+ # P: #missing
+ # mode: initial_guess
+ # state: [1, 2]
+ # dynamic: true
+ O:
+ mode: initial_guess
+ state: [3]
+ dynamic: true
+ noise_std: [0.1, 0.2]
+ k_disp_to_disp: 0.5
+ k_rot_to_rot: 0.8
+
+ sensor_factor_dynamic_wrong:
+ states:
+ P:
+ mode: factor
+ state: [1, 2]
+ noise_std: [0.1, 0.2, 0.3] #wrong size
+ dynamic: true
+ O:
+ mode: factor
+ state: [3]
+ noise_std: [0.3]
+ dynamic: true
+ noise_std: [0.1, 0.2]
+ k_disp_to_disp: 0.5
+ k_rot_to_rot: 0.8
+
+ sensor_fix_dynamic_drift_wrong:
+ states:
+ P:
+ mode: fix
+ state: [1, 2]
+ dynamic: true
+ drift_std: [0.1, 0.2, 0.3] #wrong size
+ O:
+ mode: fix
+ state: [3]
+ dynamic: true
+ drift_std: [0.3]
+ noise_std: [0.1, 0.2]
+ k_disp_to_disp: 0.5
+ k_rot_to_rot: 0.8
+
+ sensor_initial_guess_dynamic_drift_wrong:
+ states:
+ P:
+ mode: initial_guess
+ state: [3] # wrong size
+ dynamic: true
+ drift_std: [0.1, 0.2]
+ O:
+ mode: initial_guess
+ state: [3]
+ dynamic: true
+ drift_std: [0.3]
+ noise_std: [0.1, 0.2]
+ k_disp_to_disp: 0.5
+ k_rot_to_rot: 0.8
+
+ sensor_factor_dynamic_drift_wrong:
+ states:
+ P:
+ mode: factor
+ state: [1, 2]
+ noise_std: [0.1, 0.2]
+ dynamic: true
+ drift_std: [0.3] # wrong size
+ O:
+ mode: factor
+ state: [3]
+ noise_std: [0.3]
+ dynamic: true
+ drift_std: [0.3]
+ noise_std: [0.1, 0.2]
+ k_disp_to_disp: 0.5
+ k_rot_to_rot: 0.8
+
+ sensor_mixed_wrong:
+ states:
+ P:
+ mode: factor
+ state: [1, 2]
+ noise_std: [0.1, 0.2, 0.3] # wrong size
+ dynamic: true
+ O:
+ mode: fix
+ state: [3]
+ dynamic: false
+ noise_std: [0.1, 0.2]
+ k_disp_to_disp: 0.5
+ k_rot_to_rot: 0.8
\ No newline at end of file