diff --git a/include/core/common/factory.h b/include/core/common/factory.h index c02474f8924ba06224221dc651f08e259bf92095..358c86895c1c8fcfdf026548925e5d290640ca7b 100644 --- a/include/core/common/factory.h +++ b/include/core/common/factory.h @@ -398,20 +398,6 @@ inline void Factory<TypeBase, TypeInput...>::printCallbacks() namespace wolf { -// Some specializations -//====================== - -// Landmarks from YAML -class LandmarkBase; -typedef Factory<std::shared_ptr<LandmarkBase>, - const YAML::Node&> FactoryLandmark; -template<> -inline std::string FactoryLandmark::getClass() const -{ - return "FactoryLandmark"; -} - - #ifdef __GNUC__ #define WOLF_UNUSED __attribute__((used)) #elif defined _MSC_VER diff --git a/include/core/landmark/factory_landmark.h b/include/core/landmark/factory_landmark.h new file mode 100644 index 0000000000000000000000000000000000000000..5e162739e35601302db94a26fd31c246f5a52b97 --- /dev/null +++ b/include/core/landmark/factory_landmark.h @@ -0,0 +1,167 @@ +//--------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-------- +#pragma once + +namespace wolf +{ +class LandmarkBase; +} + +#include <unordered_map> + +// wolf +#include "core/common/factory.h" + +// yaml +#include "yaml-cpp/yaml.h" + +namespace wolf +{ + +/** \brief Landmark factory class + * + * This factory can create objects of classes deriving from LandmarkBase. + * + * Specific object creation is invoked by `create(TYPE, ... )`, and the TYPE of landmark is identified with a string. + * + * Find general Factory documentation in class Factory: + * - Access the factory + * - Register/unregister creators + * - Invoke object creation + * + * This documentation shows you how to use the FactoryLandmark specifically: + * - Write landmark creators. + * - Create landmarks + * + * #### Write landmark creators + * Landmark creators have the following API: + * + * \code + * static LandmarkBasePtr create(const YAML::Node&); + * \endcode + * + * They follow the general implementations shown below: + * + * \code + * static LandmarkBasePtr create(const YAML::Node& _node) + * { + * return std::make_shared<LandmarkDerived>("LandmarkDerived", _node); + * } + * \endcode + * + * #### Creating landmarks + * Note: Prior to invoking the creation of a landmark of a particular type, + * you must register the creator for this type into the factory. + * + * To create e.g. a LandmarkCorner in 3D, you type: + * + * \code + * auto corner_ptr = FactoryLandmark::create("LandmarkCorner", _node); + * \endcode + * + * We RECOMMEND using the macro WOLF_LANDMARK_CREATE(LandmarkClass) to automatically add the landmark creator, provided in 'landmark_base.h'. + * + * To do so, you should implement a constructor with the API: + * + * \code + * LandmarkDerived(const YAML::Node& _node) : + * LandmarkBase("LandmarkDerived", _node) + * { + * < CODE > + * } + * \endcode + * + * #### Registering landmark creator into the factory + * Registration can be done manually or automatically. It involves the call to static functions. + * It is advisable to put these calls within unnamed namespaces. + * + * - __Manual registration__: you control registration at application level. + * Put the code either at global scope (you must define a dummy variable for this), + * \code + * namespace { + * const bool registered_corner = FactoryLandmark::registerCreator("LandmarkCorner", LandmarkCorner::create); + * } + * main () { ... } + * \endcode + * or inside your main(), where a direct call is possible: + * \code + * main () { + * FactoryLandmark::registerCreator("LandmarkCorner", LandmarkCorner::create); + * ... + * } + * \endcode + * + * - __Automatic registration__: registration is performed at library level. + * Put the code at the last line of the landmark_xxx.cpp file, + * \code + * namespace { + * const bool registered_corner = FactoryLandmark::registerCreator("LandmarkCorner", LandmarkCorner::create); + * } + * \endcode + * + * Automatic registration is recommended in wolf, and implemented in the classes shipped with it using the macro + * WOLF_REGISTER_LANDMARK(LandmarkType). + * You are free to comment out these lines and place them wherever you consider it more convenient for your design. + * + * #### Example 2: creating landmarks + * We finally provide the necessary steps to create a landmark of class LandmarkCorner in our application: + * + * \code + * #include "core/landmark/factory_landmark.h" + * #include "vision/landmark/landmark_corner.h" // provides LandmarkCorner + * + * // Note: LandmarkCorner::create() is already registered, automatically. + * + * using namespace wolf; + * int main() { + * + * // To create a corner, provide: + * // a type = "LandmarkCorner", + * // a YAML node with the info + * + * LandmarkBasePtr corner_1_ptr = + * FactoryLandmark::create ( "LandmarkCorner", + * node ); + * + * LandmarkBasePtr corner_2_ptr = + * FactoryLandmark::create( "LandmarkCorner" , + * node2 ); + * + * return 0; + * } + * \endcode + */ + +typedef Factory<std::shared_ptr<LandmarkBase>, + const YAML::Node&> FactoryLandmark; + +template<> +inline std::string FactoryLandmark::getClass() const +{ + return "FactoryLandmark"; +} + +#define WOLF_REGISTER_LANDMARK(LandmarkType) \ + namespace{ const bool WOLF_UNUSED LandmarkType##Registered = \ + FactoryLandmark::registerCreator(#LandmarkType, LandmarkType::create);} \ + +} /* namespace wolf */ \ No newline at end of file diff --git a/include/core/landmark/landmark_base.h b/include/core/landmark/landmark_base.h index 3f7bd7adb38a3f263361bd206534e6f06a63a691..fc62fc8068317a66ae97f47320d85b532eca672d 100644 --- a/include/core/landmark/landmark_base.h +++ b/include/core/landmark/landmark_base.h @@ -39,6 +39,22 @@ class StateBlock; #include "yaml-cpp/yaml.h" namespace wolf { +/* + * Macro for defining Autoconf landmark creator. + * + * Place a call to this macro inside your class declaration (in the landmark_class.h file), + * preferably just after the constructors. + * + * In order to use this macro, the derived landmark class, LandmarkClass, + * must have two constructors available with the API: + * + * LandmarkClass(const YAML::Node& _node); + */ +#define WOLF_LANDMARK_CREATE(LandmarkClass) \ +static LandmarkBasePtr create(const YAML::Node& _node) \ +{ \ + return std::make_shared<LandmarkClass>(_node); \ +} \ //class LandmarkBase class LandmarkBase : public NodeBase, public HasStateBlocks, public std::enable_shared_from_this<LandmarkBase> @@ -48,9 +64,6 @@ class LandmarkBase : public NodeBase, public HasStateBlocks, public std::enable_ private: MapBaseWPtr map_ptr_; FactorBasePtrList constrained_by_list_; - //std::vector<StateBlockPtr> state_block_vec_; ///< vector of state blocks, additional to those in the state_block_map from HasStateBlocks. - //std::vector<StateBlockConstPtr> state_block_const_vec_; ///< vector of state blocks, additional to those in the state_block_map from HasStateBlocks. - static unsigned int landmark_id_count_; protected: @@ -58,7 +71,6 @@ class LandmarkBase : public NodeBase, public HasStateBlocks, public std::enable_ void setProblem(ProblemPtr _problem) override final; unsigned int landmark_id_; ///< landmark unique id TimeStamp stamp_; ///< stamp of the creation of the landmark - Eigen::VectorXd descriptor_; //TODO: agree? JS: No: It is not general enough as descriptor to be in LmkBase. public: @@ -68,27 +80,37 @@ class LandmarkBase : public NodeBase, public HasStateBlocks, public std::enable_ * \param _tp indicates landmark type.(types defined at wolf.h) * \param _p_ptr StateBlock pointer to the position * \param _o_ptr StateBlock pointer to the orientation (default: nullptr) - * **/ LandmarkBase(const std::string& _type, StateBlockPtr _p_ptr, StateBlockPtr _o_ptr = nullptr); + + /** \brief Constructor with type and a YAML node (to be used by the derived classes YAML node constructors) + * + * Constructor with type, and YAML node + * \param _tp indicates landmark type.(types defined at wolf.h) + * \param _n YAML node containing the necessary information + **/ + LandmarkBase(const std::string& _type, const YAML::Node& _n); + + /** \brief Constructor with YAML node + * + * Constructor with YAML node (type assumed "LandmarkBase") + * \param _n YAML node containing the necessary information + **/ + LandmarkBase(const YAML::Node& _n); + WOLF_LANDMARK_CREATE(LandmarkBase); + ~LandmarkBase() override; virtual void remove(bool viral_remove_empty_parent=false); - virtual YAML::Node saveToYaml() const; + virtual YAML::Node toYaml() const; // Properties unsigned int id() const; void setId(unsigned int _id); // State blocks - //std::vector<StateBlockConstPtr> getUsedStateBlockVec() const; - //std::vector<StateBlockPtr> getUsedStateBlockVec(); bool getCovariance(Eigen::MatrixXd& _cov) const; - // Descriptor public: - const Eigen::VectorXd& getDescriptor() const; - double getDescriptor(unsigned int _ii) const; - void setDescriptor(const Eigen::VectorXd& _descriptor); unsigned int getHits() const; FactorBaseConstPtrList getConstrainedByList() const; @@ -102,12 +124,6 @@ class LandmarkBase : public NodeBase, public HasStateBlocks, public std::enable_ template<typename classType, typename... T> static std::shared_ptr<classType> emplace(MapBasePtr _map_ptr, T&&... all); - /** \brief Creator for Factory<LandmarkBase, YAML::Node> - * Caution: This creator does not set the landmark's anchor frame and sensor. - * These need to be set afterwards. - */ - static LandmarkBasePtr create(const YAML::Node& _node); - virtual void printHeader(int depth, // bool constr_by, // bool metric, // @@ -193,20 +209,4 @@ inline FactorBasePtrList LandmarkBase::getConstrainedByList() return constrained_by_list_; } -inline void LandmarkBase::setDescriptor(const Eigen::VectorXd& _descriptor) -{ - descriptor_ = _descriptor; -} - -inline double LandmarkBase::getDescriptor(unsigned int _ii) const -{ - assert(_ii < descriptor_.size() && "LandmarkBase::getDescriptor: bad index"); - return descriptor_(_ii); -} - -inline const Eigen::VectorXd& LandmarkBase::getDescriptor() const -{ - return descriptor_; -} - } // namespace wolf \ No newline at end of file diff --git a/include/core/state_block/composite.h b/include/core/state_block/composite.h index 2e1abc2dd54cb29c2fb96139ff381289cda690b0..6fec7d76a82fed670bbf367237213e1fc195d8fe 100644 --- a/include/core/state_block/composite.h +++ b/include/core/state_block/composite.h @@ -21,6 +21,7 @@ //--------LICENSE_END-------- #pragma once +#include "core/common/wolf.h" #include <string> #include <map> #include <algorithm> @@ -57,6 +58,8 @@ class Composite : public map<char, T> } return _os; } + + YAML::Node toYaml() const; }; template <typename T> @@ -74,15 +77,22 @@ inline CompositeOther Composite<T>::cast() const template <typename T> inline Composite<T>::Composite(const YAML::Node& _n) { - if (not _n.IsMap()) + WOLF_INFO("constructor composite..."); + if (_n.IsDefined()) { - throw std::runtime_error("SpecComposite: constructor with a non-map yaml node"); - } + WOLF_INFO("defined...", _n); + if (not _n.IsMap()) + { + throw std::runtime_error("SpecComposite: constructor with a non-map yaml node"); + } - for (auto spec_pair : _n) - { - this->emplace(spec_pair.first.as<char>(), T(spec_pair.second)); + for (auto spec_pair : _n) + { + WOLF_INFO("iterating...", spec_pair.first, spec_pair.second); + this->emplace(spec_pair.first.as<char>(), T(spec_pair.second)); + } } + WOLF_INFO("constructor composite done"); } template <typename T> @@ -114,4 +124,16 @@ inline StateKeys Composite<T>::getKeys() const return keys; } +template <typename T> +inline YAML::Node Composite<T>::toYaml() const +{ + YAML::Node n; + for (auto&& pair : *this) + { + n[pair.first] = pair.second.toYaml(); + } + + return n; +} + } // namespace wolf \ No newline at end of file diff --git a/include/core/state_block/has_state_blocks.h b/include/core/state_block/has_state_blocks.h index d143262215279bf247464e017a7cb842423c351d..71308a397edb94da2f06b628b6838443495a15cd 100644 --- a/include/core/state_block/has_state_blocks.h +++ b/include/core/state_block/has_state_blocks.h @@ -43,6 +43,7 @@ class HasStateBlocks virtual ~HasStateBlocks(); StateKeys getKeys() const { return keys_order_; } + SpecComposite getSpecs() const; void appendToStructure(const char& _new_key){ keys_order_ += _new_key; } bool isInStructure(const char& _sb_key) const { return keys_order_.find(_sb_key) != std::string::npos; } diff --git a/include/core/state_block/spec_composite.h b/include/core/state_block/spec_composite.h index 688caea505b38e199f24bbd51c370b21cad76f47..81e38cd10165e352ef431ad7c636fb1666c7492f 100644 --- a/include/core/state_block/spec_composite.h +++ b/include/core/state_block/spec_composite.h @@ -26,7 +26,6 @@ #include "core/state_block/vector_composite.h" #include <string> -#include <unordered_map> #include <eigen3/Eigen/Dense> #include "yaml-cpp/yaml.h" @@ -64,6 +63,8 @@ class SpecState virtual std::string print(const std::string& _spaces = "") const; friend std::ostream& operator <<(std::ostream &_os, const wolf::SpecState &_x); + + YAML::Node toYaml() const; }; template <typename T> diff --git a/include/core/state_block/state_angle.h b/include/core/state_block/state_angle.h index d33f53a1a923b1380c8a04c1a4266cb434cf1aa1..160ddbce32e96453eb43572db72bfb8f8761d28b 100644 --- a/include/core/state_block/state_angle.h +++ b/include/core/state_block/state_angle.h @@ -43,13 +43,13 @@ class StateAngle : public StateBlock }; inline StateAngle::StateAngle(double _angle, bool _fixed, bool _transformable) : - StateBlock(1, _fixed, std::make_shared<LocalParametrizationAngle>(), _transformable) + StateBlock("StateAngle", 1, _fixed, std::make_shared<LocalParametrizationAngle>(), _transformable) { state_(0) = pi2pi(_angle); } inline StateAngle::StateAngle(const VectorXd& _angle, bool _fixed, bool _transformable) : - StateBlock(1, _fixed, std::make_shared<LocalParametrizationAngle>(), _transformable) + StateBlock("StateAngle", 1, _fixed, std::make_shared<LocalParametrizationAngle>(), _transformable) { if(_angle.size() != 1) throw std::runtime_error("The angle state vector must be of size 1!"); diff --git a/include/core/state_block/state_block.h b/include/core/state_block/state_block.h index d19d3155a28d8b95e273a3f8f9e9bbde97a8564b..6a65d6447c1a7591cd0158405cc3cfde86473c4c 100644 --- a/include/core/state_block/state_block.h +++ b/include/core/state_block/state_block.h @@ -77,6 +77,7 @@ public: protected: + std::string type_; ///< Type of the derived class std::atomic_bool fixed_; ///< Key to indicate whether the state is fixed or not std::atomic<SizeEigen> state_size_; ///< State vector size @@ -98,7 +99,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, bool _transformable = true); + StateBlock(const std::string& _type, const SizeEigen _size, bool _fixed = false, LocalParametrizationBasePtr _local_param_ptr = nullptr, bool _transformable = true); /** \brief Constructor from vector * @@ -106,7 +107,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, bool _transformable = true); + StateBlock(const std::string& _type, const Eigen::VectorXd& _state, bool _fixed = false, LocalParametrizationBasePtr _local_param_ptr = nullptr, bool _transformable = true); ///< Explicitly not copyable/movable StateBlock(const StateBlock& o) = delete; @@ -117,6 +118,10 @@ public: **/ virtual ~StateBlock(); + /** \brief Returns the type of the state block + **/ + std::string getType() const; + /** \brief Returns the state vector **/ Eigen::VectorXd getState() const; @@ -229,7 +234,8 @@ public: namespace wolf { -inline StateBlock::StateBlock(const Eigen::VectorXd& _state, bool _fixed, LocalParametrizationBasePtr _local_param_ptr, bool _transformable) : +inline StateBlock::StateBlock(const std::string& _type, const Eigen::VectorXd& _state, bool _fixed, LocalParametrizationBasePtr _local_param_ptr, bool _transformable) : + type_(_type), fixed_(_fixed), state_size_(_state.size()), state_(_state), @@ -242,7 +248,8 @@ inline StateBlock::StateBlock(const Eigen::VectorXd& _state, bool _fixed, LocalP // std::cout << "constructed +sb" << std::endl; } -inline StateBlock::StateBlock(const SizeEigen _size, bool _fixed, LocalParametrizationBasePtr _local_param_ptr, bool _transformable) : +inline StateBlock::StateBlock(const std::string& _type, const SizeEigen _size, bool _fixed, LocalParametrizationBasePtr _local_param_ptr, bool _transformable) : + type_(_type), fixed_(_fixed), state_size_(_size), state_(Eigen::VectorXd::Zero(_size)), @@ -260,6 +267,11 @@ inline StateBlock::~StateBlock() // std::cout << "destructed -sb" << std::endl; } +inline std::string StateBlock::getType() const +{ + return type_; +} + inline Eigen::VectorXd StateBlock::getState() const { std::lock_guard<std::mutex> lock(mut_state_); diff --git a/include/core/state_block/state_block_derived.h b/include/core/state_block/state_block_derived.h index 6f0c50feeb7301ae6e90ea076cf5d311b07418cd..8e07be2c06de256f2d90524d902fc3851ef1c752 100644 --- a/include/core/state_block/state_block_derived.h +++ b/include/core/state_block/state_block_derived.h @@ -40,10 +40,10 @@ class StateParams : public StateBlock { public: StateParams(const Eigen::VectorXd& _state, bool _fixed = false) : - StateBlock(_state, _fixed, nullptr, false) + StateBlock("StateParams" + toString(size), _state, _fixed, nullptr, false) { if (_state.size() != size) - throw std::runtime_error("Wrong vector size for StateParams."); + throw std::runtime_error("Wrong vector size for StateParams" + toString(size) + ":" + toString(_state.size())); } static Eigen::VectorXd Identity() { @@ -80,10 +80,10 @@ class StatePoint2d : public StateBlock { public: StatePoint2d(const Eigen::VectorXd& _state, bool _fixed = false, bool _transformable = true) - : StateBlock(_state, _fixed, nullptr, _transformable) + : StateBlock("StatePoint2d", _state, _fixed, nullptr, _transformable) { if (_state.size() != 2) - throw std::runtime_error("Wrong vector size for StatePoint2d."); + throw std::runtime_error("Wrong vector size for StatePoint2d:" + toString(_state.size())); } static Eigen::VectorXd Identity() { @@ -110,10 +110,10 @@ class StateVector2d : public StateBlock { public: StateVector2d(const Eigen::VectorXd& _state, bool _fixed = false, bool _transformable = true) - : StateBlock(_state, _fixed, nullptr, _transformable) + : StateBlock("StateVector2d", _state, _fixed, nullptr, _transformable) { if (_state.size() != 2) - throw std::runtime_error("Wrong vector size for StateVector2d."); + throw std::runtime_error("Wrong vector size for StateVector2d:" + toString(_state.size())); } static Eigen::VectorXd Identity() { @@ -141,10 +141,10 @@ class StatePoint3d : public StateBlock StatePoint3d(const Eigen::VectorXd& _state, bool _fixed = false, bool _transformable = true) - : StateBlock(_state, _fixed, nullptr, _transformable) + : StateBlock("StatePoint3d", _state, _fixed, nullptr, _transformable) { if (_state.size() != 3) - throw std::runtime_error("Wrong vector size for StatePoint3d."); + throw std::runtime_error("Wrong vector size for StatePoint3d:" + toString(_state.size())); } static Eigen::VectorXd Identity() { @@ -172,10 +172,10 @@ class StateVector3d : public StateBlock { public: StateVector3d(const Eigen::VectorXd& _state, bool _fixed = false, bool _transformable = true) - : StateBlock(_state, _fixed, nullptr, _transformable) + : StateBlock("StateVector3d", _state, _fixed, nullptr, _transformable) { if (_state.size() != 3) - throw std::runtime_error("Wrong vector size for StateVector3d."); + throw std::runtime_error("Wrong vector size for StateVector3d:" + toString(_state.size())); } static Eigen::VectorXd Identity() { diff --git a/include/core/state_block/state_homogeneous_3d.h b/include/core/state_block/state_homogeneous_3d.h index 357a976044f3574e17d9c405e0119aa338725e22..9d62b312ef2c3c98f2d207725cece7b2ae69ffbc 100644 --- a/include/core/state_block/state_homogeneous_3d.h +++ b/include/core/state_block/state_homogeneous_3d.h @@ -42,7 +42,7 @@ class StateHomogeneous3d : public StateBlock }; inline StateHomogeneous3d::StateHomogeneous3d(const Eigen::VectorXd _state, bool _fixed, bool _transformable) : - StateBlock(_state, _fixed, nullptr, _transformable) + StateBlock("StateHomogeneous3d", _state, _fixed, nullptr, _transformable) { if(_state.size() != 4) throw std::runtime_error("Homogeneous 3d must be size 4."); @@ -50,7 +50,7 @@ inline StateHomogeneous3d::StateHomogeneous3d(const Eigen::VectorXd _state, bool } inline StateHomogeneous3d::StateHomogeneous3d(bool _fixed, bool _transformable) : - StateBlock(4, _fixed, nullptr, _transformable) + StateBlock("StateHomogeneous3d", 4, _fixed, nullptr, _transformable) { local_param_ptr_ = std::make_shared<LocalParametrizationHomogeneous>(); state_ << 0, 0, 0, 1; // Set origin diff --git a/include/core/state_block/state_quaternion.h b/include/core/state_block/state_quaternion.h index 13632c59ee2d6f1cd102a9cb7996ffcbff85d1a3..4246e72fb3500bbb5e0ca807566dc2f7eedc54a5 100644 --- a/include/core/state_block/state_quaternion.h +++ b/include/core/state_block/state_quaternion.h @@ -43,7 +43,7 @@ class StateQuaternion : public StateBlock }; inline StateQuaternion::StateQuaternion(const Eigen::Quaterniond& _quaternion, bool _fixed, bool _transformable) : - StateBlock(_quaternion.coeffs(), _fixed, std::make_shared<LocalParametrizationQuaternion<DQ_LOCAL>>(), _transformable) + StateBlock("StateQuaternion", _quaternion.coeffs(), _fixed, std::make_shared<LocalParametrizationQuaternion<DQ_LOCAL>>(), _transformable) { // TODO: remove these lines after issue #381 is addressed if(not isValid(1e-5)) @@ -53,7 +53,7 @@ inline StateQuaternion::StateQuaternion(const Eigen::Quaterniond& _quaternion, b } inline StateQuaternion::StateQuaternion(const Eigen::VectorXd& _state, bool _fixed, bool _transformable) : - StateBlock(_state, _fixed, std::make_shared<LocalParametrizationQuaternion<DQ_LOCAL>>(), _transformable) + StateBlock("StateQuaternion", _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!"); @@ -66,7 +66,7 @@ inline StateQuaternion::StateQuaternion(const Eigen::VectorXd& _state, bool _fix } inline StateQuaternion::StateQuaternion(bool _fixed, bool _transformable) : - StateBlock(4, _fixed, std::make_shared<LocalParametrizationQuaternion<DQ_LOCAL>>(), _transformable) + StateBlock("StateQuaternion", 4, _fixed, std::make_shared<LocalParametrizationQuaternion<DQ_LOCAL>>(), _transformable) { state_ = Eigen::Quaterniond::Identity().coeffs(); // diff --git a/include/core/state_block/type_composite.h b/include/core/state_block/type_composite.h index 5835e2e2b232434475249a88ee71feb8afec9079..392f718b57b47dd7b677d10b823e3d352a9c3820 100644 --- a/include/core/state_block/type_composite.h +++ b/include/core/state_block/type_composite.h @@ -41,10 +41,24 @@ inline Composite<std::string>::Composite(const YAML::Node& _n) for (auto spec_pair : _n) { - this->emplace(spec_pair.first.as<char>(), spec_pair.second.as<std::string>()); + this->emplace(spec_pair.first.as<char>(), spec_pair.second["type"].as<std::string>()); } } +template <> +inline YAML::Node Composite<std::string>::toYaml() const +{ + YAML::Node n; + for (auto&& pair : *this) + { + YAML::Node n_type; + n_type["type"] = pair.second; + n[pair.first] = n_type; + } + + return n; +} + inline bool isTypeCompositeValid(const TypeComposite& _types, SizeEigen _dim) { try diff --git a/include/core/state_block/vector_composite.h b/include/core/state_block/vector_composite.h index ebed1706395b60e87650c1d621bad052a8c44b45..5e1447f858b63a10610b764c2e3e5bdced073b58 100644 --- a/include/core/state_block/vector_composite.h +++ b/include/core/state_block/vector_composite.h @@ -24,6 +24,8 @@ #include "core/common/wolf.h" #include "core/state_block/composite.h" +#include "yaml-schema-cpp/yaml_conversion.hpp" + #include <unordered_map> #include <iostream> @@ -81,4 +83,34 @@ class VectorComposite : public Composite<Eigen::VectorXd> // friend wolf::VectorComposite operator -(const wolf::VectorComposite &_x); }; +template <> +inline Composite<Eigen::VectorXd>::Composite(const YAML::Node& _n) +{ + if (not _n.IsMap()) + { + throw std::runtime_error("SpecComposite: constructor with a non-map yaml node"); + } + + for (auto spec_pair : _n) + { + this->emplace(spec_pair.first.as<char>(), spec_pair.second["state"].as<Eigen::VectorXd>()); + } +} + +template <> +inline YAML::Node Composite<Eigen::VectorXd>::toYaml() const +{ + YAML::Node n; + for (auto&& pair : *this) + { + YAML::Node n_type; + n_type["state"] = pair.second; + n[pair.first] = n_type; + } + + return n; +} + + + } \ No newline at end of file diff --git a/schema/sensor/SensorMotionModel.schema b/schema/sensor/SensorMotionModel.schema new file mode 100644 index 0000000000000000000000000000000000000000..9ff4a8f839cc1bec658f6c111fc6214889d29b27 --- /dev/null +++ b/schema/sensor/SensorMotionModel.schema @@ -0,0 +1 @@ +follow: SensorBase.schema \ No newline at end of file diff --git a/src/landmark/landmark_base.cpp b/src/landmark/landmark_base.cpp index 82f91377428bb34ab7b46c29bac5b1adc5890626..bd9a2fe5f13657b8bafd79df7547645647d0e3f4 100644 --- a/src/landmark/landmark_base.cpp +++ b/src/landmark/landmark_base.cpp @@ -38,7 +38,6 @@ LandmarkBase::LandmarkBase(const std::string& _type, StateBlockPtr _p_ptr, State NodeBase("LANDMARK", _type), HasStateBlocks(""), map_ptr_(), - //state_block_vec_(0), // Resize in derived constructors if needed. landmark_id_(++landmark_id_count_) { if (_p_ptr) @@ -52,6 +51,22 @@ LandmarkBase::LandmarkBase(const std::string& _type, StateBlockPtr _p_ptr, State } +LandmarkBase::LandmarkBase(const std::string& _type, const YAML::Node& _n) : + NodeBase("LANDMARK", _type), + HasStateBlocks(SpecComposite(_n["states"])), + map_ptr_(), + landmark_id_(_n["id"].as<int>()) +{ +} + +LandmarkBase::LandmarkBase(const YAML::Node& _n) : + NodeBase("LANDMARK", "LandmarkBase"), + HasStateBlocks(SpecComposite(_n["states"])), + map_ptr_(), + landmark_id_(_n["id"].as<int>()) +{ +} + LandmarkBase::~LandmarkBase() { removeStateBlocks(getProblem()); @@ -85,66 +100,17 @@ void LandmarkBase::remove(bool viral_remove_empty_parent) } } -// std::vector<StateBlockConstPtr> LandmarkBase::getUsedStateBlockVec() const -// { -// std::vector<StateBlockConstPtr> used_state_block_vec(0); - -// // normal state blocks in {P,O,V,W} -// for (auto key : getKeys()) -// { -// const auto sbp = getStateBlock(key); -// if (sbp) -// used_state_block_vec.push_back(sbp); -// } - -// // // other state blocks in a vector -// // for (auto sbp : state_block_vec_) -// // if (sbp) -// // used_state_block_vec.push_back(sbp); - -// return used_state_block_vec; -// } - -// std::vector<StateBlockPtr> LandmarkBase::getUsedStateBlockVec() -// { -// std::vector<StateBlockPtr> used_state_block_vec(0); - -// // normal state blocks in {P,O,V,W} -// for (auto key : getKeys()) -// { -// auto sbp = getStateBlock(key); -// if (sbp) -// used_state_block_vec.push_back(sbp); -// } - -// // // other state blocks in a vector -// // for (auto sbp : state_block_vec_) -// // if (sbp) -// // used_state_block_vec.push_back(sbp); - -// return used_state_block_vec; -// } - bool LandmarkBase::getCovariance(Eigen::MatrixXd& _cov) const { return getProblem()->getLandmarkCovariance(shared_from_this(), _cov); } -YAML::Node LandmarkBase::saveToYaml() const +YAML::Node LandmarkBase::toYaml() const { YAML::Node node; node["id"] = landmark_id_; node["type"] = node_type_; - if (getP() != nullptr) - { - node["position"] = getP()->getState(); - node["position fixed"] = getP()->isFixed(); - } - if (getO() != nullptr) - { - node["orientation"] = getO()->getState(); - node["orientation fixed"] = getO()->isFixed(); - } + node["states"] = HasStateBlocks::getSpecs().toYaml(); return node; } @@ -211,31 +177,6 @@ void LandmarkBase::print(int _depth, bool _constr_by, bool _metric, bool _state_ { printHeader(_depth, _constr_by, _metric, _state_blocks, _stream, _tabs); } -LandmarkBasePtr LandmarkBase::create(const YAML::Node& _node) -{ - unsigned int id = _node["id"] .as< unsigned int >(); - Eigen::VectorXd pos = _node["position"] .as< Eigen::VectorXd >(); - bool pos_fixed = _node["position fixed"] .as< bool >(); - - StateBlockPtr pos_sb = FactoryStateBlock::create("P", pos, pos_fixed); - StateBlockPtr ori_sb = nullptr; - - if (_node["orientation"]) - { - Eigen::VectorXd ori = _node["orientation"].as< Eigen::VectorXd >(); - bool ori_fixed = _node["orientation fixed"].as< bool >(); - - if (ori.size() == 4) - ori_sb = std::make_shared<StateQuaternion>(ori, ori_fixed); - else - ori_sb = std::make_shared<StateAngle>(ori(0), ori_fixed); - } - - LandmarkBasePtr lmk = std::make_shared<LandmarkBase>("LandmarkBase", pos_sb, ori_sb); - lmk->setId(id); - - return lmk; -} CheckLog LandmarkBase::localCheck(bool _verbose, LandmarkBaseConstPtr _lmk_ptr, std::ostream& _stream, std::string _tabs) const { @@ -324,10 +265,11 @@ bool LandmarkBase::check(CheckLog& _log, NodeBaseConstPtr _node_ptr, bool _verbo return _log.is_consistent_; } -// Register landmark creator -namespace -{ -const bool WOLF_UNUSED registered_lmk_base = FactoryLandmark::registerCreator("LandmarkBase", LandmarkBase::create); -} } // namespace wolf + +// Register landmark creator +#include "core/landmark/factory_landmark.h" +namespace wolf { +WOLF_REGISTER_LANDMARK(LandmarkBase); +} \ No newline at end of file diff --git a/src/map/map_base.cpp b/src/map/map_base.cpp index ebd6038e83a51604023853e17d78dede44e8e34d..c2a59554ae320987eeb8b71acd30107881b95b5b 100644 --- a/src/map/map_base.cpp +++ b/src/map/map_base.cpp @@ -23,7 +23,7 @@ // wolf #include "core/map/map_base.h" #include "core/landmark/landmark_base.h" -#include "core/common/factory.h" +#include "core/landmark/factory_landmark.h" // YAML #include <yaml-cpp/yaml.h> @@ -79,7 +79,6 @@ void MapBase::load(const std::string& _map_file_dot_yaml) LandmarkBasePtr lmk_ptr = FactoryLandmark::create(lmk_node["type"].as<std::string>(), lmk_node); lmk_ptr->link(shared_from_this()); } - } void MapBase::save(const std::string& _map_file_yaml, const std::string& _map_name) @@ -96,7 +95,7 @@ void MapBase::save(const std::string& _map_file_yaml, const std::string& _map_na for (LandmarkBasePtr lmk_ptr : getLandmarkList()) { - emitter << YAML::Flow << lmk_ptr->saveToYaml(); + emitter << YAML::Flow << lmk_ptr->toYaml(); } emitter << YAML::EndSeq << YAML::EndMap; diff --git a/src/state_block/has_state_blocks.cpp b/src/state_block/has_state_blocks.cpp index 84668cfff6d8ba7f9996255bef11d82d3bc0ad8c..b7e3568c608a36084b3d04ff95e6f4239a0e7dad 100644 --- a/src/state_block/has_state_blocks.cpp +++ b/src/state_block/has_state_blocks.cpp @@ -93,6 +93,18 @@ HasStateBlocks::HasStateBlocks(const TypeComposite& _types, const VectorComposit } } +SpecComposite HasStateBlocks::getSpecs() const +{ + SpecComposite specs; + for (auto && state_pair : state_block_map_) + { + specs.emplace(state_pair.first, SpecState(state_pair.second->getType(), + state_pair.second->getState(), + state_pair.second->isFixed() ? "fix" : "initial_guess")); + } + return specs; +} + StateBlockPtr HasStateBlocks::addStateBlock(const char& _sb_key, const StateBlockPtr& _sb, ProblemPtr _problem) { assert(state_block_map_.count(_sb_key) == 0 and diff --git a/src/state_block/spec_composite.cpp b/src/state_block/spec_composite.cpp index 00234e6c75f5cf26eec05e033402a81637d6a639..7d95b118b27bfddc044e097666e8c59334902bfa 100644 --- a/src/state_block/spec_composite.cpp +++ b/src/state_block/spec_composite.cpp @@ -87,4 +87,16 @@ std::ostream& operator <<(std::ostream &_os, const wolf::SpecState &_x) return _os; } +YAML::Node SpecState::toYaml() const +{ + YAML::Node node; + node["type"] = type_; + node["state"] = state_; + node["mode"] = mode_; + if (mode_ == "factor") + node["noise_std"] = noise_std_; + + return node; +} + } // namespace wolf \ No newline at end of file diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index 3951bf2f092fdcce0c518bcabfb7d9d46e66325f..782a06f05f0cd39dac17e2bdf74047d8161b878d 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -159,32 +159,32 @@ wolf_add_gtest(gtest_factor_pose_2d gtest_factor_pose_2d.cpp) # FactorPose3d class test wolf_add_gtest(gtest_factor_pose_3d gtest_factor_pose_3d.cpp) -# # FactorRelativePose2d class test -# wolf_add_gtest(gtest_factor_relative_pose_2d gtest_factor_relative_pose_2d.cpp) +# FactorRelativePose2d class test +wolf_add_gtest(gtest_factor_relative_pose_2d gtest_factor_relative_pose_2d.cpp) -# # FactorRelativePose2dWithExtrinsics class test -# wolf_add_gtest(gtest_factor_relative_pose_2d_with_extrinsics gtest_factor_relative_pose_2d_with_extrinsics.cpp) +# FactorRelativePose2dWithExtrinsics class test +wolf_add_gtest(gtest_factor_relative_pose_2d_with_extrinsics gtest_factor_relative_pose_2d_with_extrinsics.cpp) -# # FactorRelativePose3d class test -# wolf_add_gtest(gtest_factor_relative_pose_3d gtest_factor_relative_pose_3d.cpp) +# FactorRelativePose3d class test +wolf_add_gtest(gtest_factor_relative_pose_3d gtest_factor_relative_pose_3d.cpp) -# # FactorRelativePose3dWithExtrinsics class test -# wolf_add_gtest(gtest_factor_relative_pose_3d_with_extrinsics gtest_factor_relative_pose_3d_with_extrinsics.cpp) +# FactorRelativePose3dWithExtrinsics class test +wolf_add_gtest(gtest_factor_relative_pose_3d_with_extrinsics gtest_factor_relative_pose_3d_with_extrinsics.cpp) -# # FactorVelocityLocalDirection3d class test -# wolf_add_gtest(gtest_factor_velocity_local_direction_3d gtest_factor_velocity_local_direction_3d.cpp) +# FactorVelocityLocalDirection3d class test +wolf_add_gtest(gtest_factor_velocity_local_direction_3d gtest_factor_velocity_local_direction_3d.cpp) -# # MakePosDef function test -# wolf_add_gtest(gtest_make_posdef gtest_make_posdef.cpp) +# MakePosDef function test +wolf_add_gtest(gtest_make_posdef gtest_make_posdef.cpp) -# # Map yaml save/load test -# wolf_add_gtest(gtest_map_yaml gtest_map_yaml.cpp) +# Map yaml save/load test +wolf_add_gtest(gtest_map_yaml gtest_map_yaml.cpp) -# # Parameter prior test -# wolf_add_gtest(gtest_param_prior gtest_param_prior.cpp) +# Parameter prior test +wolf_add_gtest(gtest_param_prior gtest_param_prior.cpp) -# # ProcessorFixedWingModel class test -# wolf_add_gtest(gtest_processor_fixed_wing_model gtest_processor_fixed_wing_model.cpp) +# ProcessorFixedWingModel class test +wolf_add_gtest(gtest_processor_fixed_wing_model gtest_processor_fixed_wing_model.cpp) # # ProcessorDiffDrive class test # wolf_add_gtest(gtest_processor_diff_drive gtest_processor_diff_drive.cpp) diff --git a/test/dummy/landmark_dummy.h b/test/dummy/landmark_dummy.h new file mode 100644 index 0000000000000000000000000000000000000000..70aa2eb93ecbb1edf9228a0433356e128ca5b6bf --- /dev/null +++ b/test/dummy/landmark_dummy.h @@ -0,0 +1,96 @@ +//--------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-------- + +#pragma once + +//Wolf includes +#include "core/landmark/landmark_base.h" +#include "core/landmark/factory_landmark.h" + +#include "core/state_block/state_quaternion.h" + +// Std includes + + +namespace wolf { + +WOLF_PTR_TYPEDEFS(LandmarkDummy); + +//class LandmarkDummy +class LandmarkDummy : public LandmarkBase +{ + public: + LandmarkDummy(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, const int& _int_param, const double& _double_param); + LandmarkDummy(const YAML::Node& _node); + WOLF_LANDMARK_CREATE(LandmarkDummy); + + ~LandmarkDummy() override = default; + + int getIntParam() const; + double getDoubleParam() const; + + YAML::Node toYaml() const override; + + private: + const int int_param_; + const double double_param_; + +}; + + +inline LandmarkDummy::LandmarkDummy(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, const int& _int_param, const double& _double_param) : + LandmarkBase("LandmarkDummy", _p_ptr, _o_ptr), + int_param_(_int_param), + double_param_(_double_param) +{ +} + +inline LandmarkDummy::LandmarkDummy(const YAML::Node& _node) : + LandmarkBase("LandmarkDummy", _node), + int_param_(_node["int_param"].as<int>()), + double_param_(_node["double_param"].as<double>()) +{ +} + +inline int LandmarkDummy::getIntParam() const +{ + return int_param_; +} + +inline double LandmarkDummy::getDoubleParam() const +{ + return double_param_; +} + +inline YAML::Node LandmarkDummy::toYaml() const +{ + YAML::Node node = LandmarkBase::toYaml(); + + node["int_param"] = int_param_; + node["double_param"] = double_param_; + + return node; +} + +WOLF_REGISTER_LANDMARK(LandmarkDummy); + +} // namespace wolf diff --git a/test/gtest_factor_relative_pose_2d.cpp b/test/gtest_factor_relative_pose_2d.cpp index 63eed7c281b4bfb8c5ae3ba213c2fc49726a151b..15a042316c76bba2fe4c5aefcfb87cad01c31cba 100644 --- a/test/gtest_factor_relative_pose_2d.cpp +++ b/test/gtest_factor_relative_pose_2d.cpp @@ -40,8 +40,8 @@ ProblemPtr problem_ptr = Problem::create("PO", 2); SolverCeres solver(problem_ptr); // Two frames -FrameBasePtr frm0 = problem_ptr->emplaceFrame(0.0, Vector3d::Zero()); -FrameBasePtr frm1 = problem_ptr->emplaceFrame(1.0, Vector3d::Zero()); +FrameBasePtr frm0 = problem_ptr->emplaceFrame(0.0, "PO", Vector3d::Zero()); +FrameBasePtr frm1 = problem_ptr->emplaceFrame(1.0, "PO", Vector3d::Zero()); // Capture from frm1 to frm0 auto cap1 = CaptureBase::emplace<CaptureOdom2d>(frm1, 1, nullptr, Vector3d::Zero(), data_cov); diff --git a/test/gtest_factor_relative_pose_2d_with_extrinsics.cpp b/test/gtest_factor_relative_pose_2d_with_extrinsics.cpp index 86578050a1fb1375fe9e18aa1fb518db07bdb4b4..c15db358f0ec0ef6cade5137841c7faf23f1fdbb 100644 --- a/test/gtest_factor_relative_pose_2d_with_extrinsics.cpp +++ b/test/gtest_factor_relative_pose_2d_with_extrinsics.cpp @@ -43,12 +43,12 @@ ProblemPtr problem_ptr = Problem::create("PO", 2); SolverCeres solver(problem_ptr); // Sensor -auto sensor_odom2d = problem_ptr->installSensor("SensorOdom", "odom", wolf_root + "/test/yaml/sensor_odom_2d.yaml"); -auto processor_odom2d = problem_ptr->installProcessor("ProcessorOdom2d", "odom", sensor_odom2d, std::make_shared<ParamsProcessorOdom2d>()); +auto sensor_odom2d = problem_ptr->installSensor("SensorOdom2d", wolf_root + "/test/yaml/sensor_odom_2d.yaml", {wolf_root}); +auto processor_odom2d = ProcessorBase::emplace<ProcessorOdom2d>(sensor_odom2d, std::make_shared<ParamsProcessorOdom2d>()); // Two frames -FrameBasePtr frm0 = problem_ptr->emplaceFrame(0, Vector3d::Zero() ); -FrameBasePtr frm1 = problem_ptr->emplaceFrame(1, Vector3d::Zero() ); +FrameBasePtr frm0 = problem_ptr->emplaceFrame(0, "PO", Vector3d::Zero() ); +FrameBasePtr frm1 = problem_ptr->emplaceFrame(1, "PO", Vector3d::Zero() ); // Capture from frm1 to frm0 auto cap1 = CaptureBase::emplace<CaptureOdom2d>(frm1, 1, sensor_odom2d, Vector3d::Zero(), data_cov); diff --git a/test/gtest_factor_relative_pose_3d.cpp b/test/gtest_factor_relative_pose_3d.cpp index 524255e52010730fad55fecdd5851a1317faa98f..a0ebf1390e044af47b9167334b937e9bea2e74ad 100644 --- a/test/gtest_factor_relative_pose_3d.cpp +++ b/test/gtest_factor_relative_pose_3d.cpp @@ -59,7 +59,7 @@ SolverCeres solver(problem_ptr); // Two frames FrameBasePtr frm0 = problem_ptr->emplaceFrame(TimeStamp(0), problem_ptr->stateZero()); -FrameBasePtr frm1 = problem_ptr->emplaceFrame(TimeStamp(1), delta); +FrameBasePtr frm1 = problem_ptr->emplaceFrame(TimeStamp(1), "PO", delta); // Capture, feature and factor from frm1 to frm0 auto cap1 = CaptureBase::emplace<CaptureMotion>(frm1, "CaptureOdom3d", 1, nullptr, delta, nullptr); diff --git a/test/gtest_factor_relative_pose_3d_with_extrinsics.cpp b/test/gtest_factor_relative_pose_3d_with_extrinsics.cpp index d9be016dd463ede52c70e9213705a563770c387c..d5f2b0b04782ec0e2c45f9a4ccbbee35b2048c9f 100644 --- a/test/gtest_factor_relative_pose_3d_with_extrinsics.cpp +++ b/test/gtest_factor_relative_pose_3d_with_extrinsics.cpp @@ -36,7 +36,7 @@ using namespace Eigen; using namespace wolf; using std::static_pointer_cast; -string wolf_root = _WOLF_ROOT_DIR; +std::string wolf_root = _WOLF_ROOT_DIR; // Use the following in case you want to initialize tests with predefines variables or methods. class FactorRelativePose3dWithExtrinsics_class : public testing::Test{ @@ -116,17 +116,16 @@ class FactorRelativePose3dWithExtrinsics_class : public testing::Test{ solver = std::make_shared<SolverCeres>(problem); // Create sensor to be able to initialize - S = problem->installSensor("SensorPose", - "sensor_pose_1", - std::make_shared<ParamsSensorPose>(), - SpecSensorComposite{{'P',SpecStateSensor("P",pos_camera)}, - {'O',SpecStateSensor("O",vquat_camera)}}); + S = SensorBase::emplace<SensorPose3d>(problem->getHardware(), + std::make_shared<ParamsSensorPose>(), + SpecSensorComposite{{'P',SpecStateSensor("StatePoint3d",pos_camera)}, + {'O',SpecStateSensor("StateQuaternion",vquat_camera)}}); // F1 is be origin KF - VectorComposite x0(pose_robot, "PO", {3,4}); - VectorComposite s0("PO", {Vector3d(1,1,1), Vector3d(1,1,1)}); - F1 = problem->setPriorFactor(x0, s0, 0.0); - + SpecComposite prior; + prior.emplace('P',SpecState("StatePoint3d", pos_robot, "factor", Vector3d::Ones())); + prior.emplace('O',SpecState("StateQuaternion", vquat_robot, "factor", Vector3d::Ones())); + F1 = problem->setPrior(prior, 0.0); // the sensor is at origin as well as the robot. The measurement matches with the pose of the tag wrt sensor (and also wrt robot and world) meas_cov = Eigen::Matrix6d::Identity(); diff --git a/test/gtest_factor_velocity_local_direction_3d.cpp b/test/gtest_factor_velocity_local_direction_3d.cpp index be2eb8584a83b27504239c8b784119e07c4e9528..85c994571cd1336630891d650e0621946e157f47 100644 --- a/test/gtest_factor_velocity_local_direction_3d.cpp +++ b/test/gtest_factor_velocity_local_direction_3d.cpp @@ -57,7 +57,7 @@ class FactorVelocityLocalDirection3dTest : public testing::Test solver = std::make_shared<SolverCeres>(problem, params_solver); // Frame - frm = problem->emplaceFrame(0.0, (Vector10d() << 0,0,0,0,0,0,1,1,0,0).finished()); + frm = problem->emplaceFrame(0.0, "POV", (Vector10d() << 0,0,0,0,0,0,1,1,0,0).finished()); sb_p = frm->getP(); sb_p->fix(); sb_o = frm->getO(); diff --git a/test/gtest_map_yaml.cpp b/test/gtest_map_yaml.cpp index 183d6dfbbc6489dd94da9875e224ef4cbbd072a9..8635bf8b48002c5631ebc1fbdb033847519be214 100644 --- a/test/gtest_map_yaml.cpp +++ b/test/gtest_map_yaml.cpp @@ -31,6 +31,7 @@ #include "core/problem/problem.h" #include "core/map/map_base.h" #include "core/landmark/landmark_base.h" +#include "dummy/landmark_dummy.h" #include "core/state_block/state_block_derived.h" #include "core/state_block/state_angle.h" #include "core/state_block/state_quaternion.h" @@ -39,31 +40,38 @@ #include <iostream> using namespace wolf; +using namespace Eigen; + +std::string map_path = std::string(_WOLF_ROOT_DIR) + "/test/yaml"; + +Vector2d p1_2d = Vector2d::Random(); +Vector2d p2_2d = Vector2d::Random(); +Vector2d p3_2d = Vector2d::Random(); +Vector1d o2_2d = Vector1d::Random(); +Vector1d o3_2d = Vector1d::Random(); + +Vector3d p1_3d = Vector3d::Random(); +Vector3d p2_3d = Vector3d::Random(); +Vector3d p3_3d = Vector3d::Random(); +Vector4d o2_3d = Vector4d::Random().normalized(); +Vector4d o3_3d = Vector4d::Random().normalized(); + +int int_param = (int) (Vector1d::Random()(0) * 1e4); +double double_param = Vector1d::Random()(0) * 1e4; TEST(MapYaml, save_2d) { ProblemPtr problem = Problem::create("PO", 2); - Eigen::Vector2d p1, p2, p3; - Eigen::Vector1d o2, o3; - p1 << 1, 2; - p2 << 3, 4; - p3 << 5, 6; - o2 << 2; - o3 << 3; - - StateBlockPtr p1_sb = std::make_shared<StatePoint2d>(p1); - StateBlockPtr p2_sb = std::make_shared<StatePoint2d>(p2); - StateBlockPtr o2_sb = std::make_shared<StateAngle>(o2(0)); - StateBlockPtr p3_sb = std::make_shared<StatePoint2d>(p3, true); - StateBlockPtr o3_sb = std::make_shared<StateAngle>(o3(0), true); + StateBlockPtr p1_sb = std::make_shared<StatePoint2d>(p1_2d); + StateBlockPtr p2_sb = std::make_shared<StatePoint2d>(p2_2d); + StateBlockPtr o2_sb = std::make_shared<StateAngle>(o2_2d(0)); + StateBlockPtr p3_sb = std::make_shared<StatePoint2d>(p3_2d, true); + StateBlockPtr o3_sb = std::make_shared<StateAngle>(o3_2d(0), true); LandmarkBase::emplace<LandmarkBase>(problem->getMap(), "LandmarkBase", p1_sb); LandmarkBase::emplace<LandmarkBase>(problem->getMap(), "LandmarkBase", p2_sb, o2_sb); - LandmarkBase::emplace<LandmarkBase>(problem->getMap(), "LandmarkBase", p3_sb, o3_sb); - - std::string wolf_root = _WOLF_ROOT_DIR; - std::string map_path = wolf_root + "/test/yaml"; + LandmarkBase::emplace<LandmarkDummy>(problem->getMap(), p3_sb, o3_sb, int_param, double_param); // Check Problem functions std::string filename = map_path + "/map_2d_problem.yaml"; @@ -80,113 +88,76 @@ TEST(MapYaml, load_2d) { ProblemPtr problem = Problem::create("PO", 2); - std::string wolf_root = _WOLF_ROOT_DIR; - std::string map_path = wolf_root + "/test/yaml"; - - // Check Problem functions - std::string filename = map_path + "/map_2d_problem.yaml"; - std::cout << "Loading map to file: " << filename << std::endl; - problem->loadMap(filename); + std::list<std::string> maps{"map_2d_problem.yaml", "map_2d_map.yaml"}; - ASSERT_EQ(problem->getMap()->getLandmarkList().size(), 3); - - for (auto lmk : problem->getMap()->getLandmarkList()) + for (auto map : maps) { - if (lmk->id() == 1) - { - ASSERT_TRUE(lmk->getP() != nullptr); - ASSERT_TRUE(lmk->getO() == nullptr); - ASSERT_MATRIX_APPROX(lmk->getP()->getState(), (Eigen::Vector2d()<<1,2).finished(), 1e-12); - ASSERT_FALSE(lmk->getP()->isFixed()); - } - else if (lmk->id() == 2) - { - ASSERT_TRUE(lmk->getP() != nullptr); - ASSERT_TRUE(lmk->getO() != nullptr); - ASSERT_MATRIX_APPROX(lmk->getP()->getState(), (Eigen::Vector2d()<<3,4).finished(), 1e-12); - ASSERT_MATRIX_APPROX(lmk->getO()->getState(), (Eigen::Vector1d()<<2).finished(), 1e-12); - ASSERT_FALSE(lmk->getP()->isFixed()); - ASSERT_FALSE(lmk->getO()->isFixed()); - } - else if (lmk->id() == 3) - { - ASSERT_TRUE(lmk->getP() != nullptr); - ASSERT_TRUE(lmk->getO() != nullptr); - ASSERT_MATRIX_APPROX(lmk->getP()->getState(), (Eigen::Vector2d()<<5,6).finished(), 1e-12); - ASSERT_MATRIX_APPROX(lmk->getO()->getState(), (Eigen::Vector1d()<<3).finished(), 1e-12); - ASSERT_TRUE(lmk->getP()->isFixed()); - ASSERT_TRUE(lmk->getO()->isFixed()); - } - } + std::string filename = map_path + "/" + map; + std::cout << "Loading map to file: " << filename << std::endl; + problem->loadMap(filename); - // empty the map - { - auto lmk_list = problem->getMap()->getLandmarkList(); - for (auto lmk : lmk_list) - lmk->remove(); - } - ASSERT_TRUE(problem->getMap()->getLandmarkList().empty()); + ASSERT_EQ(problem->getMap()->getLandmarkList().size(), 3); - // Check Map functions - filename = map_path + "/map_2d_map.yaml"; - std::cout << "Loading map to file: " << filename << std::endl; - problem->getMap()->load(filename); - - ASSERT_EQ(problem->getMap()->getLandmarkList().size(), 3); - - for (auto lmk : problem->getMap()->getLandmarkList()) - { - if (lmk->id() == 1) + for (auto lmk : problem->getMap()->getLandmarkList()) { - ASSERT_TRUE(lmk->getP() != nullptr); - ASSERT_TRUE(lmk->getO() == nullptr); - ASSERT_MATRIX_APPROX(lmk->getP()->getState(), (Eigen::Vector2d()<<1,2).finished(), 1e-12); - ASSERT_FALSE(lmk->getP()->isFixed()); + if (lmk->id() == 1) + { + ASSERT_TRUE(lmk->getP() != nullptr); + ASSERT_TRUE(lmk->getO() == nullptr); + ASSERT_MATRIX_APPROX(lmk->getP()->getState(), p1_2d, 1e-12); + ASSERT_FALSE(lmk->getP()->isFixed()); + ASSERT_EQ(lmk->getType(), "LandmarkBase"); + } + else if (lmk->id() == 2) + { + ASSERT_TRUE(lmk->getP() != nullptr); + ASSERT_TRUE(lmk->getO() != nullptr); + ASSERT_MATRIX_APPROX(lmk->getP()->getState(), p2_2d, 1e-12); + ASSERT_MATRIX_APPROX(lmk->getO()->getState(), o2_2d, 1e-12); + ASSERT_FALSE(lmk->getP()->isFixed()); + ASSERT_FALSE(lmk->getO()->isFixed()); + ASSERT_EQ(lmk->getType(), "LandmarkBase"); + } + else if (lmk->id() == 3) + { + ASSERT_TRUE(lmk->getP() != nullptr); + ASSERT_TRUE(lmk->getO() != nullptr); + ASSERT_MATRIX_APPROX(lmk->getP()->getState(), p3_2d, 1e-12); + ASSERT_MATRIX_APPROX(lmk->getO()->getState(), o3_2d, 1e-12); + ASSERT_TRUE(lmk->getP()->isFixed()); + ASSERT_TRUE(lmk->getO()->isFixed()); + ASSERT_EQ(lmk->getType(), "LandmarkDummy"); + + auto lmk_dummy = std::dynamic_pointer_cast<LandmarkDummy>(lmk); + ASSERT_TRUE(lmk_dummy != nullptr); + ASSERT_EQ(lmk_dummy->getIntParam(), int_param); + ASSERT_NEAR(lmk_dummy->getDoubleParam(), double_param, 1e-12); + } } - else if (lmk->id() == 2) - { - ASSERT_TRUE(lmk->getP() != nullptr); - ASSERT_TRUE(lmk->getO() != nullptr); - ASSERT_MATRIX_APPROX(lmk->getP()->getState(), (Eigen::Vector2d()<<3,4).finished(), 1e-12); - ASSERT_MATRIX_APPROX(lmk->getO()->getState(), (Eigen::Vector1d()<<2).finished(), 1e-12); - ASSERT_FALSE(lmk->getP()->isFixed()); - ASSERT_FALSE(lmk->getO()->isFixed()); - } - else if (lmk->id() == 3) + + // empty the map { - ASSERT_TRUE(lmk->getP() != nullptr); - ASSERT_TRUE(lmk->getO() != nullptr); - ASSERT_MATRIX_APPROX(lmk->getP()->getState(), (Eigen::Vector2d()<<5,6).finished(), 1e-12); - ASSERT_MATRIX_APPROX(lmk->getO()->getState(), (Eigen::Vector1d()<<3).finished(), 1e-12); - ASSERT_TRUE(lmk->getP()->isFixed()); - ASSERT_TRUE(lmk->getO()->isFixed()); + auto lmk_list = problem->getMap()->getLandmarkList(); + for (auto lmk : lmk_list) + lmk->remove(); } + ASSERT_TRUE(problem->getMap()->getLandmarkList().empty()); } } + TEST(MapYaml, save_3d) { ProblemPtr problem = Problem::create("PO", 3); - Eigen::Vector3d p1, p2, p3; - Eigen::Vector4d q2, q3; - p1 << 1, 2, 3; - p2 << 4, 5, 6; - p3 << 7, 8, 9; - q2 << 0, 1, 0, 0; - q3 << 0, 0, 1, 0; - - auto p1_sb = std::make_shared<StatePoint2d>(p1); - auto p2_sb = std::make_shared<StatePoint2d>(p2); - auto q2_sb = std::make_shared<StateQuaternion>(q2); - auto p3_sb = std::make_shared<StatePoint2d>(p3, true); - auto q3_sb = std::make_shared<StateQuaternion>(q3, true); + auto p1_sb = std::make_shared<StatePoint3d>(p1_3d); + auto p2_sb = std::make_shared<StatePoint3d>(p2_3d); + auto o2_sb = std::make_shared<StateQuaternion>(o2_3d); + auto p3_sb = std::make_shared<StatePoint3d>(p3_3d, true); + auto o3_sb = std::make_shared<StateQuaternion>(o3_3d, true); LandmarkBase::emplace<LandmarkBase>(problem->getMap(), "LandmarkBase", p1_sb); - LandmarkBase::emplace<LandmarkBase>(problem->getMap(), "LandmarkBase", p2_sb, q2_sb); - LandmarkBase::emplace<LandmarkBase>(problem->getMap(), "LandmarkBase", p3_sb, q3_sb); - - std::string wolf_root = _WOLF_ROOT_DIR; - std::string map_path = wolf_root + "/test/yaml"; + LandmarkBase::emplace<LandmarkBase>(problem->getMap(), "LandmarkBase", p2_sb, o2_sb); + LandmarkBase::emplace<LandmarkDummy>(problem->getMap(), p3_sb, o3_sb, int_param, double_param); // Check Problem functions std::string filename = map_path + "/map_3d_problem.yaml"; @@ -202,100 +173,68 @@ TEST(MapYaml, save_3d) TEST(MapYaml, load_3d) { ProblemPtr problem = Problem::create("PO", 3); + + std::list<std::string> maps{"map_3d_problem.yaml", "map_3d_map.yaml"}; - std::string wolf_root = _WOLF_ROOT_DIR; - std::string map_path = wolf_root + "/test/yaml"; - - // Check Problem functions - std::string filename = map_path + "/map_3d_problem.yaml"; - std::cout << "Loading map to file: " << filename << std::endl; - problem->loadMap(filename); - - ASSERT_EQ(problem->getMap()->getLandmarkList().size(), 3); - - for (auto lmk : problem->getMap()->getLandmarkList()) + for (auto map : maps) { - if (lmk->id() == 1) - { - ASSERT_TRUE(lmk->getP() != nullptr); - ASSERT_TRUE(lmk->getO() == nullptr); - ASSERT_MATRIX_APPROX(lmk->getP()->getState(), (Eigen::Vector3d()<<1,2,3).finished(), 1e-12); - ASSERT_FALSE(lmk->getP()->isFixed()); - } - else if (lmk->id() == 2) - { - ASSERT_TRUE(lmk->getP() != nullptr); - ASSERT_TRUE(lmk->getO() != nullptr); - ASSERT_MATRIX_APPROX(lmk->getP()->getState(), (Eigen::Vector3d()<<4,5,6).finished(), 1e-12); - ASSERT_MATRIX_APPROX(lmk->getO()->getState(), (Eigen::Vector4d()<<0,1,0,0).finished(), 1e-12); - ASSERT_FALSE(lmk->getP()->isFixed()); - ASSERT_FALSE(lmk->getO()->isFixed()); - ASSERT_TRUE(lmk->getO()->hasLocalParametrization()); - } - else if (lmk->id() == 3) - { - ASSERT_TRUE(lmk->getP() != nullptr); - ASSERT_TRUE(lmk->getO() != nullptr); - ASSERT_MATRIX_APPROX(lmk->getP()->getState(), (Eigen::Vector3d()<<7,8,9).finished(), 1e-12); - ASSERT_MATRIX_APPROX(lmk->getO()->getState(), (Eigen::Vector4d()<<0,0,1,0).finished(), 1e-12); - ASSERT_TRUE(lmk->getP()->isFixed()); - ASSERT_TRUE(lmk->getO()->isFixed()); - ASSERT_TRUE(lmk->getO()->hasLocalParametrization()); - } - } + std::string filename = map_path + "/" + map; + std::cout << "Loading map to file: " << filename << std::endl; + problem->loadMap(filename); - // empty the map - { - auto lmk_list = problem->getMap()->getLandmarkList(); - for (auto lmk : lmk_list) - lmk->remove(); - } - ASSERT_TRUE(problem->getMap()->getLandmarkList().empty()); - - // Check Map functions - filename = map_path + "/map_3d_map.yaml"; - std::cout << "Loading map to file: " << filename << std::endl; - problem->getMap()->load(filename); + ASSERT_EQ(problem->getMap()->getLandmarkList().size(), 3); - ASSERT_EQ(problem->getMap()->getLandmarkList().size(), 3); - - for (auto lmk : problem->getMap()->getLandmarkList()) - { - if (lmk->id() == 1) - { - ASSERT_TRUE(lmk->getP() != nullptr); - ASSERT_TRUE(lmk->getO() == nullptr); - ASSERT_MATRIX_APPROX(lmk->getP()->getState(), (Eigen::Vector3d()<<1,2,3).finished(), 1e-12); - ASSERT_FALSE(lmk->getP()->isFixed()); - } - else if (lmk->id() == 2) + for (auto lmk : problem->getMap()->getLandmarkList()) { - ASSERT_TRUE(lmk->getP() != nullptr); - ASSERT_TRUE(lmk->getO() != nullptr); - ASSERT_MATRIX_APPROX(lmk->getP()->getState(), (Eigen::Vector3d()<<4,5,6).finished(), 1e-12); - ASSERT_MATRIX_APPROX(lmk->getO()->getState(), (Eigen::Vector4d()<<0,1,0,0).finished(), 1e-12); - ASSERT_FALSE(lmk->getP()->isFixed()); - ASSERT_FALSE(lmk->getO()->isFixed()); - ASSERT_TRUE(lmk->getO()->hasLocalParametrization()); + if (lmk->id() == 1) + { + ASSERT_TRUE(lmk->getP() != nullptr); + ASSERT_TRUE(lmk->getO() == nullptr); + ASSERT_MATRIX_APPROX(lmk->getP()->getState(), p1_3d, 1e-12); + ASSERT_FALSE(lmk->getP()->isFixed()); + ASSERT_EQ(lmk->getType(), "LandmarkBase"); + } + else if (lmk->id() == 2) + { + ASSERT_TRUE(lmk->getP() != nullptr); + ASSERT_TRUE(lmk->getO() != nullptr); + ASSERT_MATRIX_APPROX(lmk->getP()->getState(), p2_3d, 1e-12); + ASSERT_MATRIX_APPROX(lmk->getO()->getState(), o2_3d, 1e-12); + ASSERT_FALSE(lmk->getP()->isFixed()); + ASSERT_FALSE(lmk->getO()->isFixed()); + ASSERT_TRUE(lmk->getO()->hasLocalParametrization()); + ASSERT_EQ(lmk->getType(), "LandmarkBase"); + } + else if (lmk->id() == 3) + { + ASSERT_TRUE(lmk->getP() != nullptr); + ASSERT_TRUE(lmk->getO() != nullptr); + ASSERT_MATRIX_APPROX(lmk->getP()->getState(), p3_3d, 1e-12); + ASSERT_MATRIX_APPROX(lmk->getO()->getState(), o3_3d, 1e-12); + ASSERT_TRUE(lmk->getP()->isFixed()); + ASSERT_TRUE(lmk->getO()->isFixed()); + ASSERT_TRUE(lmk->getO()->hasLocalParametrization()); + ASSERT_EQ(lmk->getType(), "LandmarkDummy"); + + auto lmk_dummy = std::dynamic_pointer_cast<LandmarkDummy>(lmk); + ASSERT_TRUE(lmk_dummy != nullptr); + ASSERT_EQ(lmk_dummy->getIntParam(), int_param); + ASSERT_NEAR(lmk_dummy->getDoubleParam(), double_param, 1e-12); + } } - else if (lmk->id() == 3) + + // empty the map { - ASSERT_TRUE(lmk->getP() != nullptr); - ASSERT_TRUE(lmk->getO() != nullptr); - ASSERT_MATRIX_APPROX(lmk->getP()->getState(), (Eigen::Vector3d()<<7,8,9).finished(), 1e-12); - ASSERT_MATRIX_APPROX(lmk->getO()->getState(), (Eigen::Vector4d()<<0,0,1,0).finished(), 1e-12); - ASSERT_TRUE(lmk->getP()->isFixed()); - ASSERT_TRUE(lmk->getO()->isFixed()); - ASSERT_TRUE(lmk->getO()->hasLocalParametrization()); + auto lmk_list = problem->getMap()->getLandmarkList(); + for (auto lmk : lmk_list) + lmk->remove(); } + ASSERT_TRUE(problem->getMap()->getLandmarkList().empty()); } } TEST(MapYaml, remove_map_files) { - std::string wolf_root = _WOLF_ROOT_DIR; - std::string map_path = wolf_root + "/test/yaml"; - std::string filename = map_path + "/map_2d_problem.yaml"; ASSERT_TRUE(std::remove(filename.c_str()) == 0); filename = map_path + "/map_2d_map.yaml"; diff --git a/test/gtest_param_prior.cpp b/test/gtest_param_prior.cpp index 3f4146a10fe9201887f3ab5a0f9c509dd39ae689..caf8b96994445d0b1dede7299d45dcbfdbb9adce 100644 --- a/test/gtest_param_prior.cpp +++ b/test/gtest_param_prior.cpp @@ -46,12 +46,11 @@ Vector7d prior_extrinsics((Vector7d() << 10, 20, 30, 0, 0, 0, 1).finished()); Vector7d prior2_extrinsics((Vector7d() << 100, 200, 300, 0, 0, 0, 1).finished()); ParamsSensorOdomPtr params = std::make_shared<ParamsSensorOdom>(); -SpecSensorComposite priors{{'P',SpecStateSensor("P",initial_extrinsics_p,"initial_guess")}, - {'O',SpecStateSensor("O",initial_extrinsics_o,"initial_guess")}}; -SensorBasePtr sensor_ptr_ = problem_ptr->installSensor("SensorOdom", - "odometer", - params, - priors); +SpecSensorComposite priors{{'P',SpecStateSensor("StatePoint3d",initial_extrinsics_p,"initial_guess")}, + {'O',SpecStateSensor("StateQuaternion",initial_extrinsics_o,"initial_guess")}}; +SensorBasePtr sensor_ptr_ = SensorBase::emplace<SensorOdom3d>(problem_ptr->getHardware(), + params, + priors); TEST(ParameterPrior, add_prior_p) { diff --git a/test/gtest_processor_fixed_wing_model.cpp b/test/gtest_processor_fixed_wing_model.cpp index 0504619c7489de34b566ab509b569b3f81fa8193..00157a56dd51207f539d1e840a297eb4fc53f68a 100644 --- a/test/gtest_processor_fixed_wing_model.cpp +++ b/test/gtest_processor_fixed_wing_model.cpp @@ -54,20 +54,15 @@ class ProcessorFixedWingModelTest : public testing::Test solver = std::make_shared<SolverCeres>(problem); // Emplace sensor - sensor = problem->installSensor("SensorMotionModel", "sensor_1", wolf_root + "/test/yaml/sensor_pose_3d.yaml"); + sensor = problem->installSensor("SensorMotionModel", wolf_root + "/test/yaml/sensor_pose_3d.yaml", {wolf_root}); // Emplace processor ParamsProcessorFixedWingModelPtr params = std::make_shared<ParamsProcessorFixedWingModel>(); params->velocity_local = (Vector3d() << 1, 0, 0).finished(); params->angle_stdev = 0.1; params->min_vel_norm = 0; - processor = problem->installProcessor("ProcessorFixedWingModel","processor_1", sensor, params); - } - - FrameBasePtr emplaceFrame(TimeStamp ts, const Vector10d& x) - { - // new frame - return problem->emplaceFrame(ts, x); + params->name = "processor_1"; + processor = ProcessorBase::emplace<ProcessorFixedWingModel>(sensor, params); } }; @@ -79,7 +74,7 @@ TEST_F(ProcessorFixedWingModelTest, setup) TEST_F(ProcessorFixedWingModelTest, keyFrameCallback) { // new frame - auto frm1 = emplaceFrame(1, (Vector10d() << 0,0,0,0,0,0,1,1,0,0).finished()); + auto frm1 = problem->emplaceFrame(1, "POV", (Vector10d() << 0,0,0,0,0,0,1,1,0,0).finished()); // keyframecallback problem->keyFrameCallback(frm1, nullptr); @@ -102,7 +97,7 @@ TEST_F(ProcessorFixedWingModelTest, keyFrameCallback) TEST_F(ProcessorFixedWingModelTest, keyFrameCallbackRepeated) { // new frame - auto frm1 = emplaceFrame(1, (Vector10d() << 0,0,0,0,0,0,1,1,0,0).finished()); + auto frm1 = problem->emplaceFrame(1, "POV", (Vector10d() << 0,0,0,0,0,0,1,1,0,0).finished()); // keyframecallback problem->keyFrameCallback(frm1, nullptr); @@ -128,7 +123,7 @@ TEST_F(ProcessorFixedWingModelTest, keyFrameCallbackRepeated) TEST_F(ProcessorFixedWingModelTest, solve_origin) { // new frame - auto frm1 = emplaceFrame(1, (Vector10d() << 0,0,0,0,0,0,1,1,0,0).finished()); + auto frm1 = problem->emplaceFrame(1, "POV", (Vector10d() << 0,0,0,0,0,0,1,1,0,0).finished()); // keyframecallback problem->keyFrameCallback(frm1, nullptr); diff --git a/test/yaml/sensor_pose_2d.yaml b/test/yaml/sensor_pose_2d.yaml index d85c3d247fda7b9f26bff9bc60f7271c2e025c53..10c6003e44afdd3a6d3d5762f3318be380fffc35 100644 --- a/test/yaml/sensor_pose_2d.yaml +++ b/test/yaml/sensor_pose_2d.yaml @@ -1,3 +1,4 @@ +name: a cool sensor pose 2d states: P: mode: fix diff --git a/test/yaml/sensor_pose_3d.yaml b/test/yaml/sensor_pose_3d.yaml index f2acfab188d82beef19a8ec507698ac1b038d19b..249af7bfb925cc97f943e6136b39c2dc08b5b1d3 100644 --- a/test/yaml/sensor_pose_3d.yaml +++ b/test/yaml/sensor_pose_3d.yaml @@ -1,3 +1,4 @@ +name: a cool sensor pose 3d states: P: mode: fix