diff --git a/CMakeLists.txt b/CMakeLists.txt index 832377f6711f059221d85374397950e7f51b3135..16f94577c6d285823875434038935bbcb9f1694a 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -101,14 +101,9 @@ IF(EXISTS "${WOLF_CONFIG_DIR}" AND NOT IS_DIRECTORY "${WOLF_CONFIG_DIR}") message(FATAL_ERROR "Bug: Specified CONFIG_DIR: " "${WOLF_CONFIG_DIR} exists, but is not a directory.") ENDIF() + # Configure config.h configure_file(${CMAKE_CURRENT_SOURCE_DIR}/internal/config.h.in "${WOLF_CONFIG_DIR}/config.h") -message(STATUS "WOLF CONFIG DIRECTORY ${WOLF_CONFIG_DIR}") -message(STATUS "WOLF CONFIG FILE ${WOLF_CONFIG_DIR}/config.h") -include_directories("${PROJECT_BINARY_DIR}/conf") - -# ============ INCLUDES ============ -INCLUDE_DIRECTORIES("include") # In this same project # ============ HEADERS ============ SET(HDRS_CAPTURE @@ -215,6 +210,7 @@ SET(HDRS_STATE_BLOCK include/${PROJECT_NAME}/state_block/prior.h include/${PROJECT_NAME}/state_block/state_angle.h include/${PROJECT_NAME}/state_block/state_block.h + include/${PROJECT_NAME}/state_block/state_block_derived.h include/${PROJECT_NAME}/state_block/state_composite.h include/${PROJECT_NAME}/state_block/state_homogeneous_3d.h include/${PROJECT_NAME}/state_block/state_quaternion.h @@ -446,9 +442,9 @@ install( ${LIB_INSTALL_DIR}/${PLUGIN_NAME}/cmake ) -# Specifies include directories to use when compiling the plugin target -# This way, include_directories does not need to be called in plugins depending on this one -target_include_directories(${PLUGIN_NAME} INTERFACE +target_include_directories(${PLUGIN_NAME} PUBLIC + $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include> + $<BUILD_INTERFACE:${PROJECT_BINARY_DIR}/conf> $<INSTALL_INTERFACE:${INCLUDE_INSTALL_DIR}> ) @@ -494,7 +490,7 @@ INSTALL(FILES ${HDRS_UTILS} INSTALL(FILES ${HDRS_YAML} DESTINATION ${INCLUDE_INSTALL_DIR}/${PROJECT_NAME}/yaml) -INSTALL(FILES "${WOLF_CONFIG_DIR}/config.h" +INSTALL(FILES ${WOLF_CONFIG_DIR}/config.h DESTINATION ${INCLUDE_INSTALL_DIR}/${PROJECT_NAME}/internal) export(PACKAGE ${PLUGIN_NAME}) diff --git a/doc/doxygen.conf b/doc/doxygen.conf index f0009526c6f9fcb8a61ae84916690a62190e32c6..29dd647045c69da17bfd41a11bd4d047216b2583 100644 --- a/doc/doxygen.conf +++ b/doc/doxygen.conf @@ -757,7 +757,8 @@ WARN_LOGFILE = INPUT = ../doc/main.dox \ ../src \ - ../include/core + ../include/core \ + conf/core/internal # This tag can be used to specify the character encoding of the source files # that doxygen parses. Internally doxygen uses the UTF-8 encoding. Doxygen uses diff --git a/include/core/common/factory.h b/include/core/common/factory.h index 0a42e1275ef5e96f505837774d0c75842c6e81cd..c73d042f6599a2e386a19b2d10fed02bf8dd3aa8 100644 --- a/include/core/common/factory.h +++ b/include/core/common/factory.h @@ -305,6 +305,7 @@ class Factory public: static bool registerCreator(const std::string& _type, CreatorCallback createFn); static bool unregisterCreator(const std::string& _type); + static bool isCreatorRegistered(const std::string& _type); static TypeBasePtr create(const std::string& _type, TypeInput... _input); std::string getClass() const; static void printAddress(); @@ -356,6 +357,12 @@ inline bool Factory<TypeBase, TypeInput...>::unregisterCreator(const std::string return get().callbacks_.erase(_type) == 1; } +template<class TypeBase, typename... TypeInput> +inline bool Factory<TypeBase, TypeInput...>::isCreatorRegistered(const std::string& _type) +{ + return get().callbacks_.count(_type) == 1; +} + template<class TypeBase, typename... TypeInput> inline typename Factory<TypeBase, TypeInput...>::TypeBasePtr Factory<TypeBase, TypeInput...>::create(const std::string& _type, TypeInput... _input) { diff --git a/include/core/common/params_base.h b/include/core/common/params_base.h index 14b8dcb18cce99dc793e89f66281b04e7c07eb2e..51825a75307752c7ee88447dd14008e435c5a9a1 100644 --- a/include/core/common/params_base.h +++ b/include/core/common/params_base.h @@ -23,7 +23,7 @@ #define PARAMS_BASE_H_ #include "core/utils/params_server.h" -#include "yaml-schema-cpp/yaml-schema-cpp.hpp" +#include "yaml-schema-cpp/yaml_server.hpp" namespace wolf { struct ParamsBase diff --git a/include/core/math/SE3.h b/include/core/math/SE3.h index 9eb2e5fd9b1c644742a7d32d055591231f10c7aa..7917935549619fbc58cc917e834d5ae735d90d41 100644 --- a/include/core/math/SE3.h +++ b/include/core/math/SE3.h @@ -108,7 +108,7 @@ inline void inverse(const MatrixBase<D1>& p, const QuaternionBase<D2>& q, MatrixSizeCheck<3, 1>::check(p); MatrixSizeCheck<3, 1>::check(ip); - ip = - q.conjugate() * p ; + ip = -(q.conjugate() * p) ; iq = q.conjugate() ; } @@ -135,6 +135,21 @@ inline Matrix<typename D::Scalar, 7, 1> inverse(const MatrixBase<D>& d) return id; } +inline void inverse(const VectorComposite& v, VectorComposite& c) +{ + Map<const Quaternion<double> > qv( & v.at('O')(0) ); + Map<Quaternion<double> > qc( & c['O'](0) ); + inverse(v.at('P'), qv, c['P'], qc); +} + +inline VectorComposite inverse(const VectorComposite& v) +{ + VectorComposite c("PO", {3,4}); + inverse(v, c); + return c; +} + + template<typename D1, typename D2, typename D4, typename D5, typename D7, typename D8> inline void compose(const MatrixBase<D1>& p1, const QuaternionBase<D2>& q1, const MatrixBase<D4>& p2, const QuaternionBase<D5>& q2, @@ -231,6 +246,13 @@ inline void compose(const VectorComposite& x1, const VectorComposite& x2, Vector compose(x1.at('P'), x1.at('O'), x2.at('P'), x2.at('O'), c['P'], c['O']); } +inline VectorComposite compose(const VectorComposite& x1, const VectorComposite& x2) +{ + VectorComposite c("PO", {3,4}); + compose(x1.at('P'), x1.at('O'), x2.at('P'), x2.at('O'), c['P'], c['O']); + return c; +} + inline void compose(const VectorComposite& x1, const VectorComposite& x2, VectorComposite& c, diff --git a/include/core/problem/problem.h b/include/core/problem/problem.h index 147a04430dd803e62321b143c194cfa6ac7f0e50..7c551678e21f932c75f084cd770b0c35165e4e2d 100644 --- a/include/core/problem/problem.h +++ b/include/core/problem/problem.h @@ -87,7 +87,11 @@ class Problem : public std::enable_shared_from_this<Problem> StateStructure frame_structure_; PriorOptionsPtr prior_options_; - private: // CAUTION: THESE METHODS ARE PRIVATE, DO NOT MAKE THEM PUBLIC !! + std::atomic_bool transformed_; + VectorComposite transformation_; + mutable std::mutex mut_transform_; + + private: // CAUTION: THESE METHODS ARE PRIVATE, DO NOT MAKE THEM PUBLIC !! Problem(const std::string& _frame_structure, SizeEigen _dim, MapBasePtr _map); // USE create() below !! void setup(); @@ -364,6 +368,10 @@ class Problem : public std::enable_shared_from_this<Problem> // All branches ------------------------------------------- // perturb states void perturb(double amplitude = 0.01); + void transform(const VectorComposite& _transformation); + bool isTransformed() const; + void resetTransformed(); + VectorComposite getTransformation() const; // Covariances void clearCovariance(); diff --git a/include/core/processor/processor_base.h b/include/core/processor/processor_base.h index 3e529411d32fff7d9a2adc907b4c1e5eaffef88f..62019c087088db640e09f86d846807ab6f84d8cb 100644 --- a/include/core/processor/processor_base.h +++ b/include/core/processor/processor_base.h @@ -98,8 +98,6 @@ static ProcessorBasePtr create(const YAML::Node& _input_node) \ auto processor = std::make_shared<ProcessorClass>(params); \ \ - processor ->setName(_unique_name); \ - \ return processor; \ } \ diff --git a/include/core/processor/processor_motion.h b/include/core/processor/processor_motion.h index be64fda51927defcaedfed31c7683a1be5b6f77f..e9b1597f96effd18b2ff273f00d7cc369eb98cfc 100644 --- a/include/core/processor/processor_motion.h +++ b/include/core/processor/processor_motion.h @@ -177,12 +177,13 @@ class ProcessorMotion : public ProcessorBase, public MotionProvider RUNNING_WITH_KF_AFTER_ORIGIN } ProcessingStep ; - protected: + protected: ParamsProcessorMotionPtr params_motion_; - ProcessingStep processing_step_; ///< State machine controlling the processing step + ProcessingStep processing_step_; ///< State machine controlling the processing step + bool bootstrapping_; ///< processor is bootstrapping - // This is the main public interface - public: + // This is the main public interface + public: ProcessorMotion(const std::string& _type, std::string _state_structure, int _dim, @@ -299,6 +300,11 @@ class ProcessorMotion : public ProcessorBase, public MotionProvider */ virtual void preProcess(){ }; + /** + * @brief Bootstrap the processor with some initialization steps + */ + virtual void bootstrap(){}; + /** Post-process * * This is called by process() after finishing the processing algorithm. diff --git a/include/core/sensor/sensor_base.h b/include/core/sensor/sensor_base.h index 2e778b40f0a2268e6ed9920a701fd832542bd793..605400d49e6a44b4acc85d264e1404c574c581c1 100644 --- a/include/core/sensor/sensor_base.h +++ b/include/core/sensor/sensor_base.h @@ -55,44 +55,62 @@ namespace wolf { * SensorClass(const std::string& _unique_name, * SizeEigen _dim, * ParamsSensorClassPtr _params, - * const ParamsServer& _server) - * - * SensorClass(const std::string& _unique_name, - * SizeEigen _dim, - * ParamsSensorClassPtr _params, * const Priors& _priors) + * + * 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(const std::string& _unique_name, \ - SizeEigen _dim, \ - const ParamsServer& _server) \ -{ \ - auto params = std::make_shared<ParamsSensorClass>(_unique_name, _server); \ - \ - return std::make_shared<SensorClass>(_unique_name, _dim, params, _server); \ -} \ -static SensorBasePtr create(const std::string& _unique_name, \ - SizeEigen _dim, \ - const std::string& _yaml_filepath) \ -{ \ - auto parser = ParserYaml(_yaml_filepath, true); \ - \ - auto server = ParamsServer(parser.getParams(), "sensor/" + _unique_name); \ - \ - auto params = std::make_shared<ParamsSensorClass>(_unique_name, server); \ - \ - return std::make_shared<SensorClass>(_unique_name, _dim, params, server); \ -} \ -static SensorBasePtr create(const std::string& _unique_name, \ - SizeEigen _dim, \ - ParamsSensorBasePtr _params, \ - const Priors& _priors) \ -{ \ - auto params_derived = std::static_pointer_cast<ParamsSensorClass>(_params); \ - \ - return std::make_shared<SensorClass>(_unique_name, _dim, params_derived, _priors); \ -} \ +#define WOLF_SENSOR_CREATE(SensorClass, ParamsSensorClass) \ +static SensorBasePtr create(SizeEigen _dim, \ + const YAML::Node& _input_node, \ + const std::vector<std::string>& folders_schema) \ +{ \ + std::stringstream log; \ + if (not checkNode(_input_node, SensorClass, folders_schema, log)) \ + { \ + WOLF_ERROR(log.str()); \ + return nullptr; \ + } \ + \ + 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 schema = yaml-schema-cpp::YamlServer(folders_schema, _yaml_filepath); \ + \ + std::stringstream log; \ + if (not schema.validate(SensorClass, log)) \ + { \ + WOLF_ERROR(log.str()); \ + return nullptr; \ + } \ + auto params = std::make_shared<ParamsSensorClass>(schema.getNode()); \ + \ + auto priors = Priors(schema.getNode()["states"]); \ + \ + return std::make_shared<SensorClass>(_dim, params, priors); \ +} \ +static SensorBasePtr create(SizeEigen _dim, \ + ParamsSensorBasePtr _params, \ + const Priors& _priors) \ +{ \ + auto params_derived = std::dynamic_pointer_cast<ParamsSensorClass>(_params); \ + if (not params_derived) \ + { \ + WOLF_ERROR("In " #SensorClass " creator:", \ + " _params is not of type " #ParamsSensorClass "!"); \ + return nullptr; \ + } \ + \ + return std::make_shared<SensorClass>(_dim, params_derived, _priors); \ +} \ /** \brief base struct for intrinsic sensor parameters * @@ -101,10 +119,10 @@ static SensorBasePtr create(const std::string& _unique_name, WOLF_STRUCT_PTR_TYPEDEFS(ParamsSensorBase); struct ParamsSensorBase: public ParamsBase { - std::string prefix = "sensor/"; + std::string name; ParamsSensorBase() = default; - ParamsSensorBase(std::string _unique_name, const wolf::ParamsServer& _server) + ParamsSensorBase(const YAML::Node& _input_node) { } ~ParamsSensorBase() override = default; @@ -115,25 +133,6 @@ struct ParamsSensorBase: public ParamsBase } }; -struct SpecState -{ - std::string type; - SizeEigen size; - std::string doc; - - SpecState(const std::string& _type, SizeEigen _size, const std::string _doc) : - type(_type), size(_size), doc(_doc) - { - assert(not(_type == "StateQuaternion" and _size != 4)); - assert(not(_type == "StateAngle" and _size != 1)); - assert(not(_type == "O" and _size != 1 and _size != 4)); - assert(not(_type == "P" and _size != 1 and _size != 4)); - assert(not(_type == "V" and _size != 1 and _size != 4)); - }; -}; - -typedef std::unordered_map<char,SpecState> SpecStates; - WOLF_PTR_TYPEDEFS(SensorBase); class SensorBase : public NodeBase, public HasStateBlocks, public std::enable_shared_from_this<SensorBase> { @@ -160,7 +159,7 @@ class SensorBase : public NodeBase, public HasStateBlocks, public std::enable_sh std::map<char, Eigen::MatrixXd> drift_covs_; // covariance of the drift of dynamic state blocks [unit²/s] void setProblem(ProblemPtr _problem) override final; - virtual void loadPriors(const Priors& _priors, SizeEigen _dim, const SpecStates& _state_specs); + virtual void loadPriors(const Priors& _priors, SizeEigen _dim); public: @@ -172,34 +171,13 @@ class SensorBase : public NodeBase, public HasStateBlocks, public std::enable_sh * \param _dim Problem dimension * \param _params params struct * \param _priors priors of the sensor state blocks - * \param _state_specs_apart_from_PO Unordered map containing the keys and the specs for each of the state blocks (apart from extrinsics: P, O) - * - **/ - SensorBase(const std::string& _type, - const std::string& _unique_name, - const SizeEigen& _dim, - ParamsSensorBasePtr _params, - const Priors& _priors, - SpecStates _state_specs_apart_from_PO={}); - - /** \brief Constructor with ParamServer and keys - * - * Constructor with parameter vector - * TODO: update - * \param _tp Type of the sensor (types defined at wolf.h) - * \param _unique_name Name of the sensor - * \param _dim Problem dimension - * \param _params params struct - * \param _server parameter server - * \param _state_specs_apart_from_PO Unordered map containing the keys and the specs for each of the state blocks (apart from extrinsics: P, O) * **/ SensorBase(const std::string& _type, const std::string& _unique_name, const SizeEigen& _dim, ParamsSensorBasePtr _params, - const ParamsServer& _server, - SpecStates _state_specs_apart_from_PO={}); + const Priors& _priors); ~SensorBase() override; diff --git a/include/core/state_block/has_state_blocks.h b/include/core/state_block/has_state_blocks.h index d8ea81ae0bbe3ec3fa4547961ab715eb810662f7..e25e7d908a18a7b0989e4b9c91bf87cb18229266 100644 --- a/include/core/state_block/has_state_blocks.h +++ b/include/core/state_block/has_state_blocks.h @@ -40,6 +40,8 @@ namespace wolf class HasStateBlocks { + friend Problem; + public: HasStateBlocks(); HasStateBlocks(const StateStructure& _structure) : structure_(_structure), state_block_map_() {} @@ -95,11 +97,13 @@ class HasStateBlocks unsigned int getLocalSize(const StateStructure& _structure="") const; // Perturb state - void perturb(double amplitude = 0.01); + void perturb(double amplitude = 0.01); - protected: - void printState(bool _metric, bool _state_blocks, std::ostream& _stream, std::string _tabs) const; + protected: + // transform state + void transform(const VectorComposite& _transformation); + void printState(bool _metric, bool _state_blocks, std::ostream& _stream, std::string _tabs) const; private: StateStructure structure_; diff --git a/include/core/state_block/state_angle.h b/include/core/state_block/state_angle.h index 1a307ce72310bcee0bf7d5b21b833aec8caf9340..fe0fb46473f64743b17ee96d47143a2b00492ac7 100644 --- a/include/core/state_block/state_angle.h +++ b/include/core/state_block/state_angle.h @@ -38,15 +38,17 @@ namespace wolf class StateAngle : public StateBlock { public: - StateAngle(double _angle = 0.0, bool _fixed = false); + StateAngle(double _angle = 0.0, bool _fixed = false, bool _transformable = true); ~StateAngle() override; + virtual void transform(const VectorComposite& _transformation) override; + static StateBlockPtr create(const Eigen::VectorXd& _state, bool _fixed); }; -inline StateAngle::StateAngle(double _angle, bool _fixed) : - StateBlock(1, _fixed, std::make_shared<LocalParametrizationAngle>()) +inline StateAngle::StateAngle(double _angle, bool _fixed, bool _transformable) : + StateBlock(1, _fixed, std::make_shared<LocalParametrizationAngle>(), _transformable) { state_(0) = pi2pi(_angle); } @@ -64,6 +66,11 @@ inline StateBlockPtr StateAngle::create (const Eigen::VectorXd& _state, bool _fi return std::make_shared<StateAngle>(_state(0), _fixed); } +inline void StateAngle::transform(const VectorComposite& _transformation) +{ + if (isTransformable()) setState(_transformation.at('O') + getState()); // 2D rotation is a sum of angles +} + } /* namespace wolf */ #endif /* STATE_ANGLE_H_ */ diff --git a/include/core/state_block/state_block.h b/include/core/state_block/state_block.h index 8f4b0468b8d9fdc519e11137a8dc491330ec3ac7..2572cc9b64a729f36496ac403fb18700846dc43c 100644 --- a/include/core/state_block/state_block.h +++ b/include/core/state_block/state_block.h @@ -31,6 +31,7 @@ class LocalParametrizationBase; //Wolf includes #include "core/common/wolf.h" +#include "core/state_block/state_composite.h" //std includes #include <iostream> @@ -65,6 +66,8 @@ public: std::atomic_bool fix_updated_; ///< Flag to indicate whether the status has been updated or not std::atomic_bool local_param_updated_; ///< Flag to indicate whether the local_parameterization has been updated or not + bool transformable_; ///< Flag to indicate if the state block can be transformed to another reference frame + public: /** \brief Constructor from size * @@ -72,7 +75,7 @@ public: * \param _fixed Indicates this state is not estimated and thus acts as a fixed parameter * \param _local_param_ptr pointer to the local parametrization for the block */ - StateBlock(const SizeEigen _size, bool _fixed = false, LocalParametrizationBasePtr _local_param_ptr = nullptr); + StateBlock(const SizeEigen _size, bool _fixed = false, LocalParametrizationBasePtr _local_param_ptr = nullptr, bool _transformable = true); /** \brief Constructor from vector * @@ -80,7 +83,7 @@ public: * \param _fixed Indicates this state is not estimated and thus acts as a fixed parameter * \param _local_param_ptr pointer to the local parametrization for the block **/ - StateBlock(const Eigen::VectorXd& _state, bool _fixed = false, LocalParametrizationBasePtr _local_param_ptr = nullptr); + StateBlock(const Eigen::VectorXd& _state, bool _fixed = false, LocalParametrizationBasePtr _local_param_ptr = nullptr, bool _transformable = true); ///< Explicitly not copyable/movable StateBlock(const StateBlock& o) = delete; @@ -177,6 +180,11 @@ public: */ void perturb(double amplitude = 0.1); + bool isTransformable() const { + return transformable_; + }; + + virtual void transform(const VectorComposite& _transformation) { }; void plus(const Eigen::VectorXd& _dv); @@ -186,6 +194,7 @@ public: }; + } // namespace wolf // IMPLEMENTATION @@ -195,31 +204,30 @@ public: namespace wolf { -inline StateBlock::StateBlock(const Eigen::VectorXd& _state, bool _fixed, LocalParametrizationBasePtr _local_param_ptr) : -// notifications_{Notification::ADD}, +inline StateBlock::StateBlock(const Eigen::VectorXd& _state, bool _fixed, LocalParametrizationBasePtr _local_param_ptr, bool _transformable) : fixed_(_fixed), state_size_(_state.size()), state_(_state), local_param_ptr_(_local_param_ptr), state_updated_(false), fix_updated_(false), - local_param_updated_(false) + local_param_updated_(false), + transformable_(_transformable) { // std::cout << "constructed +sb" << std::endl; } -inline StateBlock::StateBlock(const SizeEigen _size, bool _fixed, LocalParametrizationBasePtr _local_param_ptr) : -// notifications_{Notification::ADD}, +inline StateBlock::StateBlock(const SizeEigen _size, bool _fixed, LocalParametrizationBasePtr _local_param_ptr, bool _transformable) : fixed_(_fixed), state_size_(_size), state_(Eigen::VectorXd::Zero(_size)), local_param_ptr_(_local_param_ptr), state_updated_(false), fix_updated_(false), - local_param_updated_(false) + local_param_updated_(false), + transformable_(_transformable) { - // -// std::cout << "constructed +sb" << std::endl; + // std::cout << "constructed +sb" << std::endl; } inline StateBlock::~StateBlock() diff --git a/include/core/state_block/state_block_derived.h b/include/core/state_block/state_block_derived.h new file mode 100644 index 0000000000000000000000000000000000000000..541b79a19453f34cedf13343de1caee2d2fca74b --- /dev/null +++ b/include/core/state_block/state_block_derived.h @@ -0,0 +1,154 @@ +//--------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 STATE_BLOCK_DERIVED_H_ +#define STATE_BLOCK_DERIVED_H_ + +#include "core/state_block/state_block.h" + +namespace wolf +{ + +/** + * @brief State block for general parameters + * + * This state block cannot be transformed with a geometric frame transformation. + * + * This state block cannot have a local parametrization + * + * @tparam size the size of the state block's vector + */ +template <size_t size> +class StateParams : public StateBlock +{ + public: + StateParams(const Eigen::VectorXd& _state, bool _fixed = false) : StateBlock(_state, _fixed, nullptr, false) {} + void transform(const VectorComposite& _transformation) override + { + // non transformable! --> do nothing + } +}; + +typedef StateParams<1> StateParams1; +typedef StateParams<2> StateParams2; +typedef StateParams<3> StateParams3; +typedef StateParams<4> StateParams4; +typedef StateParams<5> StateParams5; +typedef StateParams<6> StateParams6; +typedef StateParams<7> StateParams7; +typedef StateParams<8> StateParams8; +typedef StateParams<9> StateParams9; +typedef StateParams<10> StateParams10; + +/** + * @brief State block for general 2D points and positions + * + * This state block can be transformed with a geometric frame transformation. + * This is controlled at construction time by parameter _transformable, + * or later via setTransformable(bool). + * + * This state block cannot have a local parametrization + */ +class StatePoint2d : public StateBlock +{ + public: + StatePoint2d(const Eigen::VectorXd& _state, bool _fixed = false, bool _transformable = true) + : StateBlock(_state, _fixed, nullptr, _transformable) + { + } + void transform(const VectorComposite& _transformation) override + { + if (transformable_) setState(_transformation.at('P') + Rotation2Dd(_transformation.at('O')(0)) * getState()); + } +}; + +/** + * @brief State block for general 2D vectors + * + * This state block can be transformed with a geometric frame transformation. + * This is controlled at construction time by parameter _transformable, + * or later via setTransformable(bool). + * Being a vector, the geometric frame transformation will rotate the vector. + * + * This state block cannot have a local parametrization + */ +class StateVector2d : public StateBlock +{ + public: + StateVector2d(const Eigen::VectorXd& _state, bool _fixed = false, bool _transformable = true) + : StateBlock(_state, _fixed, nullptr, _transformable) + { + } + void transform(const VectorComposite& _transformation) override + { + if (transformable_) setState(Rotation2Dd(_transformation.at('O')(0)) * getState()); + } +}; + +/** + * @brief State block for general 3D points and positions + * + * This state block can be transformed with a geometric frame transformation. + * This is controlled at construction time by parameter _transformable, + * or later via setTransformable(bool). + * + * This state block cannot have a local parametrization + */ +class StatePoint3d : public StateBlock +{ + public: + StatePoint3d(const Eigen::VectorXd& _state, bool _fixed = false, bool _transformable = true) + : StateBlock(_state, _fixed, nullptr, _transformable) + { + } + void transform(const VectorComposite& _transformation) override + { + if (transformable_) + setState(_transformation.at('P') + Quaterniond(_transformation.at('O').data()) * getState()); + } +}; + +/** + * @brief State block for general 3D vectors + * + * This state block can be transformed with a geometric frame transformation. + * This is controlled at construction time by parameter _transformable, + * or later via setTransformable(bool). + * Being a vector, the geometric frame transformation will rotate the vector. + * + * This state block cannot have a local parametrization + */ +class StateVector3d : public StateBlock +{ + public: + StateVector3d(const Eigen::VectorXd& _state, bool _fixed = false, bool _transformable = true) + : StateBlock(_state, _fixed, nullptr, _transformable) + { + } + void transform(const VectorComposite& _transformation) override + { + if (transformable_) setState(Quaterniond(_transformation.at('O').data()) * getState()); + } +}; + +} // namespace wolf + +#endif // STATE_BLOCK_DERIVED_H_ \ No newline at end of file diff --git a/include/core/state_block/state_composite.h b/include/core/state_block/state_composite.h index e3569b5b4ed30159e0593de789b3b7a6c3644387..dcfa71155597334de8ff4a3daedbb760314f1856 100644 --- a/include/core/state_block/state_composite.h +++ b/include/core/state_block/state_composite.h @@ -246,7 +246,7 @@ class MatrixComposite : public std::unordered_map < char, std::unordered_map < c * S["V"]["V"] S["V"]["W"] * S["W"]["V"] S["W"]["W"] */ - MatrixComposite propagate(const MatrixComposite & _Cov); // This performs Jac * this * Jac.tr + MatrixComposite propagate(const MatrixComposite & _Cov); // This performs this * _Cov * this.tr /** * \brief Matrix-vector product diff --git a/include/core/state_block/state_homogeneous_3d.h b/include/core/state_block/state_homogeneous_3d.h index 6ee8a85c28725805ff5a28fb2149950f7cb8e132..8af6059aa6badcfc91665307e92df8fd9379694b 100644 --- a/include/core/state_block/state_homogeneous_3d.h +++ b/include/core/state_block/state_homogeneous_3d.h @@ -37,25 +37,27 @@ namespace wolf { class StateHomogeneous3d : public StateBlock { public: - StateHomogeneous3d(bool _fixed = false); - StateHomogeneous3d(const Eigen::VectorXd _state, bool _fixed = false); + StateHomogeneous3d(bool _fixed = false, bool _transformable = true); + StateHomogeneous3d(const Eigen::VectorXd _state, bool _fixed = false, bool _transformable = true); ~StateHomogeneous3d() override; void setIdentity(bool _notify = true) override; Eigen::VectorXd identity() const override; + virtual void transform(const VectorComposite& _transformation) override; + static StateBlockPtr create(const Eigen::VectorXd& _state, bool _fixed); }; -inline StateHomogeneous3d::StateHomogeneous3d(const Eigen::VectorXd _state, bool _fixed) : - StateBlock(_state, _fixed) +inline StateHomogeneous3d::StateHomogeneous3d(const Eigen::VectorXd _state, bool _fixed, bool _transformable) : + StateBlock(_state, _fixed, nullptr, _transformable) { assert(_state.size() == 4 && "Homogeneous 3d must be size 4."); local_param_ptr_ = std::make_shared<LocalParametrizationHomogeneous>(); } -inline StateHomogeneous3d::StateHomogeneous3d(bool _fixed) : - StateBlock(4, _fixed) +inline StateHomogeneous3d::StateHomogeneous3d(bool _fixed, bool _transformable) : + StateBlock(4, _fixed, nullptr, _transformable) { local_param_ptr_ = std::make_shared<LocalParametrizationHomogeneous>(); state_ << 0, 0, 0, 1; // Set origin @@ -77,6 +79,12 @@ inline Eigen::VectorXd StateHomogeneous3d::identity() const return Eigen::Quaterniond::Identity().coeffs(); } +inline void StateHomogeneous3d::transform(const VectorComposite& _transformation) +{ + if (transformable_) + setState((Quaterniond(_transformation.at('O').data()) * Quaterniond(getState().data())).coeffs()); // TODO is this correct?????? probably not!!! +} + inline StateBlockPtr StateHomogeneous3d::create (const Eigen::VectorXd& _state, bool _fixed) { MatrixSizeCheck<4,1>::check(_state); diff --git a/include/core/state_block/state_quaternion.h b/include/core/state_block/state_quaternion.h index d5278e1a6f4e3580c22056db7cc2c9ef073c21fb..ae7ed2f6381e9ce1149bff585df744d71e74b59e 100644 --- a/include/core/state_block/state_quaternion.h +++ b/include/core/state_block/state_quaternion.h @@ -37,19 +37,21 @@ namespace wolf { class StateQuaternion : public StateBlock { public: - StateQuaternion(bool _fixed = false); - StateQuaternion(const Eigen::VectorXd& _state, bool _fixed = false); - StateQuaternion(const Eigen::Quaterniond& _quaternion, bool _fixed = false); + StateQuaternion(bool _fixed = false, bool _transformable = true); + StateQuaternion(const Eigen::VectorXd& _state, bool _fixed = false, bool _transformable = true); + StateQuaternion(const Eigen::Quaterniond& _quaternion, bool _fixed = false, bool _transformable = true); ~StateQuaternion() override; void setIdentity(bool _notify = true) override; Eigen::VectorXd identity() const override; + virtual void transform(const VectorComposite& _transformation) override; + static StateBlockPtr create(const Eigen::VectorXd& _state, bool _fixed); }; -inline StateQuaternion::StateQuaternion(const Eigen::Quaterniond& _quaternion, bool _fixed) : - StateBlock(_quaternion.coeffs(), _fixed, std::make_shared<LocalParametrizationQuaternion<DQ_LOCAL>>()) +inline StateQuaternion::StateQuaternion(const Eigen::Quaterniond& _quaternion, bool _fixed, bool _transformable) : + StateBlock(_quaternion.coeffs(), _fixed, std::make_shared<LocalParametrizationQuaternion<DQ_LOCAL>>(), _transformable) { // TODO: remove these lines after issue #381 is addressed if(not isValid(1e-5)) @@ -58,8 +60,8 @@ inline StateQuaternion::StateQuaternion(const Eigen::Quaterniond& _quaternion, b state_.normalize(); } -inline StateQuaternion::StateQuaternion(const Eigen::VectorXd& _state, bool _fixed) : - StateBlock(_state, _fixed, std::make_shared<LocalParametrizationQuaternion<DQ_LOCAL>>()) +inline StateQuaternion::StateQuaternion(const Eigen::VectorXd& _state, bool _fixed, bool _transformable) : + StateBlock(_state, _fixed, std::make_shared<LocalParametrizationQuaternion<DQ_LOCAL>>(), _transformable) { if(state_.size() != 4) throw std::runtime_error("The quaternion state vector must be of size 4!"); @@ -71,8 +73,8 @@ inline StateQuaternion::StateQuaternion(const Eigen::VectorXd& _state, bool _fix state_.normalize(); } -inline StateQuaternion::StateQuaternion(bool _fixed) : - StateBlock(4, _fixed, std::make_shared<LocalParametrizationQuaternion<DQ_LOCAL>>()) +inline StateQuaternion::StateQuaternion(bool _fixed, bool _transformable) : + StateBlock(4, _fixed, std::make_shared<LocalParametrizationQuaternion<DQ_LOCAL>>(), _transformable) { state_ = Eigen::Quaterniond::Identity().coeffs(); // @@ -94,6 +96,12 @@ inline Eigen::VectorXd StateQuaternion::identity() const return Eigen::Quaterniond::Identity().coeffs(); } +inline void StateQuaternion::transform(const VectorComposite& _transformation) +{ + if (transformable_) + setState((Quaterniond(_transformation.at('O').data()) * Quaterniond(getState().data())).coeffs()); +} + inline StateBlockPtr StateQuaternion::create (const Eigen::VectorXd& _state, bool _fixed) { return std::make_shared<StateQuaternion>(_state, _fixed); diff --git a/src/landmark/landmark_base.cpp b/src/landmark/landmark_base.cpp index 921fe7896652ba7b3f3eec865136a6c6dda05237..266c0ef1925bf90725d317eb9eb17a4ccb1b3299 100644 --- a/src/landmark/landmark_base.cpp +++ b/src/landmark/landmark_base.cpp @@ -27,7 +27,7 @@ #include "core/state_block/state_angle.h" #include "core/state_block/state_quaternion.h" #include "core/common/factory.h" -// #include "core/yaml/yaml_conversion.h" +#include "yaml-schema-cpp/yaml_conversion.hpp" namespace wolf { diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp index 59299e8bd8b7665d0cbd2b266135fb0d755bf73c..70e80e1ce766a5fee328f1a9ffeab3dd5ddc9d91 100644 --- a/src/problem/problem.cpp +++ b/src/problem/problem.cpp @@ -44,7 +44,8 @@ Problem::Problem(const std::string& _frame_structure, SizeEigen _dim, MapBasePtr map_ptr_(_map), motion_provider_map_(), frame_structure_(_frame_structure), - prior_options_(std::make_shared<PriorOptions>()) + prior_options_(std::make_shared<PriorOptions>()), + transformed_(false) { dim_ = _dim; if (_frame_structure == "PO" and _dim == 2) @@ -134,20 +135,35 @@ ProblemPtr Problem::autoSetup(ParamsServer &_server) loaders.push_back(l); } - // Install sensors and processors + // Install sensors and processors -- load plugins only if sensors and processor are not registered auto sensors = _server.getParam<std::vector<std::map<std::string, std::string>>>("sensors"); - for(auto sen : sensors) - problem->installSensor(sen["type"], - sen["name"], - _server); - - auto processors = _server.getParam<std::vector<std::map<std::string, std::string>>>("processors"); - for(auto prc : processors) - problem->installProcessor(prc["type"], - prc["name"], - prc["sensor_name"], - _server); - + for (auto sen : sensors) + { + if (not FactorySensor::isCreatorRegistered(sen["type"])) + { + auto plugin_name = sen["plugin"]; + std::string plugin = plugins_path + "libwolf" + plugin_name + lib_extension; + WOLF_TRACE("Loading plugin " + plugin_name + " via " + plugin); + auto l = std::make_shared<LoaderRaw>(plugin); + l->load(); + loaders.push_back(l); + } + problem->installSensor(sen["type"], sen["name"], _server); + } + auto processors = _server.getParam<std::vector<std::map<std::string, std::string>>>("processors"); + for (auto prc : processors) + { + if (not FactoryProcessor::isCreatorRegistered(prc["type"])) + { + auto plugin_name = prc["plugin"]; + std::string plugin = plugins_path + "libwolf" + plugin_name + lib_extension; + WOLF_TRACE("Loading plugin " + plugin_name + " via " + plugin); + auto l = std::make_shared<LoaderRaw>(plugin); + l->load(); + loaders.push_back(l); + } + problem->installProcessor(prc["type"], prc["name"], prc["sensor_name"], _server); + } // Map (optional) std::string map_type, map_plugin; @@ -1262,4 +1278,42 @@ void Problem::perturb(double amplitude) L->perturb(amplitude); } +void Problem::transform(const VectorComposite& _transformation) +{ + std::lock_guard<std::mutex> lock_transform (mut_transform_); // Protect especially from SolverManager::update() + + transformation_ = _transformation; + transformed_.store(true); + + // Sensors + for (auto S : getHardware()->getSensorList()) + S->transform(_transformation); + + // Frames and Captures + for (auto F : getTrajectory()->getFrameMap()) + { + F.second->transform(_transformation); + for (auto& C : F.second->getCaptureList()) + C->transform(_transformation); + } + + // Landmarks + for (auto L : getMap()->getLandmarkList()) + L->transform(_transformation); +} + +bool Problem::isTransformed() const +{ + return transformed_.load(); +} +void Problem::resetTransformed() +{ + transformed_.store(false); +} +VectorComposite Problem::getTransformation() const +{ + std::lock_guard<std::mutex> lock(mut_transform_); + return transformation_; +} + } // namespace wolf diff --git a/src/processor/processor_motion.cpp b/src/processor/processor_motion.cpp index 31b2b770afc2e1c7ac80adc9d793cb24648960b7..a934fe6b741722c6931b3cfb698e36da7c1c69ae 100644 --- a/src/processor/processor_motion.cpp +++ b/src/processor/processor_motion.cpp @@ -47,6 +47,7 @@ ProcessorMotion::ProcessorMotion(const std::string& _type, MotionProvider(_state_structure, _params_motion), params_motion_(_params_motion), processing_step_(RUNNING_WITHOUT_KF), + bootstrapping_(false), x_size_(_state_size), data_size_(_data_size), delta_size_(_delta_size), @@ -202,19 +203,19 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr) } case FIRST_TIME_WITH_KF_BEFORE_INCOMING : { - // cannot joint to the KF: create own origin + // cannot join to the KF: create own origin setOrigin(getProblem()->getState(getStateStructure()), _incoming_ptr->getTimeStamp()); break; } case FIRST_TIME_WITH_KF_ON_INCOMING : { - // can joint to the KF + // can join to the KF setOrigin(keyframe_from_callback); break; } case FIRST_TIME_WITH_KF_AFTER_INCOMING : { - // cannot joint to the KF: create own origin + // cannot join to the KF: create own origin setOrigin(getProblem()->getState(getStateStructure()), _incoming_ptr->getTimeStamp()); break; } @@ -231,8 +232,10 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr) // Done at this place because setPrior() needs integrateOneStep(); + // perform bootstrap steps (usually only IMU requires this) + if (bootstrapping_) bootstrap(); - switch(processing_step_) + switch (processing_step_) { case RUNNING_WITH_KF_BEFORE_ORIGIN : { @@ -1066,7 +1069,7 @@ void ProcessorMotion::printHeader(int _depth, bool _constr_by, bool _metric, boo { _stream << _tabs << "PrcM" << id() << " " << getType() << " \"" << getName() << "\"" << std::endl; if (getOrigin()) - _stream << _tabs << " " << "o: Cap" << getOrigin()->id() << " - " << " Frm" + _stream << _tabs << " " << "o: Cap" << getOrigin()->id() << " - " << " Frm" << getOrigin()->getFrame()->id() << std::endl; if (getLast()) _stream << _tabs << " " << "l: Cap" << getLast()->id() << " - " << " Frm" diff --git a/src/sensor/sensor_base.cpp b/src/sensor/sensor_base.cpp index ab2e921d8ca025fbbc205eb3f162ac16e3b8e2a8..8f61345d8622d3417eef1bc2eac870477aa6290b 100644 --- a/src/sensor/sensor_base.cpp +++ b/src/sensor/sensor_base.cpp @@ -33,17 +33,11 @@ namespace wolf { unsigned int SensorBase::sensor_id_count_ = 0; -SpecState spec_p_2d = SpecState("StateBlock", 2,"Sensor extrinsics in 2D: position (x, y) of the sensor in robot coordinates [m]."); -SpecState spec_p_3d = SpecState("StateBlock", 3,"Sensor extrinsics in 3D: position (x, y, z) of the sensor in robot coordinates [m]."); -SpecState spec_o_2d = SpecState("StateAngle", 1,"Sensor extrinsics in 2D: orientation (yaw) of the sensor in robot coordinates [rad]."); -SpecState spec_o_3d = SpecState("StateQuaternion",4,"Sensor extrinsics in 3D: orientation (quaternion) of the sensor in robot coordinates."); - SensorBase::SensorBase(const std::string& _type, const std::string& _unique_name, const SizeEigen& _dim, ParamsSensorBasePtr _params, - const Priors& _priors, - SpecStates _state_specs) : + const Priors& _priors) : NodeBase("SENSOR", _type, _unique_name), HasStateBlocks("PO"), hardware_ptr_(), @@ -59,50 +53,8 @@ SensorBase::SensorBase(const std::string& _type, if (not params_) throw std::runtime_error("SensorBase: in constructor, ParamsSensorBase pointer is null"); - // append default P and O specs to _state_specs (if missing) - if (_state_specs.count('P') == 0) - _state_specs.emplace('P',_dim == 2 ? spec_p_2d : spec_p_3d); - if (_state_specs.count('O') == 0) - _state_specs.emplace('O', _dim == 2 ? spec_o_2d : spec_o_3d); - - // load priors - loadPriors(_priors, _dim, _state_specs); -} - -SensorBase::SensorBase(const std::string& _type, - const std::string& _unique_name, - const SizeEigen& _dim, - ParamsSensorBasePtr _params, - const ParamsServer& _server, - SpecStates _state_specs) : - NodeBase("SENSOR", _type, _unique_name), - HasStateBlocks("PO"), - hardware_ptr_(), - sensor_id_(++sensor_id_count_), // simple ID factory - params_(_params), - state_block_dynamic_(), - params_prior_map_(), - last_capture_(nullptr) -{ - assert(_dim == 2 or _dim == 3); - - // check params valid - if (not params_) - throw std::runtime_error("SensorBase: in constructor, ParamsSensorBase pointer is null"); - - // append default P and O specs to _state_specs (if missing) - if (_state_specs.count('P') == 0) - _state_specs.emplace('P',_dim == 2 ? spec_p_2d : spec_p_3d); - if (_state_specs.count('O') == 0) - _state_specs.emplace('O', _dim == 2 ? spec_o_2d : spec_o_3d); - - // create priors - Priors priors; - for (auto pair : _state_specs) - priors[pair.first] = Prior("sensor/" + _unique_name + "/states/" + std::string(1, pair.first), pair.second.type, _server); - // load priors - loadPriors(priors, _dim, _state_specs); + loadPriors(_priors, _dim); } SensorBase::~SensorBase() @@ -112,64 +64,26 @@ SensorBase::~SensorBase() } void SensorBase::loadPriors(const Priors& _priors, - SizeEigen _dim, - const SpecStates& _state_specs) + SizeEigen _dim) { assert(_dim == 2 or _dim == 3); - // check not specified priors - for (auto&& prior_pair : _priors) + // Iterate all keys in _priors + for (auto state_pair : _priors) { - if (_state_specs.count(prior_pair.first) == 0) - throw std::runtime_error("SensorBase::loadPriors: Given a prior for key " + std::string(1,prior_pair.first) + " that is not required"); - } + auto key = state_pair.first; + auto prior = state_pair.second; - // Iterate all keys in _state_specs - for (auto state_spec_pair : _state_specs) - { - auto key = state_spec_pair.first; - auto state_spec = state_spec_pair.second; - - // --- CHECKS --- - // Existence - if (_priors.count(key) == 0) - throw std::runtime_error("SensorBase: No prior found for key '" - + std::string(1,key) + - "' - should have type: " + state_spec.type + - "\nDoc:\n" + state_spec.doc); - - const Prior& prior = _priors.at(key); - // type - if (key != 'P' and key != 'O' and key != 'V') - { - if (prior.getType() != state_spec.type) - throw std::runtime_error("SensorBase: The provided prior for key '" + - std::string(1,key) + "' has wrong type: " + - prior.getType() + - " and should be: " + state_spec.type + - "\nDoc:\n" + state_spec.doc); - } - else - { - if (key == 'P' and prior.getType() != "P" and prior.getType() != "StateBlock") - throw std::runtime_error("Prior type for key P has to be 'P' or 'StateBlock'"); - if (key == 'V' and prior.getType() != "V" and prior.getType() != "StateBlock") - throw std::runtime_error("Prior type for key V has to be 'V' or 'StateBlock'"); - 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() != "StateBlock") + throw std::runtime_error("Prior type for key P has to be 'P' or 'StateBlock'"); + if (key == 'V' and prior.getType() != "V" and prior.getType() != "StateBlock") + throw std::runtime_error("Prior type for key V has to be 'V' or 'StateBlock'"); + 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'"); - // size - if (_priors.at(key).getState().size() != state_spec.size) - throw std::runtime_error("SensorBase: In constructor, provided '_priors' for key '" + - std::string(1,key) + "' has wrong size: " + - toString(prior.getState()) + - " and should be of size: " + toString(state_spec.size) + - "\nDoc:\n" + state_spec.doc); - // --- CREATE STATE BLOCK --- auto sb = FactoryStateBlock::create(prior.getType(), prior.getState(), prior.isFixed()); diff --git a/src/solver/solver_manager.cpp b/src/solver/solver_manager.cpp index c9d192a536151f0340a7fd1f8f9f5e88acf54c26..a9d3ee25ad0cc1e5d7c2a8991c221835fbcd4334 100644 --- a/src/solver/solver_manager.cpp +++ b/src/solver/solver_manager.cpp @@ -57,6 +57,9 @@ SolverManager::~SolverManager() void SolverManager::update() { + // lock mutex to prevent Problem::transform() during this update() + std::lock_guard<std::mutex> lock_transform (wolf_problem_->mut_transform_); + // Consume notification maps std::map<StateBlockPtr,Notification> sb_notification_map; std::map<FactorBasePtr,Notification> fac_notification_map; @@ -147,8 +150,9 @@ void SolverManager::update() state_ptr->resetFixUpdated(); state_ptr->resetLocalParamUpdated(); } + wolf_problem_->resetTransformed(); - #ifdef _WOLF_DEBUG +#ifdef _WOLF_DEBUG assert(check("after update()")); #endif } @@ -188,7 +192,15 @@ std::string SolverManager::solve(const ReportVerbosity report_level) { // Avoid usuless copies if (!stateblock_statevector.first->isFixed()) - stateblock_statevector.first->setState(stateblock_statevector.second, false); // false = do not raise the flag state_updated_ + { + stateblock_statevector.first->setState(stateblock_statevector.second, + false); // false = do not raise the flag state_updated_ + + // Transform the whole set of state blocks if necessary + // (this may happen when Problem::transform() has been called during the execution of this solve()) + if (wolf_problem_->isTransformed()) + stateblock_statevector.first->transform(wolf_problem_->getTransformation()); + } } auto duration_state = std::chrono::duration_cast<std::chrono::microseconds>(std::chrono::high_resolution_clock::now() - start_state); acc_duration_state_ += duration_state; diff --git a/src/state_block/has_state_blocks.cpp b/src/state_block/has_state_blocks.cpp index d152ebcc0b0361d6a222e5679b863edcb1621e2c..7e399e7d48e1af44d75c743c9c8168c5c748b8fc 100644 --- a/src/state_block/has_state_blocks.cpp +++ b/src/state_block/has_state_blocks.cpp @@ -91,6 +91,13 @@ void HasStateBlocks::perturb(double amplitude) } } +void HasStateBlocks::transform(const VectorComposite& _transformation) +{ + for (auto& pair_key_sb : state_block_map_) + pair_key_sb.second->transform(_transformation); +} + + void HasStateBlocks::printState (bool _metric, bool _state_blocks, std::ostream& _stream, std::string _tabs) const { if (_metric && _state_blocks) diff --git a/src/utils/params_server.cpp b/src/utils/params_server.cpp index 249dabd38183def44e8339129405fcc417e2ec98..2aa43047fe569b9c35a23d164ff812e78fa9a60d 100644 --- a/src/utils/params_server.cpp +++ b/src/utils/params_server.cpp @@ -108,5 +108,4 @@ std::string toString(long double _arg) return std::to_string(_arg); } - -} \ No newline at end of file +} diff --git a/test/gtest_SE3.cpp b/test/gtest_SE3.cpp index ff1ddb6d1ab166f9ee11c2dbc5821669c5114e78..5d9df91f1f67018ef774c3f3ee1eea2833f34a0f 100644 --- a/test/gtest_SE3.cpp +++ b/test/gtest_SE3.cpp @@ -103,6 +103,56 @@ TEST(SE3, log_of_power) ASSERT_MATRIX_APPROX(tau3, log(pose3), 1e-8); } +TEST(SE3, inverse) +{ + Vector3d p; p.setRandom(); + Vector4d qvec; qvec.setRandom().normalized(); + Quaterniond q(qvec); + + Vector3d pi_true = -(q.conjugate() * p); + Quaterniond qi_true = q.conjugate(); + + // separate API + Vector3d pi_out; + Quaterniond qi_out; + + inverse(p, q, pi_out, qi_out); + ASSERT_MATRIX_APPROX(pi_out, pi_true, 1e-8); + ASSERT_MATRIX_APPROX(qi_out.coeffs(), qi_true.coeffs(), 1e-8); + + // Vector7d API + Vector7d pose; pose << p, q.coeffs(); + Vector7d posei_true; posei_true << pi_true, qi_true.coeffs(); + Vector7d posei_out; + inverse(pose, posei_out); + ASSERT_MATRIX_APPROX(posei_out, posei_true, 1e-8); + + posei_out = inverse(pose); + ASSERT_MATRIX_APPROX(posei_out, posei_true, 1e-8); +} + +TEST(SE3, inverseComposite) +{ + Vector3d p; p.setRandom(); + Vector4d qvec; qvec.setRandom().normalized(); + VectorComposite pose_vc("PO", {p, qvec}); + Quaterniond q(qvec); + + // ground truth + Vector3d pi_true = -(q.conjugate() * p); + Quaterniond qi_true = q.conjugate(); + + VectorComposite pose_vc_out("PO", {3, 4}); + inverse(pose_vc, pose_vc_out); + ASSERT_MATRIX_APPROX(pose_vc_out.at('P'), pi_true, 1e-8); + ASSERT_MATRIX_APPROX(pose_vc_out.at('O'), qi_true.coeffs(), 1e-8); + + VectorComposite pose_vc_out_bis = inverse(pose_vc); + ASSERT_MATRIX_APPROX(pose_vc_out_bis.at('P'), pi_true, 1e-8); + ASSERT_MATRIX_APPROX(pose_vc_out_bis.at('O'), qi_true.coeffs(), 1e-8); + +} + TEST(SE3, composeBlocks) { Vector3d p1, p2, pc; @@ -176,6 +226,10 @@ TEST(SE3, composeVectorComposite) ASSERT_MATRIX_APPROX(xc.at('P'), pc, 1e-8); ASSERT_QUATERNION_VECTOR_APPROX(xc.at('O'), qc.coeffs(), 1e-8); + + VectorComposite vc = compose(x1, x2); + ASSERT_MATRIX_APPROX(vc.at('P'), pc, 1e-8); + ASSERT_QUATERNION_VECTOR_APPROX(vc.at('O'), qc.coeffs(), 1e-8); } TEST(SE3, composeVectorComposite_Jacobians) @@ -397,6 +451,7 @@ TEST(SE3, interpolate_third) ASSERT_POSE3d_APPROX(p_pre, p_gt, 1e-8); } + TEST(SE3, toSE3_I) { Vector7d pose; pose << 0,0,0, 0,0,0,1; diff --git a/test/gtest_factory.cpp b/test/gtest_factory.cpp index 6527cfd9bd95798cc033948d64b3878c43ce4d81..9e891659916011d79b4b719de92f6a57b3075850 100644 --- a/test/gtest_factory.cpp +++ b/test/gtest_factory.cpp @@ -44,6 +44,19 @@ TEST(TestFactory, DummyObjectFactory) DummyObjectDerived obj_derived = DummyObjectDerived("AnotherCoolDummyObject", server); } +TEST(TestFactory, isCreatorRegistered) +{ + ParserYaml parser = ParserYaml("test/yaml/params_basic.yaml", wolf_root); + ParamsServer server = ParamsServer(parser.getParams()); + + bool object_creator_registered = FactoryDummyObject::isCreatorRegistered("DummyObjectDerived"); + + ASSERT_TRUE(object_creator_registered); + + // FORCE LOADING + DummyObjectDerived obj_derived = DummyObjectDerived("AnotherCoolDummyObject", server); +} + int main(int argc, char **argv) { testing::InitGoogleTest(&argc, argv); diff --git a/test/gtest_rotation.cpp b/test/gtest_rotation.cpp index bdc5e696b1e4a012abdee9fc93b16bd647b85172..7ec49fdee46c118d8d7907e17a43f5a2c598e661 100644 --- a/test/gtest_rotation.cpp +++ b/test/gtest_rotation.cpp @@ -813,6 +813,6 @@ int main(int argc, char **argv) */ testing::InitGoogleTest(&argc, argv); - ::testing::GTEST_FLAG(filter) = "Conversions.R2q_norm_of_q"; + // ::testing::GTEST_FLAG(filter) = "Conversions.R2q_norm_of_q"; return RUN_ALL_TESTS(); } diff --git a/test/gtest_state_block.cpp b/test/gtest_state_block.cpp index 9bce46a4856e123124e3b62aed99c4e2a09a7b01..e1696bcc92f15f5f87a1aeed24fbed554e8cdbab 100644 --- a/test/gtest_state_block.cpp +++ b/test/gtest_state_block.cpp @@ -28,21 +28,21 @@ #include "core/utils/utils_gtest.h" - #include "core/state_block/state_block.h" #include "core/state_block/state_quaternion.h" #include "core/state_block/state_angle.h" +#include "core/state_block/state_block_derived.h" +#include "core/state_block/state_composite.h" #include <iostream> - using namespace Eigen; using namespace std; using namespace wolf; TEST(StateBlock, block_perturb) { - Vector3d x(10,20,30); + Vector3d x(10, 20, 30); StateBlock sb(x); sb.perturb(0.5); @@ -50,12 +50,12 @@ TEST(StateBlock, block_perturb) WOLF_INFO("original ", x.transpose(), ", perturbed = ", sb.getState().transpose()); ASSERT_NE(x.norm(), sb.getState().norm()); - ASSERT_MATRIX_APPROX(x , sb.getState() , 0.5 * 4); // 4-sigma test... + ASSERT_MATRIX_APPROX(x, sb.getState(), 0.5 * 4); // 4-sigma test... } TEST(StateBlock, angle_perturb) { - Vector1d x(1.0); + Vector1d x(1.0); StateAngle sb(x(0)); sb.perturb(0.1); @@ -63,28 +63,174 @@ TEST(StateBlock, angle_perturb) WOLF_INFO("original ", x.transpose(), ", perturbed = ", sb.getState().transpose()); ASSERT_NE(x.norm(), sb.getState().norm()); - ASSERT_MATRIX_APPROX(x , sb.getState() , 0.1 * 4); // 4-sigma test... + ASSERT_MATRIX_APPROX(x, sb.getState(), 0.1 * 4); // 4-sigma test... } TEST(StateBlock, quaternion_perturb) { - Vector4d x(0.5,0.5,0.5,0.5); + Vector4d x(0.5, 0.5, 0.5, 0.5); StateQuaternion sb(x); sb.perturb(0.01); WOLF_INFO("original ", x.transpose(), ", perturbed = ", sb.getState().transpose()); - ASSERT_LT((sb.getState().transpose() * x).norm() , 1.0); // quaternions are not parallel ==> not equal - ASSERT_MATRIX_APPROX(x , sb.getState() , 0.01 * 4); // 4-sigma test... + ASSERT_LT((sb.getState().transpose() * x).norm(), 1.0); // quaternions are not parallel ==> not equal + ASSERT_MATRIX_APPROX(x, sb.getState(), 0.01 * 4); // 4-sigma test... } -int main(int argc, char **argv) +TEST(StatePoint3d, transformable) +{ + Vector3d x(1, 2, 3); + + // default is transformable + StatePoint3d sb_tr(x); + ASSERT_TRUE(sb_tr.isTransformable()); + + // not fixed, non transformable + StatePoint3d sb_ntr(x, false, false); + ASSERT_FALSE(sb_ntr.isTransformable()); + + // not fixed, transformable + StatePoint3d sb_tr2(x, false, true); + ASSERT_TRUE(sb_tr2.isTransformable()); +} + +TEST(StatePoint3d, transform_identity) +{ + Vector3d x0(1, 2, 3); + StatePoint3d sb(x0); + + Vector7d xtrf; + xtrf << 0, 0, 0, 0, 0, 0, 1; + VectorComposite trf(xtrf, "PO", {3, 4}); + + sb.transform(trf); + ASSERT_MATRIX_APPROX(sb.getState(), x0, 1e-15); +} + +TEST(StatePoint3d, transform_translation_4x5y6z) +{ + Vector3d x0(1, 2, 3); + StatePoint3d sb(x0); + + Vector7d xtrf; + xtrf << 4, 5, 6, 0, 0, 0, 1; + VectorComposite trf(xtrf, "PO", {3, 4}); + + Vector3d x(5, 7, 9); // x0 translated (4,5,6) + + sb.transform(trf); + ASSERT_MATRIX_APPROX(sb.getState(), x, 1e-15); +} + +TEST(StatePoint3d, transform_rotation_90x) { - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); + Vector3d x0(1, 2, 3); + StatePoint3d sb(x0); + + Vector7d xtrf; + xtrf << 0, 0, 0, sqrt(2) / 2, 0, 0, sqrt(2) / 2; + VectorComposite trf(xtrf, "PO", {3, 4}); + + Vector3d x(1, -3, 2); // x0 rotated (90,0,0) + + sb.transform(trf); + ASSERT_MATRIX_APPROX(sb.getState(), x, 1e-15); } +TEST(StatePoint3d, transform_translation_1y_rotation_90x) +{ + Vector3d x0(1, 2, 3); + StatePoint3d sb(x0); + + Vector7d xtrf; + xtrf << 0, 1, 0, sqrt(2) / 2, 0, 0, sqrt(2) / 2; + VectorComposite trf(xtrf, "PO", {3, 4}); + + Vector3d x(1, -2, 2); // x0 translated (0,1,0) and rotated (90,0,0) + + sb.transform(trf); + ASSERT_MATRIX_APPROX(sb.getState(), x, 1e-15); +} + +TEST(StatePoint3d, non_transformable_translation_rotation) +{ + Vector3d x0(1, 2, 3); + StatePoint3d sb(x0, false, false); // non transformable + Vector7d xtrf; + xtrf << 0, 1, 0, sqrt(2) / 2, 0, 0, sqrt(2) / 2; + VectorComposite trf(xtrf, "PO", {3, 4}); + Vector3d x(1, 2, 3); // x0 NOT translated (0,1,0) and NOT rotated (90,0,0) + sb.transform(trf); + ASSERT_MATRIX_APPROX(sb.getState(), x, 1e-15); +} + +TEST(StateVector3d, transform_translation_1y_rotation_90x) +{ + Vector3d x0(1, 2, 3); + StateVector3d sb(x0); + + Vector7d xtrf; + xtrf << 0, 1, 0, sqrt(2) / 2, 0, 0, sqrt(2) / 2; + VectorComposite trf(xtrf, "PO", {3, 4}); + + Vector3d x(1, -3, 2); // x0 NOT translated (0,1,0) and rotated (90,0,0) + + sb.transform(trf); + ASSERT_MATRIX_APPROX(sb.getState(), x, 1e-15); +} + +TEST(StatePoint2d, transform_translation_1y_rotation_90) +{ + Vector2d x0(1, 2); + StatePoint2d sb(x0); + + Vector3d xtrf; + xtrf << 0, 1, M_PI / 2; + VectorComposite trf(xtrf, "PO", {2, 1}); + + Vector2d x(-2, 2); // x0 translated (0,1) and rotated 90 deg + + sb.transform(trf); + ASSERT_MATRIX_APPROX(sb.getState(), x, 1e-15); +} + +TEST(StateVector2d, transform_translation_1y_rotation_90) +{ + Vector2d x0(1, 2); + StateVector2d sb(x0); + + Vector3d xtrf; + xtrf << 0, 1, M_PI / 2; + VectorComposite trf(xtrf, "PO", {2, 1}); + + Vector2d x(-2, 1); // x0 NON translated (0,1) and rotated 90 deg + + sb.transform(trf); + ASSERT_MATRIX_APPROX(sb.getState(), x, 1e-15); +} + +TEST(StateParams4, transform_translation_1y_rotation_90x) +{ + Vector4d x0(1, 2, 3, 4); + StateParams<4> sb(x0); + + Vector7d xtrf; + xtrf << 0, 1, 0, sqrt(2) / 2, 0, 0, sqrt(2) / 2; + VectorComposite trf(xtrf, "PO", {3, 4}); + + Vector4d x(1, 2, 3, 4); // x0 NOT translated (0,1,0) and NOT rotated (90,0,0) + + sb.transform(trf); + ASSERT_MATRIX_APPROX(sb.getState(), x, 1e-15); +} + +int main(int argc, char **argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +}