From 6e4bc5910ff716c386125942d8b2777577386d6b Mon Sep 17 00:00:00 2001
From: jvallve <jvallve@iri.upc.edu>
Date: Mon, 9 May 2022 15:37:34 +0200
Subject: [PATCH] wip
---
CMakeLists.txt | 1 +
include/core/sensor/sensor_base.h | 2 +-
include/core/state_block/prior.h | 149 +++++++++++-------------------
src/sensor/sensor_base.cpp | 9 +-
src/state_block/prior.cpp | 110 ++++++++++++++++++++++
src/state_block/state_block.cpp | 14 ++-
test/CMakeLists.txt | 88 +++++++++---------
test/gtest_prior.cpp | 149 ++++++++++++++++++++++++------
test/gtest_sensor_base.cpp | 22 ++---
9 files changed, 358 insertions(+), 186 deletions(-)
create mode 100644 src/state_block/prior.cpp
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 723c125ca..e2b959b0b 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -335,6 +335,7 @@ SET(SRCS_STATE_BLOCK
src/state_block/local_parametrization_base.cpp
src/state_block/local_parametrization_homogeneous.cpp
src/state_block/local_parametrization_quaternion.cpp
+ src/state_block/prior.cpp
src/state_block/state_block.cpp
src/state_block/state_composite.cpp
)
diff --git a/include/core/sensor/sensor_base.h b/include/core/sensor/sensor_base.h
index 984ec4fa5..931db3689 100644
--- a/include/core/sensor/sensor_base.h
+++ b/include/core/sensor/sensor_base.h
@@ -133,7 +133,7 @@ class SensorBase : public NodeBase, public HasStateBlocks, public std::enable_sh
void setProblem(ProblemPtr _problem) override final;
virtual void loadParams(ParamsSensorBasePtr _params);
- virtual void loadPriors(Priors _priors, SizeEigen _dim);
+ virtual void loadPriors(const Priors& _priors, SizeEigen _dim);
public:
diff --git a/include/core/state_block/prior.h b/include/core/state_block/prior.h
index cf36d9fc3..24fa589c8 100644
--- a/include/core/state_block/prior.h
+++ b/include/core/state_block/prior.h
@@ -22,13 +22,16 @@
#ifndef PRIOR_H_
#define PRIOR_H_
-#include "core/utils/params_server.h"
+#include <string>
+#include <unordered_map>
+#include <eigen3/Eigen/Dense>
using std::to_string;
namespace wolf
{
+class ParamsServer;
class Prior;
typedef std::unordered_map<char, Prior> Priors;
@@ -47,7 +50,7 @@ class Prior
Prior(char _key,
const std::string& _mode,
const Eigen::VectorXd& _state,
- const Eigen::VectorXd& _sigma,
+ const Eigen::VectorXd& _sigma = Eigen::VectorXd(0),
bool _dynamic = false,
const Eigen::VectorXd& _sigma_drift = Eigen::VectorXd(0));
@@ -55,113 +58,65 @@ class Prior
virtual ~Prior() = default;
- char getKey() const
- {
- return key_;
- }
-
- const std::string& getMode() const
- {
- return mode_;
- }
-
- const Eigen::VectorXd& getState() const
- {
- return state_;
- }
-
- const Eigen::VectorXd& getSigma() const
- {
- return sigma_;
- }
-
- bool isDynamic() const
- {
- return dynamic_;
- }
-
- bool isFixed() const
- {
- return mode_ == "fix";
- }
-
- bool isInitialGuess() const
- {
- return mode_ == "initial_guess";
- }
-
- bool isFactor() const
- {
- return mode_ == "factor";
- }
-
- const Eigen::VectorXd& getSigmaDrift() const
- {
- return sigma_drift_;
- }
-
- void check() const
- {
- if (mode_ != "nothing" and mode_ != "initial_guess" and mode_ != "fix" and mode_ == "factor")
- throw std::runtime_error("wrong mode value, it should be: 'nothing', 'initial_guess', 'fix' or 'factor'");
-
- if ( mode_ == "nothing" and (key_ == 'P' or key_ == 'O'))
- throw std::runtime_error("For P and O keys, mode 'nothing' is not valid");
-
- if (dynamic_ and (key_ == 'P' or key_ == 'O') )
- throw std::runtime_error("Dynamic state blocks not implemented for extrinsics");
- }
-
- virtual std::string print() const final
- {
- return "Prior " + std::string(1,key_) + "\n"
- + "mode: " + mode_ + "\n"
- //+ "state: " + to_string(state_) + "\n"
- //+ (mode_ == "factor" ? "sigma: " + to_string(sigma_) + "\n" : "")
- + "dynamic: " + to_string(dynamic_) + "\n";
- //+ (dynamic_ ? "sigma_drift: "+ to_string(sigma_drift_) + "\n" : "");
- }
+ char getKey() const;
+ const std::string& getMode() const;
+ const Eigen::VectorXd& getState() const;
+ const Eigen::VectorXd& getSigma() const;
+ bool isDynamic() const;
+ bool isFixed() const;
+ bool isInitialGuess() const;
+ bool isFactor() const;
+ const Eigen::VectorXd& getSigmaDrift() const;
+
+ void check() const;
+
+ virtual std::string print() const final;
};
-inline Prior::Prior(char _key,
- const std::string& _mode,
- const Eigen::VectorXd& _state,
- const Eigen::VectorXd& _sigma,
- bool _dynamic,
- const Eigen::VectorXd& _sigma_drift) :
- key_(_key),
- mode_(_mode),
- state_(_state),
- sigma_(_sigma),
- dynamic_(_dynamic),
- sigma_drift_(_sigma_drift)
+
+inline char Prior::getKey() const
{
- check();
+ return key_;
}
-inline Prior::Prior(const std::string& _prefix, char _key, const ParamsServer& _server)
+inline const std::string& Prior::getMode() const
{
- mode_ = _server.getParam<double>(_prefix + _key + "/mode");
+ return mode_;
+}
- if (mode_ != "nothing")
- state_ = _server.getParam<Eigen::VectorXd>(_prefix + _key + "/state");
- else
- state_ = Eigen::VectorXd(0);
+inline const Eigen::VectorXd& Prior::getState() const
+{
+ return state_;
+}
+
+inline const Eigen::VectorXd& Prior::getSigma() const
+{
+ return sigma_;
+}
- if (mode_ == "factor")
- sigma_ = _server.getParam<Eigen::VectorXd>(_prefix + _key + "/sigma");
- else
- sigma_ = Eigen::VectorXd(0);
+inline bool Prior::isDynamic() const
+{
+ return dynamic_;
+}
- dynamic_ = _server.getParam<bool>(_prefix + _key + "/dynamic");
+inline bool Prior::isFixed() const
+{
+ return mode_ == "fix";
+}
- if (dynamic_)
- sigma_drift_ = _server.getParam<Eigen::VectorXd>(_prefix + _key + "/sigma_drift");
- else
- sigma_drift_ = Eigen::VectorXd(0);
+inline bool Prior::isInitialGuess() const
+{
+ return mode_ == "initial_guess";
+}
- check();
+inline bool Prior::isFactor() const
+{
+ return mode_ == "factor";
}
+inline const Eigen::VectorXd& Prior::getSigmaDrift() const
+{
+ return sigma_drift_;
+}
}
#endif
diff --git a/src/sensor/sensor_base.cpp b/src/sensor/sensor_base.cpp
index 47b7685ec..fc8e5582b 100644
--- a/src/sensor/sensor_base.cpp
+++ b/src/sensor/sensor_base.cpp
@@ -93,7 +93,7 @@ void SensorBase::loadParams(ParamsSensorBasePtr _params)
setNoiseStd(_params->noise_std);
}
-void SensorBase::loadPriors(Priors _priors, SizeEigen _dim)
+void SensorBase::loadPriors(const Priors& _priors, SizeEigen _dim)
{
assert(_dim == 2 or _dim == 3);
@@ -110,6 +110,13 @@ void SensorBase::loadPriors(Priors _priors, SizeEigen _dim)
// create state block
auto sb = FactoryStateBlock::create(std::string(1, prior.getKey()), prior.getState(), prior.isFixed());
+ // check local param
+ if (not sb->isValid())
+ {
+ WOLF_ERROR("The created stateblock ", std::string(1, prior.getKey()), " is not valid (local param violation)");
+ throw std::runtime_error("StateBlock not valid");
+ }
+
// Add state block
addStateBlock(prior.getKey(), sb, prior.isDynamic());
diff --git a/src/state_block/prior.cpp b/src/state_block/prior.cpp
new file mode 100644
index 000000000..130a43859
--- /dev/null
+++ b/src/state_block/prior.cpp
@@ -0,0 +1,110 @@
+//--------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/state_block/prior.h"
+#include "core/state_block/factory_state_block.h"
+#include "core/utils/params_server.h"
+
+namespace wolf {
+
+Prior::Prior(char _key,
+ const std::string& _mode,
+ const Eigen::VectorXd& _state,
+ const Eigen::VectorXd& _sigma,
+ bool _dynamic,
+ const Eigen::VectorXd& _sigma_drift) :
+ key_(_key),
+ mode_(_mode),
+ state_(_state),
+ sigma_(_sigma),
+ dynamic_(_dynamic),
+ sigma_drift_(_sigma_drift)
+{
+ check();
+}
+
+Prior::Prior(const std::string& _prefix, char _key, const ParamsServer& _server)
+{
+ mode_ = _server.getParam<double>(_prefix + _key + "/mode");
+
+ if (mode_ != "nothing")
+ state_ = _server.getParam<Eigen::VectorXd>(_prefix + _key + "/state");
+ else
+ state_ = Eigen::VectorXd(0);
+
+ if (mode_ == "factor")
+ sigma_ = _server.getParam<Eigen::VectorXd>(_prefix + _key + "/sigma");
+ else
+ sigma_ = Eigen::VectorXd(0);
+
+ dynamic_ = _server.getParam<bool>(_prefix + _key + "/dynamic");
+
+ if (dynamic_)
+ sigma_drift_ = _server.getParam<Eigen::VectorXd>(_prefix + _key + "/sigma_drift");
+ else
+ sigma_drift_ = Eigen::VectorXd(0);
+
+ check();
+}
+
+void Prior::check() const
+{
+ if (mode_ != "nothing" and mode_ != "initial_guess" and mode_ != "fix" and mode_ != "factor")
+ throw std::runtime_error("wrong mode value: " + mode_ + ", it should be: 'nothing', 'initial_guess', 'fix' or 'factor'");
+
+ if ( mode_ == "nothing" and (key_ == 'P' or key_ == 'O'))
+ throw std::runtime_error("For P and O keys, mode 'nothing' is not valid");
+
+ if (dynamic_ and (key_ == 'P' or key_ == 'O') )
+ throw std::runtime_error("Dynamic state blocks not implemented for extrinsics");
+
+ // try to create a state block and check for local parameterization and dimensions consistency
+ auto sb = FactoryStateBlock::create(std::string(1, key_), state_, mode_ == "fix");
+
+ // check local param
+ if (not sb->isValid())
+ throw std::runtime_error("Prior " + std::string(1, key_) + " state is not valid (local param violation)");
+
+ // check state size
+ if (mode_ != "nothing" and state_.size() != sb->getLocalSize())
+ throw std::runtime_error("Prior " + std::string(1, key_) + " state size different of local size");
+
+ // check sigma size
+ if (mode_ == "factor" and sigma_.size() != sb->getLocalSize())
+ throw std::runtime_error("Prior " + std::string(1, key_) + " sigma size different of local size");
+
+ // check sigma size
+ if (dynamic_ and sigma_drift_.size() != sb->getLocalSize())
+ throw std::runtime_error("Prior " + std::string(1, key_) + " sigma_drift size different of local size");
+}
+
+std::string Prior::print() const
+{
+ return "Prior " + std::string(1,key_) + "\n"
+ + "mode: " + mode_ + "\n"
+ //+ "state: " + to_string(state_) + "\n"
+ //+ (mode_ == "factor" ? "sigma: " + to_string(sigma_) + "\n" : "")
+ + "dynamic: " + to_string(dynamic_) + "\n";
+ //+ (dynamic_ ? "sigma_drift: "+ to_string(sigma_drift_) + "\n" : "");
+}
+
+} // namespace wolf
\ No newline at end of file
diff --git a/src/state_block/state_block.cpp b/src/state_block/state_block.cpp
index f3ade24eb..de541a5a8 100644
--- a/src/state_block/state_block.cpp
+++ b/src/state_block/state_block.cpp
@@ -92,7 +92,17 @@ StateBlockPtr create_orientation(const Eigen::VectorXd& _state, bool _fixed)
return nullptr;
}
+StateBlockPtr create_position(const Eigen::VectorXd& _state, bool _fixed)
+{
+ if (_state.size() != 2 and _state.size() != 3)
+ throw std::runtime_error("Wrong vector size for position. Must be 2 or 3 (2D or 3D).");
+
+ return StateBlock::create(_state, _fixed);
+}
+
//WOLF_REGISTER_STATEBLOCK_WITH_KEY(O, wolf::create_orientation);
-namespace{ const bool __attribute__((used)) create_orientationRegisteredWithO = \
- FactoryStateBlock::registerCreator("O", wolf::create_orientation); }
+namespace{
+ const bool __attribute__((used)) create_orientationRegisteredWithO = FactoryStateBlock::registerCreator("O", wolf::create_orientation);
+ const bool __attribute__((used)) create_positionRegisteredWithO = FactoryStateBlock::registerCreator("P", wolf::create_position);
+}
}
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index f913970e6..bac2b0d4c 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -45,13 +45,13 @@ target_link_libraries(dummy ${PLUGIN_NAME})
# ------- First Core classes ----------
-# BufferFrame
-wolf_add_gtest(gtest_buffer_frame gtest_buffer_frame.cpp)
-target_link_libraries(gtest_buffer_frame ${PLUGIN_NAME} dummy)
+# # BufferFrame
+# wolf_add_gtest(gtest_buffer_frame gtest_buffer_frame.cpp)
+# target_link_libraries(gtest_buffer_frame ${PLUGIN_NAME} dummy)
-# CaptureBase class test
-wolf_add_gtest(gtest_capture_base gtest_capture_base.cpp)
-target_link_libraries(gtest_capture_base ${PLUGIN_NAME})
+# # CaptureBase class test
+# wolf_add_gtest(gtest_capture_base gtest_capture_base.cpp)
+# target_link_libraries(gtest_capture_base ${PLUGIN_NAME})
# Converter from String to various types used by the parameters server
wolf_add_gtest(gtest_converter gtest_converter.cpp)
@@ -73,17 +73,17 @@ target_link_libraries(gtest_factory ${PLUGIN_NAME} dummy)
wolf_add_gtest(gtest_factory_state_block gtest_factory_state_block.cpp)
target_link_libraries(gtest_factory_state_block ${PLUGIN_NAME})
-# Node Emplace test
-wolf_add_gtest(gtest_emplace gtest_emplace.cpp)
-target_link_libraries(gtest_emplace ${PLUGIN_NAME} dummy)
+# # Node Emplace test
+# wolf_add_gtest(gtest_emplace gtest_emplace.cpp)
+# target_link_libraries(gtest_emplace ${PLUGIN_NAME} dummy)
# FeatureBase classes test
wolf_add_gtest(gtest_feature_base gtest_feature_base.cpp)
target_link_libraries(gtest_feature_base ${PLUGIN_NAME})
-# FrameBase classes test
-wolf_add_gtest(gtest_frame_base gtest_frame_base.cpp)
-target_link_libraries(gtest_frame_base ${PLUGIN_NAME})
+# # FrameBase classes test
+# wolf_add_gtest(gtest_frame_base gtest_frame_base.cpp)
+# target_link_libraries(gtest_frame_base ${PLUGIN_NAME})
# GraphSearch class test
wolf_add_gtest(gtest_graph_search gtest_graph_search.cpp)
@@ -113,21 +113,25 @@ target_link_libraries(gtest_motion_buffer ${PLUGIN_NAME})
wolf_add_gtest(gtest_param_server gtest_param_server.cpp ${CMAKE_CURRENT_LIST_DIR})
target_link_libraries(gtest_param_server ${PLUGIN_NAME})
+# Parameters server
+wolf_add_gtest(gtest_prior gtest_prior.cpp)
+target_link_libraries(gtest_prior ${PLUGIN_NAME})
+
# YAML parser
wolf_add_gtest(gtest_parser_yaml gtest_parser_yaml.cpp)
target_link_libraries(gtest_parser_yaml ${PLUGIN_NAME})
-# Problem class test
-wolf_add_gtest(gtest_problem gtest_problem.cpp)
-target_link_libraries(gtest_problem ${PLUGIN_NAME} dummy)
+# # Problem class test
+# wolf_add_gtest(gtest_problem gtest_problem.cpp)
+# target_link_libraries(gtest_problem ${PLUGIN_NAME} dummy)
-# ProcessorBase class test
-wolf_add_gtest(gtest_processor_base gtest_processor_base.cpp)
-target_link_libraries(gtest_processor_base ${PLUGIN_NAME} dummy)
+# # ProcessorBase class test
+# wolf_add_gtest(gtest_processor_base gtest_processor_base.cpp)
+# target_link_libraries(gtest_processor_base ${PLUGIN_NAME} dummy)
-# ProcessorMotion class test
-wolf_add_gtest(gtest_processor_motion gtest_processor_motion.cpp)
-target_link_libraries(gtest_processor_motion ${PLUGIN_NAME})
+# # ProcessorMotion class test
+# wolf_add_gtest(gtest_processor_motion gtest_processor_motion.cpp)
+# target_link_libraries(gtest_processor_motion ${PLUGIN_NAME})
# Rotation test
wolf_add_gtest(gtest_rotation gtest_rotation.cpp)
@@ -195,9 +199,9 @@ target_link_libraries(gtest_factor_autodiff_distance_3d ${PLUGIN_NAME})
wolf_add_gtest(gtest_factor_block_difference gtest_factor_block_difference.cpp)
target_link_libraries(gtest_factor_block_difference ${PLUGIN_NAME})
-# FactorDiffDrive class test
-wolf_add_gtest(gtest_factor_diff_drive gtest_factor_diff_drive.cpp)
-target_link_libraries(gtest_factor_diff_drive ${PLUGIN_NAME})
+# # FactorDiffDrive class test
+# wolf_add_gtest(gtest_factor_diff_drive gtest_factor_diff_drive.cpp)
+# target_link_libraries(gtest_factor_diff_drive ${PLUGIN_NAME})
# FactorOdom2dAutodiff class test
wolf_add_gtest(gtest_factor_odom_2d_autodiff gtest_factor_odom_2d_autodiff.cpp)
@@ -211,21 +215,21 @@ target_link_libraries(gtest_factor_pose_2d ${PLUGIN_NAME})
wolf_add_gtest(gtest_factor_pose_3d gtest_factor_pose_3d.cpp)
target_link_libraries(gtest_factor_pose_3d ${PLUGIN_NAME})
-# FactorRelativePose2d class test
-wolf_add_gtest(gtest_factor_relative_pose_2d gtest_factor_relative_pose_2d.cpp)
-target_link_libraries(gtest_factor_relative_pose_2d ${PLUGIN_NAME})
+# # FactorRelativePose2d class test
+# wolf_add_gtest(gtest_factor_relative_pose_2d gtest_factor_relative_pose_2d.cpp)
+# target_link_libraries(gtest_factor_relative_pose_2d ${PLUGIN_NAME})
-# FactorRelativePose2dWithExtrinsics class test
-wolf_add_gtest(gtest_factor_relative_pose_2d_with_extrinsics gtest_factor_relative_pose_2d_with_extrinsics.cpp)
-target_link_libraries(gtest_factor_relative_pose_2d_with_extrinsics ${PLUGIN_NAME})
+# # FactorRelativePose2dWithExtrinsics class test
+# wolf_add_gtest(gtest_factor_relative_pose_2d_with_extrinsics gtest_factor_relative_pose_2d_with_extrinsics.cpp)
+# target_link_libraries(gtest_factor_relative_pose_2d_with_extrinsics ${PLUGIN_NAME})
-# FactorRelativePose3d class test
-wolf_add_gtest(gtest_factor_relative_pose_3d gtest_factor_relative_pose_3d.cpp)
-target_link_libraries(gtest_factor_relative_pose_3d ${PLUGIN_NAME})
+# # FactorRelativePose3d class test
+# wolf_add_gtest(gtest_factor_relative_pose_3d gtest_factor_relative_pose_3d.cpp)
+# target_link_libraries(gtest_factor_relative_pose_3d ${PLUGIN_NAME})
-# FactorRelativePose3dWithExtrinsics class test
-wolf_add_gtest(gtest_factor_relative_pose_3d_with_extrinsics gtest_factor_relative_pose_3d_with_extrinsics.cpp)
-target_link_libraries(gtest_factor_relative_pose_3d_with_extrinsics ${PLUGIN_NAME})
+# # FactorRelativePose3dWithExtrinsics class test
+# wolf_add_gtest(gtest_factor_relative_pose_3d_with_extrinsics gtest_factor_relative_pose_3d_with_extrinsics.cpp)
+# target_link_libraries(gtest_factor_relative_pose_3d_with_extrinsics ${PLUGIN_NAME})
# FactorVelocityLocalDirection3d class test
wolf_add_gtest(gtest_factor_velocity_local_direction_3d gtest_factor_velocity_local_direction_3d.cpp)
@@ -239,9 +243,9 @@ target_link_libraries(gtest_make_posdef ${PLUGIN_NAME})
wolf_add_gtest(gtest_map_yaml gtest_map_yaml.cpp)
target_link_libraries(gtest_map_yaml ${PLUGIN_NAME})
-# Parameter prior test
-wolf_add_gtest(gtest_param_prior gtest_param_prior.cpp)
-target_link_libraries(gtest_param_prior ${PLUGIN_NAME})
+# # Parameter prior test
+# wolf_add_gtest(gtest_param_prior gtest_param_prior.cpp)
+# target_link_libraries(gtest_param_prior ${PLUGIN_NAME})
# ProcessorFixedWingModel class test
# wolf_add_gtest(gtest_processor_fixed_wing_model gtest_processor_fixed_wing_model.cpp)
@@ -251,9 +255,9 @@ target_link_libraries(gtest_param_prior ${PLUGIN_NAME})
# wolf_add_gtest(gtest_processor_diff_drive gtest_processor_diff_drive.cpp)
# target_link_libraries(gtest_processor_diff_drive ${PLUGIN_NAME})
-# ProcessorLoopClosure class test
-wolf_add_gtest(gtest_processor_loop_closure gtest_processor_loop_closure.cpp)
-target_link_libraries(gtest_processor_loop_closure ${PLUGIN_NAME})
+# # ProcessorLoopClosure class test
+# wolf_add_gtest(gtest_processor_loop_closure gtest_processor_loop_closure.cpp)
+# target_link_libraries(gtest_processor_loop_closure ${PLUGIN_NAME})
# ProcessorFrameNearestNeighborFilter class test
# wolf_add_gtest(gtest_processor_frame_nearest_neighbor_filter_2d gtest_processor_frame_nearest_neighbor_filter_2d.cpp)
diff --git a/test/gtest_prior.cpp b/test/gtest_prior.cpp
index 6b7015a1e..4a2f6a1d5 100644
--- a/test/gtest_prior.cpp
+++ b/test/gtest_prior.cpp
@@ -21,45 +21,136 @@
//--------LICENSE_END--------
#include "core/utils/utils_gtest.h"
+#include "core/common/wolf.h"
#include "core/state_block/prior.h"
using namespace wolf;
+using namespace Eigen;
-testPrior(char _key,
+struct PriorTestSetup
+{
+ char key;
+ std::string mode;
+ VectorXd state;
+ VectorXd sigma;
+ bool dynamic;
+ VectorXd sigma_drift;
+};
+
+void testPrior(const Prior& P,
+ char _key,
const std::string& _mode,
- const Eigen::VectorXd& _state,
- const Eigen::VectorXd& _sigma,
- bool _dynamic = false,
- const Eigen::VectorXd& _sigma_drift = Eigen::VectorXd(0))
+ const VectorXd& _state,
+ const VectorXd& _sigma,
+ bool _dynamic,
+ const VectorXd& _sigma_drift)
{
- auto P = Prior(key,
- mode,
- state,
- sigma,
- dynamic,
- sigma_drift);
-
- ASSERT_EQ(key, P.getKey());
- ASSERT_EQ(mode, P.getMode());
- ASSERT_MATRIX_APPROX(state, P.getState(), 1e-8);
- if (mode == "factor")
- ASSERT_MATRIX_APPROX(sigma, P.getSigma(), 1e-8);
- ASSERT_EQ(dynamic, P.isDynamic());
- if (dynamic)
- ASSERT_MATRIX_APPROX(state, P.getState(), 1e-8);
-}
+ ASSERT_EQ(_key, P.getKey());
+ ASSERT_EQ(_mode, P.getMode());
+ ASSERT_MATRIX_APPROX(_state, P.getState(), 1e-8);
+
+ if (_mode == "factor")
+ {
+ ASSERT_MATRIX_APPROX(_sigma, P.getSigma(), 1e-8);
+ }
+ else
+ {
+ ASSERT_EQ(0,P.getSigma().size());
+ }
+
+ ASSERT_EQ(_dynamic, P.isDynamic());
+
+ if (_dynamic)
+ {
+ ASSERT_MATRIX_APPROX(_sigma_drift, P.getSigmaDrift(), 1e-8);
+ }
+ else
+ {
+ ASSERT_EQ(0,P.getSigmaDrift().size());
+ }
+};
+
+void createAndTestPrior(char _key,
+ const std::string& _mode,
+ const VectorXd& _state,
+ const VectorXd& _sigma,
+ bool _dynamic,
+ const VectorXd& _sigma_drift)
+{
+ // check constructor with default values
+ if (not _dynamic)
+ {
+ auto P = Prior(_key, _mode, _state, _sigma);
+ testPrior(P, _key, _mode, _state, _sigma, _dynamic, _sigma_drift);
+ }
-TEST(Prior, Pfix)
+ auto P = Prior(_key, _mode, _state, _sigma, _dynamic, _sigma_drift);
+ testPrior(P, _key, _mode, _state, _sigma, _dynamic, _sigma_drift);
+};
+
+TEST(Prior, PFix)
{
- dim=2;
- auto key = 'P';
- auto mode = "fix";
- Eigen::VectorXd state = Eigen::VectorXd::Zero(dim);
- Eigen::VectorXd sigma = Eigen::VectorXd(0);
- bool dynamic = false;
- Eigen::VectorXd sigma_drift = Eigen::VectorXd(0);
+ std::vector<PriorTestSetup> setups_ok, setups_death;
+
+ // SHOULD FAIL: Nothing - not dynamic (not allowed for P and O)
+ setups_death.push_back(PriorTestSetup({'P',"nothing",VectorXd::Random(2),VectorXd::Random(0),false}));
+ setups_death.push_back(PriorTestSetup({'P',"nothing",VectorXd::Random(3),VectorXd::Random(0),false}));
+ // SHOULD FAIL: Nothing - dynamic (not implemented)
+ setups_death.push_back(PriorTestSetup({'P',"nothing",VectorXd::Random(2),VectorXd::Random(0),true}));
+ setups_death.push_back(PriorTestSetup({'P',"nothing",VectorXd::Random(3),VectorXd::Random(0),true}));
+ // Initial guess - not dynamic
+ setups_ok. push_back(PriorTestSetup({'P',"initial_guess",VectorXd::Random(2),VectorXd::Random(0),false}));
+ setups_ok. push_back(PriorTestSetup({'P',"initial_guess",VectorXd::Random(3),VectorXd::Random(0),false}));
+ setups_death.push_back(PriorTestSetup({'P',"initial_guess",VectorXd::Random(4),VectorXd::Random(0),false})); // wrong size
+
+ // SHOULD FAIL: Initial guess - dynamic (not implemented)
+ setups_death.push_back(PriorTestSetup({'P',"initial_guess",VectorXd::Random(2),VectorXd::Random(0),true}));
+ setups_death.push_back(PriorTestSetup({'P',"initial_guess",VectorXd::Random(3),VectorXd::Random(0),true}));
+
+ // Fix - not dynamic
+ setups_ok. push_back(PriorTestSetup({'P',"fix",VectorXd::Random(2),VectorXd::Random(0),false}));
+ setups_ok. push_back(PriorTestSetup({'P',"fix",VectorXd::Random(3),VectorXd::Random(0),false}));
+ setups_death.push_back(PriorTestSetup({'P',"fix",VectorXd::Random(4),VectorXd::Random(0),false})); // wrong size
+
+ // SHOULD FAIL: Fix - dynamic (not implemented)
+ setups_death.push_back(PriorTestSetup({'P',"fix",VectorXd::Random(2),VectorXd::Random(0),true}));
+ setups_death.push_back(PriorTestSetup({'P',"fix",VectorXd::Random(3),VectorXd::Random(0),true}));
+
+ // Factor - not dynamic
+ setups_ok. push_back(PriorTestSetup({'P',"factor",VectorXd::Random(2),VectorXd::Random(2),false}));
+ setups_ok. push_back(PriorTestSetup({'P',"factor",VectorXd::Random(3),VectorXd::Random(3),false}));
+ setups_death.push_back(PriorTestSetup({'P',"factor",VectorXd::Random(4),VectorXd::Random(4),false})); // wrong size
+ setups_death.push_back(PriorTestSetup({'P',"factor",VectorXd::Random(2),VectorXd::Random(3),false})); // inconsistent size
+
+ // SHOULD FAIL: Factor - dynamic (not implemented)
+ setups_death.push_back(PriorTestSetup({'P',"factor",VectorXd::Random(2),VectorXd::Random(2),true}));
+ setups_death.push_back(PriorTestSetup({'P',"factor",VectorXd::Random(3),VectorXd::Random(3),true}));
+
+ for (auto setup : setups_ok)
+ {
+ WOLF_INFO("Testing setup_ok: ",
+ std::string(1,setup.key), " ",
+ setup.mode, " ",
+ setup.state.transpose(), " ",
+ setup.sigma.transpose(), " ",
+ setup.dynamic, " ",
+ setup.sigma_drift.transpose());
+ createAndTestPrior(setup.key, setup.mode, setup.state,setup.sigma,setup.dynamic,setup.sigma_drift);
+ }
+
+ for (auto setup : setups_death)
+ {
+ WOLF_INFO("Testing setup_death: ",
+ std::string(1,setup.key), " ",
+ setup.mode, " ",
+ setup.state.transpose(), " ",
+ setup.sigma.transpose(), " ",
+ setup.dynamic, " ",
+ setup.sigma_drift.transpose());
+ ASSERT_THROW(createAndTestPrior(setup.key, setup.mode, setup.state,setup.sigma,setup.dynamic,setup.sigma_drift),std::runtime_error);
+ }
}
int main(int argc, char **argv)
diff --git a/test/gtest_sensor_base.cpp b/test/gtest_sensor_base.cpp
index 513036095..ff1b4f5e3 100644
--- a/test/gtest_sensor_base.cpp
+++ b/test/gtest_sensor_base.cpp
@@ -34,23 +34,17 @@ using namespace wolf;
TEST(SensorBase, POfix)
{
+ auto noise_std = Eigen::Vector2d::Constant(0.1);
+ Eigen::MatrixXd noise_cov = noise_std.cwiseAbs2().asDiagonal();
+
+ auto params = std::make_shared<ParamsSensorBase>();
+ params->noise_std = Eigen::Vector2d::Constant(0.1);
auto S = std::make_shared<SensorBase>("SensorBase",
"sensor1",
2,
- Priors({{'P',Prior('P',
- "fix",
- Eigen::Vector2d::Zero(),
- Eigen::VectorXd(0),
- false,
- Eigen::VectorXd(0))},
- {'O',Prior('O',
- "fix",
- Eigen::Vector1d::Zero(),
- Eigen::VectorXd(0),
- false,
- Eigen::VectorXd(0))},
- }),
- std::make_shared<ParamsSensorBase>({Eigen::Vector2d::Constant(0.1)}));
+ Priors({{'P',Prior('P',"fix", Eigen::Vector2d::Zero())},
+ {'O',Prior('O',"fix", Eigen::Vector1d::Zero())}}),
+ params);
ASSERT_MATRIX_APPROX(noise_std, S->getNoiseStd(), 1e-8);
ASSERT_MATRIX_APPROX(noise_cov, S->getNoiseCov(), 1e-8);
--
GitLab