diff --git a/CMakeLists.txt b/CMakeLists.txt
index 5d60969f803754dbcaccb4516f04dbcbe535e6d3..0a360182419dd86b64a8377d976c14759e5bed62 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 b2b8b4fb6add295a6196e5992a729f6d96f3b8d2..366d68f47c5f675fba20ad28ee1ccbe6162c3e8c 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 fea39c4d6808a7a798a83d7b8b99dfbb47abec9d..aa9011ea6efc08609182185d1c0e8e2cd6f8ba75 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 02cdbd5ef4480ccd1a0ba28ab0db5c44886fa58c..3008bbc807747525f549cf8a349d73a38b088c0f 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 117245f896f4362bba30c740bd7b5974588d0282..0000000000000000000000000000000000000000
--- 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 f1977980b7187128302dc2ed7425168ca12a0c68..0000000000000000000000000000000000000000
--- 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 e653fe458d39107c143da729a006c727df3959e3..72f5891837bfeb8b1fba1490a76424f8baae951b 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 98f55c0f0d6c5ff9af08d627c1ef4d67b351783e..78ecb0fddf8fce5ecbc50c70bbc2d72ff04a80c6 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 471dd627361266aad20356543345355a5d18bdbf..e96f3a7de719b94da693d72a1aa13482d7852dea 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 cf8191714fc59c19891e107828f51b1baec6b20b..3f9b4901757503b36287962b06f570c38b822431 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 0abef3899bf3ef237139876dafb97cf869c1f5cc..0000000000000000000000000000000000000000
--- 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 b44561ff402bef4971b211cfa2eae43724457fc3..0000000000000000000000000000000000000000
--- 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 87f2880b8f3e58e1898470572e630375bd792d8c..5be1e7473109691139f8ff4e042e45a2779378dd 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 c2bb2e5306e46e51c28cca201b8031669b362d47..915df791c424fa7842f47ff815326ec637068682 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 04d838944dd50139da61d890c0171e0b41a33cb0..8d5caccb0533e14ced1993fa733717362d481485 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 559cb5883146cda4ec6a39694a9ba7ca3c1f300a..75412f792c6907be50aa661f12aecb71a6c2bdb3 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 0000000000000000000000000000000000000000..9416dceb34a31ecdcaaa1d34fe6c557078e292c0
--- /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 f5ee28b608f483f55a72f8c681e5b38a8c0a3b97..0000000000000000000000000000000000000000
--- 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 d37c700a7daf6a45862a4d8b830b8b8e67767282..b62d81e5b75e743824ee3139256eff3d3dcc037f 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 59aec3c70cee0e147e38dbfb9408c8879bc203e2..5a75360a6772fa49c037b16070f60dc9c84d53ca 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 419125468ba5155eba8f0f75c972a5d52b5dbcef..555052c63d6b1aec789f1e1674ed2506e903dd0f 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 4125b1f4a1ef3db7ed874aafbe1969a65330dde6..0000000000000000000000000000000000000000
--- 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 f706700135da85f6ceab0083c42e7736dafe4783..0000000000000000000000000000000000000000
--- 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 b349248945347e66422f20281b9c714eefa2b0ec..0000000000000000000000000000000000000000
--- 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 99c0f477f995c26cc21187dda5c481ff1415d0af..0000000000000000000000000000000000000000
--- 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 6e1111173262fa4a5fc6a61d25e18b8cb335103f..0000000000000000000000000000000000000000
--- 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 89467b4cfd264014428152d4704d0f5ef570d13a..0000000000000000000000000000000000000000
--- 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 16adc66acf10995f74007686e2722bb5a7518f7c..0ab13d73f2b06b324bfa5689eb522d08ddec0251 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 25318d527db4bae12bdfe01e1248a252e72671fc..0000000000000000000000000000000000000000
--- 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 05b7c5cb1904fd45867467585b25b3570959551b..0000000000000000000000000000000000000000
--- 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 76d8a922249aa941d36f4cf31e220501bdd71459..0000000000000000000000000000000000000000
--- 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 b2c4971c1dba5771184623eba88780240229c0ea..0000000000000000000000000000000000000000
--- 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 7095775fa1327b141db927575769124b3dd8880f..0000000000000000000000000000000000000000
--- 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 b8a86c874abd41bb90352c1e5b9a9a6d7c69da0d..0000000000000000000000000000000000000000
--- 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 fd24b509f20858576a94089a123e9272973530bb..0000000000000000000000000000000000000000
--- 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 a4b81b49058b477ed153bd9f6610a6e9cf02d38e..0000000000000000000000000000000000000000
--- 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 0c5beb28c659ec0e5dbd929597fbacfeff40d748..0000000000000000000000000000000000000000
--- 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 ca9746a491bb1a82e32f7d2f0d677a7f600d5d8c..0000000000000000000000000000000000000000
--- 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 d927120f2690d8f1e42ce2cb5dd66e57bbd770bd..0000000000000000000000000000000000000000
--- 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 7d8038b84aeb2abf4acd1f7cc5e53bc6c2cb8be4..0000000000000000000000000000000000000000
--- 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 547c9ea892375464645b6723c5b6e257f2cad0fa..0000000000000000000000000000000000000000
--- 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 693e91bc24ccdb0e1d8ff0a82764cfa64e28d50f..0000000000000000000000000000000000000000
--- 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 49d6a56c2df7915a0704ca9f24fc669dd261bcff..0000000000000000000000000000000000000000
--- 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 4dcf14f132cea51f61daa6f15621857fb1a4b760..0000000000000000000000000000000000000000
--- 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 d6e901058b599ca4db1d8275df37919759c4d428..0000000000000000000000000000000000000000
--- 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 42777646ea7c3a31a2ef78cbfab707bcc77a5a4d..0000000000000000000000000000000000000000
--- 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 7b1023e84d563a3d71f54a76c0ccf63a3b4e5e6d..0000000000000000000000000000000000000000
--- 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 8eb2b235011cb3cfe4f35b1f73da6344991af0da..ea22ca723fbf7fedaf121ded4d041866bc0f65a1 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 16adc66acf10995f74007686e2722bb5a7518f7c..d85c3d247fda7b9f26bff9bc60f7271c2e025c53 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 d3614ede680d2a9346c276077752812a715338b5..f2acfab188d82beef19a8ec507698ac1b038d19b 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 7ec74b1faecd03323b78b47397113019602945a7..397f1d2795b704bfce9f0d8c7078f83200788e0f 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 5782bcc25c61e4a3dd200081b960c822f84f84a3..fab3cf9b19291061da5e1b96f315db02c7f249aa 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 a7a59d8ba28336a87846612296557fb3b7d9dbab..9bfc63bcdf637b1f6081484cd45a657adce38621 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 9bed5e2bfed7ed1b06bca865482ef49e91f9e4ea..ce4287ed5836202b2fe4717dec9007423c497e5b 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 d7b3ecb231ae656b545e31e32ca5d737ddcf0bdb..d4c05d48d052a977b814f2a21454cf8d07d660e2 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 abf288b221ee56363146efc4f65dfabf206851f6..a6787204c54157700a27ce4f051a3404eaa8c9de 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 9ed9a58fa58264bc1db5602ba4e3e6bc88efaa4e..c66c96442b96409021e8dd22199354d574210db3 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 a781d137fae907f7731cb9d8e09d07f9eb5a6fe5..b99691f908e478f3bd0d148a21c7fc9b26773e14 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 0000000000000000000000000000000000000000..e96ee1849a3a5034e05e0ce725d4dcbd7fa98828
--- /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 0000000000000000000000000000000000000000..715da540e07244f4e384e8189dc4b19b68d0d7ef
--- /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 25a40db4275bed61a5ecb5543d723eab08f1acd8..4e668002900941f2d896f323c24220931a0a02c6 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 622158be35117453734590e2b9987bd84e8210e1..0bcee886ca1f46fe9ec042261a498fd81ec13b58 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 d66f2f21c6bf734aee5f8c5030554b77dde43fba..1c9d8167985835870cbd2cc089de68e1006954c2 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 0000000000000000000000000000000000000000..229f89094fb3846ae70aa2bb8a5a5271d5ecd645
--- /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 937ddf2ab0946987fd28cf4e68e53ef9e0b450df..f593fc53c121358f71a7b9f30d7b4d70f1735c3e 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 e979574969acf4eb2f2c861ed2ee127c4fdfb3bd..50705368a30d24e1b72e9c9af72d0f405af9225c 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 49aaaaac83f3451c1fde21276768cca629403507..d5060eeaffbc0aabad00ffe8368a90cb32955cb1 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 cc9e32246bbfc97c6e8739db00399e4a2e4398bc..9a53f8300eba2490c4464822396303e0a56824e5 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 046ede375380deaec7e0a98dfcf570cad341bea2..9b3da538f19f6ed71374af434650e78aae8fe1fd 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 ce2792d2251d9ae705029e36bef3c976f8f42340..65d170426b4b6821e0667ffe196c4ddf6b8304db 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 6084ec5d397a7ca54888c967198b73eee7be53bc..23737df980572e74aa0c84e85c5cd29256d5e05a 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 ab974d37a2abef05b685f7bb918ee2bdf29431a2..8c9342ab801b23a97cb68dd74e9577d57858e568 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 65940ecb858217588bdec4c0a8ba61ea79f05033..9f7e14d047a4ac2b9e2d4f783d088edd97005e35 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 f00748d655526a3f185f8ad91cfd85d699ee3b81..a4980b27d66a48f1573013cb052d6ebc7f0f56fe 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 095862ddc7758660387d5e69e0e9ce5966deeb38..104237208910962c2753cd1a28305c89f5c10821 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 ba143662c6d832f0a4096e9d39d21575b1258baf..d99e580743dea4589ce17618bc99024c68cd6bf1 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 0000000000000000000000000000000000000000..44eef97f1280399a7fe500f6695aef5b171d0143
--- /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 0329cd837b057de960640354da2baf38979f5a7e..606751a7ba35e7faa66c1752244c05c55a8c4e4d 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 3c2bedefe52348215f84e8c4f5bdb16eec9078d8..f6166c91b00ab312bc72fa54e1e117553ce8ee2d 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 47286195af2d766d52506dc0ab45e35e90c52005..17467074e5baef59a958cdf2c7c1b438de2ef371 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 c5f6ed7a6e4075b845fae7f4627fa75714aa2291..4e6da9d215eaab580db71827212cfab78f19db28 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 0000000000000000000000000000000000000000..acc56356647c3791c3745520f8218bf0f005cd89
--- /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 bb396634cc3bd5e72899710f502e2ecf1a3c176e..7b5e1398db21a1fb70147ddf976a1f0c801a4e24 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 ae08f21dbb73fa9992d61940b5699c59d36ac0b8..e758e87035caac6b14b72ac9baf4e7fef054ed0c 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 2d9773d128d776ef8606cf93901eee2ae91a6ef8..1de0b8d93e365ceb6584a2bf2eaedef2e764171f 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 d9094f9d2b2ce72da62436efaca27e8b635bc21f..5322cd09f7b22c20c7acb43d7fe45c00495e8ee6 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 ced2cbcf5fd3a803b6d0393b002a43a6f9a2fbe6..873cf74259327908d2840c070d087a53fc8778cd 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 842d84d35b0c6a2eaf80136865aa9ce0823c18c1..5eed19d6210ca7729c35e813edf94c2b23dd26ba 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