From a04ba988f921833faef7932d13e4ed5fbf12fabe Mon Sep 17 00:00:00 2001
From: jvallve <jvallve@iri.upc.edu>
Date: Fri, 27 May 2022 19:10:36 +0200
Subject: [PATCH] [skip ci] sensor_pose and gtests
---
CMakeLists.txt | 14 +-
demos/hello_wolf/hello_wolf.cpp | 6 +-
include/core/processor/processor_odom_2d.h | 48 +-
include/core/processor/processor_odom_3d.h | 1 -
include/core/sensor/sensor_odom_2d.h | 113 ---
include/core/sensor/sensor_odom_3d.h | 149 ----
include/core/sensor/sensor_pose.h | 40 +-
src/processor/processor_odom_2d.cpp | 6 +-
src/processor/processor_odom_3d.cpp | 6 +-
src/sensor/sensor_base.cpp | 8 +
src/sensor/sensor_odom_2d.cpp | 90 --
src/sensor/sensor_odom_3d.cpp | 84 --
src/sensor/sensor_pose.cpp | 59 +-
test/CMakeLists.txt | 22 +-
test/gtest_odom_2d.cpp | 6 +-
test/gtest_sensor_base.cpp | 24 +-
test/gtest_sensor_odom.cpp | 803 ++++++++++++++++++
test/gtest_sensor_odom_2d.cpp | 474 -----------
test/gtest_sensor_pose.cpp | 250 +++++-
test/yaml/params_tree_manager1.yaml | 13 +-
test/yaml/params_tree_manager2.yaml | 13 +-
test/yaml/sensor_base/sensor_PO_2D_fix.yaml | 12 -
.../sensor_base/sensor_PO_2D_fix_dynamic.yaml | 12 -
.../sensor_base/sensor_PO_2D_fix_wrong.yaml | 12 -
.../sensor_base/sensor_PO_3D_fix_wrong.yaml | 12 -
test/yaml/sensor_base/sensor_dummy.yaml | 24 -
test/yaml/sensor_base/sensor_dummy_wrong.yaml | 24 -
test/yaml/sensor_odom_2d.yaml | 6 +-
.../sensor_odom_2d/sensor_odom_2d_factor.yaml | 14 -
.../sensor_odom_2d_factor_dynamic.yaml | 14 -
.../sensor_odom_2d_factor_dynamic_drift.yaml | 16 -
...or_odom_2d_factor_dynamic_drift_wrong.yaml | 16 -
.../sensor_odom_2d_factor_dynamic_wrong.yaml | 14 -
.../sensor_odom_2d_factor_wrong.yaml | 14 -
.../sensor_odom_2d_fix_dynamic.yaml | 12 -
.../sensor_odom_2d_fix_dynamic_drift.yaml | 14 -
...ensor_odom_2d_fix_dynamic_drift_wrong.yaml | 14 -
.../sensor_odom_2d_fix_dynamic_wrong.yaml | 12 -
.../sensor_odom_2d_fix_wrong.yaml | 12 -
.../sensor_odom_2d_initial_guess.yaml | 12 -
.../sensor_odom_2d_initial_guess_dynamic.yaml | 12 -
...r_odom_2d_initial_guess_dynamic_drift.yaml | 14 -
..._2d_initial_guess_dynamic_drift_wrong.yaml | 14 -
...r_odom_2d_initial_guess_dynamic_wrong.yaml | 12 -
.../sensor_odom_2d_initial_guess_wrong.yaml | 12 -
.../sensor_odom_2d/sensor_odom_2d_mixed.yaml | 14 -
.../sensor_odom_2d_mixed_wrong.yaml | 13 -
test/yaml/sensor_odom_3d.yaml | 12 +-
...r_odom_2d_fix.yaml => sensor_pose_2d.yaml} | 7 +-
...sor_PO_3D_fix.yaml => sensor_pose_3d.yaml} | 8 +-
.../sensor_POIA_3D.yaml | 13 +-
.../sensor_POIA_3D_wrong.yaml | 12 +-
.../sensor_PO_2D_factor.yaml | 12 +-
.../sensor_PO_2D_factor_dynamic.yaml | 12 +-
.../sensor_PO_2D_factor_dynamic_drift.yaml | 11 +-
...nsor_PO_2D_factor_dynamic_drift_wrong.yaml | 11 +-
.../sensor_PO_2D_factor_dynamic_wrong.yaml | 11 +-
.../sensor_PO_2D_factor_wrong.yaml | 12 +-
test/yaml/sensor_tests/sensor_PO_2D_fix.yaml | 22 +
.../sensor_PO_2D_fix_dynamic.yaml | 22 +
.../sensor_PO_2D_fix_dynamic_drift.yaml | 12 +-
.../sensor_PO_2D_fix_dynamic_drift_wrong.yaml | 12 +-
.../sensor_PO_2D_fix_dynamic_wrong.yaml | 12 +-
.../sensor_tests/sensor_PO_2D_fix_wrong.yaml | 22 +
.../sensor_PO_2D_initial_guess.yaml | 12 +-
.../sensor_PO_2D_initial_guess_dynamic.yaml | 12 +-
...sor_PO_2D_initial_guess_dynamic_drift.yaml | 12 +-
..._2D_initial_guess_dynamic_drift_wrong.yaml | 12 +-
...sor_PO_2D_initial_guess_dynamic_wrong.yaml | 12 +-
.../sensor_PO_2D_initial_guess_wrong.yaml | 12 +-
.../sensor_PO_3D_factor.yaml | 12 +-
.../sensor_PO_3D_factor_dynamic.yaml | 12 +-
.../sensor_PO_3D_factor_dynamic_drift.yaml | 12 +-
...nsor_PO_3D_factor_dynamic_drift_wrong.yaml | 12 +-
.../sensor_PO_3D_factor_dynamic_wrong.yaml | 12 +-
.../sensor_PO_3D_factor_wrong.yaml | 12 +-
test/yaml/sensor_tests/sensor_PO_3D_fix.yaml | 22 +
.../sensor_PO_3D_fix_dynamic.yaml | 12 +-
.../sensor_PO_3D_fix_dynamic_drift.yaml | 12 +-
.../sensor_PO_3D_fix_dynamic_drift_wrong.yaml | 12 +-
.../sensor_PO_3D_fix_dynamic_wrong.yaml | 12 +-
.../sensor_tests/sensor_PO_3D_fix_wrong.yaml | 22 +
.../sensor_PO_3D_initial_guess.yaml | 12 +-
.../sensor_PO_3D_initial_guess_dynamic.yaml | 12 +-
...sor_PO_3D_initial_guess_dynamic_drift.yaml | 12 +-
..._3D_initial_guess_dynamic_drift_wrong.yaml | 12 +-
...sor_PO_3D_initial_guess_dynamic_wrong.yaml | 12 +-
.../sensor_PO_3D_initial_guess_wrong.yaml | 12 +-
88 files changed, 1673 insertions(+), 1444 deletions(-)
delete mode 100644 include/core/sensor/sensor_odom_2d.h
delete mode 100644 include/core/sensor/sensor_odom_3d.h
delete mode 100644 src/sensor/sensor_odom_2d.cpp
delete mode 100644 src/sensor/sensor_odom_3d.cpp
create mode 100644 test/gtest_sensor_odom.cpp
delete mode 100644 test/gtest_sensor_odom_2d.cpp
delete mode 100644 test/yaml/sensor_base/sensor_PO_2D_fix.yaml
delete mode 100644 test/yaml/sensor_base/sensor_PO_2D_fix_dynamic.yaml
delete mode 100644 test/yaml/sensor_base/sensor_PO_2D_fix_wrong.yaml
delete mode 100644 test/yaml/sensor_base/sensor_PO_3D_fix_wrong.yaml
delete mode 100644 test/yaml/sensor_base/sensor_dummy.yaml
delete mode 100644 test/yaml/sensor_base/sensor_dummy_wrong.yaml
delete mode 100644 test/yaml/sensor_odom_2d/sensor_odom_2d_factor.yaml
delete mode 100644 test/yaml/sensor_odom_2d/sensor_odom_2d_factor_dynamic.yaml
delete mode 100644 test/yaml/sensor_odom_2d/sensor_odom_2d_factor_dynamic_drift.yaml
delete mode 100644 test/yaml/sensor_odom_2d/sensor_odom_2d_factor_dynamic_drift_wrong.yaml
delete mode 100644 test/yaml/sensor_odom_2d/sensor_odom_2d_factor_dynamic_wrong.yaml
delete mode 100644 test/yaml/sensor_odom_2d/sensor_odom_2d_factor_wrong.yaml
delete mode 100644 test/yaml/sensor_odom_2d/sensor_odom_2d_fix_dynamic.yaml
delete mode 100644 test/yaml/sensor_odom_2d/sensor_odom_2d_fix_dynamic_drift.yaml
delete mode 100644 test/yaml/sensor_odom_2d/sensor_odom_2d_fix_dynamic_drift_wrong.yaml
delete mode 100644 test/yaml/sensor_odom_2d/sensor_odom_2d_fix_dynamic_wrong.yaml
delete mode 100644 test/yaml/sensor_odom_2d/sensor_odom_2d_fix_wrong.yaml
delete mode 100644 test/yaml/sensor_odom_2d/sensor_odom_2d_initial_guess.yaml
delete mode 100644 test/yaml/sensor_odom_2d/sensor_odom_2d_initial_guess_dynamic.yaml
delete mode 100644 test/yaml/sensor_odom_2d/sensor_odom_2d_initial_guess_dynamic_drift.yaml
delete mode 100644 test/yaml/sensor_odom_2d/sensor_odom_2d_initial_guess_dynamic_drift_wrong.yaml
delete mode 100644 test/yaml/sensor_odom_2d/sensor_odom_2d_initial_guess_dynamic_wrong.yaml
delete mode 100644 test/yaml/sensor_odom_2d/sensor_odom_2d_initial_guess_wrong.yaml
delete mode 100644 test/yaml/sensor_odom_2d/sensor_odom_2d_mixed.yaml
delete mode 100644 test/yaml/sensor_odom_2d/sensor_odom_2d_mixed_wrong.yaml
rename test/yaml/{sensor_odom_2d/sensor_odom_2d_fix.yaml => sensor_pose_2d.yaml} (66%)
rename test/yaml/{sensor_base/sensor_PO_3D_fix.yaml => sensor_pose_3d.yaml} (57%)
rename test/yaml/{sensor_base => sensor_tests}/sensor_POIA_3D.yaml (71%)
rename test/yaml/{sensor_base => sensor_tests}/sensor_POIA_3D_wrong.yaml (72%)
rename test/yaml/{sensor_base => sensor_tests}/sensor_PO_2D_factor.yaml (55%)
rename test/yaml/{sensor_base => sensor_tests}/sensor_PO_2D_factor_dynamic.yaml (55%)
rename test/yaml/{sensor_base => sensor_tests}/sensor_PO_2D_factor_dynamic_drift.yaml (61%)
rename test/yaml/{sensor_base => sensor_tests}/sensor_PO_2D_factor_dynamic_drift_wrong.yaml (62%)
rename test/yaml/{sensor_base => sensor_tests}/sensor_PO_2D_factor_dynamic_wrong.yaml (58%)
rename test/yaml/{sensor_base => sensor_tests}/sensor_PO_2D_factor_wrong.yaml (56%)
create mode 100644 test/yaml/sensor_tests/sensor_PO_2D_fix.yaml
create mode 100644 test/yaml/sensor_tests/sensor_PO_2D_fix_dynamic.yaml
rename test/yaml/{sensor_base => sensor_tests}/sensor_PO_2D_fix_dynamic_drift.yaml (55%)
rename test/yaml/{sensor_base => sensor_tests}/sensor_PO_2D_fix_dynamic_drift_wrong.yaml (57%)
rename test/yaml/{sensor_base => sensor_tests}/sensor_PO_2D_fix_dynamic_wrong.yaml (51%)
create mode 100644 test/yaml/sensor_tests/sensor_PO_2D_fix_wrong.yaml
rename test/yaml/{sensor_base => sensor_tests}/sensor_PO_2D_initial_guess.yaml (51%)
rename test/yaml/{sensor_base => sensor_tests}/sensor_PO_2D_initial_guess_dynamic.yaml (52%)
rename test/yaml/{sensor_base => sensor_tests}/sensor_PO_2D_initial_guess_dynamic_drift.yaml (58%)
rename test/yaml/{sensor_base => sensor_tests}/sensor_PO_2D_initial_guess_dynamic_drift_wrong.yaml (60%)
rename test/yaml/{sensor_base => sensor_tests}/sensor_PO_2D_initial_guess_dynamic_wrong.yaml (55%)
rename test/yaml/{sensor_base => sensor_tests}/sensor_PO_2D_initial_guess_wrong.yaml (53%)
rename test/yaml/{sensor_base => sensor_tests}/sensor_PO_3D_factor.yaml (58%)
rename test/yaml/{sensor_base => sensor_tests}/sensor_PO_3D_factor_dynamic.yaml (58%)
rename test/yaml/{sensor_base => sensor_tests}/sensor_PO_3D_factor_dynamic_drift.yaml (64%)
rename test/yaml/{sensor_base => sensor_tests}/sensor_PO_3D_factor_dynamic_drift_wrong.yaml (66%)
rename test/yaml/{sensor_base => sensor_tests}/sensor_PO_3D_factor_dynamic_wrong.yaml (60%)
rename test/yaml/{sensor_base => sensor_tests}/sensor_PO_3D_factor_wrong.yaml (59%)
create mode 100644 test/yaml/sensor_tests/sensor_PO_3D_fix.yaml
rename test/yaml/{sensor_base => sensor_tests}/sensor_PO_3D_fix_dynamic.yaml (50%)
rename test/yaml/{sensor_base => sensor_tests}/sensor_PO_3D_fix_dynamic_drift.yaml (58%)
rename test/yaml/{sensor_base => sensor_tests}/sensor_PO_3D_fix_dynamic_drift_wrong.yaml (59%)
rename test/yaml/{sensor_base => sensor_tests}/sensor_PO_3D_fix_dynamic_wrong.yaml (53%)
create mode 100644 test/yaml/sensor_tests/sensor_PO_3D_fix_wrong.yaml
rename test/yaml/{sensor_base => sensor_tests}/sensor_PO_3D_initial_guess.yaml (53%)
rename test/yaml/{sensor_base => sensor_tests}/sensor_PO_3D_initial_guess_dynamic.yaml (54%)
rename test/yaml/{sensor_base => sensor_tests}/sensor_PO_3D_initial_guess_dynamic_drift.yaml (61%)
rename test/yaml/{sensor_base => sensor_tests}/sensor_PO_3D_initial_guess_dynamic_drift_wrong.yaml (62%)
rename test/yaml/{sensor_base => sensor_tests}/sensor_PO_3D_initial_guess_dynamic_wrong.yaml (56%)
rename test/yaml/{sensor_base => sensor_tests}/sensor_PO_3D_initial_guess_wrong.yaml (56%)
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 5d60969f8..0a3601824 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -204,7 +204,7 @@ SET(HDRS_PROCESSOR
include/core/processor/processor_motion.h
include/core/processor/processor_odom_2d.h
# include/core/processor/processor_odom_3d.h
- # include/core/processor/processor_pose.h
+ include/core/processor/processor_pose.h
include/core/processor/processor_tracker.h
include/core/processor/processor_tracker_feature.h
include/core/processor/processor_tracker_landmark.h
@@ -216,9 +216,7 @@ SET(HDRS_SENSOR
# include/core/sensor/sensor_diff_drive.h
# include/core/sensor/sensor_motion_model.h
include/core/sensor/sensor_odom.h
- include/core/sensor/sensor_odom_2d.h
- # include/core/sensor/sensor_odom_3d.h
- # include/core/sensor/sensor_pose.h
+ include/core/sensor/sensor_pose.h
)
SET(HDRS_SOLVER
include/core/solver/solver_manager.h
@@ -313,8 +311,8 @@ SET(SRCS_PROCESSOR
# src/processor/processor_loop_closure.cpp
src/processor/processor_motion.cpp
src/processor/processor_odom_2d.cpp
- # src/processor/processor_odom_3d.cpp
- # src/processor/processor_pose.cpp
+ src/processor/processor_odom_3d.cpp
+ src/processor/processor_pose.cpp
src/processor/processor_tracker.cpp
src/processor/processor_tracker_feature.cpp
src/processor/processor_tracker_landmark.cpp
@@ -325,9 +323,7 @@ SET(SRCS_SENSOR
# src/sensor/sensor_diff_drive.cpp
# src/sensor/sensor_motion_model.cpp
src/sensor/sensor_odom.cpp
- src/sensor/sensor_odom_2d.cpp
- # src/sensor/sensor_odom_3d.cpp
- # src/sensor/sensor_pose.cpp
+ src/sensor/sensor_pose.cpp
)
SET(SRCS_SOLVER
src/solver/solver_manager.cpp
diff --git a/demos/hello_wolf/hello_wolf.cpp b/demos/hello_wolf/hello_wolf.cpp
index b2b8b4fb6..366d68f47 100644
--- a/demos/hello_wolf/hello_wolf.cpp
+++ b/demos/hello_wolf/hello_wolf.cpp
@@ -37,7 +37,7 @@
// wolf core includes
#include "../../include/core/ceres_wrapper/solver_ceres.h"
#include "core/common/wolf.h"
-#include "core/sensor/sensor_odom_2d.h"
+#include "core/sensor/sensor_odom.h"
#include "core/processor/processor_odom_2d.h"
#include "core/capture/capture_odom_2d.h"
#include "sensor_range_bearing.h"
@@ -129,10 +129,10 @@ int main()
ceres->getSolverOptions().max_num_iterations = 1000; // We depart far from solution, need a lot of iterations
// sensor odometer 2d
- ParamsSensorOdom2dPtr intrinsics_odo = std::make_shared<ParamsSensorOdom2d>();
+ ParamsSensorOdomPtr intrinsics_odo = std::make_shared<ParamsSensorOdom>();
Priors priors_odo = {{'P',Prior("P", Vector2d::Zero())},
{'O',Prior("O", Vector1d::Zero())}};
- SensorBasePtr sensor_odo = problem->installSensor("SensorOdom2d", "sensor odo", intrinsics_odo, priors_odo);
+ SensorBasePtr sensor_odo = problem->installSensor("SensorOdom", "sensor odo", intrinsics_odo, priors_odo);
// processor odometer 2d
ParamsProcessorOdom2dPtr params_odo = std::make_shared<ParamsProcessorOdom2d>();
diff --git a/include/core/processor/processor_odom_2d.h b/include/core/processor/processor_odom_2d.h
index fea39c4d6..aa9011ea6 100644
--- a/include/core/processor/processor_odom_2d.h
+++ b/include/core/processor/processor_odom_2d.h
@@ -72,38 +72,38 @@ class ProcessorOdom2d : public ProcessorMotion
protected:
void 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& _jacobian_calib) const override;
+ const Eigen::MatrixXd& _data_cov,
+ const Eigen::VectorXd& _calib,
+ const double _dt,
+ Eigen::VectorXd& _delta,
+ Eigen::MatrixXd& _delta_cov,
+ Eigen::MatrixXd& _jacobian_calib) const override;
void deltaPlusDelta(const Eigen::VectorXd& _delta1,
- const Eigen::VectorXd& _delta2,
- const double _Dt2,
- Eigen::VectorXd& _delta1_plus_delta2) const override;
+ const Eigen::VectorXd& _delta2,
+ const double _Dt2,
+ Eigen::VectorXd& _delta1_plus_delta2) const override;
void deltaPlusDelta(const Eigen::VectorXd& _delta1,
- const Eigen::VectorXd& _delta2,
- const double _Dt2,
- Eigen::VectorXd& _delta1_plus_delta2,
- Eigen::MatrixXd& _jacobian1,
- Eigen::MatrixXd& _jacobian2) const override;
+ const Eigen::VectorXd& _delta2,
+ const double _Dt2,
+ Eigen::VectorXd& _delta1_plus_delta2,
+ Eigen::MatrixXd& _jacobian1,
+ Eigen::MatrixXd& _jacobian2) const override;
void statePlusDelta(const VectorComposite& _x,
- const Eigen::VectorXd& _delta,
- const double _Dt,
- VectorComposite& _x_plus_delta) const override;
+ const Eigen::VectorXd& _delta,
+ const double _Dt,
+ VectorComposite& _x_plus_delta) const override;
Eigen::VectorXd deltaZero() const override;
Eigen::VectorXd correctDelta(const Eigen::VectorXd& delta_preint,
const Eigen::VectorXd& delta_step) const override;
CaptureMotionPtr 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_ptr) override;
+ const SensorBasePtr& _sensor,
+ const TimeStamp& _ts,
+ const VectorXd& _data,
+ const MatrixXd& _data_cov,
+ const VectorXd& _calib,
+ const VectorXd& _calib_preint,
+ const CaptureBasePtr& _capture_origin_ptr) override;
FeatureBasePtr emplaceFeature(CaptureMotionPtr _capture_motion) override;
FactorBasePtr emplaceFactor(FeatureBasePtr _feature,
CaptureBasePtr _capture_origin) override;
diff --git a/include/core/processor/processor_odom_3d.h b/include/core/processor/processor_odom_3d.h
index 02cdbd5ef..3008bbc80 100644
--- a/include/core/processor/processor_odom_3d.h
+++ b/include/core/processor/processor_odom_3d.h
@@ -30,7 +30,6 @@
#define SRC_PROCESSOR_ODOM_3d_H_
#include "core/processor/processor_motion.h"
-#include "core/sensor/sensor_odom_3d.h"
#include "core/capture/capture_odom_3d.h"
#include "core/math/rotations.h"
#include "core/factor/factor_relative_pose_3d.h"
diff --git a/include/core/sensor/sensor_odom_2d.h b/include/core/sensor/sensor_odom_2d.h
deleted file mode 100644
index 117245f89..000000000
--- a/include/core/sensor/sensor_odom_2d.h
+++ /dev/null
@@ -1,113 +0,0 @@
-//--------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--------
-#ifndef SENSOR_ODOM_2d_H_
-#define SENSOR_ODOM_2d_H_
-
-//wolf includes
-#include "core/sensor/sensor_base.h"
-#include "core/utils/params_server.h"
-
-namespace wolf {
-
-WOLF_STRUCT_PTR_TYPEDEFS(ParamsSensorOdom2d);
-
-struct ParamsSensorOdom2d : public ParamsSensorBase
-{
- 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"
- + "k_disp_to_disp: " + std::to_string(k_disp_to_disp) + "\n"
- + "k_rot_to_rot: " + std::to_string(k_rot_to_rot) + "\n";
- }
-};
-
-WOLF_PTR_TYPEDEFS(SensorOdom2d);
-
-class SensorOdom2d : public SensorBase
-{
- protected:
- ParamsSensorOdom2dPtr params_odom2d_;
-
- public:
- SensorOdom2d(const std::string& _unique_name,
- const SizeEigen& _dim,
- ParamsSensorOdom2dPtr _params,
- const Priors& _priors);
-
- SensorOdom2d(const std::string& _unique_name,
- const SizeEigen& _dim,
- ParamsSensorOdom2dPtr _params,
- const ParamsServer& _server);
-
- WOLF_SENSOR_CREATE(SensorOdom2d, ParamsSensorOdom2d);
-
- ~SensorOdom2d() override;
-
- /** \brief Returns displacement noise factor
- *
- * Returns displacement noise factor
- *
- **/
- double getDispVarToDispNoiseFactor() const;
-
- /** \brief Returns rotation noise factor
- *
- * Returns rotation noise factor
- *
- **/
- double getRotVarToRotNoiseFactor() const;
-
- /**
- * Compute covariance of odometry given the motion data.
- *
- * computeCovFromMotion() produces a diagonal covariance that depends on the amount of motion:
- * - a linear dependence with total displacement
- * - a linear dependence with total rotation
- *
- * The formula for the variances is as follows:
- *
- * - disp_var = k_disp_to_disp * displacement
- * - rot_var = k_rot_to_rot * rotation
- *
- * See implementation for details.
- */
- // noise
- Eigen::MatrixXd computeNoiseCov(const Eigen::VectorXd & _data) const override;
-
-};
-
-} // namespace wolf
-
-#endif // SENSOR_ODOM_2d_H_
diff --git a/include/core/sensor/sensor_odom_3d.h b/include/core/sensor/sensor_odom_3d.h
deleted file mode 100644
index f1977980b..000000000
--- a/include/core/sensor/sensor_odom_3d.h
+++ /dev/null
@@ -1,149 +0,0 @@
-//--------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--------
-/**
- * \file sensor_odom_3d.h
- *
- * Created on: Oct 7, 2016
- * \author: jsola
- */
-
-#ifndef SRC_SENSOR_ODOM_3d_H_
-#define SRC_SENSOR_ODOM_3d_H_
-
-//wolf includes
-#include "core/sensor/sensor_base.h"
-#include "core/utils/params_server.h"
-
-namespace wolf {
-
-WOLF_STRUCT_PTR_TYPEDEFS(ParamsSensorOdom3d);
-
-struct ParamsSensorOdom3d : public ParamsSensorBase
-{
- double k_disp_to_disp; ///< ratio of displacement variance to displacement, for odometry noise calculation
- double k_disp_to_rot; ///< ratio of displacement variance to rotation, for odometry noise calculation
- double k_rot_to_rot; ///< ratio of rotation variance to rotation, for odometry noise calculation
- double min_disp_var;
- double min_rot_var;
-
- ParamsSensorOdom3d() = default;
- ParamsSensorOdom3d(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_disp_to_rot = _server.getParam<double>(prefix + _unique_name + "/k_disp_to_rot");
- k_rot_to_rot = _server.getParam<double>(prefix + _unique_name + "/k_rot_to_rot");
- min_disp_var = _server.getParam<double>(prefix + _unique_name + "/min_disp_var");
- min_rot_var = _server.getParam<double>(prefix + _unique_name + "/min_rot_var");
- }
- ~ParamsSensorOdom3d() override = default;
- std::string print() const override
- {
- return ParamsSensorBase::print() + "\n"
- + "k_disp_to_disp: " + std::to_string(k_disp_to_disp) + "\n"
- + "k_disp_to_rot: " + std::to_string(k_disp_to_rot) + "\n"
- + "k_rot_to_rot: " + std::to_string(k_rot_to_rot) + "\n"
- + "min_disp_var: " + std::to_string(min_disp_var) + "\n"
- + "min_rot_var: " + std::to_string(min_rot_var) + "\n";
- }
-};
-
-WOLF_PTR_TYPEDEFS(SensorOdom3d);
-
-class SensorOdom3d : public SensorBase
-{
- protected:
- double k_disp_to_disp_; ///< ratio of displacement variance to displacement, for odometry noise calculation
- double k_disp_to_rot_; ///< ratio of displacement variance to rotation, for odometry noise calculation
- double k_rot_to_rot_; ///< ratio of rotation variance to rotation, for odometry noise calculation
- double min_disp_var_;
- double min_rot_var_;
-
- public:
- SensorOdom3d(const std::string& _unique_name,
- const SizeEigen& _dim,
- ParamsSensorOdom2dPtr _params,
- const Priors& _priors);
-
- SensorOdom3d(const std::string& _unique_name,
- const SizeEigen& _dim,
- ParamsSensorOdom3dPtr _params,
- const ParamsServer& _server);
-
- WOLF_SENSOR_CREATE(SensorOdom3d, ParamsSensorOdom3dPtr);
-
- ~SensorOdom3d() override;
-
- double getDispVarToDispNoiseFactor() const;
- double getDispVarToRotNoiseFactor() const;
- double getRotVarToRotNoiseFactor() const;
- double getMinDispVar() const;
- double getMinRotVar() const;
-
- /**
- * Compute covariance of odometry given the motion data.
- *
- * computeCovFromMotion() produces a diagonal covariance that depends on the amount of motion:
- * - a minimal value for displacement
- * - a minimal value for rotation
- * - a linear dependence with total displacement
- * - a linear dependence with total rotation
- *
- * The formula for the variances is as follows:
- *
- * - disp_var = disp_var_min + k_disp_to_disp * displacement
- * - rot_var = rot_var_min + k_disp_to_rot * displacement + k_rot_to_rot * rotation
- *
- * See implementation for details.
- */
- Matrix6d computeNoiseCov (const VectorXd& _data) const override;
-
-};
-
-inline double SensorOdom3d::getDispVarToDispNoiseFactor() const
-{
- return k_disp_to_disp_;
-}
-
-inline double SensorOdom3d::getDispVarToRotNoiseFactor() const
-{
- return k_disp_to_rot_;
-}
-
-inline double SensorOdom3d::getRotVarToRotNoiseFactor() const
-{
- return k_rot_to_rot_;
-}
-
-inline double SensorOdom3d::getMinDispVar() const
-{
- return min_disp_var_;
-}
-
-inline double SensorOdom3d::getMinRotVar() const
-{
- return min_rot_var_;
-}
-
-} /* namespace wolf */
-
-#endif /* SRC_SENSOR_ODOM_3d_H_ */
diff --git a/include/core/sensor/sensor_pose.h b/include/core/sensor/sensor_pose.h
index e653fe458..72f589183 100644
--- a/include/core/sensor/sensor_pose.h
+++ b/include/core/sensor/sensor_pose.h
@@ -41,23 +41,20 @@ struct ParamsSensorPose : public ParamsSensorBase
{
double std_p = 1; // m
double std_o = 1; // rad
- ParamsSensorPose()
- {
- //DEFINED FOR COMPATIBILITY PURPOSES. TO BE REMOVED IN THE FUTURE.
- }
+ ParamsSensorPose() = default;
ParamsSensorPose(std::string _unique_name, const ParamsServer& _server):
ParamsSensorBase(_unique_name, _server)
{
std_p = _server.getParam<double>(prefix + _unique_name + "/std_p");
- std_o = _server.getParam<double>(prefix + _unique_name + "/std_o");
+ std_o = _server.getParam<double>(prefix + _unique_name + "/std_o");
}
+ ~ParamsSensorPose() override = default;
std::string print() const override
{
return ParamsSensorBase::print() + "\n"
+ "std_p: " + std::to_string(std_p) + "\n"
- + "std_o: " + std::to_string(std_o) + "\n";
+ + "std_o: " + std::to_string(std_o) + "\n";
}
- ~ParamsSensorPose() override = default;
};
WOLF_PTR_TYPEDEFS(SensorPose);
@@ -65,36 +62,41 @@ WOLF_PTR_TYPEDEFS(SensorPose);
class SensorPose : public SensorBase
{
protected:
- double std_p_; // standard deviation of translation measurements
- double std_o_; // standard deviation of orientation measurements
+ SizeEigen dim_;
+ ParamsSensorPosePtr params_pose_;
public:
- SensorPose(const Eigen::VectorXd& _extrinsics_pq, const ParamsSensorPose& params);
- SensorPose(const Eigen::VectorXd& _extrinsics_pq, ParamsSensorPosePtr params);
- WOLF_SENSOR_CREATE(SensorPose, ParamsSensorPose, 7);
+ SensorPose(const std::string& _unique_name,
+ const SizeEigen& _dim,
+ ParamsSensorPosePtr _params,
+ const Priors& _priors);
+
+ SensorPose(const std::string& _unique_name,
+ const SizeEigen& _dim,
+ ParamsSensorPosePtr _params,
+ const ParamsServer& _server);
+
+ WOLF_SENSOR_CREATE(SensorPose, ParamsSensorPose);
~SensorPose() override;
double getStdP() const;
double getStdO() const;
+ Eigen::MatrixXd computeNoiseCov(const Eigen::VectorXd & _data) const override;
+
};
inline double SensorPose::getStdP() const
{
- return std_p_;
+ return params_pose_->std_p;
}
inline double SensorPose::getStdO() const
{
- return std_o_;
+ return params_pose_->std_o;
}
-// inline Matrix6d SensorPose::computeDataCov() const
-// {
-// return (Vector6d() << std_p_*std_p_, std_p_*std_p_, std_p_*std_p_, std_o_*std_o_, std_o_*std_o_, std_o_*std_o_).finished().asDiagonal();
-// }
-
} /* namespace wolf */
#endif /* SRC_SENSOR_POSE_H_ */
diff --git a/src/processor/processor_odom_2d.cpp b/src/processor/processor_odom_2d.cpp
index 98f55c0f0..78ecb0fdd 100644
--- a/src/processor/processor_odom_2d.cpp
+++ b/src/processor/processor_odom_2d.cpp
@@ -20,7 +20,7 @@
//
//--------LICENSE_END--------
#include "core/processor/processor_odom_2d.h"
-#include "core/sensor/sensor_odom_2d.h"
+#include "core/sensor/sensor_odom.h"
#include "core/math/covariance.h"
#include "core/state_block/state_composite.h"
#include "core/factor/factor_relative_pose_2d.h"
@@ -29,8 +29,8 @@ namespace wolf
{
ProcessorOdom2d::ProcessorOdom2d(ParamsProcessorOdom2dPtr _params) :
- ProcessorMotion("ProcessorOdom2d", "PO", 2, 3, 3, 3, 2, 0, _params),
- params_odom_2d_(_params)
+ ProcessorMotion("ProcessorOdom2d", "PO", 2, 3, 3, 3, 2, 0, _params),
+ params_odom_2d_(_params)
{
//
}
diff --git a/src/processor/processor_odom_3d.cpp b/src/processor/processor_odom_3d.cpp
index 471dd6273..e96f3a7de 100644
--- a/src/processor/processor_odom_3d.cpp
+++ b/src/processor/processor_odom_3d.cpp
@@ -20,6 +20,7 @@
//
//--------LICENSE_END--------
#include "core/processor/processor_odom_3d.h"
+#include "core/sensor/sensor_odom.h"
#include "core/math/SE3.h"
namespace wolf
@@ -38,9 +39,8 @@ ProcessorOdom3d::~ProcessorOdom3d()
void ProcessorOdom3d::configure(SensorBasePtr _sensor)
{
- assert (_sensor && "Trying to configure processor with a sensor but provided sensor is nullptr.");
-
- SensorOdom3dPtr sen_ptr = std::static_pointer_cast<SensorOdom3d>(_sensor);
+ if (not std::dynamic_pointer_cast<SensorOdom>(_sensor))
+ throw std::runtime_error("Configuring ProcessorOdom3d: provided sensor is null or not of type 'SensorOdom'");
}
void ProcessorOdom3d::computeCurrentDelta(const Eigen::VectorXd& _data,
diff --git a/src/sensor/sensor_base.cpp b/src/sensor/sensor_base.cpp
index cf8191714..3f9b49017 100644
--- a/src/sensor/sensor_base.cpp
+++ b/src/sensor/sensor_base.cpp
@@ -52,6 +52,10 @@ SensorBase::SensorBase(const std::string& _type,
assert(_keys_types_apart_from_PO.count('P') == 0 and
_keys_types_apart_from_PO.count('O') == 0 and
"SensorBase: _keys_types_apart_from_PO should not contain 'P' or 'O' keys");
+
+ // check params valid
+ if (not params_)
+ throw std::runtime_error("SensorBase: in constructor, ParamsSensorBase pointer is null");
// check priors have all _keys_types_apart_from_PO (PO will be checked in loadPriors())
for (auto key_type : _keys_types_apart_from_PO)
@@ -89,6 +93,10 @@ SensorBase::SensorBase(const std::string& _type,
_keys_types_apart_from_PO.count('O') == 0 and
"SensorBase: _keys_types_apart_from_PO should not contain 'P' or 'O' keys");
+ // check params valid
+ if (not params_)
+ throw std::runtime_error("SensorBase: in constructor, ParamsSensorBase pointer is null");
+
// create priors
std::unordered_map<char, std::string> keys_types = _keys_types_apart_from_PO;
keys_types['P'] = "P"; // equivalent to "StateBlock";
diff --git a/src/sensor/sensor_odom_2d.cpp b/src/sensor/sensor_odom_2d.cpp
deleted file mode 100644
index 0abef3899..000000000
--- a/src/sensor/sensor_odom_2d.cpp
+++ /dev/null
@@ -1,90 +0,0 @@
-//--------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/state_block/state_block.h"
-#include "core/state_block/state_angle.h"
-
-namespace wolf {
-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)
-{
- if (_dim != 2)
- throw std::runtime_error("SensorOdom2d only works with 2D problems");
-}
-
-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()
-{
- //
-}
-
-double SensorOdom2d::getDispVarToDispNoiseFactor() const
-{
- return params_odom2d_->k_disp_to_disp;
-}
-
-double SensorOdom2d::getRotVarToRotNoiseFactor() const
-{
- return params_odom2d_->k_rot_to_rot;
-}
-
-Eigen::MatrixXd SensorOdom2d::computeNoiseCov(const Eigen::VectorXd & _data) const
-{
- assert(_data.size() == 2);
- double d = fabs(_data(0));
- double r = fabs(_data(1));
-
- double dvar = params_odom2d_->k_disp_to_disp * d;
- double rvar = params_odom2d_->k_rot_to_rot * r;
-
- return (Vector2d() << dvar, rvar).finished().asDiagonal();
-}
-
-}
-
-// Register in the FactorySensor
-#include "core/sensor/factory_sensor.h"
-namespace wolf {
-WOLF_REGISTER_SENSOR(SensorOdom2d);
-} // namespace wolf
diff --git a/src/sensor/sensor_odom_3d.cpp b/src/sensor/sensor_odom_3d.cpp
deleted file mode 100644
index b44561ff4..000000000
--- a/src/sensor/sensor_odom_3d.cpp
+++ /dev/null
@@ -1,84 +0,0 @@
-//--------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--------
-/**
- * \file sensor_odom_3d.cpp
- *
- * Created on: Oct 7, 2016
- * \author: jsola
- */
-
-#include "core/sensor/sensor_odom_3d.h"
-
-#include "core/state_block/state_block.h"
-#include "core/state_block/state_quaternion.h"
-#include "core/math/rotations.h"
-
-namespace wolf {
-
-SensorOdom3d::SensorOdom3d(const Eigen::VectorXd& _extrinsics_pq, const ParamsSensorOdom3d& _intrinsics) :
- SensorBase("SensorOdom3d", std::make_shared<StateBlock>(_extrinsics_pq.head(3), true), std::make_shared<StateQuaternion>(_extrinsics_pq.tail(4), true), nullptr, 6),
- k_disp_to_disp_(_intrinsics.k_disp_to_disp),
- k_disp_to_rot_(_intrinsics.k_disp_to_rot),
- k_rot_to_rot_(_intrinsics.k_rot_to_rot),
- min_disp_var_(_intrinsics.min_disp_var),
- min_rot_var_(_intrinsics.min_rot_var)
-{
- assert(_extrinsics_pq.size() == 7 && "Bad extrinsics vector size! Should be 7 for 3d.");
-
- noise_cov_ = (Eigen::Vector6d() << min_disp_var_, min_disp_var_, min_disp_var_, min_rot_var_, min_rot_var_, min_rot_var_).finished().asDiagonal();
- setNoiseCov(noise_cov_); // sets also noise_std_
-}
-
-SensorOdom3d::SensorOdom3d(const Eigen::VectorXd& _extrinsics_pq, ParamsSensorOdom3dPtr _intrinsics) :
- SensorOdom3d(_extrinsics_pq, *_intrinsics)
-{
- //
-}
-
-SensorOdom3d::~SensorOdom3d()
-{
- //
-}
-
-Eigen::Matrix6d SensorOdom3d::computeCovFromMotion (const VectorXd& _data) const
-{
- double d = _data.head<3>().norm();
- double r;
- if (_data.size() == 6)
- r = _data.tail<3>().norm();
- else
- r = 2 * acos(_data(6)); // arc cos of real part of quaternion
-
- double dvar = min_disp_var_ + k_disp_to_disp_ * d;
- double rvar = min_rot_var_ + k_disp_to_rot_ * d + k_rot_to_rot_ * r;
-
- return (Vector6d() << dvar, dvar, dvar, rvar, rvar, rvar).finished().asDiagonal();
-}
-
-} // namespace wolf
-
-// Register in the FactorySensor
-#include "core/sensor/factory_sensor.h"
-namespace wolf {
-WOLF_REGISTER_SENSOR(SensorOdom3d);
-WOLF_REGISTER_SENSOR_AUTO(SensorOdom3d);
-}
diff --git a/src/sensor/sensor_pose.cpp b/src/sensor/sensor_pose.cpp
index 87f2880b8..5be1e7473 100644
--- a/src/sensor/sensor_pose.cpp
+++ b/src/sensor/sensor_pose.cpp
@@ -28,32 +28,56 @@
#include "core/sensor/sensor_pose.h"
-#include "core/state_block/state_block.h"
-#include "core/state_block/state_quaternion.h"
-#include "core/math/rotations.h"
-
namespace wolf {
-
-SensorPose::SensorPose(const Eigen::VectorXd& _extrinsics_pq, const ParamsSensorPose& _intrinsics) :
- SensorBase("SensorPose", std::make_shared<StateBlock>(_extrinsics_pq.head(3), true), std::make_shared<StateQuaternion>(_extrinsics_pq.tail(4), true), nullptr, 6),
- std_p_(_intrinsics.std_p),
- std_o_(_intrinsics.std_o)
+
+SensorPose::SensorPose(const std::string& _unique_name,
+ const SizeEigen& _dim,
+ ParamsSensorPosePtr _params,
+ const Priors& _priors) :
+ SensorBase("SensorPose",
+ _unique_name,
+ _dim,
+ _params,
+ _priors),
+ dim_(_dim),
+ params_pose_(_params)
{
- assert(_extrinsics_pq.size() == 7 && "Bad extrinsics vector size! Should be 7 for 3d.");
-
- noise_cov_ = (Eigen::Vector6d() << std_p_*std_p_, std_p_*std_p_, std_p_*std_p_, std_o_*std_o_, std_o_*std_o_, std_o_*std_o_).finished().asDiagonal();
- setNoiseCov(noise_cov_); // sets also noise_std_
+ assert(dim_ == 2 or dim_ == 3);
}
-SensorPose::SensorPose(const Eigen::VectorXd& _extrinsics_pq, ParamsSensorPosePtr _intrinsics) :
- SensorPose(_extrinsics_pq, *_intrinsics)
+SensorPose::SensorPose(const std::string& _unique_name,
+ const SizeEigen& _dim,
+ ParamsSensorPosePtr _params,
+ const ParamsServer& _server) :
+ SensorBase("SensorPose",
+ _unique_name,
+ _dim,
+ _params,
+ _server),
+ dim_(_dim),
+ params_pose_(_params)
{
- //
+ assert(dim_ == 2 or dim_ == 3);
}
SensorPose::~SensorPose()
{
- //
+
+}
+
+Eigen::MatrixXd SensorPose::computeNoiseCov(const Eigen::VectorXd & _data) const
+{
+ if (dim_ == 2)
+ return (Eigen::Vector3d() << params_pose_->std_p,
+ params_pose_->std_p,
+ params_pose_->std_o).finished().cwiseAbs2().asDiagonal();
+ else
+ return (Eigen::Vector6d() << params_pose_->std_p,
+ params_pose_->std_p,
+ params_pose_->std_p,
+ params_pose_->std_o,
+ params_pose_->std_o,
+ params_pose_->std_o).finished().cwiseAbs2().asDiagonal();
}
} // namespace wolf
@@ -62,5 +86,4 @@ SensorPose::~SensorPose()
#include "core/sensor/factory_sensor.h"
namespace wolf {
WOLF_REGISTER_SENSOR(SensorPose);
-WOLF_REGISTER_SENSOR_AUTO(SensorPose);
}
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index c2bb2e530..915df791c 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -264,12 +264,12 @@ target_link_libraries(gtest_map_yaml ${PLUGIN_NAME})
# target_link_libraries(gtest_processor_frame_nearest_neighbor_filter_2d ${PLUGIN_NAME})
# ProcessorMotion in 2d
-# wolf_add_gtest(gtest_odom_2d gtest_odom_2d.cpp)
-# target_link_libraries(gtest_odom_2d ${PLUGIN_NAME})
+wolf_add_gtest(gtest_odom_2d gtest_odom_2d.cpp)
+target_link_libraries(gtest_odom_2d ${PLUGIN_NAME})
# ProcessorOdom3d class test
-# wolf_add_gtest(gtest_processor_odom_3d gtest_processor_odom_3d.cpp)
-# target_link_libraries(gtest_processor_odom_3d ${PLUGIN_NAME})
+wolf_add_gtest(gtest_processor_odom_3d gtest_processor_odom_3d.cpp)
+target_link_libraries(gtest_processor_odom_3d ${PLUGIN_NAME})
# FactorPose3dWithExtrinsics class test
# wolf_add_gtest(gtest_processor_and_factor_pose_3d_with_extrinsics gtest_processor_and_factor_pose_3d_with_extrinsics.cpp)
@@ -288,13 +288,17 @@ 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})
-# SensorOdom2d class test
-wolf_add_gtest(gtest_sensor_odom_2d gtest_sensor_odom_2d.cpp)
-target_link_libraries(gtest_sensor_odom_2d ${PLUGIN_NAME})
+# SensorOdom class test
+wolf_add_gtest(gtest_sensor_odom gtest_sensor_odom.cpp)
+target_link_libraries(gtest_sensor_odom ${PLUGIN_NAME})
+
+# # 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})
+wolf_add_gtest(gtest_sensor_pose gtest_sensor_pose.cpp)
+target_link_libraries(gtest_sensor_pose ${PLUGIN_NAME})
IF (Ceres_FOUND)
# SolverCeres test
diff --git a/test/gtest_odom_2d.cpp b/test/gtest_odom_2d.cpp
index 04d838944..8d5caccb0 100644
--- a/test/gtest_odom_2d.cpp
+++ b/test/gtest_odom_2d.cpp
@@ -29,7 +29,7 @@
#include "core/utils/utils_gtest.h"
// Classes under test
-#include "core/sensor/sensor_odom_2d.h"
+#include "core/sensor/sensor_odom.h"
#include "core/processor/processor_odom_2d.h"
#include "core/capture/capture_odom_2d.h"
#include "core/factor/factor_relative_pose_2d.h"
@@ -218,7 +218,7 @@ TEST(Odom2d, VoteForKfAndSolve)
// Create Wolf tree nodes
ProblemPtr problem = Problem::create("PO", 2);
- SensorBasePtr sensor_odom2d = problem->installSensor("SensorOdom2d", "odom", Vector3d(0,0,0), wolf_root + "/test/yaml/sensor_odom_2d.yaml");
+ SensorBasePtr sensor_odom2d = problem->installSensor("SensorOdom", "odom", wolf_root + "/test/yaml/sensor_odom_2d.yaml");
ParamsProcessorOdom2dPtr params(std::make_shared<ParamsProcessorOdom2d>());
params->voting_active = true;
params->dist_traveled = 100;
@@ -362,7 +362,7 @@ TEST(Odom2d, KF_callback)
// Create Wolf tree nodes
ProblemPtr problem = Problem::create("PO", 2);
- SensorBasePtr sensor_odom2d = problem->installSensor("SensorOdom2d", "odom", Vector3d(0,0,0), wolf_root + "/test/yaml/sensor_odom_2d.yaml");
+ SensorBasePtr sensor_odom2d = problem->installSensor("SensorOdom", "odom", wolf_root + "/test/yaml/sensor_odom_2d.yaml");
ParamsProcessorOdom2dPtr params(std::make_shared<ParamsProcessorOdom2d>());
params->dist_traveled = 100;
params->angle_turned = data(1)*2.5; // force KF at every third process()
diff --git a/test/gtest_sensor_base.cpp b/test/gtest_sensor_base.cpp
index 559cb5883..75412f792 100644
--- a/test/gtest_sensor_base.cpp
+++ b/test/gtest_sensor_base.cpp
@@ -19,12 +19,6 @@
// along with this program. If not, see <http://www.gnu.org/licenses/>.
//
//--------LICENSE_END--------
-/**
- * \file gtest_sensor_base.cpp
- *
- * Created on: Mar 27, 2018
- * \author: jsola
- */
#include "core/sensor/sensor_base.h"
#include "core/utils/utils_gtest.h"
@@ -417,7 +411,7 @@ TEST(SensorBase, makeshared_server_PO)
(wrong ? "_wrong" : "");
// Yaml parser
- ParserYaml parser = ParserYaml("test/yaml/sensor_base/" + name + ".yaml", wolf_root, true);
+ ParserYaml parser = ParserYaml("test/yaml/sensor_tests/" + name + ".yaml", wolf_root, true);
ParamsServer server = ParamsServer(parser.getParams(), "/sensor/sensor_1");
WOLF_INFO("Creating sensor from ", name, ".yaml");
@@ -459,7 +453,7 @@ TEST(SensorBase, makeshared_server_PO)
// POIA - 3D - CORRECT YAML
{
// Yaml parser
- ParserYaml parser = ParserYaml("test/yaml/sensor_base/sensor_POIA_3D.yaml", wolf_root, true);
+ ParserYaml parser = ParserYaml("test/yaml/sensor_tests/sensor_POIA_3D.yaml", wolf_root, true);
ParamsServer server = ParamsServer(parser.getParams(), "/sensor/sensor_1");
server.print();
@@ -485,7 +479,7 @@ TEST(SensorBase, makeshared_server_PO)
// POIA - 3D - INCORRECT YAML
{
// Yaml parser
- ParserYaml parser = ParserYaml("test/yaml/sensor_base/sensor_POIA_3D_wrong.yaml", wolf_root, true);
+ ParserYaml parser = ParserYaml("test/yaml/sensor_tests/sensor_POIA_3D_wrong.yaml", wolf_root, true);
ParamsServer server = ParamsServer(parser.getParams(), "/sensor/sensor_1");
WOLF_INFO("Creating sensor from sensor_POIA_3D_wrong.yaml");
@@ -532,7 +526,7 @@ TEST(SensorBase, factory)
(drift ? "_drift" : "") +
(wrong ? "_wrong" : "");
// Yaml parser
- ParserYaml parser = ParserYaml("test/yaml/sensor_base/" + name + ".yaml", wolf_root, true);
+ ParserYaml parser = ParserYaml("test/yaml/sensor_tests/" + name + ".yaml", wolf_root, true);
ParamsServer server = ParamsServer(parser.getParams(), "/sensor/sensor_1");
WOLF_INFO("Creating sensor from ", name, ".yaml");
@@ -573,7 +567,7 @@ TEST(SensorBase, factory)
WOLF_INFO("Creating sensor from name sensor_POIA_3D.yaml");
// Yaml parser
- ParserYaml parser = ParserYaml("test/yaml/sensor_base/sensor_POIA_3D.yaml", wolf_root, true);
+ ParserYaml parser = ParserYaml("test/yaml/sensor_tests/sensor_POIA_3D.yaml", wolf_root, true);
ParamsServer server = ParamsServer(parser.getParams(), "/sensor/sensor_1");
server.print();
@@ -599,7 +593,7 @@ TEST(SensorBase, factory)
WOLF_INFO("Creating sensor from name sensor_POIA_3D_wrong.yaml");
// Yaml parser
- ParserYaml parser = ParserYaml("test/yaml/sensor_base/sensor_POIA_3D_wrong.yaml", wolf_root, true);
+ ParserYaml parser = ParserYaml("test/yaml/sensor_tests/sensor_POIA_3D_wrong.yaml", wolf_root, true);
ParamsServer server = ParamsServer(parser.getParams(), "/sensor/sensor_1");
// create sensor
@@ -635,7 +629,7 @@ TEST(SensorBase, factory_yaml)
continue;
std::string yaml_filepath = wolf_root +
- "/test/yaml/sensor_base/" +
+ "/test/yaml/sensor_tests/" +
"sensor_PO_" +
to_string(dim) +
"D_" +
@@ -686,7 +680,7 @@ TEST(SensorBase, factory_yaml)
auto S = FactorySensorYaml::create("SensorDummyPoia",
"sensor_1",
3,
- wolf_root + "/test/yaml/sensor_base/sensor_POIA_3D.yaml");
+ wolf_root + "/test/yaml/sensor_tests/sensor_POIA_3D.yaml");
// noise
ASSERT_MATRIX_APPROX(S->computeNoiseCov(vector0),
@@ -710,7 +704,7 @@ TEST(SensorBase, factory_yaml)
ASSERT_THROW(FactorySensorYaml::create("SensorDummyPoia",
"sensor_1",
3,
- wolf_root + "/test/yaml/sensor_base/sensor_POIA_3D_wrong.yaml"),
+ wolf_root + "/test/yaml/sensor_tests/sensor_POIA_3D_wrong.yaml"),
std::runtime_error);
}
}
diff --git a/test/gtest_sensor_odom.cpp b/test/gtest_sensor_odom.cpp
new file mode 100644
index 000000000..9416dceb3
--- /dev/null
+++ b/test/gtest_sensor_odom.cpp
@@ -0,0 +1,803 @@
+//--------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.h"
+#include "core/sensor/factory_sensor.h"
+#include "core/utils/utils_gtest.h"
+#include "core/yaml/parser_yaml.h"
+#include "core/utils/params_server.h"
+
+using namespace wolf;
+using namespace Eigen;
+
+std::string wolf_root = _WOLF_ROOT_DIR;
+
+VectorXd vector0 = VectorXd(0);
+VectorXd p_state_2D = VectorXd::Random(2);
+VectorXd p_state_3D = VectorXd::Random(3);
+VectorXd o_state_2D = VectorXd::Random(1);
+VectorXd o_state_3D = VectorXd::Random(4).normalized();
+VectorXd p_std_2D = VectorXd::Random(2).cwiseAbs();
+VectorXd p_std_3D = VectorXd::Random(3).cwiseAbs();
+VectorXd o_std_2D = VectorXd::Random(1).cwiseAbs();
+VectorXd o_std_3D = VectorXd::Random(3).cwiseAbs();
+MatrixXd p_cov_2D = p_std_2D.cwiseAbs2().asDiagonal();
+MatrixXd p_cov_3D = p_std_3D.cwiseAbs2().asDiagonal();
+MatrixXd o_cov_2D = o_std_2D.cwiseAbs2().asDiagonal();
+MatrixXd o_cov_3D = o_std_3D.cwiseAbs2().asDiagonal();
+double k_disp_to_disp = 0.1;
+double k_disp_to_rot = 0.2;
+double k_rot_to_rot = 0.3;
+double min_disp_var = 0.01;
+double min_rot_var = 0.02;
+double noise_p_std = 0.1;
+double noise_o_std = 0.01;
+MatrixXd noise_cov_dummy = Vector2d(noise_p_std, noise_o_std).cwiseAbs2().asDiagonal();
+
+void checkSensor(SensorBasePtr S,
+ char _key,
+ const VectorXd& _state,
+ bool _fixed,
+ const VectorXd& _noise_std,
+ bool _dynamic,
+ const VectorXd& _drift_std)
+{
+ MatrixXd noise_cov = _noise_std.cwiseAbs2().asDiagonal();
+ MatrixXd drift_cov = _drift_std.cwiseAbs2().asDiagonal();
+
+ // state
+ ASSERT_MATRIX_APPROX(_state, S->getStateBlockDynamic(_key)->getState(), Constants::EPS);
+ // fixed
+ ASSERT_EQ(S->getStateBlockDynamic(_key)->isFixed(), _fixed);
+ // dynamic
+ ASSERT_EQ(S->isStateBlockDynamic(_key), _dynamic);
+ // drift
+ ASSERT_EQ(_drift_std.size(), S->getDriftStd(_key).size());
+ ASSERT_EQ(drift_cov.size(), S->getDriftCov(_key).size());
+ if (_drift_std.size() != 0)
+ {
+ ASSERT_MATRIX_APPROX(_drift_std, S->getDriftStd(_key), Constants::EPS);
+ ASSERT_MATRIX_APPROX(drift_cov, S->getDriftCov(_key), Constants::EPS);
+ }
+ // factor
+ if (_noise_std.size() != 0)
+ {
+ ASSERT_TRUE(S->getPriorFeature(_key) != nullptr);
+ ASSERT_EQ(_state.size(), S->getPriorFeature(_key)->getMeasurement().size());
+ ASSERT_MATRIX_APPROX(_state,
+ S->getPriorFeature(_key)->getMeasurement(),
+ Constants::EPS);
+ ASSERT_EQ(noise_cov.size(), S->getPriorFeature(_key)->getMeasurementCovariance().size());
+ ASSERT_MATRIX_APPROX(noise_cov,
+ S->getPriorFeature(_key)->getMeasurementCovariance(),
+ Constants::EPS);
+ }
+ else
+ {
+ ASSERT_TRUE(S->getPriorFeature(_key) == nullptr);
+ }
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////
+////////////////////////////////// CONSTRUCTOR WITH PRIORS AND PARAMS ////////////////////////////////
+//////////////////////////////////////////////////////////////////////////////////////////////////////
+// 2D Fix
+TEST(SensorOdom, makeshared_priors_fix_2D)
+{
+ auto params = std::make_shared<ParamsSensorOdom>();
+ params->k_disp_to_disp = k_disp_to_disp;
+ params->k_disp_to_rot = k_disp_to_rot;
+ params->k_rot_to_rot = k_rot_to_rot;
+ params->min_disp_var = min_disp_var;
+ params->min_rot_var = min_rot_var;
+
+ auto S = std::make_shared<SensorOdom>("sensor1", 2, params,
+ Priors({{'P',Prior("P", p_state_2D)}, //default "fix", not dynamic
+ {'O',Prior("O", o_state_2D)}}));
+
+ // checks
+ checkSensor(S, 'P', p_state_2D, true, vector0, false, vector0);
+ checkSensor(S, 'O', o_state_2D, true, vector0, false, vector0);
+}
+
+// 3D Fix
+TEST(SensorOdom, makeshared_priors_fix_3D)
+{
+ auto params = std::make_shared<ParamsSensorOdom>();
+ params->k_disp_to_disp = k_disp_to_disp;
+ params->k_disp_to_rot = k_disp_to_rot;
+ params->k_rot_to_rot = k_rot_to_rot;
+ params->min_disp_var = min_disp_var;
+ params->min_rot_var = min_rot_var;
+
+ auto S = std::make_shared<SensorOdom>("sensor1", 3, params,
+ Priors({{'P',Prior("P", p_state_3D)}, //default "fix", not dynamic
+ {'O',Prior("O", o_state_3D)}}));
+
+ // factors
+ ASSERT_EQ(S->getPriorFeatures().size(), 0);
+
+ // check
+ checkSensor(S, 'P', p_state_3D, true, vector0, false, vector0);
+ checkSensor(S, 'O', o_state_3D, true, vector0, false, vector0);
+}
+
+// 2D Initial guess
+TEST(SensorOdom, makeshared_priors_initial_guess_2D)
+{
+ auto params = std::make_shared<ParamsSensorOdom>();
+ params->k_disp_to_disp = k_disp_to_disp;
+ params->k_disp_to_rot = k_disp_to_rot;
+ params->k_rot_to_rot = k_rot_to_rot;
+ params->min_disp_var = min_disp_var;
+ params->min_rot_var = min_rot_var;
+
+ auto S = std::make_shared<SensorOdom>("sensor1", 2, params,
+ Priors({{'P',Prior("P", p_state_2D, "initial_guess")},
+ {'O',Prior("O", o_state_2D, "initial_guess")}}));
+
+ // factors
+ ASSERT_EQ(S->getPriorFeatures().size(), 0);
+
+ // check
+ checkSensor(S, 'P', p_state_2D, false, vector0, false, vector0);
+ checkSensor(S, 'O', o_state_2D, false, vector0, false, vector0);
+}
+
+// 3D Initial guess
+TEST(SensorOdom, makeshared_priors_initial_guess_3D)
+{
+ auto params = std::make_shared<ParamsSensorOdom>();
+ params->k_disp_to_disp = k_disp_to_disp;
+ params->k_disp_to_rot = k_disp_to_rot;
+ params->k_rot_to_rot = k_rot_to_rot;
+ params->min_disp_var = min_disp_var;
+ params->min_rot_var = min_rot_var;
+
+ auto S = std::make_shared<SensorOdom>("sensor1", 3, params,
+ Priors({{'P',Prior("P", p_state_3D, "initial_guess")},
+ {'O',Prior("O", o_state_3D, "initial_guess")}}));
+
+ // factors
+ ASSERT_EQ(S->getPriorFeatures().size(), 0);
+
+ // check
+ checkSensor(S, 'P', p_state_3D, false, vector0, false, vector0);
+ checkSensor(S, 'O', o_state_3D, false, vector0, false, vector0);
+}
+
+// 2D Factor
+TEST(SensorOdom, makeshared_priors_factor_2D)
+{
+ auto params = std::make_shared<ParamsSensorOdom>();
+ params->k_disp_to_disp = k_disp_to_disp;
+ params->k_disp_to_rot = k_disp_to_rot;
+ params->k_rot_to_rot = k_rot_to_rot;
+ params->min_disp_var = min_disp_var;
+ params->min_rot_var = min_rot_var;
+
+ auto S = std::make_shared<SensorOdom>("sensor1", 2, params,
+ Priors({{'P',Prior("P", p_state_2D, "factor", p_std_2D)},
+ {'O',Prior("O", o_state_2D, "factor", o_std_2D)}}));
+
+ // factors
+ ASSERT_EQ(S->getPriorFeatures().size(), 2);
+
+ // check
+ 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
+TEST(SensorOdom, makeshared_priors_factor_3D)
+{
+ auto params = std::make_shared<ParamsSensorOdom>();
+ params->k_disp_to_disp = k_disp_to_disp;
+ params->k_disp_to_rot = k_disp_to_rot;
+ params->k_rot_to_rot = k_rot_to_rot;
+ params->min_disp_var = min_disp_var;
+ params->min_rot_var = min_rot_var;
+
+ auto S = std::make_shared<SensorOdom>("sensor1", 3, params,
+ Priors({{'P',Prior("P", p_state_3D, "factor", p_std_3D)},
+ {'O',Prior("O", o_state_3D, "factor", o_std_3D)}}));
+
+ // factors
+ ASSERT_EQ(S->getPriorFeatures().size(), 2);
+
+ // check
+ 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
+TEST(SensorOdom, makeshared_priors_initial_guess_dynamic_2D)
+{
+ auto params = std::make_shared<ParamsSensorOdom>();
+ params->k_disp_to_disp = k_disp_to_disp;
+ params->k_disp_to_rot = k_disp_to_rot;
+ params->k_rot_to_rot = k_rot_to_rot;
+ params->min_disp_var = min_disp_var;
+ params->min_rot_var = min_rot_var;
+
+ auto S = std::make_shared<SensorOdom>("sensor1", 2, params,
+ Priors({{'P',Prior("P", p_state_2D, "initial_guess", vector0, true)},
+ {'O',Prior("O", o_state_2D, "initial_guess", vector0, true)}}));
+
+ // factors
+ ASSERT_EQ(S->getPriorFeatures().size(), 0);
+
+ // check
+ checkSensor(S, 'P', p_state_2D, false, vector0, true, vector0);
+ checkSensor(S, 'O', o_state_2D, false, vector0, true, vector0);
+}
+
+// 3D Initial guess dynamic
+TEST(SensorOdom, makeshared_priors_initial_guess_dynamic_3D)
+{
+ auto params = std::make_shared<ParamsSensorOdom>();
+ params->k_disp_to_disp = k_disp_to_disp;
+ params->k_disp_to_rot = k_disp_to_rot;
+ params->k_rot_to_rot = k_rot_to_rot;
+ params->min_disp_var = min_disp_var;
+ params->min_rot_var = min_rot_var;
+
+ auto S = std::make_shared<SensorOdom>("sensor1", 3, params,
+ Priors({{'P',Prior("P", p_state_3D, "initial_guess", vector0, true)},
+ {'O',Prior("O", o_state_3D, "initial_guess", vector0, true)}}));
+
+ // factors
+ ASSERT_EQ(S->getPriorFeatures().size(), 0);
+
+ // check
+ 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
+TEST(SensorOdom, makeshared_priors_initial_guess_dynamic_drift_2D)
+{
+ auto params = std::make_shared<ParamsSensorOdom>();
+ params->k_disp_to_disp = k_disp_to_disp;
+ params->k_disp_to_rot = k_disp_to_rot;
+ params->k_rot_to_rot = k_rot_to_rot;
+ params->min_disp_var = min_disp_var;
+ params->min_rot_var = min_rot_var;
+
+ auto S = std::make_shared<SensorOdom>("sensor1", 2, params,
+ Priors({{'P',Prior("P", p_state_2D, "initial_guess", vector0, true, p_std_2D)},
+ {'O',Prior("O", o_state_2D, "initial_guess", vector0, true, o_std_2D)}}));
+
+ // factors
+ ASSERT_EQ(S->getPriorFeatures().size(), 0);
+
+ // check
+ 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
+TEST(SensorOdom, makeshared_priors_initial_guess_dynamic_drift_3D)
+{
+ auto params = std::make_shared<ParamsSensorOdom>();
+ params->k_disp_to_disp = k_disp_to_disp;
+ params->k_disp_to_rot = k_disp_to_rot;
+ params->k_rot_to_rot = k_rot_to_rot;
+ params->min_disp_var = min_disp_var;
+ params->min_rot_var = min_rot_var;
+
+ auto S = std::make_shared<SensorOdom>("sensor1", 3, params,
+ Priors({{'P',Prior("P", p_state_3D, "initial_guess", vector0, true, p_std_3D)},
+ {'O',Prior("O", o_state_3D, "initial_guess", vector0, true, o_std_3D)}}));
+
+ // factors
+ ASSERT_EQ(S->getPriorFeatures().size(), 0);
+
+ // check
+ checkSensor(S, 'P', p_state_3D, false, vector0, true, p_std_3D);
+ checkSensor(S, 'O', o_state_3D, false, vector0, true, o_std_3D);
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////
+///////////////////////////// CONSTRUCTOR WITH PARAM SERVER and KEY_TYPES ////////////////////////////
+//////////////////////////////////////////////////////////////////////////////////////////////////////
+TEST(SensorOdom, makeshared_server)
+{
+ std::vector<int> dims({2, 3});
+ std::vector<std::string> modes({"fix", "initial_guess", "factor"});
+ std::vector<bool> dynamics({false, true});
+ std::vector<bool> drifts({false, true});
+ std::vector<bool> wrongs({false, true});
+
+ VectorXd p_state(4), o_state(4), po_std(4);
+ p_state << 1, 2, 3, 4;
+ o_state << 1, 0, 0, 0;
+ po_std << 0.1, 0.2, 0.3, 0.4;
+
+ // P & O
+ for (auto dim : dims)
+ for (auto mode : modes)
+ for (auto dynamic : dynamics)
+ for (auto drift : drifts)
+ for (auto wrong : wrongs)
+ {
+ // nonsense combination
+ if (not dynamic and drift)
+ continue;
+
+ std::string name = "sensor_PO_" +
+ to_string(dim) +
+ "D_" +
+ mode +
+ (dynamic ? "_dynamic" : "") +
+ (drift ? "_drift" : "") +
+ (wrong ? "_wrong" : "");
+
+ // Yaml parser
+ ParserYaml parser = ParserYaml("test/yaml/sensor_tests/" + name + ".yaml", wolf_root, true);
+ ParamsServer server = ParamsServer(parser.getParams(), "/sensor/sensor_1");
+
+ WOLF_INFO("Creating sensor from ", name, ".yaml");
+
+ // CORRECT YAML
+ if (not wrong)
+ {
+ auto params = std::make_shared<ParamsSensorOdom>("sensor_1", server);
+ auto S = std::make_shared<SensorOdom>("sensor_1", dim, params, server);
+
+ auto p_size = dim;
+ auto o_size = dim == 2 ? 1 : 4;
+ auto p_size_std = mode == "factor" ? dim : 0;
+ auto o_size_std = mode == "factor" ? (dim == 2 ? 1 : 3) : 0;
+ auto p_size_std_drift = drift ? dim : 0;
+ auto o_size_std_drift = drift ? (dim == 2 ? 1 : 3) : 0;
+
+ // factors
+ ASSERT_EQ(S->getPriorFeatures().size(), mode == "factor" ? 2 : 0);
+
+ // check
+ 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
+ else
+ {
+ ASSERT_THROW(std::make_shared<SensorOdom>("sensor_1", dim,
+ std::make_shared<ParamsSensorOdom>("sensor_1", server),
+ server),std::runtime_error);
+ }
+ }
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////
+//////////////////////////////////////////// FactorySensor ///////////////////////////////////////////
+//////////////////////////////////////////////////////////////////////////////////////////////////////
+TEST(SensorOdom, factory)
+{
+ std::vector<int> dims({2, 3});
+ std::vector<std::string> modes({"fix", "initial_guess", "factor"});
+ std::vector<bool> dynamics({false, true});
+ std::vector<bool> drifts({false, true});
+ std::vector<bool> wrongs({false, true});
+
+ VectorXd p_state(4), o_state(4), po_std(4);
+ p_state << 1, 2, 3, 4;
+ o_state << 1, 0, 0, 0;
+ po_std << 0.1, 0.2, 0.3, 0.4;
+
+ // P & O
+ for (auto dim : dims)
+ for (auto mode : modes)
+ for (auto dynamic : dynamics)
+ for (auto drift : drifts)
+ for (auto wrong : wrongs)
+ {
+ // nonsense combination
+ if (not dynamic and drift)
+ continue;
+
+ std::string name = "sensor_PO_" +
+ to_string(dim) +
+ "D_" +
+ mode +
+ (dynamic ? "_dynamic" : "") +
+ (drift ? "_drift" : "") +
+ (wrong ? "_wrong" : "");
+ // Yaml parser
+ ParserYaml parser = ParserYaml("test/yaml/sensor_tests/" + name + ".yaml", wolf_root, true);
+ ParamsServer server = ParamsServer(parser.getParams(), "/sensor/sensor_1");
+
+ WOLF_INFO("Creating sensor from ", name, ".yaml");
+
+ // CORRECT YAML
+ if (not wrong)
+ {
+ auto S = FactorySensor::create("SensorOdom", "sensor_1", dim, server);
+
+ auto p_size = dim;
+ auto o_size = dim == 2 ? 1 : 4;
+ auto p_size_std = mode == "factor" ? dim : 0;
+ auto o_size_std = mode == "factor" ? (dim == 2 ? 1 : 3) : 0;
+ auto p_size_std_drift = drift ? dim : 0;
+ auto o_size_std_drift = drift ? (dim == 2 ? 1 : 3) : 0;
+
+ // factors
+ ASSERT_EQ(S->getPriorFeatures().size(), mode == "factor" ? 2 : 0);
+
+ // check
+ 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
+ else
+ {
+ ASSERT_THROW(FactorySensor::create("SensorOdom", "sensor_1", dim, server),std::runtime_error);
+ }
+ }
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////
+////////////////////////////////////////// FactorySensorYaml /////////////////////////////////////////
+//////////////////////////////////////////////////////////////////////////////////////////////////////
+TEST(SensorOdom, factory_yaml)
+{
+ std::vector<int> dims({2, 3});
+ std::vector<std::string> modes({"fix", "initial_guess", "factor"});
+ std::vector<bool> dynamics({false, true});
+ std::vector<bool> drifts({false, true});
+ std::vector<bool> wrongs({false, true});
+
+ VectorXd p_state(4), o_state(4), po_std(4);
+ p_state << 1, 2, 3, 4;
+ o_state << 1, 0, 0, 0;
+ po_std << 0.1, 0.2, 0.3, 0.4;
+
+ // P & O
+ for (auto dim : dims)
+ for (auto mode : modes)
+ for (auto dynamic : dynamics)
+ for (auto drift : drifts)
+ for (auto wrong : wrongs)
+ {
+ // nonsense combination
+ if (not dynamic and drift)
+ continue;
+
+ std::string yaml_filepath = wolf_root +
+ "/test/yaml/sensor_tests/" +
+ "sensor_PO_" +
+ to_string(dim) +
+ "D_" +
+ mode +
+ (dynamic ? "_dynamic" : "") +
+ (drift ? "_drift" : "") +
+ (wrong ? "_wrong" : "") +
+ ".yaml";
+
+ WOLF_INFO("Creating sensor from ", yaml_filepath);
+
+ // CORRECT YAML
+ if (not wrong)
+ {
+ auto S = FactorySensorYaml::create("SensorOdom", "sensor_1", dim, yaml_filepath);
+
+ auto p_size = dim;
+ auto o_size = dim == 2 ? 1 : 4;
+ auto p_size_std = mode == "factor" ? dim : 0;
+ auto o_size_std = mode == "factor" ? (dim == 2 ? 1 : 3) : 0;
+ auto p_size_std_drift = drift ? dim : 0;
+ auto o_size_std_drift = drift ? (dim == 2 ? 1 : 3) : 0;
+
+ // factors
+ ASSERT_EQ(S->getPriorFeatures().size(), mode == "factor" ? 2 : 0);
+
+ // check
+ 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
+ else
+ {
+ ASSERT_THROW(FactorySensorYaml::create("SensorOdom", "sensor_1", dim, yaml_filepath),std::runtime_error);
+ }
+ }
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////
+///////////////////////////////////////// FactorySensorPriors ////////////////////////////////////////
+//////////////////////////////////////////////////////////////////////////////////////////////////////
+
+// 2D Fix
+TEST(SensorOdom, factory_priors_fix_2D)
+{
+ auto params = std::make_shared<ParamsSensorOdom>();
+ params->k_disp_to_disp = k_disp_to_disp;
+ params->k_disp_to_rot = k_disp_to_rot;
+ params->k_rot_to_rot = k_rot_to_rot;
+ params->min_disp_var = min_disp_var;
+ params->min_rot_var = min_rot_var;
+
+ auto S = FactorySensorPriors::create("SensorOdom","sensor1", 2, params,
+ Priors({{'P',Prior("P", p_state_2D)}, //default "fix", not dynamic
+ {'O',Prior("O", o_state_2D)}}));
+
+ // checks
+ checkSensor(S, 'P', p_state_2D, true, vector0, false, vector0);
+ checkSensor(S, 'O', o_state_2D, true, vector0, false, vector0);
+}
+
+// 3D Fix
+TEST(SensorOdom, factory_priors_fix_3D)
+{
+ auto params = std::make_shared<ParamsSensorOdom>();
+ params->k_disp_to_disp = k_disp_to_disp;
+ params->k_disp_to_rot = k_disp_to_rot;
+ params->k_rot_to_rot = k_rot_to_rot;
+ params->min_disp_var = min_disp_var;
+ params->min_rot_var = min_rot_var;
+
+ auto S = FactorySensorPriors::create("SensorOdom","sensor1", 3, params,
+ Priors({{'P',Prior("P", p_state_3D)}, //default "fix", not dynamic
+ {'O',Prior("O", o_state_3D)}}));
+
+ // factors
+ ASSERT_EQ(S->getPriorFeatures().size(), 0);
+
+ // check
+ checkSensor(S, 'P', p_state_3D, true, vector0, false, vector0);
+ checkSensor(S, 'O', o_state_3D, true, vector0, false, vector0);
+}
+
+// 2D Initial guess
+TEST(SensorOdom, factory_priors_initial_guess_2D)
+{
+ auto params = std::make_shared<ParamsSensorOdom>();
+ params->k_disp_to_disp = k_disp_to_disp;
+ params->k_disp_to_rot = k_disp_to_rot;
+ params->k_rot_to_rot = k_rot_to_rot;
+ params->min_disp_var = min_disp_var;
+ params->min_rot_var = min_rot_var;
+
+ auto S = FactorySensorPriors::create("SensorOdom","sensor1", 2, params,
+ Priors({{'P',Prior("P", p_state_2D, "initial_guess")},
+ {'O',Prior("O", o_state_2D, "initial_guess")}}));
+
+ // factors
+ ASSERT_EQ(S->getPriorFeatures().size(), 0);
+
+ // check
+ checkSensor(S, 'P', p_state_2D, false, vector0, false, vector0);
+ checkSensor(S, 'O', o_state_2D, false, vector0, false, vector0);
+}
+
+// 3D Initial guess
+TEST(SensorOdom, factory_priors_initial_guess_3D)
+{
+ auto params = std::make_shared<ParamsSensorOdom>();
+ params->k_disp_to_disp = k_disp_to_disp;
+ params->k_disp_to_rot = k_disp_to_rot;
+ params->k_rot_to_rot = k_rot_to_rot;
+ params->min_disp_var = min_disp_var;
+ params->min_rot_var = min_rot_var;
+
+ auto S = FactorySensorPriors::create("SensorOdom","sensor1", 3, params,
+ Priors({{'P',Prior("P", p_state_3D, "initial_guess")},
+ {'O',Prior("O", o_state_3D, "initial_guess")}}));
+
+ // factors
+ ASSERT_EQ(S->getPriorFeatures().size(), 0);
+
+ // check
+ checkSensor(S, 'P', p_state_3D, false, vector0, false, vector0);
+ checkSensor(S, 'O', o_state_3D, false, vector0, false, vector0);
+}
+
+// 2D Factor
+TEST(SensorOdom, factory_priors_factor_2D)
+{
+ auto params = std::make_shared<ParamsSensorOdom>();
+ params->k_disp_to_disp = k_disp_to_disp;
+ params->k_disp_to_rot = k_disp_to_rot;
+ params->k_rot_to_rot = k_rot_to_rot;
+ params->min_disp_var = min_disp_var;
+ params->min_rot_var = min_rot_var;
+
+ auto S = FactorySensorPriors::create("SensorOdom","sensor1", 2, params,
+ Priors({{'P',Prior("P", p_state_2D, "factor", p_std_2D)},
+ {'O',Prior("O", o_state_2D, "factor", o_std_2D)}}));
+
+ // factors
+ ASSERT_EQ(S->getPriorFeatures().size(), 2);
+
+ // check
+ 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
+TEST(SensorOdom, factory_priors_factor_3D)
+{
+ auto params = std::make_shared<ParamsSensorOdom>();
+ params->k_disp_to_disp = k_disp_to_disp;
+ params->k_disp_to_rot = k_disp_to_rot;
+ params->k_rot_to_rot = k_rot_to_rot;
+ params->min_disp_var = min_disp_var;
+ params->min_rot_var = min_rot_var;
+
+ auto S = FactorySensorPriors::create("SensorOdom","sensor1", 3, params,
+ Priors({{'P',Prior("P", p_state_3D, "factor", p_std_3D)},
+ {'O',Prior("O", o_state_3D, "factor", o_std_3D)}}));
+
+ // factors
+ ASSERT_EQ(S->getPriorFeatures().size(), 2);
+
+ // check
+ 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
+TEST(SensorOdom, factory_priors_initial_guess_dynamic_2D)
+{
+ auto params = std::make_shared<ParamsSensorOdom>();
+ params->k_disp_to_disp = k_disp_to_disp;
+ params->k_disp_to_rot = k_disp_to_rot;
+ params->k_rot_to_rot = k_rot_to_rot;
+ params->min_disp_var = min_disp_var;
+ params->min_rot_var = min_rot_var;
+
+ auto S = FactorySensorPriors::create("SensorOdom","sensor1", 2, params,
+ Priors({{'P',Prior("P", p_state_2D, "initial_guess", vector0, true)},
+ {'O',Prior("O", o_state_2D, "initial_guess", vector0, true)}}));
+
+ // factors
+ ASSERT_EQ(S->getPriorFeatures().size(), 0);
+
+ // check
+ checkSensor(S, 'P', p_state_2D, false, vector0, true, vector0);
+ checkSensor(S, 'O', o_state_2D, false, vector0, true, vector0);
+}
+
+// 3D Initial guess dynamic
+TEST(SensorOdom, factory_priors_initial_guess_dynamic_3D)
+{
+ auto params = std::make_shared<ParamsSensorOdom>();
+ params->k_disp_to_disp = k_disp_to_disp;
+ params->k_disp_to_rot = k_disp_to_rot;
+ params->k_rot_to_rot = k_rot_to_rot;
+ params->min_disp_var = min_disp_var;
+ params->min_rot_var = min_rot_var;
+
+ auto S = FactorySensorPriors::create("SensorOdom","sensor1", 3, params,
+ Priors({{'P',Prior("P", p_state_3D, "initial_guess", vector0, true)},
+ {'O',Prior("O", o_state_3D, "initial_guess", vector0, true)}}));
+
+ // factors
+ ASSERT_EQ(S->getPriorFeatures().size(), 0);
+
+ // check
+ 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
+TEST(SensorOdom, factory_priors_initial_guess_dynamic_drift_2D)
+{
+ auto params = std::make_shared<ParamsSensorOdom>();
+ params->k_disp_to_disp = k_disp_to_disp;
+ params->k_disp_to_rot = k_disp_to_rot;
+ params->k_rot_to_rot = k_rot_to_rot;
+ params->min_disp_var = min_disp_var;
+ params->min_rot_var = min_rot_var;
+
+ auto S = FactorySensorPriors::create("SensorOdom","sensor1", 2, params,
+ Priors({{'P',Prior("P", p_state_2D, "initial_guess", vector0, true, p_std_2D)},
+ {'O',Prior("O", o_state_2D, "initial_guess", vector0, true, o_std_2D)}}));
+
+ // factors
+ ASSERT_EQ(S->getPriorFeatures().size(), 0);
+
+ // check
+ 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
+TEST(SensorOdom, factory_priors_initial_guess_dynamic_drift_3D)
+{
+ auto params = std::make_shared<ParamsSensorOdom>();
+ params->k_disp_to_disp = k_disp_to_disp;
+ params->k_disp_to_rot = k_disp_to_rot;
+ params->k_rot_to_rot = k_rot_to_rot;
+ params->min_disp_var = min_disp_var;
+ params->min_rot_var = min_rot_var;
+
+ auto S = FactorySensorPriors::create("SensorOdom","sensor1", 3, params,
+ Priors({{'P',Prior("P", p_state_3D, "initial_guess", vector0, true, p_std_3D)},
+ {'O',Prior("O", o_state_3D, "initial_guess", vector0, true, o_std_3D)}}));
+
+ // factors
+ ASSERT_EQ(S->getPriorFeatures().size(), 0);
+
+ // check
+ checkSensor(S, 'P', p_state_3D, false, vector0, true, p_std_3D);
+ checkSensor(S, 'O', o_state_3D, false, vector0, true, o_std_3D);
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////
+////////////////////////////////////////// COMPUTE NOISE COV /////////////////////////////////////////
+//////////////////////////////////////////////////////////////////////////////////////////////////////
+TEST(SensorOdom, compute_noise_cov_3D)
+{
+ auto params = std::make_shared<ParamsSensorOdom>();
+ params->k_disp_to_disp = k_disp_to_disp;
+ params->k_disp_to_rot = k_disp_to_rot;
+ params->k_rot_to_rot = k_rot_to_rot;
+ params->min_disp_var = min_disp_var;
+ params->min_rot_var = min_rot_var;
+
+ auto S = FactorySensorPriors::create("SensorOdom","sensor1", 3, params,
+ Priors({{'P',Prior("P", p_state_3D)}, //default "fix", not dynamic
+ {'O',Prior("O", o_state_3D)}}));
+
+ Vector6d disp_elements, rot_elements;
+ disp_elements << 1, 1, 1, 0, 0, 0;
+ rot_elements << 0, 0, 0, 1, 1, 1;
+ Vector6d min_var = disp_elements * min_disp_var + rot_elements * min_rot_var;
+
+ // compute with zero movement (size 6)
+ ASSERT_MATRIX_APPROX(S->computeNoiseCov(Vector6d::Zero()),
+ Matrix6d(min_var.asDiagonal()),
+ Constants::EPS);
+
+ // compute with zero movement (size 7)
+ ASSERT_MATRIX_APPROX(S->computeNoiseCov((Vector7d() << 0, 0, 0, 0, 0, 0, 1).finished()),
+ Matrix6d(min_var.asDiagonal()),
+ Constants::EPS);
+
+ // compute with displacement (3m) but not rotation (size 6)
+ ASSERT_MATRIX_APPROX(S->computeNoiseCov((Vector6d() << 3, 0, 0, 0, 0, 0).finished()),
+ Matrix6d((min_var + disp_elements*k_disp_to_disp*3 + rot_elements*k_disp_to_rot*3).asDiagonal()),
+ Constants::EPS);
+
+ // compute with displacement (3m) but not rotation (size 7)
+ ASSERT_MATRIX_APPROX(S->computeNoiseCov((Vector7d() << 0, -3, 0, 0, 0, 0, 1).finished()),
+ Matrix6d((min_var + disp_elements*k_disp_to_disp*3 + rot_elements*k_disp_to_rot*3).asDiagonal()),
+ Constants::EPS);
+
+ // compute with rotation (M_PI) but not displacement (size 6)
+ ASSERT_MATRIX_APPROX(S->computeNoiseCov((Vector6d() << 0, 0, 0, -M_PI, 0, 0).finished()),
+ Matrix6d((min_var + rot_elements*k_rot_to_rot*M_PI).asDiagonal()),
+ Constants::EPS);
+
+ // compute with rotation (M_PI) but not displacement (size 7)
+ ASSERT_MATRIX_APPROX(S->computeNoiseCov((Vector7d() << 0, 0, 0, 0, 0, 1, 0).finished()),
+ Matrix6d((min_var + rot_elements*k_rot_to_rot*M_PI).asDiagonal()),
+ Constants::EPS);
+}
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
+
diff --git a/test/gtest_sensor_odom_2d.cpp b/test/gtest_sensor_odom_2d.cpp
deleted file mode 100644
index f5ee28b60..000000000
--- a/test/gtest_sensor_odom_2d.cpp
+++ /dev/null
@@ -1,474 +0,0 @@
-//--------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(SensorBasePtr S_base,
- 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();
-
- // cast
- SensorOdom2dPtr S = std::dynamic_pointer_cast<SensorOdom2d>(S_base);
- ASSERT_TRUE(S!=nullptr);
- // 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->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->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->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->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->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, mixed)
-{
- auto params = std::make_shared<ParamsSensorOdom2d>();
- 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)
-{
- std::vector<std::string> modes({"fix", "initial_guess", "factor"});
- std::vector<bool> dynamics({false, true});
- std::vector<bool> drifts({false, true});
- std::vector<bool> wrongs({false, true});
-
- // P & O
- for (auto mode : modes)
- for (auto dynamic : dynamics)
- for (auto drift : drifts)
- for (auto wrong : wrongs)
- {
- // nonsense combination
- if (not dynamic and drift)
- continue;
-
- // Yaml parser
- std::string name = "sensor_odom_2d_" +
- mode +
- (dynamic ? "_dynamic" : "") +
- (drift ? "_drift" : "") +
- (wrong ? "_wrong" : "");
- ParserYaml parser = ParserYaml("test/yaml/sensor_odom_2d/" + name + ".yaml", wolf_root, true);
- ParamsServer server = ParamsServer(parser.getParams(), "sensor/" + name);
-
- WOLF_INFO("Creating sensor from name ", name);
-
- // CORRECT YAML
- if (not wrong)
- {
- 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
- else
- {
- 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");
-
- // Yaml parser
- ParserYaml parser = ParserYaml("test/yaml/sensor_odom_2d/sensor_odom_2d_mixed.yaml", wolf_root, true);
- ParamsServer server = ParamsServer(parser.getParams(), "sensor/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");
-
- // Yaml parser
- ParserYaml parser = ParserYaml("test/yaml/sensor_odom_2d/sensor_odom_2d_mixed_wrong.yaml", wolf_root, true);
- ParamsServer server = ParamsServer(parser.getParams(), "sensor/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)
-{
- std::vector<std::string> modes({"fix", "initial_guess", "factor"});
- std::vector<bool> dynamics({false, true});
- std::vector<bool> drifts({false, true});
- std::vector<bool> wrongs({false, true});
-
- // P & O
- for (auto mode : modes)
- for (auto dynamic : dynamics)
- for (auto drift : drifts)
- for (auto wrong : wrongs)
- {
- // nonsense combination
- if (not dynamic and drift)
- continue;
-
- // Yaml parser
- std::string name = "sensor_odom_2d_" +
- mode +
- (dynamic ? "_dynamic" : "") +
- (drift ? "_drift" : "") +
- (wrong ? "_wrong" : "");
- ParserYaml parser = ParserYaml("test/yaml/sensor_odom_2d/" + name + ".yaml", wolf_root, true);
- ParamsServer server = ParamsServer(parser.getParams(), "sensor/" + name);
-
- WOLF_INFO("Creating sensor from name ", name);
-
- // CORRECT YAML
- if (not wrong)
- {
- auto S = FactorySensor::create("SensorOdom2d", name, 2, 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
- else
- {
- ASSERT_THROW(FactorySensor::create("SensorOdom2d", name, 2, server),
- std::runtime_error);
- }
- }
-
- // MIXED - CORRECT YAML
- {
- WOLF_INFO("Creating sensor from name sensor_mixed");
-
- // Yaml parser
- ParserYaml parser = ParserYaml("test/yaml/sensor_odom_2d/sensor_odom_2d_mixed.yaml", wolf_root, true);
- ParamsServer server = ParamsServer(parser.getParams(), "sensor/sensor_mixed");
-
- auto S = FactorySensor::create("SensorOdom2d", "sensor_mixed", 2, 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");
-
- // Yaml parser
- ParserYaml parser = ParserYaml("test/yaml/sensor_odom_2d/sensor_odom_2d_mixed_wrong.yaml", wolf_root, true);
- ParamsServer server = ParamsServer(parser.getParams(), "sensor/sensor_mixed_wrong");
-
- ASSERT_THROW(FactorySensor::create("SensorOdom2d", "sensor_mixed_wrong", 2, server),
- std::runtime_error);
- }
-}
-
-TEST(SensorOdom2d, factory_yaml)
-{
- std::vector<std::string> modes({"fix", "initial_guess", "factor"});
- std::vector<bool> dynamics({false, true});
- std::vector<bool> drifts({false, true});
- std::vector<bool> wrongs({false, true});
-
- // P & O
- for (auto mode : modes)
- for (auto dynamic : dynamics)
- for (auto drift : drifts)
- for (auto wrong : wrongs)
- {
- // nonsense combination
- if (not dynamic and drift)
- continue;
-
- std::string name = "sensor_odom_2d_" +
- mode +
- (dynamic ? "_dynamic" : "") +
- (drift ? "_drift" : "") +
- (wrong ? "_wrong" : "");
-
- WOLF_INFO("Creating sensor from name ", name);
-
- // CORRECT YAML
- if (not wrong)
- {
- auto S = FactorySensorYaml::create("SensorOdom2d", name, 2,
- wolf_root + "/test/yaml/sensor_odom_2d/" + name + ".yaml");
-
- 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
- else
- {
- ASSERT_THROW(FactorySensorYaml::create("SensorOdom2d", name, 2,
- wolf_root + "/test/yaml/sensor_odom_2d/" + name + ".yaml"),
- std::runtime_error);
- }
- }
-
- // MIXED - 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/sensor_odom_2d_mixed.yaml");
-
- // 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(FactorySensorYaml::create("SensorOdom2d", "sensor_mixed_wrong", 2,
- wolf_root + "/test/yaml/sensor_odom_2d/sensor_odom_2d_mixed_wrong.yaml"),
- std::runtime_error);
- }
-}
-
-int main(int argc, char **argv)
-{
- testing::InitGoogleTest(&argc, argv);
- return RUN_ALL_TESTS();
-}
-
diff --git a/test/gtest_sensor_pose.cpp b/test/gtest_sensor_pose.cpp
index d37c700a7..b62d81e5b 100644
--- a/test/gtest_sensor_pose.cpp
+++ b/test/gtest_sensor_pose.cpp
@@ -27,6 +27,7 @@
*/
#include "core/sensor/sensor_pose.h"
+#include "core/sensor/factory_sensor.h"
#include "core/utils/utils_gtest.h"
#include <cstdio>
@@ -34,52 +35,247 @@
using namespace wolf;
using namespace Eigen;
-TEST(Pose, constructor)
+std::string wolf_root = _WOLF_ROOT_DIR;
+
+Vector2d p_state_2D = (Vector2d() << 1,2).finished();
+Vector3d p_state_3D = (Vector3d() << 1,2,3).finished();
+Vector1d o_state_2D = (Vector1d() << 3).finished();
+Vector4d o_state_3D = (Vector4d() << .5,.5,.5,.5).finished();
+double std_p = 0.2;
+double std_o = 0.1;
+Matrix3d noise_cov_2D = (Vector3d() << std_p, std_p, std_o).finished().cwiseAbs2().asDiagonal();
+Matrix6d noise_cov_3D = (Vector6d() << std_p, std_p, std_p, std_o, std_o, std_o).finished().cwiseAbs2().asDiagonal();
+
+TEST(Pose, constructor_priors_2D)
+{
+ auto param = std::make_shared<ParamsSensorPose>();
+ param->std_p = std_p;
+ param->std_o = std_o;
+
+ Priors priors{{'P',Prior("P", p_state_2D)}, //default "fix", not dynamic
+ {'O',Prior("O", o_state_2D)}}; //default "fix", not dynamic
+
+ auto sen = std::make_shared<SensorPose>("sensor_1", 2, param, priors);
+
+ ASSERT_NE(sen, nullptr);
+
+ ASSERT_TRUE(sen->getP()->isFixed());
+ ASSERT_TRUE(sen->getO()->isFixed());
+
+ ASSERT_MATRIX_APPROX(sen->getP()->getState(), p_state_2D, Constants::EPS);
+ ASSERT_MATRIX_APPROX(sen->getO()->getState(), o_state_2D, Constants::EPS);
+
+ ASSERT_EQ(sen->getStdP(), std_p);
+ ASSERT_EQ(sen->getStdO(), std_o);
+
+ ASSERT_MATRIX_APPROX(sen->computeNoiseCov(VectorXd(0)), noise_cov_2D, Constants::EPS)
+}
+
+TEST(Pose, constructor_priors_3D)
+{
+ auto param = std::make_shared<ParamsSensorPose>();
+ param->std_p = std_p;
+ param->std_o = std_o;
+
+ Priors priors{{'P',Prior("P", p_state_3D)}, //default "fix", not dynamic
+ {'O',Prior("O", o_state_3D)}}; //default "fix", not dynamic
+
+ auto sen = std::make_shared<SensorPose>("sensor_1", 3, param, priors);
+
+ ASSERT_NE(sen, nullptr);
+
+ ASSERT_TRUE(sen->getP()->isFixed());
+ ASSERT_TRUE(sen->getO()->isFixed());
+
+ ASSERT_MATRIX_APPROX(sen->getP()->getState(), p_state_3D, Constants::EPS);
+ ASSERT_MATRIX_APPROX(sen->getO()->getState(), o_state_3D, Constants::EPS);
+
+ ASSERT_EQ(sen->getStdP(), std_p);
+ ASSERT_EQ(sen->getStdO(), std_o);
+
+ ASSERT_MATRIX_APPROX(sen->computeNoiseCov(VectorXd(0)), noise_cov_3D, Constants::EPS)
+}
+
+TEST(Pose, constructor_server_2D)
{
+ ParserYaml parser = ParserYaml("test/yaml/sensor_pose_2d.yaml", wolf_root, true);
+ ParamsServer server = ParamsServer(parser.getParams(), "/sensor/sensor_1");
+
+ auto params = std::make_shared<ParamsSensorPose>("sensor_1", server);
+ auto sen = std::make_shared<SensorPose>("sensor_1", 2, params, server);
+
+ ASSERT_NE(sen, nullptr);
+
+ ASSERT_TRUE(sen->getP()->isFixed());
+ ASSERT_TRUE(sen->getO()->isFixed());
- auto intr = std::make_shared<ParamsSensorPose>();
- Vector7d extr; extr << 1,2,3,.5,.5,.5,.5;
+ ASSERT_MATRIX_APPROX(sen->getP()->getState(), p_state_2D, Constants::EPS);
+ ASSERT_MATRIX_APPROX(sen->getO()->getState(), o_state_2D, Constants::EPS);
+
+ ASSERT_EQ(sen->getStdP(), std_p);
+ ASSERT_EQ(sen->getStdO(), std_o);
+
+ ASSERT_MATRIX_APPROX(sen->computeNoiseCov(VectorXd(0)), noise_cov_2D, Constants::EPS)
+}
+
+TEST(Pose, constructor_server_3D)
+{
+ ParserYaml parser = ParserYaml("test/yaml/sensor_pose_3d.yaml", wolf_root, true);
+ ParamsServer server = ParamsServer(parser.getParams(), "/sensor/sensor_1");
- auto sen = std::make_shared<SensorPose>(extr, intr);
+ auto params = std::make_shared<ParamsSensorPose>("sensor_1", server);
+ auto sen = std::make_shared<SensorPose>("sensor_1", 3, params, server);
ASSERT_NE(sen, nullptr);
- ASSERT_MATRIX_APPROX(sen->getP()->getState(), extr.head<3>(), 1e-12);
- ASSERT_MATRIX_APPROX(sen->getO()->getState(), extr.tail<4>(), 1e-12);
+ ASSERT_TRUE(sen->getP()->isFixed());
+ ASSERT_TRUE(sen->getO()->isFixed());
+
+ ASSERT_MATRIX_APPROX(sen->getP()->getState(), p_state_3D, Constants::EPS);
+ ASSERT_MATRIX_APPROX(sen->getO()->getState(), o_state_3D, Constants::EPS);
+
+ ASSERT_EQ(sen->getStdP(), std_p);
+ ASSERT_EQ(sen->getStdO(), std_o);
+
+ ASSERT_MATRIX_APPROX(sen->computeNoiseCov(VectorXd(0)), noise_cov_3D, Constants::EPS)
}
-TEST(Pose, getParams)
+TEST(Pose, factory_2D)
{
- auto intr = std::make_shared<ParamsSensorPose>();
- intr->std_p = 2;
- intr->std_o = 3;
+ ParserYaml parser = ParserYaml("test/yaml/sensor_pose_2d.yaml", wolf_root, true);
+ ParamsServer server = ParamsServer(parser.getParams(), "/sensor/sensor_1");
- Vector7d extr; extr << 1,2,3,.5,.5,.5,.5;
+ auto sb = FactorySensor::create("SensorPose","sensor_1", 2, server);
+
+ SensorPosePtr sen = std::dynamic_pointer_cast<SensorPose>(sb);
+
+ ASSERT_NE(sen, nullptr);
- auto sen = std::make_shared<SensorPose>(extr, intr);
+ ASSERT_TRUE(sen->getP()->isFixed());
+ ASSERT_TRUE(sen->getO()->isFixed());
- ASSERT_EQ(sen->getStdP(), intr->std_p);
- ASSERT_EQ(sen->getStdO(), intr->std_o);
- // ASSERT_EQ(sen->getNoiseCov()(0,0), (Vector1d() << 4).finished());
- // ASSERT_EQ(sen->getNoiseCov()(3,3), (Vector1d() << 9).finished());
+ ASSERT_MATRIX_APPROX(sen->getP()->getState(), p_state_2D, Constants::EPS);
+ ASSERT_MATRIX_APPROX(sen->getO()->getState(), o_state_2D, Constants::EPS);
+
+ ASSERT_EQ(sen->getStdP(), std_p);
+ ASSERT_EQ(sen->getStdO(), std_o);
+
+ ASSERT_MATRIX_APPROX(sen->computeNoiseCov(VectorXd(0)), noise_cov_2D, Constants::EPS)
}
-TEST(Pose, create)
+TEST(Pose, factory_3D)
{
- auto intr = std::make_shared<ParamsSensorPose>();
- intr->std_p = 2;
- intr->std_o = 3;
+ ParserYaml parser = ParserYaml("test/yaml/sensor_pose_3d.yaml", wolf_root, true);
+ ParamsServer server = ParamsServer(parser.getParams(), "/sensor/sensor_1");
+
+ auto sb = FactorySensor::create("SensorPose","sensor_1", 3, server);
+
+ SensorPosePtr sen = std::dynamic_pointer_cast<SensorPose>(sb);
+
+ ASSERT_NE(sen, nullptr);
+
+ ASSERT_TRUE(sen->getP()->isFixed());
+ ASSERT_TRUE(sen->getO()->isFixed());
+
+ ASSERT_MATRIX_APPROX(sen->getP()->getState(), p_state_3D, Constants::EPS);
+ ASSERT_MATRIX_APPROX(sen->getO()->getState(), o_state_3D, Constants::EPS);
+
+ ASSERT_EQ(sen->getStdP(), std_p);
+ ASSERT_EQ(sen->getStdO(), std_o);
+
+ ASSERT_MATRIX_APPROX(sen->computeNoiseCov(VectorXd(0)), noise_cov_3D, Constants::EPS)
+}
+
+TEST(Pose, factory_yaml_2D)
+{
+ auto sb = FactorySensorYaml::create("SensorPose","sensor_1", 2, wolf_root + "/test/yaml/sensor_pose_2d.yaml");
+
+ SensorPosePtr sen = std::dynamic_pointer_cast<SensorPose>(sb);
+
+ ASSERT_NE(sen, nullptr);
+
+ ASSERT_TRUE(sen->getP()->isFixed());
+ ASSERT_TRUE(sen->getO()->isFixed());
+
+ ASSERT_MATRIX_APPROX(sen->getP()->getState(), p_state_2D, Constants::EPS);
+ ASSERT_MATRIX_APPROX(sen->getO()->getState(), o_state_2D, Constants::EPS);
+
+ ASSERT_EQ(sen->getStdP(), std_p);
+ ASSERT_EQ(sen->getStdO(), std_o);
+
+ ASSERT_MATRIX_APPROX(sen->computeNoiseCov(VectorXd(0)), noise_cov_2D, Constants::EPS)
+}
+
+TEST(Pose, factory_yaml_3D)
+{
+ auto sb = FactorySensorYaml::create("SensorPose","sensor_1", 3, wolf_root + "/test/yaml/sensor_pose_3d.yaml");
+
+ SensorPosePtr sen = std::dynamic_pointer_cast<SensorPose>(sb);
+
+ ASSERT_NE(sen, nullptr);
+
+ ASSERT_TRUE(sen->getP()->isFixed());
+ ASSERT_TRUE(sen->getO()->isFixed());
+
+ ASSERT_MATRIX_APPROX(sen->getP()->getState(), p_state_3D, Constants::EPS);
+ ASSERT_MATRIX_APPROX(sen->getO()->getState(), o_state_3D, Constants::EPS);
+
+ ASSERT_EQ(sen->getStdP(), std_p);
+ ASSERT_EQ(sen->getStdO(), std_o);
+
+ ASSERT_MATRIX_APPROX(sen->computeNoiseCov(VectorXd(0)), noise_cov_3D, Constants::EPS)
+}
+
+TEST(Pose, factory_priors_2D)
+{
+ auto param = std::make_shared<ParamsSensorPose>();
+ param->std_p = std_p;
+ param->std_o = std_o;
+
+ Priors priors{{'P',Prior("P", p_state_2D)}, //default "fix", not dynamic
+ {'O',Prior("O", o_state_2D)}}; //default "fix", not dynamic
+
+ auto sb = FactorySensorPriors::create("SensorPose","sensor_1", 2, param, priors);
+
+ SensorPosePtr sen = std::dynamic_pointer_cast<SensorPose>(sb);
+
+ ASSERT_NE(sen, nullptr);
+
+ ASSERT_TRUE(sen->getP()->isFixed());
+ ASSERT_TRUE(sen->getO()->isFixed());
+
+ ASSERT_MATRIX_APPROX(sen->getP()->getState(), p_state_2D, Constants::EPS);
+ ASSERT_MATRIX_APPROX(sen->getO()->getState(), o_state_2D, Constants::EPS);
+
+ ASSERT_EQ(sen->getStdP(), std_p);
+ ASSERT_EQ(sen->getStdO(), std_o);
+
+ ASSERT_MATRIX_APPROX(sen->computeNoiseCov(VectorXd(0)), noise_cov_2D, Constants::EPS)
+}
+
+TEST(Pose, factory_priors_3D)
+{
+ auto param = std::make_shared<ParamsSensorPose>();
+ param->std_p = std_p;
+ param->std_o = std_o;
+
+ Priors priors{{'P',Prior("P", p_state_3D)}, //default "fix", not dynamic
+ {'O',Prior("O", o_state_3D)}}; //default "fix", not dynamic
+
+ auto sb = FactorySensorPriors::create("SensorPose","sensor_1", 3, param, priors);
+
+ SensorPosePtr sen = std::dynamic_pointer_cast<SensorPose>(sb);
- Vector7d extr; extr << 1,2,3,.5,.5,.5,.5;
+ ASSERT_TRUE(sen->getP()->isFixed());
+ ASSERT_TRUE(sen->getO()->isFixed());
- auto sen_base = SensorPose::create("name", extr, intr);
+ ASSERT_MATRIX_APPROX(sen->getP()->getState(), p_state_3D, Constants::EPS);
+ ASSERT_MATRIX_APPROX(sen->getO()->getState(), o_state_3D, Constants::EPS);
- auto sen = std::static_pointer_cast<SensorPose>(sen_base);
+ ASSERT_EQ(sen->getStdP(), std_p);
+ ASSERT_EQ(sen->getStdO(), std_o);
- ASSERT_EQ(sen->getStdP(), intr->std_p);
- ASSERT_EQ(sen->getStdO(), intr->std_o);
- // ASSERT_EQ(sen->getNoiseCov()(0,0), (Vector1d() << 4).finished());
- // ASSERT_EQ(sen->getNoiseCov()(3,3), (Vector1d() << 9).finished());
+ ASSERT_MATRIX_APPROX(sen->computeNoiseCov(VectorXd(0)), noise_cov_3D, Constants::EPS)
}
int main(int argc, char **argv)
diff --git a/test/yaml/params_tree_manager1.yaml b/test/yaml/params_tree_manager1.yaml
index 59aec3c70..5a75360a6 100644
--- a/test/yaml/params_tree_manager1.yaml
+++ b/test/yaml/params_tree_manager1.yaml
@@ -21,16 +21,23 @@ config:
toy_param: 0
sensors:
-
- type: "SensorOdom3d"
+ type: "SensorOdom"
name: "odom"
plugin: "core"
+ states:
+ P:
+ mode: fix
+ state: [1, 2, 3]
+ dynamic: false
+ O:
+ mode: fix
+ state: [0, 0, 0, 1]
+ dynamic: false
k_disp_to_disp: 0.1
k_disp_to_rot: 0.1
k_rot_to_rot: 0.1
min_disp_var: 0.1
min_rot_var: 0.1
- extrinsic:
- pose: [1,2,3,0,0,0,1]
processors:
-
type: "ProcessorOdom3d"
diff --git a/test/yaml/params_tree_manager2.yaml b/test/yaml/params_tree_manager2.yaml
index 419125468..555052c63 100644
--- a/test/yaml/params_tree_manager2.yaml
+++ b/test/yaml/params_tree_manager2.yaml
@@ -19,16 +19,23 @@ config:
type: "None"
sensors:
-
- type: "SensorOdom3d"
+ type: "SensorOdom"
name: "odom"
plugin: "core"
+ states:
+ P:
+ mode: fix
+ state: [1, 2, 3]
+ dynamic: false
+ O:
+ mode: fix
+ state: [0, 0, 0, 1]
+ dynamic: false
k_disp_to_disp: 0.1
k_disp_to_rot: 0.1
k_rot_to_rot: 0.1
min_disp_var: 0.1
min_rot_var: 0.1
- extrinsic:
- pose: [1,2,3,0,0,0,1]
processors:
-
type: "ProcessorOdom3d"
diff --git a/test/yaml/sensor_base/sensor_PO_2D_fix.yaml b/test/yaml/sensor_base/sensor_PO_2D_fix.yaml
deleted file mode 100644
index 4125b1f4a..000000000
--- a/test/yaml/sensor_base/sensor_PO_2D_fix.yaml
+++ /dev/null
@@ -1,12 +0,0 @@
-# sensor_PO_2D_fix:
-states:
- P:
- mode: fix
- state: [1, 2]
- dynamic: false
- O:
- mode: fix
- state: [1]
- dynamic: false
-noise_p_std: 0.1
-noise_o_std: 0.01
\ No newline at end of file
diff --git a/test/yaml/sensor_base/sensor_PO_2D_fix_dynamic.yaml b/test/yaml/sensor_base/sensor_PO_2D_fix_dynamic.yaml
deleted file mode 100644
index f70670013..000000000
--- a/test/yaml/sensor_base/sensor_PO_2D_fix_dynamic.yaml
+++ /dev/null
@@ -1,12 +0,0 @@
-# sensor_PO_2D_fix_dynamic:
-states:
- P:
- mode: fix
- state: [1, 2]
- dynamic: true
- O:
- mode: fix
- state: [1]
- dynamic: true
-noise_p_std: 0.1
-noise_o_std: 0.01
\ No newline at end of file
diff --git a/test/yaml/sensor_base/sensor_PO_2D_fix_wrong.yaml b/test/yaml/sensor_base/sensor_PO_2D_fix_wrong.yaml
deleted file mode 100644
index b34924894..000000000
--- a/test/yaml/sensor_base/sensor_PO_2D_fix_wrong.yaml
+++ /dev/null
@@ -1,12 +0,0 @@
-# sensor_PO_2D_fix_wrong:
-states:
- P:
- mode: fix
- state: [1, 2]
- #dynamic: false #missing
- O:
- mode: fix
- state: [1]
- dynamic: false
-noise_p_std: 0.1
-noise_o_std: 0.01
\ No newline at end of file
diff --git a/test/yaml/sensor_base/sensor_PO_3D_fix_wrong.yaml b/test/yaml/sensor_base/sensor_PO_3D_fix_wrong.yaml
deleted file mode 100644
index 99c0f477f..000000000
--- a/test/yaml/sensor_base/sensor_PO_3D_fix_wrong.yaml
+++ /dev/null
@@ -1,12 +0,0 @@
-# sensor_PO_3D_fix_wrong:
-states:
- P:
- mode: fix
- state: [1, 2, 3]
- dynamic: false
- O:
- mode: fix
- state: [1, 0, 0, 0]
- dynamic: false
-noise_p_std: 0.1
-#noise_o_std: 0.01 #missing
\ No newline at end of file
diff --git a/test/yaml/sensor_base/sensor_dummy.yaml b/test/yaml/sensor_base/sensor_dummy.yaml
deleted file mode 100644
index 6e1111173..000000000
--- a/test/yaml/sensor_base/sensor_dummy.yaml
+++ /dev/null
@@ -1,24 +0,0 @@
-param1: 1.2
-param2: 3
-noise_std: [0.1, 0.2]
-states:
- P:
- mode: factor
- state: [1, 2, 3]
- noise_std: [0.1, 0.2, 0.3]
- dynamic: true
- O:
- mode: fix
- state: [1, 0, 0, 0]
- dynamic: false
- I:
- mode: initial_guess
- state: [1, 2, 3, 4]
- dynamic: true
- drift_std: [0.1, 0.2, 0.3, 0.4]
- A:
- mode: factor
- state: [1, 0, 0, 0]
- noise_std: [0.1, 0.2, 0.3]
- dynamic: true
- drift_std: [0.1, 0.2, 0.3]
\ No newline at end of file
diff --git a/test/yaml/sensor_base/sensor_dummy_wrong.yaml b/test/yaml/sensor_base/sensor_dummy_wrong.yaml
deleted file mode 100644
index 89467b4cf..000000000
--- a/test/yaml/sensor_base/sensor_dummy_wrong.yaml
+++ /dev/null
@@ -1,24 +0,0 @@
-param1: 1.2
-param2: 3
-noise_std: [0.1, 0.2]
-states:
- P:
- mode: factor
- state: [1, 2, 3]
- noise_std: [0.1, 0.2, 0.3]
- dynamic: true
- O:
- mode: fix
- state: [1, 0, 0, 0]
- dynamic: false
- # I:
- # mode: initial_guess
- # state: [1, 2, 3, 4]
- # dynamic: true
- # drift_std: [0.1, 0.2, 0.3, 0.4]
- A:
- mode: factor
- state: [1, 0, 0, 0]
- noise_std: [0.1, 0.2, 0.3]
- dynamic: true
- drift_std: [0.1, 0.2, 0.3]
\ No newline at end of file
diff --git a/test/yaml/sensor_odom_2d.yaml b/test/yaml/sensor_odom_2d.yaml
index 16adc66ac..0ab13d73f 100644
--- a/test/yaml/sensor_odom_2d.yaml
+++ b/test/yaml/sensor_odom_2d.yaml
@@ -7,6 +7,10 @@ states:
mode: fix
state: [3]
dynamic: false
-noise_std: [0.1, 0.2]
+
k_disp_to_disp: 0.5
+k_disp_to_rot: 0.0
k_rot_to_rot: 0.8
+min_disp_var: 0.01
+min_rot_var: 0.01
+
diff --git a/test/yaml/sensor_odom_2d/sensor_odom_2d_factor.yaml b/test/yaml/sensor_odom_2d/sensor_odom_2d_factor.yaml
deleted file mode 100644
index 25318d527..000000000
--- a/test/yaml/sensor_odom_2d/sensor_odom_2d_factor.yaml
+++ /dev/null
@@ -1,14 +0,0 @@
-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
\ No newline at end of file
diff --git a/test/yaml/sensor_odom_2d/sensor_odom_2d_factor_dynamic.yaml b/test/yaml/sensor_odom_2d/sensor_odom_2d_factor_dynamic.yaml
deleted file mode 100644
index 05b7c5cb1..000000000
--- a/test/yaml/sensor_odom_2d/sensor_odom_2d_factor_dynamic.yaml
+++ /dev/null
@@ -1,14 +0,0 @@
-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
\ No newline at end of file
diff --git a/test/yaml/sensor_odom_2d/sensor_odom_2d_factor_dynamic_drift.yaml b/test/yaml/sensor_odom_2d/sensor_odom_2d_factor_dynamic_drift.yaml
deleted file mode 100644
index 76d8a9222..000000000
--- a/test/yaml/sensor_odom_2d/sensor_odom_2d_factor_dynamic_drift.yaml
+++ /dev/null
@@ -1,16 +0,0 @@
-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
\ No newline at end of file
diff --git a/test/yaml/sensor_odom_2d/sensor_odom_2d_factor_dynamic_drift_wrong.yaml b/test/yaml/sensor_odom_2d/sensor_odom_2d_factor_dynamic_drift_wrong.yaml
deleted file mode 100644
index b2c4971c1..000000000
--- a/test/yaml/sensor_odom_2d/sensor_odom_2d_factor_dynamic_drift_wrong.yaml
+++ /dev/null
@@ -1,16 +0,0 @@
-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
\ No newline at end of file
diff --git a/test/yaml/sensor_odom_2d/sensor_odom_2d_factor_dynamic_wrong.yaml b/test/yaml/sensor_odom_2d/sensor_odom_2d_factor_dynamic_wrong.yaml
deleted file mode 100644
index 7095775fa..000000000
--- a/test/yaml/sensor_odom_2d/sensor_odom_2d_factor_dynamic_wrong.yaml
+++ /dev/null
@@ -1,14 +0,0 @@
-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
\ No newline at end of file
diff --git a/test/yaml/sensor_odom_2d/sensor_odom_2d_factor_wrong.yaml b/test/yaml/sensor_odom_2d/sensor_odom_2d_factor_wrong.yaml
deleted file mode 100644
index b8a86c874..000000000
--- a/test/yaml/sensor_odom_2d/sensor_odom_2d_factor_wrong.yaml
+++ /dev/null
@@ -1,14 +0,0 @@
-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
\ No newline at end of file
diff --git a/test/yaml/sensor_odom_2d/sensor_odom_2d_fix_dynamic.yaml b/test/yaml/sensor_odom_2d/sensor_odom_2d_fix_dynamic.yaml
deleted file mode 100644
index fd24b509f..000000000
--- a/test/yaml/sensor_odom_2d/sensor_odom_2d_fix_dynamic.yaml
+++ /dev/null
@@ -1,12 +0,0 @@
-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
\ No newline at end of file
diff --git a/test/yaml/sensor_odom_2d/sensor_odom_2d_fix_dynamic_drift.yaml b/test/yaml/sensor_odom_2d/sensor_odom_2d_fix_dynamic_drift.yaml
deleted file mode 100644
index a4b81b490..000000000
--- a/test/yaml/sensor_odom_2d/sensor_odom_2d_fix_dynamic_drift.yaml
+++ /dev/null
@@ -1,14 +0,0 @@
-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
diff --git a/test/yaml/sensor_odom_2d/sensor_odom_2d_fix_dynamic_drift_wrong.yaml b/test/yaml/sensor_odom_2d/sensor_odom_2d_fix_dynamic_drift_wrong.yaml
deleted file mode 100644
index 0c5beb28c..000000000
--- a/test/yaml/sensor_odom_2d/sensor_odom_2d_fix_dynamic_drift_wrong.yaml
+++ /dev/null
@@ -1,14 +0,0 @@
-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
diff --git a/test/yaml/sensor_odom_2d/sensor_odom_2d_fix_dynamic_wrong.yaml b/test/yaml/sensor_odom_2d/sensor_odom_2d_fix_dynamic_wrong.yaml
deleted file mode 100644
index ca9746a49..000000000
--- a/test/yaml/sensor_odom_2d/sensor_odom_2d_fix_dynamic_wrong.yaml
+++ /dev/null
@@ -1,12 +0,0 @@
-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
\ No newline at end of file
diff --git a/test/yaml/sensor_odom_2d/sensor_odom_2d_fix_wrong.yaml b/test/yaml/sensor_odom_2d/sensor_odom_2d_fix_wrong.yaml
deleted file mode 100644
index d927120f2..000000000
--- a/test/yaml/sensor_odom_2d/sensor_odom_2d_fix_wrong.yaml
+++ /dev/null
@@ -1,12 +0,0 @@
-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
diff --git a/test/yaml/sensor_odom_2d/sensor_odom_2d_initial_guess.yaml b/test/yaml/sensor_odom_2d/sensor_odom_2d_initial_guess.yaml
deleted file mode 100644
index 7d8038b84..000000000
--- a/test/yaml/sensor_odom_2d/sensor_odom_2d_initial_guess.yaml
+++ /dev/null
@@ -1,12 +0,0 @@
-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
\ No newline at end of file
diff --git a/test/yaml/sensor_odom_2d/sensor_odom_2d_initial_guess_dynamic.yaml b/test/yaml/sensor_odom_2d/sensor_odom_2d_initial_guess_dynamic.yaml
deleted file mode 100644
index 547c9ea89..000000000
--- a/test/yaml/sensor_odom_2d/sensor_odom_2d_initial_guess_dynamic.yaml
+++ /dev/null
@@ -1,12 +0,0 @@
-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
diff --git a/test/yaml/sensor_odom_2d/sensor_odom_2d_initial_guess_dynamic_drift.yaml b/test/yaml/sensor_odom_2d/sensor_odom_2d_initial_guess_dynamic_drift.yaml
deleted file mode 100644
index 693e91bc2..000000000
--- a/test/yaml/sensor_odom_2d/sensor_odom_2d_initial_guess_dynamic_drift.yaml
+++ /dev/null
@@ -1,14 +0,0 @@
-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
diff --git a/test/yaml/sensor_odom_2d/sensor_odom_2d_initial_guess_dynamic_drift_wrong.yaml b/test/yaml/sensor_odom_2d/sensor_odom_2d_initial_guess_dynamic_drift_wrong.yaml
deleted file mode 100644
index 49d6a56c2..000000000
--- a/test/yaml/sensor_odom_2d/sensor_odom_2d_initial_guess_dynamic_drift_wrong.yaml
+++ /dev/null
@@ -1,14 +0,0 @@
-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
\ No newline at end of file
diff --git a/test/yaml/sensor_odom_2d/sensor_odom_2d_initial_guess_dynamic_wrong.yaml b/test/yaml/sensor_odom_2d/sensor_odom_2d_initial_guess_dynamic_wrong.yaml
deleted file mode 100644
index 4dcf14f13..000000000
--- a/test/yaml/sensor_odom_2d/sensor_odom_2d_initial_guess_dynamic_wrong.yaml
+++ /dev/null
@@ -1,12 +0,0 @@
-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
\ No newline at end of file
diff --git a/test/yaml/sensor_odom_2d/sensor_odom_2d_initial_guess_wrong.yaml b/test/yaml/sensor_odom_2d/sensor_odom_2d_initial_guess_wrong.yaml
deleted file mode 100644
index d6e901058..000000000
--- a/test/yaml/sensor_odom_2d/sensor_odom_2d_initial_guess_wrong.yaml
+++ /dev/null
@@ -1,12 +0,0 @@
-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
\ No newline at end of file
diff --git a/test/yaml/sensor_odom_2d/sensor_odom_2d_mixed.yaml b/test/yaml/sensor_odom_2d/sensor_odom_2d_mixed.yaml
deleted file mode 100644
index 42777646e..000000000
--- a/test/yaml/sensor_odom_2d/sensor_odom_2d_mixed.yaml
+++ /dev/null
@@ -1,14 +0,0 @@
-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
\ No newline at end of file
diff --git a/test/yaml/sensor_odom_2d/sensor_odom_2d_mixed_wrong.yaml b/test/yaml/sensor_odom_2d/sensor_odom_2d_mixed_wrong.yaml
deleted file mode 100644
index 7b1023e84..000000000
--- a/test/yaml/sensor_odom_2d/sensor_odom_2d_mixed_wrong.yaml
+++ /dev/null
@@ -1,13 +0,0 @@
-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
diff --git a/test/yaml/sensor_odom_3d.yaml b/test/yaml/sensor_odom_3d.yaml
index 8eb2b2350..ea22ca723 100644
--- a/test/yaml/sensor_odom_3d.yaml
+++ b/test/yaml/sensor_odom_3d.yaml
@@ -1,5 +1,13 @@
-type: "SensorOdom3d" # This must match the KEY used in the FactorySensor. Otherwise it is an error.
-
+states:
+ P:
+ mode: fix
+ state: [1, 2, 3]
+ dynamic: false
+ O:
+ mode: fix
+ state: [1, 0, 0, 0]
+ dynamic: false
+
k_disp_to_disp: 0.02 # m^2 / m
k_disp_to_rot: 0.02 # rad^2 / m
k_rot_to_rot: 0.01 # rad^2 / rad
diff --git a/test/yaml/sensor_odom_2d/sensor_odom_2d_fix.yaml b/test/yaml/sensor_pose_2d.yaml
similarity index 66%
rename from test/yaml/sensor_odom_2d/sensor_odom_2d_fix.yaml
rename to test/yaml/sensor_pose_2d.yaml
index 16adc66ac..d85c3d247 100644
--- a/test/yaml/sensor_odom_2d/sensor_odom_2d_fix.yaml
+++ b/test/yaml/sensor_pose_2d.yaml
@@ -7,6 +7,7 @@ states:
mode: fix
state: [3]
dynamic: false
-noise_std: [0.1, 0.2]
-k_disp_to_disp: 0.5
-k_rot_to_rot: 0.8
+
+std_p: 0.2
+std_o: 0.1
+
diff --git a/test/yaml/sensor_base/sensor_PO_3D_fix.yaml b/test/yaml/sensor_pose_3d.yaml
similarity index 57%
rename from test/yaml/sensor_base/sensor_PO_3D_fix.yaml
rename to test/yaml/sensor_pose_3d.yaml
index d3614ede6..f2acfab18 100644
--- a/test/yaml/sensor_base/sensor_PO_3D_fix.yaml
+++ b/test/yaml/sensor_pose_3d.yaml
@@ -1,4 +1,3 @@
-# sensor_PO_3D_fix:
states:
P:
mode: fix
@@ -6,7 +5,8 @@ states:
dynamic: false
O:
mode: fix
- state: [1, 0, 0, 0]
+ state: [0.5, 0.5, 0.5, 0.5]
dynamic: false
-noise_p_std: 0.1
-noise_o_std: 0.01
\ No newline at end of file
+
+std_p: 0.2
+std_o: 0.1
diff --git a/test/yaml/sensor_base/sensor_POIA_3D.yaml b/test/yaml/sensor_tests/sensor_POIA_3D.yaml
similarity index 71%
rename from test/yaml/sensor_base/sensor_POIA_3D.yaml
rename to test/yaml/sensor_tests/sensor_POIA_3D.yaml
index 7ec74b1fa..397f1d279 100644
--- a/test/yaml/sensor_base/sensor_POIA_3D.yaml
+++ b/test/yaml/sensor_tests/sensor_POIA_3D.yaml
@@ -20,5 +20,16 @@ states:
noise_std: [0.1, 0.2, 0.3]
dynamic: true
drift_std: [0.1, 0.2, 0.3]
+
+
+
+# used in gtest_sensor_base
noise_p_std: 0.1
-noise_o_std: 0.01
\ No newline at end of file
+noise_o_std: 0.01
+
+# used in gtest_sensor_odom
+k_disp_to_disp: 0.5
+k_disp_to_rot: 0.0
+k_rot_to_rot: 0.8
+min_disp_var: 0.01
+min_rot_var: 0.01
diff --git a/test/yaml/sensor_base/sensor_POIA_3D_wrong.yaml b/test/yaml/sensor_tests/sensor_POIA_3D_wrong.yaml
similarity index 72%
rename from test/yaml/sensor_base/sensor_POIA_3D_wrong.yaml
rename to test/yaml/sensor_tests/sensor_POIA_3D_wrong.yaml
index 5782bcc25..fab3cf9b1 100644
--- a/test/yaml/sensor_base/sensor_POIA_3D_wrong.yaml
+++ b/test/yaml/sensor_tests/sensor_POIA_3D_wrong.yaml
@@ -20,5 +20,15 @@ states:
# noise_std: [0.1, 0.2, 0.3]
# dynamic: true
# drift_std: [0.1, 0.2, 0.3]
+
+
+# used in gtest_sensor_base
noise_p_std: 0.1
-noise_o_std: 0.01
\ No newline at end of file
+noise_o_std: 0.01
+
+# used in gtest_sensor_odom
+k_disp_to_disp: 0.5
+k_disp_to_rot: 0.0
+k_rot_to_rot: 0.8
+min_disp_var: 0.01
+min_rot_var: 0.01
diff --git a/test/yaml/sensor_base/sensor_PO_2D_factor.yaml b/test/yaml/sensor_tests/sensor_PO_2D_factor.yaml
similarity index 55%
rename from test/yaml/sensor_base/sensor_PO_2D_factor.yaml
rename to test/yaml/sensor_tests/sensor_PO_2D_factor.yaml
index a7a59d8ba..9bfc63bcd 100644
--- a/test/yaml/sensor_base/sensor_PO_2D_factor.yaml
+++ b/test/yaml/sensor_tests/sensor_PO_2D_factor.yaml
@@ -10,5 +10,15 @@ states:
state: [1]
noise_std: [0.1]
dynamic: false
+
+
+# used in gtest_sensor_base
noise_p_std: 0.1
-noise_o_std: 0.01
\ No newline at end of file
+noise_o_std: 0.01
+
+# used in gtest_sensor_odom
+k_disp_to_disp: 0.5
+k_disp_to_rot: 0.0
+k_rot_to_rot: 0.8
+min_disp_var: 0.01
+min_rot_var: 0.01
diff --git a/test/yaml/sensor_base/sensor_PO_2D_factor_dynamic.yaml b/test/yaml/sensor_tests/sensor_PO_2D_factor_dynamic.yaml
similarity index 55%
rename from test/yaml/sensor_base/sensor_PO_2D_factor_dynamic.yaml
rename to test/yaml/sensor_tests/sensor_PO_2D_factor_dynamic.yaml
index 9bed5e2bf..ce4287ed5 100644
--- a/test/yaml/sensor_base/sensor_PO_2D_factor_dynamic.yaml
+++ b/test/yaml/sensor_tests/sensor_PO_2D_factor_dynamic.yaml
@@ -10,5 +10,15 @@ states:
state: [1]
noise_std: [0.1]
dynamic: true
+
+
+# used in gtest_sensor_base
noise_p_std: 0.1
-noise_o_std: 0.01
\ No newline at end of file
+noise_o_std: 0.01
+
+# used in gtest_sensor_odom
+k_disp_to_disp: 0.5
+k_disp_to_rot: 0.0
+k_rot_to_rot: 0.8
+min_disp_var: 0.01
+min_rot_var: 0.01
diff --git a/test/yaml/sensor_base/sensor_PO_2D_factor_dynamic_drift.yaml b/test/yaml/sensor_tests/sensor_PO_2D_factor_dynamic_drift.yaml
similarity index 61%
rename from test/yaml/sensor_base/sensor_PO_2D_factor_dynamic_drift.yaml
rename to test/yaml/sensor_tests/sensor_PO_2D_factor_dynamic_drift.yaml
index d7b3ecb23..d4c05d48d 100644
--- a/test/yaml/sensor_base/sensor_PO_2D_factor_dynamic_drift.yaml
+++ b/test/yaml/sensor_tests/sensor_PO_2D_factor_dynamic_drift.yaml
@@ -12,5 +12,14 @@ states:
noise_std: [0.1]
dynamic: true
drift_std: [0.1]
+
+# used in gtest_sensor_base
noise_p_std: 0.1
-noise_o_std: 0.01
\ No newline at end of file
+noise_o_std: 0.01
+
+# used in gtest_sensor_odom
+k_disp_to_disp: 0.5
+k_disp_to_rot: 0.0
+k_rot_to_rot: 0.8
+min_disp_var: 0.01
+min_rot_var: 0.01
diff --git a/test/yaml/sensor_base/sensor_PO_2D_factor_dynamic_drift_wrong.yaml b/test/yaml/sensor_tests/sensor_PO_2D_factor_dynamic_drift_wrong.yaml
similarity index 62%
rename from test/yaml/sensor_base/sensor_PO_2D_factor_dynamic_drift_wrong.yaml
rename to test/yaml/sensor_tests/sensor_PO_2D_factor_dynamic_drift_wrong.yaml
index abf288b22..a6787204c 100644
--- a/test/yaml/sensor_base/sensor_PO_2D_factor_dynamic_drift_wrong.yaml
+++ b/test/yaml/sensor_tests/sensor_PO_2D_factor_dynamic_drift_wrong.yaml
@@ -12,5 +12,14 @@ states:
noise_std: [0.1]
dynamic: true
drift_std: [0.1]
+
+# used in gtest_sensor_base
noise_p_std: 0.1
-noise_o_std: 0.01
\ No newline at end of file
+noise_o_std: 0.01
+
+# used in gtest_sensor_odom
+k_disp_to_disp: 0.5
+k_disp_to_rot: 0.0
+k_rot_to_rot: 0.8
+min_disp_var: 0.01
+min_rot_var: 0.01
diff --git a/test/yaml/sensor_base/sensor_PO_2D_factor_dynamic_wrong.yaml b/test/yaml/sensor_tests/sensor_PO_2D_factor_dynamic_wrong.yaml
similarity index 58%
rename from test/yaml/sensor_base/sensor_PO_2D_factor_dynamic_wrong.yaml
rename to test/yaml/sensor_tests/sensor_PO_2D_factor_dynamic_wrong.yaml
index 9ed9a58fa..c66c96442 100644
--- a/test/yaml/sensor_base/sensor_PO_2D_factor_dynamic_wrong.yaml
+++ b/test/yaml/sensor_tests/sensor_PO_2D_factor_dynamic_wrong.yaml
@@ -10,5 +10,14 @@ states:
state: [1]
noise_std: [0.1]
dynamic: true
+
+# used in gtest_sensor_base
noise_p_std: 0.1
-noise_o_std: 0.01
\ No newline at end of file
+noise_o_std: 0.01
+
+# used in gtest_sensor_odom
+k_disp_to_disp: 0.5
+k_disp_to_rot: 0.0
+k_rot_to_rot: 0.8
+min_disp_var: 0.01
+min_rot_var: 0.01
diff --git a/test/yaml/sensor_base/sensor_PO_2D_factor_wrong.yaml b/test/yaml/sensor_tests/sensor_PO_2D_factor_wrong.yaml
similarity index 56%
rename from test/yaml/sensor_base/sensor_PO_2D_factor_wrong.yaml
rename to test/yaml/sensor_tests/sensor_PO_2D_factor_wrong.yaml
index a781d137f..b99691f90 100644
--- a/test/yaml/sensor_base/sensor_PO_2D_factor_wrong.yaml
+++ b/test/yaml/sensor_tests/sensor_PO_2D_factor_wrong.yaml
@@ -10,5 +10,15 @@ states:
#state: [1] #missing
noise_std: [0.1]
dynamic: false
+
+
+# used in gtest_sensor_base
noise_p_std: 0.1
-noise_o_std: 0.01
\ No newline at end of file
+noise_o_std: 0.01
+
+# used in gtest_sensor_odom
+k_disp_to_disp: 0.5
+k_disp_to_rot: 0.0
+k_rot_to_rot: 0.8
+min_disp_var: 0.01
+min_rot_var: 0.01
diff --git a/test/yaml/sensor_tests/sensor_PO_2D_fix.yaml b/test/yaml/sensor_tests/sensor_PO_2D_fix.yaml
new file mode 100644
index 000000000..e96ee1849
--- /dev/null
+++ b/test/yaml/sensor_tests/sensor_PO_2D_fix.yaml
@@ -0,0 +1,22 @@
+# sensor_PO_2D_fix:
+states:
+ P:
+ mode: fix
+ state: [1, 2]
+ dynamic: false
+ O:
+ mode: fix
+ state: [1]
+ dynamic: false
+
+
+# used in gtest_sensor_base
+noise_p_std: 0.1
+noise_o_std: 0.01
+
+# used in gtest_sensor_odom
+k_disp_to_disp: 0.5
+k_disp_to_rot: 0.0
+k_rot_to_rot: 0.8
+min_disp_var: 0.01
+min_rot_var: 0.01
diff --git a/test/yaml/sensor_tests/sensor_PO_2D_fix_dynamic.yaml b/test/yaml/sensor_tests/sensor_PO_2D_fix_dynamic.yaml
new file mode 100644
index 000000000..715da540e
--- /dev/null
+++ b/test/yaml/sensor_tests/sensor_PO_2D_fix_dynamic.yaml
@@ -0,0 +1,22 @@
+# sensor_PO_2D_fix_dynamic:
+states:
+ P:
+ mode: fix
+ state: [1, 2]
+ dynamic: true
+ O:
+ mode: fix
+ state: [1]
+ dynamic: true
+
+
+# used in gtest_sensor_base
+noise_p_std: 0.1
+noise_o_std: 0.01
+
+# used in gtest_sensor_odom
+k_disp_to_disp: 0.5
+k_disp_to_rot: 0.0
+k_rot_to_rot: 0.8
+min_disp_var: 0.01
+min_rot_var: 0.01
diff --git a/test/yaml/sensor_base/sensor_PO_2D_fix_dynamic_drift.yaml b/test/yaml/sensor_tests/sensor_PO_2D_fix_dynamic_drift.yaml
similarity index 55%
rename from test/yaml/sensor_base/sensor_PO_2D_fix_dynamic_drift.yaml
rename to test/yaml/sensor_tests/sensor_PO_2D_fix_dynamic_drift.yaml
index 25a40db42..4e6680029 100644
--- a/test/yaml/sensor_base/sensor_PO_2D_fix_dynamic_drift.yaml
+++ b/test/yaml/sensor_tests/sensor_PO_2D_fix_dynamic_drift.yaml
@@ -10,5 +10,15 @@ states:
state: [1]
dynamic: true
drift_std: [0.1]
+
+
+# used in gtest_sensor_base
noise_p_std: 0.1
-noise_o_std: 0.01
\ No newline at end of file
+noise_o_std: 0.01
+
+# used in gtest_sensor_odom
+k_disp_to_disp: 0.5
+k_disp_to_rot: 0.0
+k_rot_to_rot: 0.8
+min_disp_var: 0.01
+min_rot_var: 0.01
diff --git a/test/yaml/sensor_base/sensor_PO_2D_fix_dynamic_drift_wrong.yaml b/test/yaml/sensor_tests/sensor_PO_2D_fix_dynamic_drift_wrong.yaml
similarity index 57%
rename from test/yaml/sensor_base/sensor_PO_2D_fix_dynamic_drift_wrong.yaml
rename to test/yaml/sensor_tests/sensor_PO_2D_fix_dynamic_drift_wrong.yaml
index 622158be3..0bcee886c 100644
--- a/test/yaml/sensor_base/sensor_PO_2D_fix_dynamic_drift_wrong.yaml
+++ b/test/yaml/sensor_tests/sensor_PO_2D_fix_dynamic_drift_wrong.yaml
@@ -10,5 +10,15 @@ states:
state: [1]
dynamic: true
drift_std: [0.1]
+
+
+# used in gtest_sensor_base
noise_p_std: 0.1
-noise_o_std: 0.01
\ No newline at end of file
+noise_o_std: 0.01
+
+# used in gtest_sensor_odom
+k_disp_to_disp: 0.5
+k_disp_to_rot: 0.0
+k_rot_to_rot: 0.8
+min_disp_var: 0.01
+min_rot_var: 0.01
diff --git a/test/yaml/sensor_base/sensor_PO_2D_fix_dynamic_wrong.yaml b/test/yaml/sensor_tests/sensor_PO_2D_fix_dynamic_wrong.yaml
similarity index 51%
rename from test/yaml/sensor_base/sensor_PO_2D_fix_dynamic_wrong.yaml
rename to test/yaml/sensor_tests/sensor_PO_2D_fix_dynamic_wrong.yaml
index d66f2f21c..1c9d81679 100644
--- a/test/yaml/sensor_base/sensor_PO_2D_fix_dynamic_wrong.yaml
+++ b/test/yaml/sensor_tests/sensor_PO_2D_fix_dynamic_wrong.yaml
@@ -8,5 +8,15 @@ states:
mode: fix
state: [1]
dynamic: true
+
+
+# used in gtest_sensor_base
noise_p_std: 0.1
-noise_o_std: 0.01
\ No newline at end of file
+noise_o_std: 0.01
+
+# used in gtest_sensor_odom
+k_disp_to_disp: 0.5
+k_disp_to_rot: 0.0
+k_rot_to_rot: 0.8
+min_disp_var: 0.01
+min_rot_var: 0.01
diff --git a/test/yaml/sensor_tests/sensor_PO_2D_fix_wrong.yaml b/test/yaml/sensor_tests/sensor_PO_2D_fix_wrong.yaml
new file mode 100644
index 000000000..229f89094
--- /dev/null
+++ b/test/yaml/sensor_tests/sensor_PO_2D_fix_wrong.yaml
@@ -0,0 +1,22 @@
+# sensor_PO_2D_fix_wrong:
+states:
+ P:
+ mode: fix
+ state: [1, 2]
+ #dynamic: false #missing
+ O:
+ mode: fix
+ state: [1]
+ dynamic: false
+
+
+# used in gtest_sensor_base
+noise_p_std: 0.1
+noise_o_std: 0.01
+
+# used in gtest_sensor_odom
+k_disp_to_disp: 0.5
+k_disp_to_rot: 0.0
+k_rot_to_rot: 0.8
+min_disp_var: 0.01
+min_rot_var: 0.01
diff --git a/test/yaml/sensor_base/sensor_PO_2D_initial_guess.yaml b/test/yaml/sensor_tests/sensor_PO_2D_initial_guess.yaml
similarity index 51%
rename from test/yaml/sensor_base/sensor_PO_2D_initial_guess.yaml
rename to test/yaml/sensor_tests/sensor_PO_2D_initial_guess.yaml
index 937ddf2ab..f593fc53c 100644
--- a/test/yaml/sensor_base/sensor_PO_2D_initial_guess.yaml
+++ b/test/yaml/sensor_tests/sensor_PO_2D_initial_guess.yaml
@@ -8,5 +8,15 @@ states:
mode: initial_guess
state: [1]
dynamic: false
+
+
+# used in gtest_sensor_base
noise_p_std: 0.1
-noise_o_std: 0.01
\ No newline at end of file
+noise_o_std: 0.01
+
+# used in gtest_sensor_odom
+k_disp_to_disp: 0.5
+k_disp_to_rot: 0.0
+k_rot_to_rot: 0.8
+min_disp_var: 0.01
+min_rot_var: 0.01
diff --git a/test/yaml/sensor_base/sensor_PO_2D_initial_guess_dynamic.yaml b/test/yaml/sensor_tests/sensor_PO_2D_initial_guess_dynamic.yaml
similarity index 52%
rename from test/yaml/sensor_base/sensor_PO_2D_initial_guess_dynamic.yaml
rename to test/yaml/sensor_tests/sensor_PO_2D_initial_guess_dynamic.yaml
index e97957496..50705368a 100644
--- a/test/yaml/sensor_base/sensor_PO_2D_initial_guess_dynamic.yaml
+++ b/test/yaml/sensor_tests/sensor_PO_2D_initial_guess_dynamic.yaml
@@ -8,5 +8,15 @@ states:
mode: initial_guess
state: [1]
dynamic: true
+
+
+# used in gtest_sensor_base
noise_p_std: 0.1
-noise_o_std: 0.01
\ No newline at end of file
+noise_o_std: 0.01
+
+# used in gtest_sensor_odom
+k_disp_to_disp: 0.5
+k_disp_to_rot: 0.0
+k_rot_to_rot: 0.8
+min_disp_var: 0.01
+min_rot_var: 0.01
diff --git a/test/yaml/sensor_base/sensor_PO_2D_initial_guess_dynamic_drift.yaml b/test/yaml/sensor_tests/sensor_PO_2D_initial_guess_dynamic_drift.yaml
similarity index 58%
rename from test/yaml/sensor_base/sensor_PO_2D_initial_guess_dynamic_drift.yaml
rename to test/yaml/sensor_tests/sensor_PO_2D_initial_guess_dynamic_drift.yaml
index 49aaaaac8..d5060eeaf 100644
--- a/test/yaml/sensor_base/sensor_PO_2D_initial_guess_dynamic_drift.yaml
+++ b/test/yaml/sensor_tests/sensor_PO_2D_initial_guess_dynamic_drift.yaml
@@ -10,5 +10,15 @@ states:
state: [1]
dynamic: true
drift_std: [0.1]
+
+
+# used in gtest_sensor_base
noise_p_std: 0.1
-noise_o_std: 0.01
\ No newline at end of file
+noise_o_std: 0.01
+
+# used in gtest_sensor_odom
+k_disp_to_disp: 0.5
+k_disp_to_rot: 0.0
+k_rot_to_rot: 0.8
+min_disp_var: 0.01
+min_rot_var: 0.01
diff --git a/test/yaml/sensor_base/sensor_PO_2D_initial_guess_dynamic_drift_wrong.yaml b/test/yaml/sensor_tests/sensor_PO_2D_initial_guess_dynamic_drift_wrong.yaml
similarity index 60%
rename from test/yaml/sensor_base/sensor_PO_2D_initial_guess_dynamic_drift_wrong.yaml
rename to test/yaml/sensor_tests/sensor_PO_2D_initial_guess_dynamic_drift_wrong.yaml
index cc9e32246..9a53f8300 100644
--- a/test/yaml/sensor_base/sensor_PO_2D_initial_guess_dynamic_drift_wrong.yaml
+++ b/test/yaml/sensor_tests/sensor_PO_2D_initial_guess_dynamic_drift_wrong.yaml
@@ -10,5 +10,15 @@ states:
state: [1]
dynamic: true
drift_std: [0.1]
+
+
+# used in gtest_sensor_base
noise_p_std: 0.1
-noise_o_std: 0.01
\ No newline at end of file
+noise_o_std: 0.01
+
+# used in gtest_sensor_odom
+k_disp_to_disp: 0.5
+k_disp_to_rot: 0.0
+k_rot_to_rot: 0.8
+min_disp_var: 0.01
+min_rot_var: 0.01
diff --git a/test/yaml/sensor_base/sensor_PO_2D_initial_guess_dynamic_wrong.yaml b/test/yaml/sensor_tests/sensor_PO_2D_initial_guess_dynamic_wrong.yaml
similarity index 55%
rename from test/yaml/sensor_base/sensor_PO_2D_initial_guess_dynamic_wrong.yaml
rename to test/yaml/sensor_tests/sensor_PO_2D_initial_guess_dynamic_wrong.yaml
index 046ede375..9b3da538f 100644
--- a/test/yaml/sensor_base/sensor_PO_2D_initial_guess_dynamic_wrong.yaml
+++ b/test/yaml/sensor_tests/sensor_PO_2D_initial_guess_dynamic_wrong.yaml
@@ -8,5 +8,15 @@ states:
mode: initial_guess
state: [1]
dynamic: true
+
+
+# used in gtest_sensor_base
noise_p_std: 0.1
-noise_o_std: 0.01
\ No newline at end of file
+noise_o_std: 0.01
+
+# used in gtest_sensor_odom
+k_disp_to_disp: 0.5
+k_disp_to_rot: 0.0
+k_rot_to_rot: 0.8
+min_disp_var: 0.01
+min_rot_var: 0.01
diff --git a/test/yaml/sensor_base/sensor_PO_2D_initial_guess_wrong.yaml b/test/yaml/sensor_tests/sensor_PO_2D_initial_guess_wrong.yaml
similarity index 53%
rename from test/yaml/sensor_base/sensor_PO_2D_initial_guess_wrong.yaml
rename to test/yaml/sensor_tests/sensor_PO_2D_initial_guess_wrong.yaml
index ce2792d22..65d170426 100644
--- a/test/yaml/sensor_base/sensor_PO_2D_initial_guess_wrong.yaml
+++ b/test/yaml/sensor_tests/sensor_PO_2D_initial_guess_wrong.yaml
@@ -8,5 +8,15 @@ states:
#mode: initial_guess #missing
state: [1]
dynamic: false
+
+
+# used in gtest_sensor_base
noise_p_std: 0.1
-noise_o_std: 0.01
\ No newline at end of file
+noise_o_std: 0.01
+
+# used in gtest_sensor_odom
+k_disp_to_disp: 0.5
+k_disp_to_rot: 0.0
+k_rot_to_rot: 0.8
+min_disp_var: 0.01
+min_rot_var: 0.01
diff --git a/test/yaml/sensor_base/sensor_PO_3D_factor.yaml b/test/yaml/sensor_tests/sensor_PO_3D_factor.yaml
similarity index 58%
rename from test/yaml/sensor_base/sensor_PO_3D_factor.yaml
rename to test/yaml/sensor_tests/sensor_PO_3D_factor.yaml
index 6084ec5d3..23737df98 100644
--- a/test/yaml/sensor_base/sensor_PO_3D_factor.yaml
+++ b/test/yaml/sensor_tests/sensor_PO_3D_factor.yaml
@@ -10,5 +10,15 @@ states:
state: [1, 0, 0, 0]
noise_std: [0.1, 0.2, 0.3]
dynamic: false
+
+
+# used in gtest_sensor_base
noise_p_std: 0.1
-noise_o_std: 0.01
\ No newline at end of file
+noise_o_std: 0.01
+
+# used in gtest_sensor_odom
+k_disp_to_disp: 0.5
+k_disp_to_rot: 0.0
+k_rot_to_rot: 0.8
+min_disp_var: 0.01
+min_rot_var: 0.01
diff --git a/test/yaml/sensor_base/sensor_PO_3D_factor_dynamic.yaml b/test/yaml/sensor_tests/sensor_PO_3D_factor_dynamic.yaml
similarity index 58%
rename from test/yaml/sensor_base/sensor_PO_3D_factor_dynamic.yaml
rename to test/yaml/sensor_tests/sensor_PO_3D_factor_dynamic.yaml
index ab974d37a..8c9342ab8 100644
--- a/test/yaml/sensor_base/sensor_PO_3D_factor_dynamic.yaml
+++ b/test/yaml/sensor_tests/sensor_PO_3D_factor_dynamic.yaml
@@ -10,5 +10,15 @@ states:
state: [1, 0, 0, 0]
noise_std: [0.1, 0.2, 0.3]
dynamic: true
+
+
+# used in gtest_sensor_base
noise_p_std: 0.1
-noise_o_std: 0.01
\ No newline at end of file
+noise_o_std: 0.01
+
+# used in gtest_sensor_odom
+k_disp_to_disp: 0.5
+k_disp_to_rot: 0.0
+k_rot_to_rot: 0.8
+min_disp_var: 0.01
+min_rot_var: 0.01
diff --git a/test/yaml/sensor_base/sensor_PO_3D_factor_dynamic_drift.yaml b/test/yaml/sensor_tests/sensor_PO_3D_factor_dynamic_drift.yaml
similarity index 64%
rename from test/yaml/sensor_base/sensor_PO_3D_factor_dynamic_drift.yaml
rename to test/yaml/sensor_tests/sensor_PO_3D_factor_dynamic_drift.yaml
index 65940ecb8..9f7e14d04 100644
--- a/test/yaml/sensor_base/sensor_PO_3D_factor_dynamic_drift.yaml
+++ b/test/yaml/sensor_tests/sensor_PO_3D_factor_dynamic_drift.yaml
@@ -12,5 +12,15 @@ states:
noise_std: [0.1, 0.2, 0.3]
dynamic: true
drift_std: [0.1, 0.2, 0.3]
+
+
+# used in gtest_sensor_base
noise_p_std: 0.1
-noise_o_std: 0.01
\ No newline at end of file
+noise_o_std: 0.01
+
+# used in gtest_sensor_odom
+k_disp_to_disp: 0.5
+k_disp_to_rot: 0.0
+k_rot_to_rot: 0.8
+min_disp_var: 0.01
+min_rot_var: 0.01
diff --git a/test/yaml/sensor_base/sensor_PO_3D_factor_dynamic_drift_wrong.yaml b/test/yaml/sensor_tests/sensor_PO_3D_factor_dynamic_drift_wrong.yaml
similarity index 66%
rename from test/yaml/sensor_base/sensor_PO_3D_factor_dynamic_drift_wrong.yaml
rename to test/yaml/sensor_tests/sensor_PO_3D_factor_dynamic_drift_wrong.yaml
index f00748d65..a4980b27d 100644
--- a/test/yaml/sensor_base/sensor_PO_3D_factor_dynamic_drift_wrong.yaml
+++ b/test/yaml/sensor_tests/sensor_PO_3D_factor_dynamic_drift_wrong.yaml
@@ -12,5 +12,15 @@ states:
noise_std: [0.1, 0.2, 0.3, 0.4] #wrong size
dynamic: true
drift_std: [0.1, 0.2, 0.3]
+
+
+# used in gtest_sensor_base
noise_p_std: 0.1
-noise_o_std: 0.01
\ No newline at end of file
+noise_o_std: 0.01
+
+# used in gtest_sensor_odom
+k_disp_to_disp: 0.5
+k_disp_to_rot: 0.0
+k_rot_to_rot: 0.8
+min_disp_var: 0.01
+min_rot_var: 0.01
diff --git a/test/yaml/sensor_base/sensor_PO_3D_factor_dynamic_wrong.yaml b/test/yaml/sensor_tests/sensor_PO_3D_factor_dynamic_wrong.yaml
similarity index 60%
rename from test/yaml/sensor_base/sensor_PO_3D_factor_dynamic_wrong.yaml
rename to test/yaml/sensor_tests/sensor_PO_3D_factor_dynamic_wrong.yaml
index 095862ddc..104237208 100644
--- a/test/yaml/sensor_base/sensor_PO_3D_factor_dynamic_wrong.yaml
+++ b/test/yaml/sensor_tests/sensor_PO_3D_factor_dynamic_wrong.yaml
@@ -10,5 +10,15 @@ states:
state: [1, 0, 0, 0]
noise_std: [0.1, 0.2, 0.3, 0.4] # wrong size
dynamic: true
+
+
+# used in gtest_sensor_base
noise_p_std: 0.1
-noise_o_std: 0.01
\ No newline at end of file
+noise_o_std: 0.01
+
+# used in gtest_sensor_odom
+k_disp_to_disp: 0.5
+k_disp_to_rot: 0.0
+k_rot_to_rot: 0.8
+min_disp_var: 0.01
+min_rot_var: 0.01
diff --git a/test/yaml/sensor_base/sensor_PO_3D_factor_wrong.yaml b/test/yaml/sensor_tests/sensor_PO_3D_factor_wrong.yaml
similarity index 59%
rename from test/yaml/sensor_base/sensor_PO_3D_factor_wrong.yaml
rename to test/yaml/sensor_tests/sensor_PO_3D_factor_wrong.yaml
index ba143662c..d99e58074 100644
--- a/test/yaml/sensor_base/sensor_PO_3D_factor_wrong.yaml
+++ b/test/yaml/sensor_tests/sensor_PO_3D_factor_wrong.yaml
@@ -10,5 +10,15 @@ states:
state: [1, 0, 0, 0]
noise_std: [0.1, 0.2, 0.3]
dynamic: false
+
+
+# used in gtest_sensor_base
noise_p_std: 0.1
-noise_o_std: 0.01
\ No newline at end of file
+noise_o_std: 0.01
+
+# used in gtest_sensor_odom
+k_disp_to_disp: 0.5
+k_disp_to_rot: 0.0
+k_rot_to_rot: 0.8
+min_disp_var: 0.01
+min_rot_var: 0.01
diff --git a/test/yaml/sensor_tests/sensor_PO_3D_fix.yaml b/test/yaml/sensor_tests/sensor_PO_3D_fix.yaml
new file mode 100644
index 000000000..44eef97f1
--- /dev/null
+++ b/test/yaml/sensor_tests/sensor_PO_3D_fix.yaml
@@ -0,0 +1,22 @@
+# sensor_PO_3D_fix:
+states:
+ P:
+ mode: fix
+ state: [1, 2, 3]
+ dynamic: false
+ O:
+ mode: fix
+ state: [1, 0, 0, 0]
+ dynamic: false
+
+
+# used in gtest_sensor_base
+noise_p_std: 0.1
+noise_o_std: 0.01
+
+# used in gtest_sensor_odom
+k_disp_to_disp: 0.5
+k_disp_to_rot: 0.0
+k_rot_to_rot: 0.8
+min_disp_var: 0.01
+min_rot_var: 0.01
diff --git a/test/yaml/sensor_base/sensor_PO_3D_fix_dynamic.yaml b/test/yaml/sensor_tests/sensor_PO_3D_fix_dynamic.yaml
similarity index 50%
rename from test/yaml/sensor_base/sensor_PO_3D_fix_dynamic.yaml
rename to test/yaml/sensor_tests/sensor_PO_3D_fix_dynamic.yaml
index 0329cd837..606751a7b 100644
--- a/test/yaml/sensor_base/sensor_PO_3D_fix_dynamic.yaml
+++ b/test/yaml/sensor_tests/sensor_PO_3D_fix_dynamic.yaml
@@ -8,5 +8,15 @@ states:
mode: fix
state: [1, 0, 0, 0]
dynamic: true
+
+
+# used in gtest_sensor_base
noise_p_std: 0.1
-noise_o_std: 0.01
\ No newline at end of file
+noise_o_std: 0.01
+
+# used in gtest_sensor_odom
+k_disp_to_disp: 0.5
+k_disp_to_rot: 0.0
+k_rot_to_rot: 0.8
+min_disp_var: 0.01
+min_rot_var: 0.01
diff --git a/test/yaml/sensor_base/sensor_PO_3D_fix_dynamic_drift.yaml b/test/yaml/sensor_tests/sensor_PO_3D_fix_dynamic_drift.yaml
similarity index 58%
rename from test/yaml/sensor_base/sensor_PO_3D_fix_dynamic_drift.yaml
rename to test/yaml/sensor_tests/sensor_PO_3D_fix_dynamic_drift.yaml
index 3c2bedefe..f6166c91b 100644
--- a/test/yaml/sensor_base/sensor_PO_3D_fix_dynamic_drift.yaml
+++ b/test/yaml/sensor_tests/sensor_PO_3D_fix_dynamic_drift.yaml
@@ -10,5 +10,15 @@ states:
state: [1, 0, 0, 0]
dynamic: true
drift_std: [0.1, 0.2, 0.3]
+
+
+# used in gtest_sensor_base
noise_p_std: 0.1
-noise_o_std: 0.01
\ No newline at end of file
+noise_o_std: 0.01
+
+# used in gtest_sensor_odom
+k_disp_to_disp: 0.5
+k_disp_to_rot: 0.0
+k_rot_to_rot: 0.8
+min_disp_var: 0.01
+min_rot_var: 0.01
diff --git a/test/yaml/sensor_base/sensor_PO_3D_fix_dynamic_drift_wrong.yaml b/test/yaml/sensor_tests/sensor_PO_3D_fix_dynamic_drift_wrong.yaml
similarity index 59%
rename from test/yaml/sensor_base/sensor_PO_3D_fix_dynamic_drift_wrong.yaml
rename to test/yaml/sensor_tests/sensor_PO_3D_fix_dynamic_drift_wrong.yaml
index 47286195a..17467074e 100644
--- a/test/yaml/sensor_base/sensor_PO_3D_fix_dynamic_drift_wrong.yaml
+++ b/test/yaml/sensor_tests/sensor_PO_3D_fix_dynamic_drift_wrong.yaml
@@ -10,5 +10,15 @@ states:
state: [1, 0, 0, 0]
#dynamic: true #missing
drift_std: [0.1, 0.2, 0.3]
+
+
+# used in gtest_sensor_base
noise_p_std: 0.1
-noise_o_std: 0.01
\ No newline at end of file
+noise_o_std: 0.01
+
+# used in gtest_sensor_odom
+k_disp_to_disp: 0.5
+k_disp_to_rot: 0.0
+k_rot_to_rot: 0.8
+min_disp_var: 0.01
+min_rot_var: 0.01
diff --git a/test/yaml/sensor_base/sensor_PO_3D_fix_dynamic_wrong.yaml b/test/yaml/sensor_tests/sensor_PO_3D_fix_dynamic_wrong.yaml
similarity index 53%
rename from test/yaml/sensor_base/sensor_PO_3D_fix_dynamic_wrong.yaml
rename to test/yaml/sensor_tests/sensor_PO_3D_fix_dynamic_wrong.yaml
index c5f6ed7a6..4e6da9d21 100644
--- a/test/yaml/sensor_base/sensor_PO_3D_fix_dynamic_wrong.yaml
+++ b/test/yaml/sensor_tests/sensor_PO_3D_fix_dynamic_wrong.yaml
@@ -8,5 +8,15 @@ states:
mode: fix
state: [1, 0, 0, 1] # not normalized
dynamic: true
+
+
+# used in gtest_sensor_base
noise_p_std: 0.1
-noise_o_std: 0.01
\ No newline at end of file
+noise_o_std: 0.01
+
+# used in gtest_sensor_odom
+k_disp_to_disp: 0.5
+k_disp_to_rot: 0.0
+k_rot_to_rot: 0.8
+min_disp_var: 0.01
+min_rot_var: 0.01
diff --git a/test/yaml/sensor_tests/sensor_PO_3D_fix_wrong.yaml b/test/yaml/sensor_tests/sensor_PO_3D_fix_wrong.yaml
new file mode 100644
index 000000000..acc563566
--- /dev/null
+++ b/test/yaml/sensor_tests/sensor_PO_3D_fix_wrong.yaml
@@ -0,0 +1,22 @@
+# sensor_PO_3D_fix_wrong:
+states:
+ P:
+ mode: fix
+ state: [1, 2, 3]
+ dynamic: false
+ O:
+ mode: fix
+ state: [1, 0, 0, 0]
+ dynamic: false
+
+
+# used in gtest_sensor_base
+noise_p_std: 0.1
+# noise_o_std: 0.01 #missing
+
+# used in gtest_sensor_odom
+k_disp_to_disp: 0.5
+k_disp_to_rot: 0.0
+# k_rot_to_rot: 0.8 #missing
+min_disp_var: 0.01
+min_rot_var: 0.01
diff --git a/test/yaml/sensor_base/sensor_PO_3D_initial_guess.yaml b/test/yaml/sensor_tests/sensor_PO_3D_initial_guess.yaml
similarity index 53%
rename from test/yaml/sensor_base/sensor_PO_3D_initial_guess.yaml
rename to test/yaml/sensor_tests/sensor_PO_3D_initial_guess.yaml
index bb396634c..7b5e1398d 100644
--- a/test/yaml/sensor_base/sensor_PO_3D_initial_guess.yaml
+++ b/test/yaml/sensor_tests/sensor_PO_3D_initial_guess.yaml
@@ -8,5 +8,15 @@ states:
mode: initial_guess
state: [1, 0, 0, 0]
dynamic: false
+
+
+# used in gtest_sensor_base
noise_p_std: 0.1
-noise_o_std: 0.01
\ No newline at end of file
+noise_o_std: 0.01
+
+# used in gtest_sensor_odom
+k_disp_to_disp: 0.5
+k_disp_to_rot: 0.0
+k_rot_to_rot: 0.8
+min_disp_var: 0.01
+min_rot_var: 0.01
diff --git a/test/yaml/sensor_base/sensor_PO_3D_initial_guess_dynamic.yaml b/test/yaml/sensor_tests/sensor_PO_3D_initial_guess_dynamic.yaml
similarity index 54%
rename from test/yaml/sensor_base/sensor_PO_3D_initial_guess_dynamic.yaml
rename to test/yaml/sensor_tests/sensor_PO_3D_initial_guess_dynamic.yaml
index ae08f21db..e758e8703 100644
--- a/test/yaml/sensor_base/sensor_PO_3D_initial_guess_dynamic.yaml
+++ b/test/yaml/sensor_tests/sensor_PO_3D_initial_guess_dynamic.yaml
@@ -8,5 +8,15 @@ states:
mode: initial_guess
state: [1, 0, 0, 0]
dynamic: true
+
+
+# used in gtest_sensor_base
noise_p_std: 0.1
-noise_o_std: 0.01
\ No newline at end of file
+noise_o_std: 0.01
+
+# used in gtest_sensor_odom
+k_disp_to_disp: 0.5
+k_disp_to_rot: 0.0
+k_rot_to_rot: 0.8
+min_disp_var: 0.01
+min_rot_var: 0.01
diff --git a/test/yaml/sensor_base/sensor_PO_3D_initial_guess_dynamic_drift.yaml b/test/yaml/sensor_tests/sensor_PO_3D_initial_guess_dynamic_drift.yaml
similarity index 61%
rename from test/yaml/sensor_base/sensor_PO_3D_initial_guess_dynamic_drift.yaml
rename to test/yaml/sensor_tests/sensor_PO_3D_initial_guess_dynamic_drift.yaml
index 2d9773d12..1de0b8d93 100644
--- a/test/yaml/sensor_base/sensor_PO_3D_initial_guess_dynamic_drift.yaml
+++ b/test/yaml/sensor_tests/sensor_PO_3D_initial_guess_dynamic_drift.yaml
@@ -10,5 +10,15 @@ states:
state: [1, 0, 0, 0]
dynamic: true
drift_std: [0.1, 0.2, 0.3]
+
+
+# used in gtest_sensor_base
noise_p_std: 0.1
-noise_o_std: 0.01
\ No newline at end of file
+noise_o_std: 0.01
+
+# used in gtest_sensor_odom
+k_disp_to_disp: 0.5
+k_disp_to_rot: 0.0
+k_rot_to_rot: 0.8
+min_disp_var: 0.01
+min_rot_var: 0.01
diff --git a/test/yaml/sensor_base/sensor_PO_3D_initial_guess_dynamic_drift_wrong.yaml b/test/yaml/sensor_tests/sensor_PO_3D_initial_guess_dynamic_drift_wrong.yaml
similarity index 62%
rename from test/yaml/sensor_base/sensor_PO_3D_initial_guess_dynamic_drift_wrong.yaml
rename to test/yaml/sensor_tests/sensor_PO_3D_initial_guess_dynamic_drift_wrong.yaml
index d9094f9d2..5322cd09f 100644
--- a/test/yaml/sensor_base/sensor_PO_3D_initial_guess_dynamic_drift_wrong.yaml
+++ b/test/yaml/sensor_tests/sensor_PO_3D_initial_guess_dynamic_drift_wrong.yaml
@@ -10,5 +10,15 @@ states:
state: [1, 0, 0, 0]
dynamic: true
drift_std: [0.1, 0.2, 0.3]
+
+
+# used in gtest_sensor_base
noise_p_std: 0.1
-noise_o_std: 0.01
\ No newline at end of file
+noise_o_std: 0.01
+
+# used in gtest_sensor_odom
+k_disp_to_disp: 0.5
+k_disp_to_rot: 0.0
+k_rot_to_rot: 0.8
+min_disp_var: 0.01
+min_rot_var: 0.01
diff --git a/test/yaml/sensor_base/sensor_PO_3D_initial_guess_dynamic_wrong.yaml b/test/yaml/sensor_tests/sensor_PO_3D_initial_guess_dynamic_wrong.yaml
similarity index 56%
rename from test/yaml/sensor_base/sensor_PO_3D_initial_guess_dynamic_wrong.yaml
rename to test/yaml/sensor_tests/sensor_PO_3D_initial_guess_dynamic_wrong.yaml
index ced2cbcf5..873cf7425 100644
--- a/test/yaml/sensor_base/sensor_PO_3D_initial_guess_dynamic_wrong.yaml
+++ b/test/yaml/sensor_tests/sensor_PO_3D_initial_guess_dynamic_wrong.yaml
@@ -8,5 +8,15 @@ states:
mode: initial_guess
state: [1, 0, 0, 0]
dynamic: true
+
+
+# used in gtest_sensor_base
noise_p_std: 0.1
-noise_o_std: 0.01
\ No newline at end of file
+noise_o_std: 0.01
+
+# used in gtest_sensor_odom
+k_disp_to_disp: 0.5
+k_disp_to_rot: 0.0
+k_rot_to_rot: 0.8
+min_disp_var: 0.01
+min_rot_var: 0.01
diff --git a/test/yaml/sensor_base/sensor_PO_3D_initial_guess_wrong.yaml b/test/yaml/sensor_tests/sensor_PO_3D_initial_guess_wrong.yaml
similarity index 56%
rename from test/yaml/sensor_base/sensor_PO_3D_initial_guess_wrong.yaml
rename to test/yaml/sensor_tests/sensor_PO_3D_initial_guess_wrong.yaml
index 842d84d35..5eed19d62 100644
--- a/test/yaml/sensor_base/sensor_PO_3D_initial_guess_wrong.yaml
+++ b/test/yaml/sensor_tests/sensor_PO_3D_initial_guess_wrong.yaml
@@ -8,5 +8,15 @@ states:
# mode: initial_guess
# state: [1, 0, 0, 0]
# dynamic: false
+
+
+# used in gtest_sensor_base
noise_p_std: 0.1
-noise_o_std: 0.01
\ No newline at end of file
+noise_o_std: 0.01
+
+# used in gtest_sensor_odom
+k_disp_to_disp: 0.5
+k_disp_to_rot: 0.0
+k_rot_to_rot: 0.8
+min_disp_var: 0.01
+min_rot_var: 0.01
--
GitLab