From cbe29459c473db92da67683f14a7d05d8b638669 Mon Sep 17 00:00:00 2001
From: jvallve <jvallve@iri.upc.edu>
Date: Wed, 6 Jul 2022 18:36:12 +0200
Subject: [PATCH] [skip ci] more tests passing
---
CMakeLists.txt | 2 -
include/core/sensor/factory_sensor.h | 3 +-
include/core/sensor/sensor_base.h | 66 +++++++------
include/core/sensor/sensor_diff_drive.h | 9 +-
include/core/sensor/sensor_motion_model.h | 3 +-
include/core/sensor/sensor_odom.h | 91 ++++++++++++++++--
include/core/sensor/sensor_pose.h | 61 +++++++++---
schema/PriorModeDynamicNoiseDrift.schema | 26 ------
schema/PriorO2d.schema | 4 +-
schema/PriorO3d.schema | 4 +-
schema/PriorP2d.schema | 4 +-
schema/PriorP3d.schema | 4 +-
schema/sensor/SensorDiffDrive.schema | 36 ++++++++
schema/sensor/SensorOdom2d.schema | 6 ++
schema/sensor/SensorOdom3d.schema | 4 +-
src/problem/problem.cpp | 6 +-
src/processor/processor_odom_3d.cpp | 4 +-
src/processor/processor_pose.cpp | 2 +-
src/sensor/sensor_base.cpp | 32 +++----
src/sensor/sensor_diff_drive.cpp | 17 +---
src/sensor/sensor_motion_model.cpp | 5 +-
src/sensor/sensor_odom.cpp | 90 ------------------
src/sensor/sensor_pose.cpp | 71 --------------
src/state_block/prior.cpp | 5 -
test/dummy/SensorDummyPoia3d.schema | 4 +-
test/dummy/sensor_dummy.h | 34 ++++---
test/gtest_capture_base.cpp | 8 +-
test/gtest_frame_base.cpp | 6 +-
test/gtest_prior.cpp | 4 +-
test/gtest_processor_motion.cpp | 4 +-
test/gtest_sensor_base.cpp | 107 ++++++++++++----------
test/yaml/sensor_diff_drive_dynamic.yaml | 1 +
32 files changed, 334 insertions(+), 389 deletions(-)
delete mode 100644 schema/PriorModeDynamicNoiseDrift.schema
create mode 100644 schema/sensor/SensorDiffDrive.schema
delete mode 100644 src/sensor/sensor_odom.cpp
delete mode 100644 src/sensor/sensor_pose.cpp
diff --git a/CMakeLists.txt b/CMakeLists.txt
index df643606e..8d1762036 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -302,8 +302,6 @@ SET(SRCS_SENSOR
src/sensor/sensor_base.cpp
src/sensor/sensor_diff_drive.cpp
src/sensor/sensor_motion_model.cpp
- src/sensor/sensor_odom.cpp
- src/sensor/sensor_pose.cpp
)
SET(SRCS_SOLVER
src/solver/solver_manager.cpp
diff --git a/include/core/sensor/factory_sensor.h b/include/core/sensor/factory_sensor.h
index 976097d45..745b2d839 100644
--- a/include/core/sensor/factory_sensor.h
+++ b/include/core/sensor/factory_sensor.h
@@ -210,11 +210,10 @@ namespace wolf
*/
typedef Factory<SensorBase,
- SizeEigen,
const YAML::Node&> FactorySensor;
typedef Factory<SensorBase,
- SizeEigen,
+ const std::string&,
const std::string&,
const std::vector<std::string>&> FactorySensorYaml;
diff --git a/include/core/sensor/sensor_base.h b/include/core/sensor/sensor_base.h
index cc1f5d408..d849b3fae 100644
--- a/include/core/sensor/sensor_base.h
+++ b/include/core/sensor/sensor_base.h
@@ -59,33 +59,31 @@ namespace wolf {
* Also, there should be the schema file 'SensorClass.schema' containing the specifications
* of the user input yaml file.
*/
-#define WOLF_SENSOR_CREATE(SensorClass, ParamsSensorClass) \
-static SensorBasePtr create(SizeEigen _dim, \
- const YAML::Node& _input_node) \
-{ \
- auto params = std::make_shared<ParamsSensorClass>(_input_node); \
- \
- auto priors = Priors(_input_node["states"]); \
- \
- return std::make_shared<SensorClass>(_dim, params, priors); \
-} \
-static SensorBasePtr create(SizeEigen _dim, \
- const std::string& _yaml_filepath, \
- const std::vector<std::string>& _folders_schema) \
-{ \
- auto server = yaml_schema_cpp::YamlServer(_folders_schema, _yaml_filepath); \
- \
- if (not server.applySchema(#SensorClass)) \
- { \
- WOLF_ERROR(server.getLog().str()); \
- return nullptr; \
- } \
- auto params = std::make_shared<ParamsSensorClass>(server.getNode()); \
- \
- auto priors = Priors(server.getNode()["states"]); \
- \
- return std::make_shared<SensorClass>(_dim, params, priors); \
-} \
+#define WOLF_SENSOR_CREATE(SensorClass, ParamsSensorClass) \
+static SensorBasePtr create(const YAML::Node& _input_node) \
+{ \
+ auto params = std::make_shared<ParamsSensorClass>(_input_node); \
+ \
+ auto priors = Priors(_input_node["states"]); \
+ \
+ return std::make_shared<SensorClass>(params, priors); \
+} \
+static SensorBasePtr create(const std::string& _schema, \
+ const std::string& _yaml_filepath, \
+ const std::vector<std::string>& _folders_schema) \
+{ \
+ auto server = yaml_schema_cpp::YamlServer(_folders_schema, _yaml_filepath); \
+ \
+ if (not server.applySchema(_schema)) \
+ { \
+ throw std::runtime_error(server.getLog().str()); \
+ } \
+ auto params = std::make_shared<ParamsSensorClass>(server.getNode()); \
+ \
+ auto priors = Priors(server.getNode()["states"]); \
+ \
+ return std::make_shared<SensorClass>(params, priors); \
+} \
/** \brief base struct for intrinsic sensor parameters
*
@@ -134,8 +132,10 @@ class SensorBase : public NodeBase, public HasStateBlocks, public std::enable_sh
protected:
std::map<char, Eigen::MatrixXd> drift_covs_; // covariance of the drift of dynamic state blocks [unit²/s]
+ int dim_; ///< Dimension compatibility of the sensor: 2: 2D, 3: 3D, -1: both
+
void setProblem(ProblemPtr _problem) override final;
- virtual void loadPriors(const Priors& _priors, SizeEigen _dim);
+ virtual void loadPriors(const Priors& _priors);
public:
@@ -143,19 +143,20 @@ class SensorBase : public NodeBase, public HasStateBlocks, public std::enable_sh
*
* Constructor with parameter vector
* \param _tp Type of the sensor (types defined at wolf.h)
- * \param _dim Problem dimension
+ * \param _dim Which dimension is the sensor compatible (2: 2D, 3: 3D, -1: both)
* \param _params params struct
* \param _priors priors of the sensor state blocks
*
**/
SensorBase(const std::string& _type,
- const SizeEigen& _dim,
+ const int& _dim,
ParamsSensorBasePtr _params,
const Priors& _priors);
~SensorBase() override;
unsigned int id() const;
+ int dim() const;
HardwareBaseConstPtr getHardware() const;
HardwareBasePtr getHardware();
@@ -302,6 +303,11 @@ inline unsigned int SensorBase::id() const
return sensor_id_;
}
+inline int SensorBase::dim() const
+{
+ return dim_;
+}
+
inline ProcessorBaseConstPtrList SensorBase::getProcessorList() const
{
ProcessorBaseConstPtrList list_const;
diff --git a/include/core/sensor/sensor_diff_drive.h b/include/core/sensor/sensor_diff_drive.h
index 97f6e451a..c516eaf01 100644
--- a/include/core/sensor/sensor_diff_drive.h
+++ b/include/core/sensor/sensor_diff_drive.h
@@ -54,14 +54,11 @@ WOLF_PTR_TYPEDEFS(SensorDiffDrive);
class SensorDiffDrive : public SensorBase
{
public:
-
-
- SensorDiffDrive(const SizeEigen& _dim,
- ParamsSensorDiffDrivePtr _params,
+ SensorDiffDrive(ParamsSensorDiffDrivePtr _params,
const Priors& _priors);
-
WOLF_SENSOR_CREATE(SensorDiffDrive, ParamsSensorDiffDrive);
- ~SensorDiffDrive() override;
+
+ ~SensorDiffDrive() override = default;
ParamsSensorDiffDriveConstPtr getParams() const;
ParamsSensorDiffDrivePtr getParams();
diff --git a/include/core/sensor/sensor_motion_model.h b/include/core/sensor/sensor_motion_model.h
index 3b6c56eff..4d68fb6df 100644
--- a/include/core/sensor/sensor_motion_model.h
+++ b/include/core/sensor/sensor_motion_model.h
@@ -31,8 +31,7 @@ WOLF_PTR_TYPEDEFS(SensorMotionModel);
class SensorMotionModel : public SensorBase
{
public:
- SensorMotionModel(const SizeEigen& _dim,
- ParamsSensorBasePtr _params,
+ SensorMotionModel(ParamsSensorBasePtr _params,
const Priors& _priors);
WOLF_SENSOR_CREATE(SensorMotionModel, ParamsSensorBase);
diff --git a/include/core/sensor/sensor_odom.h b/include/core/sensor/sensor_odom.h
index 6cb43d327..1ed6e3c87 100644
--- a/include/core/sensor/sensor_odom.h
+++ b/include/core/sensor/sensor_odom.h
@@ -59,17 +59,25 @@ struct ParamsSensorOdom : public ParamsSensorBase
}
};
-WOLF_PTR_TYPEDEFS(SensorOdom);
+template <unsigned int DIM>
class SensorOdom : public SensorBase
{
protected:
- SizeEigen dim_;
ParamsSensorOdomPtr params_odom_;
public:
- SensorOdom(const SizeEigen& _dim, ParamsSensorOdomPtr _params, const Priors& _priors);
- WOLF_SENSOR_CREATE(SensorOdom, ParamsSensorOdom);
+ SensorOdom(ParamsSensorOdomPtr _params,
+ const Priors& _priors) :
+ SensorBase("SensorOdom"+toString(DIM)+"d",
+ DIM,
+ _params,
+ _priors),
+ params_odom_(_params)
+ {
+ static_assert(DIM == 2 or DIM == 3);
+ }
+ WOLF_SENSOR_CREATE(SensorOdom<DIM>, ParamsSensorOdom);
~SensorOdom() override = default;
@@ -83,29 +91,92 @@ class SensorOdom : public SensorBase
};
-inline double SensorOdom::getDispVarToDispNoiseFactor() const
+template <unsigned int DIM>
+inline double SensorOdom<DIM>::getDispVarToDispNoiseFactor() const
{
return params_odom_->k_disp_to_disp;
}
-inline double SensorOdom::getDispVarToRotNoiseFactor() const
+template <unsigned int DIM>
+inline double SensorOdom<DIM>::getDispVarToRotNoiseFactor() const
{
return params_odom_->k_disp_to_rot;
}
-inline double SensorOdom::getRotVarToRotNoiseFactor() const
+template <unsigned int DIM>
+inline double SensorOdom<DIM>::getRotVarToRotNoiseFactor() const
{
return params_odom_->k_rot_to_rot;
}
-inline double SensorOdom::getMinDispVar() const
+template <unsigned int DIM>
+inline double SensorOdom<DIM>::getMinDispVar() const
{
return params_odom_->min_disp_var;
}
-inline double SensorOdom::getMinRotVar() const
+template <unsigned int DIM>
+inline double SensorOdom<DIM>::getMinRotVar() const
{
return params_odom_->min_rot_var;
}
-} // namespace wolf
\ No newline at end of file
+template <unsigned int DIM>
+inline Eigen::MatrixXd SensorOdom<DIM>::computeNoiseCov(const Eigen::VectorXd & _data) const
+{
+ double d; // displacement
+ double r; // rotation
+
+ // 2D
+ if (DIM ==2)
+ {
+ assert(_data.size() == 2 or _data.size() == 3);
+
+ // data = [forward_x, rotation_z] 2D
+ if (_data.size() == 2)
+ {
+ d = fabs(_data(0));
+ r = fabs(_data(1));
+ }
+ else
+ {
+ d = _data.head<2>().norm();
+ r = fabs(_data(2));
+ }
+ }
+ // 3D
+ else
+ {
+ assert(_data.size() == 6 or _data.size() == 7);
+
+ d = _data.head<3>().norm();
+ if (_data.size() == 6)
+ r = _data.tail<3>().norm();
+ else
+ r = 2 * acos(_data(6)); // arc cos of real part of quaternion
+ }
+
+ // variances
+ double dvar = std::max(params_odom_->min_disp_var, params_odom_->k_disp_to_disp * d);
+ double rvar = std::max(params_odom_->min_rot_var, params_odom_->k_disp_to_rot * d + params_odom_->k_rot_to_rot * r);
+
+ // return
+ if (DIM == 2)
+ return (Vector2d() << dvar, rvar).finished().asDiagonal();
+ else
+ return (Vector6d() << dvar, dvar, dvar, rvar, rvar, rvar).finished().asDiagonal();
+}
+
+typedef SensorOdom<2> SensorOdom2d;
+typedef SensorOdom<3> SensorOdom3d;
+WOLF_DECLARED_PTR_TYPEDEFS(SensorOdom2d);
+WOLF_DECLARED_PTR_TYPEDEFS(SensorOdom3d);
+
+} /* namespace wolf */
+
+// Register in the FactorySensor
+#include "core/sensor/factory_sensor.h"
+namespace wolf {
+WOLF_REGISTER_SENSOR(SensorOdom2d);
+WOLF_REGISTER_SENSOR(SensorOdom3d);
+}
\ No newline at end of file
diff --git a/include/core/sensor/sensor_pose.h b/include/core/sensor/sensor_pose.h
index c5ecbc516..abf7e9e11 100644
--- a/include/core/sensor/sensor_pose.h
+++ b/include/core/sensor/sensor_pose.h
@@ -49,38 +49,73 @@ struct ParamsSensorPose : public ParamsSensorBase
}
};
-WOLF_PTR_TYPEDEFS(SensorPose);
-
+template <unsigned int DIM>
class SensorPose : public SensorBase
{
protected:
- SizeEigen dim_;
ParamsSensorPosePtr params_pose_;
public:
- SensorPose(const SizeEigen& _dim,
- ParamsSensorPosePtr _params,
- const Priors& _priors);
-
- WOLF_SENSOR_CREATE(SensorPose, ParamsSensorPose);
+ SensorPose(ParamsSensorPosePtr _params,
+ const Priors& _priors) :
+ SensorBase("SensorPose"+toString(DIM)+"d",
+ DIM,
+ _params,
+ _priors),
+ params_pose_(_params)
+ {
+ static_assert(DIM == 2 or DIM == 3);
+ }
+ WOLF_SENSOR_CREATE(SensorPose<DIM>, ParamsSensorPose);
- ~SensorPose() override;
+ ~SensorPose() override = default;
double getStdP() const;
double getStdO() const;
Eigen::MatrixXd computeNoiseCov(const Eigen::VectorXd & _data) const override;
-
};
-inline double SensorPose::getStdP() const
+template <unsigned int DIM>
+inline double SensorPose<DIM>::getStdP() const
{
return params_pose_->std_p;
}
-inline double SensorPose::getStdO() const
+template <unsigned int DIM>
+inline double SensorPose<DIM>::getStdO() const
{
return params_pose_->std_o;
}
-} /* namespace wolf */
\ No newline at end of file
+template <>
+inline Eigen::MatrixXd SensorPose<2>::computeNoiseCov(const Eigen::VectorXd & _data) const
+{
+ return (Eigen::Vector3d() << params_pose_->std_p,
+ params_pose_->std_p,
+ params_pose_->std_o).finished().cwiseAbs2().asDiagonal();
+}
+template <>
+inline Eigen::MatrixXd SensorPose<3>::computeNoiseCov(const Eigen::VectorXd & _data) const
+{
+ 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();
+}
+
+typedef SensorPose<2> SensorPose2d;
+typedef SensorPose<3> SensorPose3d;
+WOLF_DECLARED_PTR_TYPEDEFS(SensorPose2d);
+WOLF_DECLARED_PTR_TYPEDEFS(SensorPose3d);
+
+} /* namespace wolf */
+
+// Register in the FactorySensor
+#include "core/sensor/factory_sensor.h"
+namespace wolf {
+WOLF_REGISTER_SENSOR(SensorPose2d);
+WOLF_REGISTER_SENSOR(SensorPose3d);
+}
\ No newline at end of file
diff --git a/schema/PriorModeDynamicNoiseDrift.schema b/schema/PriorModeDynamicNoiseDrift.schema
deleted file mode 100644
index 4c14a5652..000000000
--- a/schema/PriorModeDynamicNoiseDrift.schema
+++ /dev/null
@@ -1,26 +0,0 @@
-mode:
- type: string
- yaml_type: scalar
- mandatory: true
- options:
- - "fix"
- - "factor"
- - "initial_guess"
- doc: The prior mode can be 'factor' to add an absolute factor (requires 'noise_std'), 'fix' to set the values constant or 'initial_guess' to just set the values
-dynamic:
- type: bool
- yaml_type: scalar
- mandatory: true
- doc: If the state is dynamic, i.e. it changes along time.
-noise_std:
- type: VectorXd
- yaml_type: scalar
- mandatory: false
- default: []
- doc: A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix.
-drift_std:
- type: VectorXd
- yaml_type: scalar
- mandatory: false
- default: []
- doc: A vector containing the stdev values of the noise of the drift factor (only if dynamic==true), i.e. the sqrt of the diagonal elements of the covariance matrix.
\ No newline at end of file
diff --git a/schema/PriorO2d.schema b/schema/PriorO2d.schema
index 402d0c37e..c5afbd849 100644
--- a/schema/PriorO2d.schema
+++ b/schema/PriorO2d.schema
@@ -1,3 +1,4 @@
+follow: Prior.schema
type:
type: string
yaml_type: scalar
@@ -9,5 +10,4 @@ state:
type: Vector1d
yaml_type: scalar
mandatory: true
- doc: A vector containing the state values
-follow: PriorModeDynamicNoiseDrift.schema
\ No newline at end of file
+ doc: A vector containing the state values
\ No newline at end of file
diff --git a/schema/PriorO3d.schema b/schema/PriorO3d.schema
index e7b1dd38b..c819039b2 100644
--- a/schema/PriorO3d.schema
+++ b/schema/PriorO3d.schema
@@ -1,3 +1,4 @@
+follow: Prior.schema
type:
type: string
yaml_type: scalar
@@ -9,5 +10,4 @@ state:
type: Vector4d
yaml_type: scalar
mandatory: true
- doc: A vector containing the state values. It should be a quaternion (i.e. four values and normalized)
-follow: PriorModeDynamicNoiseDrift.schema
\ No newline at end of file
+ doc: A vector containing the state values. It should be a quaternion (i.e. four values and normalized)
\ No newline at end of file
diff --git a/schema/PriorP2d.schema b/schema/PriorP2d.schema
index d8f19390f..ef3331992 100644
--- a/schema/PriorP2d.schema
+++ b/schema/PriorP2d.schema
@@ -1,3 +1,4 @@
+follow: Prior.schema
type:
type: string
yaml_type: scalar
@@ -9,5 +10,4 @@ state:
type: Vector2d
yaml_type: scalar
mandatory: true
- doc: A vector containing the state values
-follow: PriorModeDynamicNoiseDrift.schema
\ No newline at end of file
+ doc: A vector containing the state values
\ No newline at end of file
diff --git a/schema/PriorP3d.schema b/schema/PriorP3d.schema
index d413a9029..de70f20a0 100644
--- a/schema/PriorP3d.schema
+++ b/schema/PriorP3d.schema
@@ -1,3 +1,4 @@
+follow: Prior.schema
type:
type: string
yaml_type: scalar
@@ -9,5 +10,4 @@ state:
type: Vector3d
yaml_type: scalar
mandatory: true
- doc: A vector containing the state 'P' values
-follow: PriorModeDynamicNoiseDrift.schema
\ No newline at end of file
+ doc: A vector containing the state 'P' values
\ No newline at end of file
diff --git a/schema/sensor/SensorDiffDrive.schema b/schema/sensor/SensorDiffDrive.schema
new file mode 100644
index 000000000..e42352f97
--- /dev/null
+++ b/schema/sensor/SensorDiffDrive.schema
@@ -0,0 +1,36 @@
+follow: SensorBase.schema
+ticks_per_wheel_revolution:
+ mandatory: true
+ type: double
+ yaml_type: scalar
+ doc: ratio of displacement variance to displacement, for odometry noise calculation.
+ticks_std_factor:
+ mandatory: true
+ type: double
+ yaml_type: scalar
+ doc: ratio of displacement variance to rotation, for odometry noise calculation.
+
+states:
+ P:
+ follow: PriorP2d.schema
+ O:
+ follow: PriorO2d.schema
+ I:
+ follow: Prior.schema
+ type:
+ type: string
+ yaml_type: scalar
+ mandatory: false
+ default: StateParam3
+ options: [StateParam3]
+ doc: The type of the SensorDiffDrive intrinsic parameters is StateParam3.
+ state:
+ type: Vector3d
+ yaml_type: scalar
+ mandatory: true
+ doc: A vector containing the intrinsic state values. 0=left wheel radius (m), 1=right wheel radius (m), 2=wheel separation (m)
+
+
+
+
+
diff --git a/schema/sensor/SensorOdom2d.schema b/schema/sensor/SensorOdom2d.schema
index bb88b9ec5..bd4574a7a 100644
--- a/schema/sensor/SensorOdom2d.schema
+++ b/schema/sensor/SensorOdom2d.schema
@@ -25,3 +25,9 @@ min_rot_var:
yaml_type: scalar
doc: minimum rotation variance, for odometry noise calculation.
+states:
+ P:
+ follow: PriorP2d.schema
+ O:
+ follow: PriorO2d.schema
+
diff --git a/schema/sensor/SensorOdom3d.schema b/schema/sensor/SensorOdom3d.schema
index 1c1f877db..36bc601c9 100644
--- a/schema/sensor/SensorOdom3d.schema
+++ b/schema/sensor/SensorOdom3d.schema
@@ -27,6 +27,6 @@ min_rot_var:
states:
P:
- follow: Prior.schema
+ follow: PriorP3d.schema
O:
- follow: Prior.schema
\ No newline at end of file
+ follow: PriorO3d.schema
\ No newline at end of file
diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp
index 0442805fe..6f1ace470 100644
--- a/src/problem/problem.cpp
+++ b/src/problem/problem.cpp
@@ -238,7 +238,7 @@ Problem::~Problem()
SensorBasePtr Problem::installSensor(const YAML::Node& _sensor_node)
{
- SensorBasePtr sen_ptr = FactorySensor::create(_sensor_node["type"].as<std::string>(), dim_, _sensor_node);
+ SensorBasePtr sen_ptr = FactorySensor::create(_sensor_node["type"].as<std::string>(), _sensor_node);
sen_ptr->link(getHardware());
return sen_ptr;
}
@@ -251,8 +251,8 @@ SensorBasePtr Problem::installSensor(const std::string& _sen_type,
{
throw std::runtime_error("Cannot install sensor: provided params YAML file: '" + _params_yaml_filename + "' does not exist.");
}
- SensorBasePtr sen_ptr = FactorySensorYaml::create(_sen_type,
- dim_,
+ SensorBasePtr sen_ptr = FactorySensorYaml::create(_sen_type,
+ _sen_type,
_params_yaml_filename,
_folders_schema);
sen_ptr->link(getHardware());
diff --git a/src/processor/processor_odom_3d.cpp b/src/processor/processor_odom_3d.cpp
index da78f58b3..f6eb7c2d3 100644
--- a/src/processor/processor_odom_3d.cpp
+++ b/src/processor/processor_odom_3d.cpp
@@ -39,8 +39,8 @@ ProcessorOdom3d::~ProcessorOdom3d()
void ProcessorOdom3d::configure(SensorBasePtr _sensor)
{
- if (not std::dynamic_pointer_cast<SensorOdom>(_sensor))
- throw std::runtime_error("Configuring ProcessorOdom3d: provided sensor is null or not of type 'SensorOdom'");
+ if (not std::dynamic_pointer_cast<SensorOdom3d>(_sensor))
+ throw std::runtime_error("Configuring ProcessorOdom3d: provided sensor is null or not of type 'SensorOdom3d'");
}
void ProcessorOdom3d::computeCurrentDelta(const Eigen::VectorXd& _data,
diff --git a/src/processor/processor_pose.cpp b/src/processor/processor_pose.cpp
index a9f53bf72..1cc44ea0d 100644
--- a/src/processor/processor_pose.cpp
+++ b/src/processor/processor_pose.cpp
@@ -44,7 +44,7 @@ void ProcessorPose::configure(SensorBasePtr _sensor)
}
void ProcessorPose::createFactorIfNecessary(){
- auto sensor_pose = std::static_pointer_cast<SensorPose>(getSensor());
+ auto sensor_pose = std::static_pointer_cast<SensorPose3d>(getSensor());
auto kf_it_last = buffer_frame_.getContainer().end();
auto kf_it = buffer_frame_.getContainer().begin();
while (kf_it != buffer_frame_.getContainer().end())
diff --git a/src/sensor/sensor_base.cpp b/src/sensor/sensor_base.cpp
index d6ad0ad51..db7c52199 100644
--- a/src/sensor/sensor_base.cpp
+++ b/src/sensor/sensor_base.cpp
@@ -33,7 +33,7 @@ namespace wolf {
unsigned int SensorBase::sensor_id_count_ = 0;
SensorBase::SensorBase(const std::string& _type,
- const SizeEigen& _dim,
+ const int& _dim,
ParamsSensorBasePtr _params,
const Priors& _priors) :
NodeBase("SENSOR", _type, _params->name),
@@ -43,12 +43,11 @@ SensorBase::SensorBase(const std::string& _type,
params_(_params),
state_block_dynamic_(),
params_prior_map_(),
- last_capture_(nullptr)
+ last_capture_(nullptr),
+ dim_(_dim)
{
- assert(_dim == 2 or _dim == 3);
-
// load priors
- loadPriors(_priors, _dim);
+ loadPriors(_priors);
}
SensorBase::~SensorBase()
@@ -57,11 +56,8 @@ SensorBase::~SensorBase()
removeStateBlocks(getProblem());
}
-void SensorBase::loadPriors(const Priors& _priors,
- SizeEigen _dim)
+void SensorBase::loadPriors(const Priors& _priors)
{
- assert(_dim == 2 or _dim == 3);
-
// Iterate all keys in _priors
for (auto state_pair : _priors)
{
@@ -69,18 +65,12 @@ void SensorBase::loadPriors(const Priors& _priors,
auto prior = state_pair.second;
// type
- if (key == 'P' and _dim == 2 and prior.getType() != "P" and prior.getType() != "StatePoint2d")
- throw std::runtime_error("Prior type for key P has to be 'P' or 'StatePoint2d'");
- if (key == 'P' and _dim == 3 and prior.getType() != "P" and prior.getType() != "StatePoint3d")
- throw std::runtime_error("Prior type for key P has to be 'P' or 'StatePoint3d'");
- if (key == 'V' and _dim == 2 and prior.getType() != "V" and prior.getType() != "StatePoint2d")
- throw std::runtime_error("Prior type for key V has to be 'V' or 'StatePoint2d'");
- if (key == 'V' and _dim == 3 and prior.getType() != "V" and prior.getType() != "StatePoint3d")
- throw std::runtime_error("Prior type for key V has to be 'V' or 'StatePoint3d'");
- if (key == 'O' and _dim == 2 and prior.getType() != "O" and prior.getType() != "StateAngle")
- throw std::runtime_error("Prior type for key O in 2D has to be 'O' or 'StateAngle'");
- if (key == 'O' and _dim == 3 and prior.getType() != "O" and prior.getType() != "StateQuaternion")
- throw std::runtime_error("Prior type for key O in 3D has to be 'O' or 'StateQuaternion'");
+ if (key == 'P' and prior.getType() != "P" and prior.getType() != "StatePoint2d" and prior.getType() != "StatePoint3d")
+ throw std::runtime_error("Prior type for key P has to be 'P', 'StatePoint2d' or 'StatePoint3d'");
+ if (key == 'V' and prior.getType() != "V" and prior.getType() != "StateVector2d" and prior.getType() != "StateVector3d")
+ throw std::runtime_error("Prior type for key V has to be 'V' 'StateVector2d' or 'StateVector3d'");
+ if (key == 'O' and prior.getType() != "O" and prior.getType() != "StateAngle" and prior.getType() != "StateQuaternion")
+ throw std::runtime_error("Prior type for key O in 3D has to be 'O', 'StateAngle' or 'StateQuaternion'");
// --- CREATE STATE BLOCK ---
auto sb = FactoryStateBlock::create(prior.getType(), prior.getState(), prior.isFixed());
diff --git a/src/sensor/sensor_diff_drive.cpp b/src/sensor/sensor_diff_drive.cpp
index 89151dd40..514769f5b 100644
--- a/src/sensor/sensor_diff_drive.cpp
+++ b/src/sensor/sensor_diff_drive.cpp
@@ -32,26 +32,13 @@
namespace wolf
{
-// SpecStates specs_states_diff_drive({{'I',SpecState("StateBlock",
-// 3,
-// "0: left wheel radius (m), 1: right wheel radius (m), 2: wheel separation (m)")}});
-
-SensorDiffDrive::SensorDiffDrive(const SizeEigen& _dim,
- ParamsSensorDiffDrivePtr _params,
+SensorDiffDrive::SensorDiffDrive(ParamsSensorDiffDrivePtr _params,
const Priors& _priors) :
SensorBase("SensorDiffDrive",
- _dim,
+ 2,
_params,
_priors),
params_diff_drive_(_params)
-{
- assert(_dim == 2 and "SensorDiffDrive only defined for 2D problems");
-
- if (this->getStateBlockDynamic('I')->getState().size() != 3)
- throw std::runtime_error("SensorDiffDrive in constructor, provided state for 'I' of wrong size. Is should be 3");
-}
-
-SensorDiffDrive::~SensorDiffDrive()
{
}
diff --git a/src/sensor/sensor_motion_model.cpp b/src/sensor/sensor_motion_model.cpp
index a3711abd5..a567d2c4f 100644
--- a/src/sensor/sensor_motion_model.cpp
+++ b/src/sensor/sensor_motion_model.cpp
@@ -23,11 +23,10 @@
namespace wolf {
-SensorMotionModel::SensorMotionModel(const SizeEigen& _dim,
- ParamsSensorBasePtr _params,
+SensorMotionModel::SensorMotionModel(ParamsSensorBasePtr _params,
const Priors& _priors) :
SensorBase("SensorMotionModel",
- _dim,
+ -1,
_params,
_priors)
{
diff --git a/src/sensor/sensor_odom.cpp b/src/sensor/sensor_odom.cpp
deleted file mode 100644
index c3ac836ca..000000000
--- a/src/sensor/sensor_odom.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.h"
-
-namespace wolf {
-
-SensorOdom::SensorOdom(const SizeEigen& _dim,
- ParamsSensorOdomPtr _params,
- const Priors& _priors) :
- SensorBase("SensorOdom",
- _dim,
- _params,
- _priors),
- dim_(_dim),
- params_odom_(_params)
-{
-}
-
-Eigen::MatrixXd SensorOdom::computeNoiseCov(const Eigen::VectorXd & _data) const
-{
- double d; // displacement
- double r; // rotation
-
- // 2D
- if (dim_ ==2)
- {
- assert(_data.size() == 2 or _data.size() == 3);
-
- // data = [forward_x, rotation_z] 2D
- if (_data.size() == 2)
- {
- d = fabs(_data(0));
- r = fabs(_data(1));
- }
- else
- {
- d = _data.head<2>().norm();
- r = fabs(_data(2));
- }
- }
- // 3D
- else
- {
- assert(_data.size() == 6 or _data.size() == 7);
-
- d = _data.head<3>().norm();
- if (_data.size() == 6)
- r = _data.tail<3>().norm();
- else
- r = 2 * acos(_data(6)); // arc cos of real part of quaternion
- }
-
- // variances
- double dvar = std::max(params_odom_->min_disp_var, params_odom_->k_disp_to_disp * d);
- double rvar = std::max(params_odom_->min_rot_var, params_odom_->k_disp_to_rot * d + params_odom_->k_rot_to_rot * r);
-
- // return
- if (dim_ == 2)
- return (Vector2d() << dvar, rvar).finished().asDiagonal();
- else
- return (Vector6d() << dvar, dvar, dvar, rvar, rvar, rvar).finished().asDiagonal();
-}
-
-}
-
-// Register in the FactorySensor
-#include "core/sensor/factory_sensor.h"
-namespace wolf {
-WOLF_REGISTER_SENSOR_WITH_KEY(SensorOdom2d, SensorOdom);
-WOLF_REGISTER_SENSOR_WITH_KEY(SensorOdom3d, SensorOdom);
-} // namespace wolf
diff --git a/src/sensor/sensor_pose.cpp b/src/sensor/sensor_pose.cpp
deleted file mode 100644
index 7af50dc3e..000000000
--- a/src/sensor/sensor_pose.cpp
+++ /dev/null
@@ -1,71 +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_pose.cpp
- *
- * Created on: Feb 18, 2020
- * \author: mfourmy
- */
-
-#include "core/sensor/sensor_pose.h"
-
-namespace wolf {
-
-SensorPose::SensorPose(const SizeEigen& _dim,
- ParamsSensorPosePtr _params,
- const Priors& _priors) :
- SensorBase("SensorPose",
- _dim,
- _params,
- _priors),
- dim_(_dim),
- params_pose_(_params)
-{
-}
-
-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
-
-// Register in the FactorySensor
-#include "core/sensor/factory_sensor.h"
-namespace wolf {
-WOLF_REGISTER_SENSOR(SensorPose);
-}
diff --git a/src/state_block/prior.cpp b/src/state_block/prior.cpp
index 781d5ebe3..eabd2ef18 100644
--- a/src/state_block/prior.cpp
+++ b/src/state_block/prior.cpp
@@ -29,7 +29,6 @@ namespace wolf
Priors::Priors(const YAML::Node& priors_node)
{
- WOLF_INFO("Priors...");
if (not priors_node.IsMap())
{
throw std::runtime_error("Priors: constructor with a non-map yaml node");
@@ -38,7 +37,6 @@ Priors::Priors(const YAML::Node& priors_node)
{
this->emplace(prior_pair.first.as<char>(), Prior(prior_pair.second));
}
- WOLF_INFO("Exiting priors constructor");
}
Prior::Prior(const std::string& _type,
@@ -54,7 +52,6 @@ Prior::Prior(const std::string& _type,
Prior::Prior(const YAML::Node& prior_node)
{
- WOLF_INFO("Prior...\n", prior_node);
type_ = prior_node["type"].as<std::string>();
state_ = prior_node["state"].as<Eigen::VectorXd>();
mode_ = prior_node["mode"].as<std::string>();
@@ -70,9 +67,7 @@ Prior::Prior(const YAML::Node& prior_node)
else
drift_std_ = Eigen::VectorXd(0);
- WOLF_INFO("checking");
check();
- WOLF_INFO("Exiting prior constructor");
}
void Prior::check() const
diff --git a/test/dummy/SensorDummyPoia3d.schema b/test/dummy/SensorDummyPoia3d.schema
index ac24447e5..3c54a371c 100644
--- a/test/dummy/SensorDummyPoia3d.schema
+++ b/test/dummy/SensorDummyPoia3d.schema
@@ -5,6 +5,7 @@ states:
O:
follow: PriorO3d.schema
I:
+ follow: Prior.schema
type:
type: string
yaml_type: scalar
@@ -17,8 +18,8 @@ states:
yaml_type: scalar
mandatory: true
doc: A vector containing the state 'I' values
- follow: PriorModeDynamicNoiseDrift.schema
A:
+ follow: Prior.schema
type:
type: string
yaml_type: scalar
@@ -31,7 +32,6 @@ states:
yaml_type: scalar
mandatory: true
doc: A vector containing the state 'A' values
- follow: PriorModeDynamicNoiseDrift.schema
noise_p_std:
mandatory: true
type: double
diff --git a/test/dummy/sensor_dummy.h b/test/dummy/sensor_dummy.h
index 23ba2a36c..5f82b090d 100644
--- a/test/dummy/sensor_dummy.h
+++ b/test/dummy/sensor_dummy.h
@@ -51,28 +51,26 @@ struct ParamsSensorDummy : public ParamsSensorBase
}
};
-WOLF_PTR_TYPEDEFS(SensorDummy);
-
+template <unsigned int DIM>
class SensorDummy : public SensorBase
{
private:
ParamsSensorDummyPtr params_dummy_;
- SizeEigen dim_;
public:
- SensorDummy(SizeEigen _dim,
- ParamsSensorDummyPtr _params,
+ SensorDummy(ParamsSensorDummyPtr _params,
const Priors& _priors) :
- SensorBase("SensorDummy",
- _dim,
+ SensorBase("SensorDummy"+toString(DIM)+"d",
+ DIM,
_params,
_priors),
params_dummy_(_params)
{
+ static_assert(DIM == 2 or DIM == 3);
}
- WOLF_SENSOR_CREATE(SensorDummy, ParamsSensorDummy);
+ WOLF_SENSOR_CREATE(SensorDummy<DIM>, ParamsSensorDummy);
- virtual ~SensorDummy(){};
+ virtual ~SensorDummy() = default;
Eigen::MatrixXd computeNoiseCov(const Eigen::VectorXd & _data) const override
{
@@ -81,13 +79,19 @@ class SensorDummy : public SensorBase
}
};
-}
+typedef SensorDummy<2> SensorDummy2d;
+typedef SensorDummy<3> SensorDummy3d;
+typedef SensorDummy<3> SensorDummyPoia3d;
+WOLF_DECLARED_PTR_TYPEDEFS(SensorDummy2d);
+WOLF_DECLARED_PTR_TYPEDEFS(SensorDummy3d);
+WOLF_DECLARED_PTR_TYPEDEFS(SensorDummyPoia3d);
+
+} // namespace wolf
// Register in the FactorySensor
#include "core/sensor/factory_sensor.h"
namespace wolf {
-WOLF_REGISTER_SENSOR_WITH_KEY(SensorDummy2d, SensorDummy);
-WOLF_REGISTER_SENSOR_WITH_KEY(SensorDummy3d, SensorDummy);
-WOLF_REGISTER_SENSOR_WITH_KEY(SensorDummyPoia2d, SensorDummy);
-WOLF_REGISTER_SENSOR_WITH_KEY(SensorDummyPoia3d, SensorDummy);
-} // namespace wolf
\ No newline at end of file
+WOLF_REGISTER_SENSOR(SensorDummy2d);
+WOLF_REGISTER_SENSOR(SensorDummy3d);
+WOLF_REGISTER_SENSOR(SensorDummyPoia3d);
+}
\ No newline at end of file
diff --git a/test/gtest_capture_base.cpp b/test/gtest_capture_base.cpp
index 1e72d21ae..e8823de12 100644
--- a/test/gtest_capture_base.cpp
+++ b/test/gtest_capture_base.cpp
@@ -43,7 +43,7 @@ TEST(CaptureBase, ConstructorNoSensor)
TEST(CaptureBase, ConstructorWithSensor)
{
- SensorBasePtr S = FactorySensorYaml::create("SensorOdom2d", 2, wolf_root + "/test/yaml/sensor_odom_2d.yaml", {wolf_root});
+ SensorBasePtr S = FactorySensorYaml::create("SensorOdom2d", "SensorOdom2d", wolf_root + "/test/yaml/sensor_odom_2d.yaml", {wolf_root});
CaptureBasePtr C(std::make_shared<CaptureBase>("DUMMY", 1.5, S)); // timestamp = 1.5
ASSERT_EQ(C->getTimeStamp(), 1.5);
ASSERT_FALSE(C->getFrame());
@@ -53,7 +53,7 @@ TEST(CaptureBase, ConstructorWithSensor)
TEST(CaptureBase, Static_sensor_params)
{
- SensorBasePtr S = FactorySensorYaml::create("SensorOdom2d", 2, wolf_root + "/test/yaml/sensor_odom_2d.yaml", {wolf_root});
+ SensorBasePtr S = FactorySensorYaml::create("SensorOdom2d", "SensorOdom2d", wolf_root + "/test/yaml/sensor_odom_2d.yaml", {wolf_root});
CaptureBasePtr C(std::make_shared<CaptureBase>("DUMMY", 1.5, S)); // timestamp = 1.5
// query capture blocks
@@ -64,7 +64,7 @@ TEST(CaptureBase, Static_sensor_params)
TEST(CaptureBase, Dynamic_sensor_params)
{
- SensorBasePtr S = FactorySensorYaml::create("SensorDiffDrive", 2, wolf_root + "/test/yaml/sensor_diff_drive_dynamic.yaml", {wolf_root});
+ SensorBasePtr S = FactorySensorYaml::create("SensorDiffDrive", "SensorDiffDrive", wolf_root + "/test/yaml/sensor_diff_drive_dynamic.yaml", {wolf_root});
StateBlockPtr p(std::make_shared<StateBlock>(2));
StateBlockPtr o(std::make_shared<StateAngle>() );
StateBlockPtr i(std::make_shared<StateBlock>(3));
@@ -96,7 +96,7 @@ TEST(CaptureBase, print)
TEST(CaptureBase, process)
{
- SensorBasePtr S = FactorySensorYaml::create("SensorOdom", 2, wolf_root + "/test/yaml/sensor_odom_2d.yaml", {wolf_root});
+ SensorBasePtr S = FactorySensorYaml::create("SensorOdom2d", "SensorOdom2d", wolf_root + "/test/yaml/sensor_odom_2d.yaml", {wolf_root});
CaptureBasePtr C(std::make_shared<CaptureBase>("DUMMY", 1.5, nullptr));
ASSERT_DEATH({C->process();},""); // No sensor in the capture should fail
C->setSensor(S);
diff --git a/test/gtest_frame_base.cpp b/test/gtest_frame_base.cpp
index 111c8e5f6..dc7ea50ec 100644
--- a/test/gtest_frame_base.cpp
+++ b/test/gtest_frame_base.cpp
@@ -66,7 +66,7 @@ TEST(FrameBase, LinksBasic)
ASSERT_FALSE(F->getTrajectory());
ASSERT_FALSE(F->getProblem());
- auto S = FactorySensorYaml::create("SensorOdom", 2, wolf_root + "/test/yaml/sensor_odom_2d.yaml", {wolf_root});
+ auto S = FactorySensorYaml::create("SensorOdom2d", "SensorOdom2d", wolf_root + "/test/yaml/sensor_odom_2d.yaml", {wolf_root});
ASSERT_FALSE(F->getCaptureOf(S));
ASSERT_TRUE(F->getCaptureList().empty());
ASSERT_TRUE(F->getConstrainedByList().empty());
@@ -78,7 +78,7 @@ TEST(FrameBase, LinksToTree)
// Problem with 2 frames and one motion factor between them
ProblemPtr P = Problem::create("PO", 2);
TrajectoryBasePtr T = P->getTrajectory();
- auto S = P->installSensor("SensorOdom", wolf_root + "/test/yaml/sensor_odom_2d.yaml", {wolf_root});
+ auto S = P->installSensor("SensorOdom2d", wolf_root + "/test/yaml/sensor_odom_2d.yaml", {wolf_root});
auto F1 = FrameBase::emplace<FrameBase>(T, 1, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
auto F2 = FrameBase::emplace<FrameBase>(T, 2, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
auto C = CaptureBase::emplace<CaptureMotion>(F1, "CaptureMotion", 1, S, Vector3d::Zero(), nullptr);
@@ -129,7 +129,7 @@ TEST(FrameBase, Frames)
// Problem with 10 frames
ProblemPtr P = Problem::create("PO", 2);
TrajectoryBasePtr T = P->getTrajectory();
- auto S = P->installSensor("SensorOdom", wolf_root + "/test/yaml/sensor_odom_2d.yaml", {wolf_root});
+ auto S = P->installSensor("SensorOdom2d", wolf_root + "/test/yaml/sensor_odom_2d.yaml", {wolf_root});
auto F0 = FrameBase::emplace<FrameBase>(T, 0, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
auto F1 = FrameBase::emplace<FrameBase>(T, 1, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
auto F2 = FrameBase::emplace<FrameBase>(T, 2, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
diff --git a/test/gtest_prior.cpp b/test/gtest_prior.cpp
index bcccda4a4..b82a64b48 100644
--- a/test/gtest_prior.cpp
+++ b/test/gtest_prior.cpp
@@ -440,7 +440,7 @@ TEST(Prior, ParamsServerWrong)
std::string prefix = key + "_" + toString(dim) + "D_" + mode + (dynamic ? "_dynamic" : "") + (drift ? "_drift" : "");
WOLF_INFO("Creating prior from prefix ", prefix);
- ASSERT_THROW(auto P = Prior(input_node[prefix]),std::runtime_error);
+ ASSERT_THROW(auto prior = Prior(input_node[prefix]),std::runtime_error);
}
// I
@@ -454,7 +454,7 @@ TEST(Prior, ParamsServerWrong)
std::string prefix = "I_" + mode + (dynamic ? "_dynamic" : "") + (drift ? "_drift" : "");
WOLF_INFO("Creating prior from prefix ", prefix);
- ASSERT_THROW(auto P = Prior(input_node[prefix]),std::runtime_error);
+ ASSERT_THROW(auto prior = Prior(input_node[prefix]),std::runtime_error);
}
}
diff --git a/test/gtest_processor_motion.cpp b/test/gtest_processor_motion.cpp
index 9e9e278c3..c4b37a73f 100644
--- a/test/gtest_processor_motion.cpp
+++ b/test/gtest_processor_motion.cpp
@@ -57,7 +57,7 @@ class ProcessorMotion_test : public testing::Test{
public:
double dt;
ProblemPtr problem;
- SensorOdomPtr sensor;
+ SensorOdom2dPtr sensor;
ProcessorOdom2dPublicPtr processor;
CaptureMotionPtr capture;
Vector2d data;
@@ -69,7 +69,7 @@ class ProcessorMotion_test : public testing::Test{
dt = 1.0;
problem = Problem::create("PO", 2);
- sensor = static_pointer_cast<SensorOdom>(problem->installSensor("SensorOdom",
+ sensor = static_pointer_cast<SensorOdom2d>(problem->installSensor("SensorOdom",
wolf_root + "/test/yaml/sensor_odom_2d.yaml",
{wolf_root}));
ParamsProcessorOdom2dPtr params(std::make_shared<ParamsProcessorOdom2d>());
diff --git a/test/gtest_sensor_base.cpp b/test/gtest_sensor_base.cpp
index 2607d430b..5280195fd 100644
--- a/test/gtest_sensor_base.cpp
+++ b/test/gtest_sensor_base.cpp
@@ -103,9 +103,9 @@ TEST(SensorBase, makeshared_priors_POfix2D)
params->noise_p_std = noise_p_std;
params->noise_o_std = noise_o_std;
- auto S = std::make_shared<SensorDummy>(2, params,
- Priors({{'P',Prior("P", p_state_2D)}, //default "fix", not dynamic
- {'O',Prior("O", o_state_2D)}}));
+ auto S = std::make_shared<SensorDummy2d>(params,
+ Priors({{'P',Prior("P", p_state_2D)}, //default "fix", not dynamic
+ {'O',Prior("O", o_state_2D)}}));
// noise
ASSERT_MATRIX_APPROX(S->computeNoiseCov(vector0), noise_cov_dummy, Constants::EPS);
@@ -123,9 +123,9 @@ TEST(SensorBase, makeshared_priors_POfix3D)
params->noise_p_std = noise_p_std;
params->noise_o_std = noise_o_std;
- auto S = std::make_shared<SensorDummy>(3, params,
- Priors({{'P',Prior("P", p_state_3D)}, //default "fix", not dynamic
- {'O',Prior("O", o_state_3D)}}));
+ auto S = std::make_shared<SensorDummy3d>(params,
+ Priors({{'P',Prior("P", p_state_3D)}, //default "fix", not dynamic
+ {'O',Prior("O", o_state_3D)}}));
// noise
ASSERT_MATRIX_APPROX(S->computeNoiseCov(vector0), noise_cov_dummy, Constants::EPS);
@@ -146,9 +146,9 @@ TEST(SensorBase, makeshared_priors_POinitial_guess2D)
params->noise_p_std = noise_p_std;
params->noise_o_std = noise_o_std;
- auto S = std::make_shared<SensorDummy>(2, params,
- Priors({{'P',Prior("P", p_state_2D, "initial_guess")},
- {'O',Prior("O", o_state_2D, "initial_guess")}}));
+ auto S = std::make_shared<SensorDummy2d>(params,
+ Priors({{'P',Prior("P", p_state_2D, "initial_guess")},
+ {'O',Prior("O", o_state_2D, "initial_guess")}}));
// noise
ASSERT_MATRIX_APPROX(S->computeNoiseCov(vector0), noise_cov_dummy, Constants::EPS);
@@ -169,9 +169,9 @@ TEST(SensorBase, makeshared_priors_POinitial_guess3D)
params->noise_p_std = noise_p_std;
params->noise_o_std = noise_o_std;
- auto S = std::make_shared<SensorDummy>(3, params,
- Priors({{'P',Prior("P", p_state_3D, "initial_guess")},
- {'O',Prior("O", o_state_3D, "initial_guess")}}));
+ auto S = std::make_shared<SensorDummy3d>(params,
+ Priors({{'P',Prior("P", p_state_3D, "initial_guess")},
+ {'O',Prior("O", o_state_3D, "initial_guess")}}));
// noise
ASSERT_MATRIX_APPROX(S->computeNoiseCov(vector0), noise_cov_dummy, Constants::EPS);
@@ -192,9 +192,9 @@ TEST(SensorBase, makeshared_priors_POfactor2D)
params->noise_p_std = noise_p_std;
params->noise_o_std = noise_o_std;
- auto S = std::make_shared<SensorDummy>(2, params,
- Priors({{'P',Prior("P", p_state_2D, "factor", p_std_2D)},
- {'O',Prior("O", o_state_2D, "factor", o_std_2D)}}));
+ auto S = std::make_shared<SensorDummy2d>(params,
+ Priors({{'P',Prior("P", p_state_2D, "factor", p_std_2D)},
+ {'O',Prior("O", o_state_2D, "factor", o_std_2D)}}));
// noise
ASSERT_MATRIX_APPROX(S->computeNoiseCov(vector0), noise_cov_dummy, Constants::EPS);
@@ -215,9 +215,9 @@ TEST(SensorBase, makeshared_priors_POfactor3D)
params->noise_p_std = noise_p_std;
params->noise_o_std = noise_o_std;
- auto S = std::make_shared<SensorDummy>(3, params,
- Priors({{'P',Prior("P", p_state_3D, "factor", p_std_3D)},
- {'O',Prior("O", o_state_3D, "factor", o_std_3D)}}));
+ auto S = std::make_shared<SensorDummy3d>(params,
+ Priors({{'P',Prior("P", p_state_3D, "factor", p_std_3D)},
+ {'O',Prior("O", o_state_3D, "factor", o_std_3D)}}));
// noise
ASSERT_MATRIX_APPROX(S->computeNoiseCov(vector0), noise_cov_dummy, Constants::EPS);
@@ -238,9 +238,9 @@ TEST(SensorBase, makeshared_priors_POinitial_guess_dynamic2D)
params->noise_p_std = noise_p_std;
params->noise_o_std = noise_o_std;
- auto S = std::make_shared<SensorDummy>(2, params,
- Priors({{'P',Prior("P", p_state_2D, "initial_guess", vector0, true)},
- {'O',Prior("O", o_state_2D, "initial_guess", vector0, true)}}));
+ auto S = std::make_shared<SensorDummy2d>(params,
+ Priors({{'P',Prior("P", p_state_2D, "initial_guess", vector0, true)},
+ {'O',Prior("O", o_state_2D, "initial_guess", vector0, true)}}));
// noise
ASSERT_MATRIX_APPROX(S->computeNoiseCov(vector0), noise_cov_dummy, Constants::EPS);
@@ -261,9 +261,9 @@ TEST(SensorBase, makeshared_priors_POinitial_guess_dynamic3D)
params->noise_p_std = noise_p_std;
params->noise_o_std = noise_o_std;
- auto S = std::make_shared<SensorDummy>(3, params,
- Priors({{'P',Prior("P", p_state_3D, "initial_guess", vector0, true)},
- {'O',Prior("O", o_state_3D, "initial_guess", vector0, true)}}));
+ auto S = std::make_shared<SensorDummy3d>(params,
+ Priors({{'P',Prior("P", p_state_3D, "initial_guess", vector0, true)},
+ {'O',Prior("O", o_state_3D, "initial_guess", vector0, true)}}));
// noise
ASSERT_MATRIX_APPROX(S->computeNoiseCov(vector0), noise_cov_dummy, Constants::EPS);
@@ -284,9 +284,9 @@ TEST(SensorBase, makeshared_priors_POinitial_guess_dynamic2D_drift)
params->noise_p_std = noise_p_std;
params->noise_o_std = noise_o_std;
- auto S = std::make_shared<SensorDummy>(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)}}));
+ auto S = std::make_shared<SensorDummy2d>(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)}}));
// noise
ASSERT_MATRIX_APPROX(S->computeNoiseCov(vector0), noise_cov_dummy, Constants::EPS);
@@ -307,9 +307,9 @@ TEST(SensorBase, makeshared_priors_POinitial_guess_dynamic3D_drift)
params->noise_p_std = noise_p_std;
params->noise_o_std = noise_o_std;
- auto S = std::make_shared<SensorDummy>(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)}}));
+ auto S = std::make_shared<SensorDummy3d>(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)}}));
// noise
ASSERT_MATRIX_APPROX(S->computeNoiseCov(vector0), noise_cov_dummy, Constants::EPS);
@@ -332,11 +332,11 @@ TEST(SensorBase, makeshared_priors_POIA_mixed)
VectorXd i_state_3D = VectorXd::Random(5);
- auto S = std::make_shared<SensorDummy>(3, params,
- Priors({{'P',Prior("P", p_state_3D, "fix", vector0, true)},
- {'O',Prior("O", o_state_3D, "factor", o_std_3D, true, o_std_3D)},
- {'I',Prior("StateBlock", i_state_3D, "initial_guess")},
- {'A',Prior("StateQuaternion", o_state_3D, "factor", o_std_3D)}}));
+ auto S = std::make_shared<SensorDummy3d>(params,
+ Priors({{'P',Prior("P", p_state_3D, "fix", vector0, true)},
+ {'O',Prior("O", o_state_3D, "factor", o_std_3D, true, o_std_3D)},
+ {'I',Prior("StateBlock", i_state_3D, "initial_guess")},
+ {'A',Prior("StateQuaternion", o_state_3D, "factor", o_std_3D)}}));
// noise
ASSERT_MATRIX_APPROX(S->computeNoiseCov(vector0), noise_cov_dummy, Constants::EPS);
@@ -400,7 +400,7 @@ TEST(SensorBase, factory)
{
ASSERT_TRUE(valid);
- auto S = FactorySensor::create("SensorDummy"+toString(dim)+"d", dim, server.getNode());
+ auto S = FactorySensor::create("SensorDummy"+toString(dim)+"d", server.getNode());
auto p_size = dim;
auto o_size = dim == 2 ? 1 : 4;
@@ -427,7 +427,7 @@ TEST(SensorBase, factory)
// either is not valid (schema) or it throws an error
if (valid)
{
- ASSERT_THROW(FactorySensor::create("SensorDummy"+toString(dim)+"d", dim, server.getNode()),std::runtime_error);
+ ASSERT_THROW(FactorySensor::create("SensorDummy"+toString(dim)+"d", server.getNode()),std::runtime_error);
}
}
}
@@ -439,8 +439,12 @@ TEST(SensorBase, factory)
// Yaml server
YamlServer server({wolf_root}, wolf_root + "/test/yaml/sensor_tests/sensor_POIA_3D.yaml");
+ auto valid = server.applySchema("SensorDummyPoia3d");
+ WOLF_WARN_COND(not valid, server.getLog().str());
+ ASSERT_TRUE(valid);
+
// create sensor
- auto S = FactorySensor::create("SensorDummyPoia3d", 3, server.getNode());
+ auto S = FactorySensor::create("SensorDummyPoia3d", server.getNode());
WOLF_INFO("created");
// noise
@@ -463,9 +467,14 @@ TEST(SensorBase, factory)
// Yaml server
YamlServer server({wolf_root}, wolf_root + "/test/yaml/sensor_tests/sensor_POIA_3D_wrong.yaml");
-
+
+ auto valid = server.applySchema("SensorDummyPoia3d");
+
// create sensor
- ASSERT_THROW(FactorySensor::create("SensorDummyPoia3d", 3, server.getNode()),std::runtime_error);
+ if (valid)
+ {
+ ASSERT_THROW(FactorySensor::create("SensorDummyPoia3d", server.getNode()),std::runtime_error);
+ }
}
}
@@ -498,6 +507,7 @@ TEST(SensorBase, factory_yaml)
if (not dynamic and drift)
continue;
+ std::string SensorType = "SensorDummy"+toString(dim)+"d";
std::string yaml_filepath = wolf_root +
"/test/yaml/sensor_tests/" +
"sensor_PO_" +
@@ -509,12 +519,13 @@ TEST(SensorBase, factory_yaml)
(wrong ? "_wrong" : "") +
".yaml";
- WOLF_INFO("Creating sensor from ", yaml_filepath);
+ WOLF_INFO("Creating sensor ", SensorType, " from ", yaml_filepath);
+
// CORRECT YAML
if (not wrong)
{
- auto S = FactorySensorYaml::create("SensorDummy"+toString(dim)+"d", dim, yaml_filepath, {wolf_root});
+ auto S = FactorySensorYaml::create(SensorType, SensorType, yaml_filepath, {wolf_root});
auto p_size = dim;
auto o_size = dim == 2 ? 1 : 4;
@@ -525,8 +536,8 @@ TEST(SensorBase, factory_yaml)
// noise
ASSERT_MATRIX_APPROX(S->computeNoiseCov(vector0),
- noise_cov_dummy,
- Constants::EPS);
+ noise_cov_dummy,
+ Constants::EPS);
// factors
ASSERT_EQ(S->getPriorFeatures().size(), mode == "factor" ? 2 : 0);
@@ -538,7 +549,7 @@ TEST(SensorBase, factory_yaml)
// INCORRECT YAML
else
{
- ASSERT_THROW(FactorySensorYaml::create("SensorDummy"+toString(dim)+"d", dim, yaml_filepath, {wolf_root}),std::runtime_error);
+ ASSERT_THROW(FactorySensorYaml::create(SensorType, SensorType, yaml_filepath, {wolf_root}),std::runtime_error);
}
}
@@ -547,8 +558,7 @@ TEST(SensorBase, factory_yaml)
WOLF_INFO("Creating sensor from name sensor_POIA_3D.yaml");
// create sensor
- auto S = FactorySensorYaml::create("SensorDummy3d",
- 3,
+ auto S = FactorySensorYaml::create("SensorDummyPoia3d", "SensorDummyPoia3d",
wolf_root + "/test/yaml/sensor_tests/sensor_POIA_3D.yaml",
{wolf_root});
@@ -571,8 +581,7 @@ TEST(SensorBase, factory_yaml)
WOLF_INFO("Creating sensor from name sensor_POIA_3D_wrong.yaml");
// create sensor
- ASSERT_THROW(FactorySensorYaml::create("SensorDummyPoia3d",
- 3,
+ ASSERT_THROW(FactorySensorYaml::create("SensorDummyPoia3d", "SensorDummyPoia3d",
wolf_root + "/test/yaml/sensor_tests/sensor_POIA_3D_wrong.yaml",
{wolf_root}),
std::runtime_error);
diff --git a/test/yaml/sensor_diff_drive_dynamic.yaml b/test/yaml/sensor_diff_drive_dynamic.yaml
index e09cb6031..013e6af1f 100644
--- a/test/yaml/sensor_diff_drive_dynamic.yaml
+++ b/test/yaml/sensor_diff_drive_dynamic.yaml
@@ -1,3 +1,4 @@
+name: a_cool_sensor_diff_drive_dynamic
states:
P:
mode: fix
--
GitLab