diff --git a/CMakeLists.txt b/CMakeLists.txt index 0b833cbbfc9d91ee015d3e905199698bfcddb294..6fb5b5b1092ee4251cf4531f7af1e6afa3245d70 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -218,6 +218,7 @@ SET(HDRS_STATE_BLOCK 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 + include/${PROJECT_NAME}/state_block/type_composite.h include/${PROJECT_NAME}/state_block/vector_composite.h ) SET(HDRS_TRAJECTORY diff --git a/demos/hello_wolf/hello_wolf.cpp b/demos/hello_wolf/hello_wolf.cpp index dde271376d13561d0055aea7f67dfe37a6fdcf4e..b64421fd8616af8b03b3f0ee92002b67bbbd5222 100644 --- a/demos/hello_wolf/hello_wolf.cpp +++ b/demos/hello_wolf/hello_wolf.cpp @@ -130,8 +130,8 @@ int main() // sensor odometer 2d ParamsSensorOdomPtr intrinsics_odo = std::make_shared<ParamsSensorOdom>(); - SpecSensorComposite priors_odo = {{'P',SpecStateSensor("P", Vector2d::Zero())}, - {'O',SpecStateSensor("O", Vector1d::Zero())}}; + SpecSensorComposite priors_odo = {{'P',SpecStateSensor("StatePoint2d", Vector2d::Zero())}, + {'O',SpecStateSensor("StateAngle", Vector1d::Zero())}}; SensorBasePtr sensor_odo = problem->installSensor("SensorOdom", "sensor odo", intrinsics_odo, priors_odo); // processor odometer 2d diff --git a/demos/hello_wolf/yaml/processor_odom_2d.yaml b/demos/hello_wolf/yaml/processor_odom_2d.yaml index 907bbfab3be6e9f66feb6c027f34fa752fd893c4..625676edf735ba2bad5ba307e300bbb866e78ab3 100644 --- a/demos/hello_wolf/yaml/processor_odom_2d.yaml +++ b/demos/hello_wolf/yaml/processor_odom_2d.yaml @@ -11,5 +11,5 @@ keyframe_vote: cov_det: 999 apply_loss_function: true -state_getter: true +state_provider: true state_priority: 1 diff --git a/include/core/common/factory.h b/include/core/common/factory.h index bd09e14228352eda82cee813692308c23cc7e2aa..c02474f8924ba06224221dc651f08e259bf92095 100644 --- a/include/core/common/factory.h +++ b/include/core/common/factory.h @@ -284,12 +284,9 @@ namespace wolf template<class TypeBase, typename... TypeInput> class Factory { - private: - typedef std::shared_ptr<TypeBase> TypeBasePtr; - public: // example of creator callback (see typedefs below) - typedef TypeBasePtr (*CreatorCallback)(TypeInput... _input); + typedef TypeBase (*CreatorCallback)(TypeInput... _input); private: typedef std::map<std::string, CreatorCallback> CallbackMap; @@ -298,7 +295,7 @@ class Factory 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); + static TypeBase create(const std::string& _type, TypeInput... _input); std::string getClass() const; static void printAddress(); static void printCallbacks(); @@ -356,7 +353,7 @@ inline bool Factory<TypeBase, TypeInput...>::isCreatorRegistered(const std::stri } template<class TypeBase, typename... TypeInput> -inline typename Factory<TypeBase, TypeInput...>::TypeBasePtr Factory<TypeBase, TypeInput...>::create(const std::string& _type, TypeInput... _input) +inline TypeBase Factory<TypeBase, TypeInput...>::create(const std::string& _type, TypeInput... _input) { typename CallbackMap::const_iterator creator_callback_it = get().callbacks_.find(_type); @@ -406,7 +403,7 @@ namespace wolf // Landmarks from YAML class LandmarkBase; -typedef Factory<LandmarkBase, +typedef Factory<std::shared_ptr<LandmarkBase>, const YAML::Node&> FactoryLandmark; template<> inline std::string FactoryLandmark::getClass() const diff --git a/include/core/frame/frame_base.h b/include/core/frame/frame_base.h index 1ff2688719d93f1a5b80ea1aaa54a1f01f68d123..4dd04a0d01e799ca4b82a9e978559d67d3743936 100644 --- a/include/core/frame/frame_base.h +++ b/include/core/frame/frame_base.h @@ -74,22 +74,22 @@ class FrameBase : public NodeBase, public HasStateBlocks, public std::enable_sha // * // * Constructor with type, time stamp and state pointer // * \param _ts is the time stamp associated to this frame, provided in seconds - // * \param _frame_structure StateStructure (string) with the state keys + // * \param _frame_structure StateKeys (string) with the state keys // * \param _state VectorComposite containing the state of each key // **/ // FrameBase(const TimeStamp& _ts, - // const StateStructure& _frame_structure, + // const StateKeys& _frame_structure, // const VectorComposite& _state); // /** \brief Constructor with type, time stamp and state pointer // * // * Constructor with type, time stamp and state pointer // * \param _ts is the time stamp associated to this frame, provided in seconds - // * \param _frame_structure StateStructure (string) with the state keys + // * \param _frame_structure StateKeys (string) with the state keys // * \param _vectors std::list of VectorXd containing the state of each key // **/ // FrameBase(const TimeStamp& _ts, - // const StateStructure& _frame_structure, + // const StateKeys& _frame_structure, // const std::list<VectorXd>& _vectors); /** \brief Constructor time stamp and specs composite diff --git a/include/core/map/factory_map.h b/include/core/map/factory_map.h index 43ba8ad739df48abc5feb531823414f19ab57eb3..8ce6f81e875742a3db35253a44c881da4fcf58b1 100644 --- a/include/core/map/factory_map.h +++ b/include/core/map/factory_map.h @@ -168,7 +168,7 @@ namespace wolf * \endcode * */ -typedef Factory<MapBase, const YAML::Node&> FactoryMap; +typedef Factory<std::shared_ptr<MapBase>, const YAML::Node&> FactoryMap; template<> inline std::string FactoryMap::getClass() const diff --git a/include/core/math/SE2.h b/include/core/math/SE2.h index 63d07cbee4ca2d169ab2103324e70410fe3d2c91..fbbc8276df890fc7982bdd7cdf8d6c361b7ff387 100644 --- a/include/core/math/SE2.h +++ b/include/core/math/SE2.h @@ -200,7 +200,7 @@ inline void exp(const VectorComposite& _tau, VectorComposite& _delta) inline VectorComposite exp(const VectorComposite& tau) { - VectorComposite res("PO", {2,1}); + VectorComposite res("PO", Vector3d::Zero(), {2,1}); exp(tau, res); diff --git a/include/core/math/SE3.h b/include/core/math/SE3.h index 9ae6c2bf38b045e77276937bda285a55a9dc82ea..c4ae8f399fbdb35de9829ed734369a27f8c15fe0 100644 --- a/include/core/math/SE3.h +++ b/include/core/math/SE3.h @@ -137,7 +137,7 @@ inline void inverse(const VectorComposite& v, VectorComposite& c) inline VectorComposite inverse(const VectorComposite& v) { - VectorComposite c("PO", {3,4}); + VectorComposite c("PO", Vector7d::Zero(), {3,4}); inverse(v, c); return c; } @@ -241,7 +241,7 @@ inline void compose(const VectorComposite& x1, const VectorComposite& x2, Vector inline VectorComposite compose(const VectorComposite& x1, const VectorComposite& x2) { - VectorComposite c("PO", {3,4}); + VectorComposite c("PO", Vector7d::Zero(), {3,4}); compose(x1.at('P'), x1.at('O'), x2.at('P'), x2.at('O'), c['P'], c['O']); return c; } diff --git a/include/core/problem/problem.h b/include/core/problem/problem.h index 2e77bda2fd8d91faab82bd5cef95f3748c41dc01..ed381086584a3db172fb73912588d869e8a342fd 100644 --- a/include/core/problem/problem.h +++ b/include/core/problem/problem.h @@ -79,8 +79,8 @@ class Problem : public std::enable_shared_from_this<Problem> std::map<StateBlockPtr, Notification> state_block_notification_map_; mutable std::mutex mut_notifications_; mutable std::mutex mut_covariances_; - //StateStructure frame_structure_; - TypeComposite frame_structure_; + //StateKeys frame_structure_; + TypeComposite frame_types_; SpecComposite prior_options_; bool prior_applied_; @@ -89,17 +89,15 @@ class Problem : public std::enable_shared_from_this<Problem> mutable std::mutex mut_transform_; private: // CAUTION: THESE METHODS ARE PRIVATE, DO NOT MAKE THEM PUBLIC !! - Problem(const TypeComposite& _frame_structure, SizeEigen _dim, MapBasePtr _map); // USE create() below !! - Problem(const std::string& _frame_structure, SizeEigen _dim, MapBasePtr _map); // USE create() below !! + Problem(const TypeComposite& _frame_structure, SizeEigen _dim); // USE create() below !! + Problem(const std::string& _frame_structure, SizeEigen _dim); // USE create() below !! void setup(); public: static ProblemPtr create(const TypeComposite& _frame_structure, - SizeEigen _dim, - MapBasePtr _map = std::make_shared<MapBase>()); // USE THIS AS A CONSTRUCTOR! + SizeEigen _dim); // USE THIS AS A CONSTRUCTOR! static ProblemPtr create(const std::string& _frame_structure, - SizeEigen _dim, - MapBasePtr _map = std::make_shared<MapBase>()); // USE THIS AS A CONSTRUCTOR! + SizeEigen _dim); // USE THIS AS A CONSTRUCTOR! static ProblemPtr autoSetup(const std::string& _input_yaml_file, const std::vector<std::string>& _primary_schema_folders={}); virtual ~Problem(); @@ -112,11 +110,12 @@ class Problem : public std::enable_shared_from_this<Problem> // Properties ----------------------------------------- public: SizeEigen getDim() const; - const StateStructure& getFrameStructure() const; + StateKeys getFrameKeys() const; + TypeComposite getFrameTypes(StateKeys _keys = "") const; protected: - //void appendToStructure(const StateStructure& _structure); - void appendToStructure(const TypeComposite& _structure); + //void appendToFrameTypes(const StateKeys& _structure); + void appendToFrameTypes(const TypeComposite& _structure); @@ -201,101 +200,64 @@ class Problem : public std::enable_shared_from_this<Problem> FrameBasePtr applyPriorOptions(const TimeStamp& _ts); FrameBasePtr setPrior(const SpecComposite& priors, const TimeStamp& _ts); - // /** \brief Emplace frame from string frame_structure, dimension and vector - // * \param _time_stamp Time stamp of the frame - // * \param _frame_structure String indicating the frame structure. - // * \param _dim variable indicating the dimension of the problem - // * \param _frame_state State vector; must match the size and format of the chosen frame structure - // * - // * This acts as a Frame factory, but also takes care to update related lists in WolfProblem: - // * - Create a Frame - // * - Add it to Trajectory - // * - If it is key-frame, update state-block lists in Problem - // */ - // FrameBasePtr emplaceFrame(const TimeStamp& _time_stamp, // - // const StateStructure& _frame_structure, // - // const SizeEigen _dim, // - // const Eigen::VectorXd& _frame_state); - - /** \brief Emplace frame from string frame_structure and state + /** \brief Emplace frame from timestamp, types and and state * \param _time_stamp Time stamp of the frame - * \param _frame_structure String indicating the frame structure. + * \param _frame_types TypeComposite indicating the types of each key. * \param _frame_state State vector; must match the size and format of the chosen frame structure * - * - The dimension is taken from Problem - * * This acts as a Frame factory, but also takes care to update related lists in WolfProblem: * - Create a Frame * - Add it to Trajectory * - If it is key-frame, update state-block lists in Problem */ - // FrameBasePtr emplaceFrame(const TimeStamp& _time_stamp, // - // const StateStructure& _frame_structure, // - // const VectorComposite& _frame_state); - FrameBasePtr emplaceFrame(const TimeStamp& _time_stamp, // - const TypeComposite& _frame_structure, // + FrameBasePtr emplaceFrame(const TimeStamp& _time_stamp, + const TypeComposite& _frame_types, const VectorComposite& _frame_state); - /** \brief Emplace frame from state + /** \brief Emplace frame from timestamp and keys * \param _time_stamp Time stamp of the frame - * \param _frame_state State; must be part of the problem's frame structure - * - * - The structure is taken from Problem - * - The dimension is taken from Problem + * \param _frame_keys String containing the keys of the desired frame. Empty means all keys available. * + * - The frame_types are taken from Problem + * - The states are taken from Problem + * * This acts as a Frame factory, but also takes care to update related lists in WolfProblem: * - Create a Frame * - Add it to Trajectory * - If it is key-frame, update state-block lists in Problem */ - FrameBasePtr emplaceFrame(const TimeStamp& _time_stamp, // - const VectorComposite& _frame_state); + FrameBasePtr emplaceFrame (const TimeStamp& _time_stamp, + const StateKeys& _frame_keys=""); - // /** \brief Emplace frame from string frame_structure and dimension - // * \param _time_stamp Time stamp of the frame - // * \param _frame_structure String indicating the frame structure. - // * \param _dim variable indicating the dimension of the problem - // * - // * - The dimension is taken from Problem - // * - The state is taken from Problem - // * - // * This acts as a Frame factory, but also takes care to update related lists in WolfProblem: - // * - Create a Frame - // * - Add it to Trajectory - // * - If it is key-frame, update state-block lists in Problem - // */ - // FrameBasePtr emplaceFrame(const TimeStamp& _time_stamp, // - // const StateStructure& _frame_structure, // - // const SizeEigen _dim); - - /** \brief Emplace frame from state vector + /** \brief Emplace frame from timestamps and state * \param _time_stamp Time stamp of the frame - * \param _frame_state State vector; must match the size and format of the chosen frame structure + * \param _frame_state State; must be part of the problem's frame structure * - * - The structure is taken from Problem - * - The dimension is taken from Problem + * - The frame_types are taken from Problem * * This acts as a Frame factory, but also takes care to update related lists in WolfProblem: * - Create a Frame * - Add it to Trajectory * - If it is key-frame, update state-block lists in Problem */ - FrameBasePtr emplaceFrame(const TimeStamp& _time_stamp, // - const Eigen::VectorXd& _frame_state); + FrameBasePtr emplaceFrame(const TimeStamp& _time_stamp, + const VectorComposite& _frame_state); - /** \brief Emplace frame, guess all values + /** \brief Emplace frame from timestamp and state vector * \param _time_stamp Time stamp of the frame + * \param _frame_keys String containing the keys of the desired frame. Empty means all keys available. + * \param _frame_state State vector; must match the size and format of the chosen frame structure * - * - The structure is taken from Problem - * - The dimension is taken from Problem - * - The state is taken from Problem + * - The frame_types are taken from Problem * * This acts as a Frame factory, but also takes care to update related lists in WolfProblem: * - Create a Frame * - Add it to Trajectory * - If it is key-frame, update state-block lists in Problem */ - FrameBasePtr emplaceFrame(const TimeStamp& _time_stamp); + FrameBasePtr emplaceFrame(const TimeStamp& _time_stamp, + const StateKeys& _frame_keys, + const Eigen::VectorXd& _frame_state); /** \brief Emplace frame from state specs composite * \param _time_stamp Time stamp of the frame @@ -310,7 +272,7 @@ class Problem : public std::enable_shared_from_this<Problem> * - Add it to Trajectory * - If it is key-frame, update state-block lists in Problem */ - FrameBasePtr emplaceFrame(const TimeStamp& _time_stamp, // + FrameBasePtr emplaceFrame(const TimeStamp& _time_stamp, const SpecComposite& _frame_spec_composite); // Frame getters @@ -331,17 +293,18 @@ class Problem : public std::enable_shared_from_this<Problem> * * New key frame callback: It should be called by any processor that creates a new key frame. It calls the keyFrameCallback of the rest of processors. */ - void keyFrameCallback(FrameBasePtr _keyframe_ptr, // + void keyFrameCallback(FrameBasePtr _keyframe_ptr, ProcessorBasePtr _processor_ptr); // State getters TimeStamp getTimeStamp ( ) const; - VectorComposite getState ( const StateStructure& _structure = "" ) const; - VectorComposite getState ( const TimeStamp& _ts, const StateStructure& _structure = "" ) const; - VectorComposite getOdometry ( const StateStructure& _structure = "" ) const; + VectorComposite getState ( const StateKeys& _structure = "" ) const; + VectorComposite getState ( const TimeStamp& _ts, const StateKeys& _structure = "" ) const; + VectorComposite getOdometry ( const StateKeys& _structure = "" ) const; // Zero state provider - VectorComposite stateZero ( const StateStructure& _structure = "" ) const; + VectorComposite stateZero ( TypeComposite _types = {} ) const; + VectorComposite stateZero ( const StateKeys& _structure) const; @@ -351,7 +314,7 @@ class Problem : public std::enable_shared_from_this<Problem> MapBasePtr getMap(); void setMap(MapBasePtr); void loadMap(const std::string& _filename_dot_yaml); - void saveMap(const std::string& _filename_dot_yaml, // + void saveMap(const std::string& _filename_dot_yaml, const std::string& _map_name = "Map automatically saved by Wolf"); diff --git a/include/core/processor/factory_processor.h b/include/core/processor/factory_processor.h index b432824fbbae94391460334009fc09191057acca..9ce372e6503f37423f9e7237aa64e14e133b8bfa 100644 --- a/include/core/processor/factory_processor.h +++ b/include/core/processor/factory_processor.h @@ -117,7 +117,7 @@ namespace wolf * \endcode * */ -typedef Factory<ProcessorBase, +typedef Factory<std::shared_ptr<ProcessorBase>, const YAML::Node&> FactoryProcessor; template<> inline std::string FactoryProcessor::getClass() const @@ -125,7 +125,7 @@ inline std::string FactoryProcessor::getClass() const return "FactoryProcessor"; } -typedef Factory<ProcessorBase, +typedef Factory<std::shared_ptr<ProcessorBase>, const std::string&, const std::vector<std::string>&> FactoryProcessorYaml; template<> diff --git a/include/core/processor/motion_provider.h b/include/core/processor/motion_provider.h index faa85b47e8ff3588ffdeebdf57ad1b77110c6498..ae25dbf4cc08e45de3a241d0764d8bee5135fa0b 100644 --- a/include/core/processor/motion_provider.h +++ b/include/core/processor/motion_provider.h @@ -23,6 +23,7 @@ #include "core/common/wolf.h" #include "core/state_block/vector_composite.h" +#include "core/state_block/type_composite.h" #include "core/common/params_base.h" // for toString #include "yaml-cpp/yaml.h" @@ -34,19 +35,19 @@ WOLF_STRUCT_PTR_TYPEDEFS(ParamsMotionProvider); struct ParamsMotionProvider { bool state_provider = true; - int state_order = 1; + int state_provider_order = 1; ParamsMotionProvider() = default; ParamsMotionProvider(const YAML::Node& _n) { state_provider = _n["state_provider"].as<bool>(); if (state_provider) - state_order = _n["state_order"].as<double>(); + state_provider_order = _n["state_provider_order"].as<double>(); } std::string print() const { return "state_provider: " + toString(state_provider) + "\n" - + "state_provider_order: " + toString(state_order) + "\n"; + + "state_provider_order: " + toString(state_provider_order) + "\n"; } }; @@ -58,14 +59,13 @@ class MotionProvider { public: - //MotionProvider(const StateStructure& _structure, ParamsMotionProviderPtr _params); - MotionProvider(const TypeComposite& _structure, ParamsMotionProviderPtr _params); + MotionProvider(const TypeComposite& _state_types, ParamsMotionProviderPtr _params); virtual ~MotionProvider(); // Queries to the processor: virtual TimeStamp getTimeStamp() const = 0; - virtual VectorComposite getState(const StateStructure& _structure = "") const = 0; - virtual VectorComposite getState(const TimeStamp& _ts, const StateStructure& _structure = "") const = 0; + virtual VectorComposite getState(const StateKeys& _structure = "") const = 0; + virtual VectorComposite getState(const TimeStamp& _ts, const StateKeys& _structure = "") const = 0; VectorComposite getOdometry ( ) const; void setOdometry(const VectorComposite&); @@ -75,25 +75,23 @@ class MotionProvider void setOrder(int); public: - //const StateStructure& getStateStructure ( ) const { return state_structure_; }; - const TypeComposite& getStateStructure ( ) const { return state_structure_; }; - //void setStateStructure(std::string _state_structure) { state_structure_ = _state_structure; }; - void setStateStructure(TypeComposite _state_structure) { state_structure_ = _state_structure; }; + TypeComposite getStateTypes ( ) const { return state_types_; }; + StateKeys getStateKeys ( ) const { return state_types_.getKeys(); }; + void setStateTypes(const TypeComposite& _state_structure) { state_types_ = _state_structure; }; void addToProblem(ProblemPtr _prb_ptr, MotionProviderPtr _motion_ptr); protected: - //StateStructure state_structure_; ///< The structure of the state vector (to retrieve state blocks from frames) - TypeComposite state_structure_; + TypeComposite state_types_; VectorComposite odometry_; ParamsMotionProviderPtr params_motion_provider_; }; -// inline MotionProvider::MotionProvider(const StateStructure& _structure, ParamsMotionProviderPtr _params) : -inline MotionProvider::MotionProvider(const TypeComposite& _structure, ParamsMotionProviderPtr _params) : - state_structure_(_structure), +// inline MotionProvider::MotionProvider(const StateKeys& _structure, ParamsMotionProviderPtr _params) : +inline MotionProvider::MotionProvider(const TypeComposite& _state_types, ParamsMotionProviderPtr _params) : + state_types_(_state_types), params_motion_provider_(_params) { - // + checkTypeComposite(_state_types); } inline wolf::VectorComposite MotionProvider::getOdometry ( ) const @@ -117,12 +115,12 @@ inline bool MotionProvider::isStateGetter() const inline int MotionProvider::getOrder() const { - return params_motion_provider_->state_order; + return params_motion_provider_->state_provider_order; } inline void MotionProvider::setOrder(int _order) { - params_motion_provider_->state_order = _order; + params_motion_provider_->state_provider_order = _order; } } /* namespace wolf */ \ No newline at end of file diff --git a/include/core/processor/processor_motion.h b/include/core/processor/processor_motion.h index 3a2e7a60c1430e42cd19ab31352984f2c1515199..dbc86ff01abcb16fa27605781326fb66e30a62b3 100644 --- a/include/core/processor/processor_motion.h +++ b/include/core/processor/processor_motion.h @@ -48,11 +48,11 @@ struct ParamsProcessorMotion : public ParamsProcessorBase, public ParamsMotionPr ParamsProcessorBase(_n), ParamsMotionProvider(_n) { - max_time_span = _n["keyframe_vote"]["max_time_span"].as<double>(); - max_buff_length = _n["keyframe_vote"]["max_buff_length"].as<unsigned int>(); - dist_traveled = _n["keyframe_vote"]["dist_traveled"].as<double>(); - angle_turned = _n["keyframe_vote"]["angle_turned"].as<double>(); - unmeasured_perturbation_std = _n["unmeasured_perturbation_std"].as<double>(); + max_time_span = _n["keyframe_vote"]["max_time_span"].as<double>(); + max_buff_length = _n["keyframe_vote"]["max_buff_length"].as<unsigned int>(); + dist_traveled = _n["keyframe_vote"]["dist_traveled"].as<double>(); + angle_turned = _n["keyframe_vote"]["angle_turned"].as<double>(); + unmeasured_perturbation_std = _n["unmeasured_perturbation_std"].as<double>(); } std::string print() const override { @@ -166,7 +166,7 @@ class ProcessorMotion : public ProcessorBase, public MotionProvider // This is the main public interface public: ProcessorMotion(const std::string& _type, - std::string _state_structure, + TypeComposite _state_types, int _dim, SizeEigen _state_size, SizeEigen _delta_size, @@ -181,8 +181,8 @@ class ProcessorMotion : public ProcessorBase, public MotionProvider // Queries to the processor: TimeStamp getTimeStamp() const override; - VectorComposite getState(const StateStructure& _structure = "") const override; - VectorComposite getState(const TimeStamp& _ts, const StateStructure& _structure = "") const override; + VectorComposite getState(const StateKeys& _structure = "") const override; + VectorComposite getState(const TimeStamp& _ts, const StateKeys& _structure = "") const override; /** \brief Gets the delta preintegrated covariance from all integrations so far * \return the delta preintegrated covariance matrix diff --git a/include/core/processor/processor_tracker.h b/include/core/processor/processor_tracker.h index b4461c7e101f031a768d24b4912a06d0a1a5dad4..faf162f88fcefe61c7643cf4ef58da58427edc01 100644 --- a/include/core/processor/processor_tracker.h +++ b/include/core/processor/processor_tracker.h @@ -119,16 +119,16 @@ class ProcessorTracker : public ProcessorBase FeatureBasePtrList new_features_incoming_; ///< list of the new features of \b last successfully tracked in \b incoming FeatureBasePtrList known_features_last_; ///< list of the known features in previous captures successfully tracked in \b last FeatureBasePtrList known_features_incoming_; ///< list of the known features in \b last successfully tracked in \b incoming - StateStructure state_structure_; ///< Structure of frames created or used by this processor + StateKeys state_keys_; ///< Keys of frames created or used by this processor public: ProcessorTracker(const std::string& _type, - const StateStructure& _structure, + const StateKeys& _structure, int _dim, ParamsProcessorTrackerPtr _params_tracker); ~ProcessorTracker() override; - const StateStructure& getStateStructure() const; + const StateKeys& getStateKeys() const; virtual CaptureBaseConstPtr getOrigin() const; virtual CaptureBasePtr getOrigin(); @@ -305,9 +305,9 @@ inline FeatureBasePtrList& ProcessorTracker::getNewFeaturesListIncoming() return new_features_incoming_; } -inline const StateStructure& ProcessorTracker::getStateStructure ( ) const +inline const StateKeys& ProcessorTracker::getStateKeys ( ) const { - return state_structure_; + return state_keys_; } inline void ProcessorTracker::addNewFeatureIncoming(FeatureBasePtr _feature_ptr) diff --git a/include/core/processor/processor_tracker_feature.h b/include/core/processor/processor_tracker_feature.h index e76d8103fea857d725c52e9bafc365940907cd61..18e724e4ed582a04d5c7d48bbf8f924f64649460 100644 --- a/include/core/processor/processor_tracker_feature.h +++ b/include/core/processor/processor_tracker_feature.h @@ -98,7 +98,7 @@ class ProcessorTrackerFeature : public ProcessorTracker /** \brief Constructor with type */ ProcessorTrackerFeature(const std::string& _type, - const StateStructure& _structure, + const StateKeys& _structure, int _dim, ParamsProcessorTrackerFeaturePtr _params_tracker_feature); ~ProcessorTrackerFeature() override; diff --git a/include/core/processor/processor_tracker_landmark.h b/include/core/processor/processor_tracker_landmark.h index b11ae12b524dd1ce31c3e7c938b164073ce6953d..c4563d754f01253be8bcf386d59a68a35341029f 100644 --- a/include/core/processor/processor_tracker_landmark.h +++ b/include/core/processor/processor_tracker_landmark.h @@ -94,7 +94,7 @@ class ProcessorTrackerLandmark : public ProcessorTracker { public: ProcessorTrackerLandmark(const std::string& _type, - const StateStructure& _structure, + const StateKeys& _structure, ParamsProcessorTrackerLandmarkPtr _params_tracker_landmark); ~ProcessorTrackerLandmark() override; diff --git a/include/core/sensor/factory_sensor.h b/include/core/sensor/factory_sensor.h index 745b2d839759a8608072c663140589298d8c153b..82aff821af7aa7ef9a5dfa656fb1b8825e48e1e7 100644 --- a/include/core/sensor/factory_sensor.h +++ b/include/core/sensor/factory_sensor.h @@ -209,10 +209,10 @@ namespace wolf * - Problem::installSensor() : to install sensors in WOLF Problem. */ -typedef Factory<SensorBase, +typedef Factory<std::shared_ptr<SensorBase>, const YAML::Node&> FactorySensor; -typedef Factory<SensorBase, +typedef Factory<std::shared_ptr<SensorBase>, const std::string&, const std::string&, const std::vector<std::string>&> FactorySensorYaml; diff --git a/include/core/solver/factory_solver.h b/include/core/solver/factory_solver.h index 72716cf487baf347a26f47b15b28f82c047a90e3..860ede1ff3bfd123e0f9dfee2115a6e3f275f5c8 100644 --- a/include/core/solver/factory_solver.h +++ b/include/core/solver/factory_solver.h @@ -40,7 +40,7 @@ namespace wolf * */ -typedef Factory<SolverManager, +typedef Factory<std::shared_ptr<SolverManager>, const ProblemPtr&, const YAML::Node&> FactorySolver; diff --git a/include/core/state_block/composite.h b/include/core/state_block/composite.h index 83f73f5ab3bde507cd9c285cea9d24ae26ca2e8a..2e1abc2dd54cb29c2fb96139ff381289cda690b0 100644 --- a/include/core/state_block/composite.h +++ b/include/core/state_block/composite.h @@ -22,22 +22,23 @@ #pragma once #include <string> -#include <unordered_map> +#include <map> +#include <algorithm> #include "yaml-cpp/yaml.h" namespace wolf { +using std::map; -typedef std::string StateStructure; -using std::unordered_map; +typedef std::string StateKeys; template <typename T> -class Composite : public unordered_map<char, T> +class Composite : public map<char, T> { public: using CompositeType = T; - using unordered_map<char, T>::unordered_map; + using map<char,T>::map; Composite(const YAML::Node& _n); virtual ~Composite() = default; @@ -45,12 +46,12 @@ class Composite : public unordered_map<char, T> CompositeOther cast() const; bool has(char _key) const; - bool has(const StateStructure &_structure) const; - StateStructure getStructure() const; - + bool has(const StateKeys &_structure) const; + StateKeys getKeys() const; + friend std::ostream& operator<<(std::ostream &_os, const Composite<T> &_x) { - for (const auto &pair : _x) + for (auto&& pair : _x) { _os << pair.first << ": " << pair.second; } @@ -63,7 +64,7 @@ template <typename CompositeOther> inline CompositeOther Composite<T>::cast() const { CompositeOther casted_composite; - for (auto pair : *this) + for (auto&& pair : *this) { casted_composite.emplace(pair.first, static_cast<typename CompositeOther::CompositeType>(pair.second)); } @@ -77,6 +78,7 @@ inline Composite<T>::Composite(const YAML::Node& _n) { 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)); @@ -90,9 +92,9 @@ inline bool Composite<T>::has(char _key) const } template <typename T> -inline bool Composite<T>::has(const StateStructure &_structure) const +inline bool Composite<T>::has(const StateKeys &_query_keys) const { - for (auto key : _structure) + for (auto&& key : _query_keys) { if (not has(key)) { @@ -103,14 +105,13 @@ inline bool Composite<T>::has(const StateStructure &_structure) const } template <typename T> -inline std::string Composite<T>::getStructure() const +inline StateKeys Composite<T>::getKeys() const { - std::string structure; - for (auto pair : *this) - { - structure.push_back(pair.first); - } - return structure; + StateKeys keys; + for (auto&& pair : *this) + keys.push_back(pair.first); + + return keys; } } // namespace wolf \ No newline at end of file diff --git a/include/core/state_block/factory_state_block.h b/include/core/state_block/factory_state_block.h index 578a8b0acdbf2fcfeb9b8b26656c676774fb67c2..31b87e960309f464278f202f1c0557730948e611 100644 --- a/include/core/state_block/factory_state_block.h +++ b/include/core/state_block/factory_state_block.h @@ -121,24 +121,33 @@ namespace wolf * * Note: for base state blocks, the size is determined by the size of the provided vector parameter `VectorXd& _state`. */ -typedef Factory<StateBlock, const Eigen::VectorXd&, bool> FactoryStateBlock; +typedef Factory<std::shared_ptr<StateBlock>, const Eigen::VectorXd&, bool> FactoryStateBlock; template<> inline std::string FactoryStateBlock::getClass() const { return "FactoryStateBlock"; } +typedef Factory<std::shared_ptr<StateBlock>> FactoryStateBlockIdentity; +template<> +inline std::string FactoryStateBlockIdentity::getClass() const +{ + return "FactoryStateBlockIdentity"; +} +typedef Factory<Eigen::VectorXd> FactoryStateBlockIdentityVector; +template<> +inline std::string FactoryStateBlockIdentityVector::getClass() const +{ + return "FactoryStateBlockIdentityVector"; +} -#define WOLF_REGISTER_STATEBLOCK(StateBlockType) \ - namespace \ - { \ - const bool WOLF_UNUSED StateBlockType##Registered = \ - FactoryStateBlock::registerCreator(#StateBlockType, StateBlockType::create); \ - } - -#define WOLF_REGISTER_STATEBLOCK_WITH_KEY(Key, StateBlockType) \ - namespace \ - { \ - const bool WOLF_UNUSED StateBlockType##RegisteredWith##Key = \ - FactoryStateBlock::registerCreator(#Key, StateBlockType::create); \ +#define WOLF_REGISTER_STATEBLOCK(StateBlockType) \ + namespace \ + { \ + const bool WOLF_UNUSED StateBlockType##Registered = \ + FactoryStateBlock::registerCreator(#StateBlockType, StateBlockType::create); \ + const bool WOLF_UNUSED StateBlockType##IdentityRegistered = \ + FactoryStateBlockIdentity::registerCreator(#StateBlockType, StateBlockType::createIdentity); \ + const bool WOLF_UNUSED StateBlockType##IdentityVectorRegistered = \ + FactoryStateBlockIdentityVector::registerCreator(#StateBlockType, StateBlockType::Identity); \ } } \ 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 f728227f91b0fd0f37c60bf1d4412c9540f3cf08..d143262215279bf247464e017a7cb842423c351d 100644 --- a/include/core/state_block/has_state_blocks.h +++ b/include/core/state_block/has_state_blocks.h @@ -37,14 +37,14 @@ class HasStateBlocks public: HasStateBlocks(); - HasStateBlocks(const StateStructure& _structure) : structure_(_structure), state_block_map_() {} + HasStateBlocks(const StateKeys& _keys) : keys_order_(_keys), state_block_map_() {} HasStateBlocks(const SpecComposite& _specs); HasStateBlocks(const TypeComposite& _types, const VectorComposite& _vectors); virtual ~HasStateBlocks(); - const StateStructure& getStructure() const { return structure_; } - void appendToStructure(const char& _new_key){ structure_ += _new_key; } - bool isInStructure(const char& _sb_key) const { return structure_.find(_sb_key) != std::string::npos; } + StateKeys getKeys() const { return keys_order_; } + 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; } std::unordered_map<char, StateBlockConstPtr> getStateBlockMap() const; std::unordered_map<char, StateBlockPtr> getStateBlockMap(); @@ -82,15 +82,15 @@ class HasStateBlocks virtual void removeStateBlocks(ProblemPtr _problem); // States - VectorComposite getState(const StateStructure& structure="") const; + VectorComposite getState(const StateKeys& _keys="") const; void setState(const VectorComposite& _state, const bool _notify = true); - void setState(const Eigen::VectorXd& _state, const StateStructure& _structure, const std::list<SizeEigen>& _sizes, const bool _notify = true); - void setState(const Eigen::VectorXd& _state, const StateStructure& _structure, const bool _notify = true); - void setState(const StateStructure& _structure, const std::list<VectorXd>& _vectors, const bool _notify = true); + void setState(const Eigen::VectorXd& _state, const StateKeys& _keys, const std::list<SizeEigen>& _sizes, const bool _notify = true); + void setState(const Eigen::VectorXd& _state, const StateKeys& _keys, const bool _notify = true); + void setState(const StateKeys& _keys, const std::list<VectorXd>& _vectors, const bool _notify = true); - VectorXd getStateVector(const StateStructure& structure) const; - unsigned int getSize(const StateStructure& _structure="") const; - unsigned int getLocalSize(const StateStructure& _structure="") const; + VectorXd getStateVector(const StateKeys& _keys) const; + unsigned int getSize(const StateKeys& _keys="") const; + unsigned int getLocalSize(const StateKeys& _keys="") const; // Perturb state void perturb(double amplitude = 0.01); @@ -102,7 +102,7 @@ class HasStateBlocks void printState(bool _metric, bool _state_blocks, std::ostream& _stream, std::string _tabs) const; private: - StateStructure structure_; + StateKeys keys_order_; std::unordered_map<char, StateBlockPtr> state_block_map_; }; @@ -117,7 +117,7 @@ class HasStateBlocks namespace wolf{ inline HasStateBlocks::HasStateBlocks() : - structure_(std::string("")), + keys_order_(std::string("")), state_block_map_() { // @@ -144,7 +144,7 @@ inline std::unordered_map<char, StateBlockPtr> HasStateBlocks::getStateBlockMap( inline std::vector<StateBlockConstPtr> HasStateBlocks::getStateBlockVec() const { std::vector<StateBlockConstPtr> sbv; - for (auto& key : structure_) + for (auto& key : keys_order_) { sbv.push_back(getStateBlock(key)); } @@ -154,7 +154,7 @@ inline std::vector<StateBlockConstPtr> HasStateBlocks::getStateBlockVec() const inline std::vector<StateBlockPtr> HasStateBlocks::getStateBlockVec() { std::vector<StateBlockPtr> sbv; - for (auto& key : structure_) + for (auto& key : keys_order_) { sbv.push_back(getStateBlock(key)); } @@ -179,7 +179,7 @@ inline std::shared_ptr<SB> HasStateBlocks::emplaceStateBlock(const char& _sb_key inline bool HasStateBlocks::setStateBlock(const char _sb_key, const StateBlockPtr& _sb) { - assert (structure_.find(_sb_key) > 0 and "Cannot set a state block out of the state structure! Use addStateBlock instead."); + assert (keys_order_.find(_sb_key) > 0 and "Cannot set a state block out of the state keys! Use addStateBlock instead."); assert ( state_block_map_.count(_sb_key) > 0 and "Cannot set an inexistent state block! Use addStateBlock instead."); state_block_map_.at(_sb_key) = _sb; return true; // success @@ -256,29 +256,29 @@ inline void HasStateBlocks::setState(const VectorComposite& _state, const bool _ const auto& vec = pair_key_vec.second; const auto& sb = getStateBlock(key); if (!sb) - WOLF_ERROR("Stateblock key ", key, " not in the structure"); - assert (sb and "Stateblock key not in the structure"); + WOLF_ERROR("Stateblock key ", key, " not in the keys"); + assert (sb and "Stateblock key not in the keys"); sb->setState(vec, _notify); } } -inline void HasStateBlocks::setState(const Eigen::VectorXd& _state, const StateStructure& _structure, const bool _notify) +inline void HasStateBlocks::setState(const Eigen::VectorXd& _state, const StateKeys& _keys, const bool _notify) { - StateStructure structure; - if (_structure == "") - structure = structure_; + StateKeys keys; + if (_keys == "") + keys = keys_order_; else - structure = _structure; + keys = _keys; - int size = getSize(structure); + int size = getSize(keys); assert(_state.size() == size and "In FrameBase::setState wrong state size"); unsigned int index = 0; - for (const char key : structure) + for (const char key : keys) { const auto& sb = getStateBlock(key); - assert (sb and "Stateblock key not in the structure"); + assert (sb and "Stateblock key not in the keys"); sb->setState(_state.segment(index, sb->getSize()), _notify); // do not notify if state block is not estimated by the solver index += sb->getSize(); @@ -287,16 +287,16 @@ inline void HasStateBlocks::setState(const Eigen::VectorXd& _state, const StateS } inline void HasStateBlocks::setState (const Eigen::VectorXd& _state, - const StateStructure& _structure, + const StateKeys& _keys, const std::list<SizeEigen>& _sizes, const bool _notify) { SizeEigen index = 0; auto size_it = _sizes.begin(); - for (const auto& key : _structure) + for (const auto& key : _keys) { const auto& sb = getStateBlock(key); - assert (sb and "Stateblock key not in the structure"); + assert (sb and "Stateblock key not in the keys"); assert(*size_it == sb->getSize() and "State block size mismatch"); sb->setState(_state.segment(index, *size_it), _notify); @@ -306,13 +306,13 @@ inline void HasStateBlocks::setState (const Eigen::VectorXd& _state, } -inline void HasStateBlocks::setState(const StateStructure& _structure, const std::list<VectorXd>& _vectors, const bool _notify) +inline void HasStateBlocks::setState(const StateKeys& _keys, const std::list<VectorXd>& _vectors, const bool _notify) { auto vec_it = _vectors.begin(); - for (const auto key : _structure) + for (const auto key : _keys) { const auto& sb = getStateBlock(key); - assert (sb and "Stateblock key not in the structure"); + assert (sb and "Stateblock key not in the keys"); assert(vec_it->size() == sb->getSize() and "State block size mismatch"); sb->setState(*vec_it, _notify); @@ -321,23 +321,23 @@ inline void HasStateBlocks::setState(const StateStructure& _structure, const std } -//// _structure can be either stateblock structure of the node or a subset of this structure -inline VectorXd HasStateBlocks::getStateVector(const StateStructure& _structure) const +//// _keys can be either stateblock keys of the node or a subset of this keys +inline VectorXd HasStateBlocks::getStateVector(const StateKeys& _keys) const { - StateStructure structure; - if (_structure == "") - structure = structure_; + StateKeys keys; + if (_keys == "") + keys = keys_order_; else - structure = _structure; + keys = _keys; - VectorXd state(getSize(structure)); + VectorXd state(getSize(keys)); unsigned int index = 0; - for (const char key : structure) + for (const char key : keys) { const auto& sb = getStateBlock(key); - assert(sb != nullptr and "Requested StateBlock key not in the structure"); + assert(sb != nullptr and "Requested StateBlock key not in the keys"); state.segment(index,sb->getSize()) = sb->getState(); index += sb->getSize(); @@ -345,13 +345,13 @@ inline VectorXd HasStateBlocks::getStateVector(const StateStructure& _structure) return state; } -inline VectorComposite HasStateBlocks::getState(const StateStructure& _structure) const +inline VectorComposite HasStateBlocks::getState(const StateKeys& _keys) const { - const StateStructure& structure = (_structure == "" ? structure_ : _structure); + const StateKeys& keys = (_keys == "" ? keys_order_ : _keys); VectorComposite state; - for (const auto key : structure) + for (const auto key : keys) { auto state_it = state_block_map_.find(key); @@ -363,16 +363,16 @@ inline VectorComposite HasStateBlocks::getState(const StateStructure& _structure return state; } -inline unsigned int HasStateBlocks::getSize(const StateStructure& _structure) const +inline unsigned int HasStateBlocks::getSize(const StateKeys& _keys) const { - const StateStructure& structure = (_structure == "" ? structure_ : _structure); + const StateKeys& keys = (_keys == "" ? keys_order_ : _keys); unsigned int size = 0; - for (const char key : structure) + for (const char key : keys) { const auto& sb = getStateBlock(key); if (!sb){ - WOLF_ERROR("Stateblock key ", key, " not in the structure"); + WOLF_ERROR("Stateblock key ", key, " not in the keys"); } size += sb->getSize(); } @@ -441,20 +441,20 @@ inline bool HasStateBlocks::stateBlockKey(const StateBlockConstPtr &_sb, char& _ } } -inline unsigned int HasStateBlocks::getLocalSize(const StateStructure& _structure) const +inline unsigned int HasStateBlocks::getLocalSize(const StateKeys& _keys) const { - StateStructure structure; - if (_structure == "") - structure = structure_; + StateKeys keys; + if (_keys == "") + keys = keys_order_; else - structure = _structure; + keys = _keys; unsigned int size = 0; - for (const char key : structure) + for (const char key : keys) { const auto& sb = getStateBlock(key); if (!sb){ - WOLF_ERROR("Stateblock key ", key, " not in the structure"); + WOLF_ERROR("Stateblock key ", key, " not in the keys"); } size += sb->getLocalSize(); } diff --git a/include/core/state_block/spec_composite.h b/include/core/state_block/spec_composite.h index 05fa48c3a0150bfba804d9428d8023a5b521faf0..688caea505b38e199f24bbd51c370b21cad76f47 100644 --- a/include/core/state_block/spec_composite.h +++ b/include/core/state_block/spec_composite.h @@ -22,6 +22,7 @@ #pragma once #include "core/state_block/composite.h" +#include "core/state_block/type_composite.h" #include "core/state_block/vector_composite.h" #include <string> @@ -32,8 +33,6 @@ namespace wolf { -using std::unordered_map; - class SpecState { protected: @@ -67,8 +66,6 @@ class SpecState friend std::ostream& operator <<(std::ostream &_os, const wolf::SpecState &_x); }; -typedef Composite<std::string> TypeComposite; - template <typename T> class SpecBaseComposite : public Composite<T> { @@ -86,9 +83,9 @@ template <> inline VectorComposite SpecBaseComposite<SpecState>::getVectorComposite() const { VectorComposite states; - for (auto spec_pair : *this) + for (auto&& pair : *this) { - states.emplace(spec_pair.first, spec_pair.second.getState()); + states.emplace(pair.first, pair.second.getState()); } return states; } @@ -103,9 +100,9 @@ template <> inline TypeComposite SpecBaseComposite<SpecState>::getTypeComposite() const { TypeComposite types; - for (auto spec_pair : *this) + for (auto&& pair : *this) { - types.emplace(spec_pair.first, spec_pair.second.getType()); + types.emplace(pair.first, pair.second.getType()); } return types; } @@ -116,7 +113,6 @@ inline TypeComposite SpecBaseComposite<T>::getTypeComposite() const throw std::runtime_error("Impossible to build a TypeComposite from this Composite"); } - inline const std::string& SpecState::getType() const { return type_; } inline const std::string& SpecState::getMode() const { return mode_; } diff --git a/include/core/state_block/state_angle.h b/include/core/state_block/state_angle.h index cce71edc7b2cd3d105ea228ef099ab6de3281d4f..d33f53a1a923b1380c8a04c1a4266cb434cf1aa1 100644 --- a/include/core/state_block/state_angle.h +++ b/include/core/state_block/state_angle.h @@ -30,14 +30,16 @@ namespace wolf class StateAngle : public StateBlock { public: - StateAngle(double _angle = 0.0, bool _fixed = false, bool _transformable = true); - StateAngle(const Vector1d& _angle, bool _fixed = false, bool _transformable = true); + StateAngle(double _angle, bool _fixed = false, bool _transformable = true); + StateAngle(const VectorXd& _angle, bool _fixed = false, bool _transformable = true); + + static Eigen::VectorXd Identity(); + + WOLF_STATE_BLOCK_CREATE(StateAngle); ~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, bool _transformable) : @@ -46,23 +48,23 @@ inline StateAngle::StateAngle(double _angle, bool _fixed, bool _transformable) : state_(0) = pi2pi(_angle); } -inline StateAngle::StateAngle(const Vector1d& _angle, bool _fixed, bool _transformable) : +inline StateAngle::StateAngle(const VectorXd& _angle, bool _fixed, bool _transformable) : StateBlock(1, _fixed, std::make_shared<LocalParametrizationAngle>(), _transformable) { + if(_angle.size() != 1) + throw std::runtime_error("The angle state vector must be of size 1!"); + state_(0) = pi2pi(_angle(0)); } -inline StateAngle::~StateAngle() +inline Eigen::VectorXd StateAngle::Identity() { - // + return Eigen::VectorXd::Zero(1); } -inline StateBlockPtr StateAngle::create (const Eigen::VectorXd& _state, bool _fixed) +inline StateAngle::~StateAngle() { - if(_state.size() != 1) - throw std::runtime_error("The angle state vector must be of size 1!"); - - return std::make_shared<StateAngle>(_state(0), _fixed); + // } inline void StateAngle::transform(const VectorComposite& _transformation) diff --git a/include/core/state_block/state_block.h b/include/core/state_block/state_block.h index ca355e307206f6f74dbb25e10e5035a14e332385..d19d3155a28d8b95e273a3f8f9e9bbde97a8564b 100644 --- a/include/core/state_block/state_block.h +++ b/include/core/state_block/state_block.h @@ -21,6 +21,31 @@ //--------LICENSE_END-------- #pragma once +/* + * Macro for defining StateBlocks creators. + * + * Place a call to this macro inside your class declaration + * (in the state_block_class.h file), preferably just after the constructors. + * + * In order to use this macro, the derived state_block class, StateBlockClass, + * must have the constructor callable as: + * + * StateBlockClass(const Eigen::VectorXd& _state, bool _fixed) + * + * And the static method: + * + * static Eigen::VectorXd Identity() + */ +#define WOLF_STATE_BLOCK_CREATE(StateBlockClass) \ +static StateBlockPtr create(const Eigen::VectorXd& _state, bool _fixed) \ +{ \ + return std::make_shared<StateBlockClass>(_state, _fixed); \ +} \ +static StateBlockPtr createIdentity() \ +{ \ + return std::make_shared<StateBlockClass>(StateBlockClass::Identity(), false); \ +} \ + // Fwd references namespace wolf{ class NodeBase; @@ -341,7 +366,8 @@ inline Eigen::VectorXd StateBlock::identity() const { return Eigen::VectorXd::Zero(state_size_); } -inline Eigen::VectorXd StateBlock::zero() const + +inline Eigen::VectorXd StateBlock::zero() const { return identity(); } diff --git a/include/core/state_block/state_block_derived.h b/include/core/state_block/state_block_derived.h index a4c34c448563e7126198f0218effe1a1d16ce0c8..6f0c50feeb7301ae6e90ea076cf5d311b07418cd 100644 --- a/include/core/state_block/state_block_derived.h +++ b/include/core/state_block/state_block_derived.h @@ -39,12 +39,21 @@ template <size_t size> class StateParams : public StateBlock { public: - StateParams(const Eigen::VectorXd& _state, bool _fixed = false) : StateBlock(_state, _fixed, nullptr, false) {} + StateParams(const Eigen::VectorXd& _state, bool _fixed = false) : + StateBlock(_state, _fixed, nullptr, false) + { + if (_state.size() != size) + throw std::runtime_error("Wrong vector size for StateParams."); + } + static Eigen::VectorXd Identity() + { + return Eigen::Matrix<double,size,1>::Zero(); + } + WOLF_STATE_BLOCK_CREATE(StateParams<size>); void transform(const VectorComposite& _transformation) override { // non transformable! --> do nothing } - static StateBlockPtr create(const Eigen::VectorXd& _state, bool _fixed); }; typedef StateParams<1> StateParams1; @@ -73,12 +82,18 @@ class StatePoint2d : public StateBlock StatePoint2d(const Eigen::VectorXd& _state, bool _fixed = false, bool _transformable = true) : StateBlock(_state, _fixed, nullptr, _transformable) { + if (_state.size() != 2) + throw std::runtime_error("Wrong vector size for StatePoint2d."); + } + static Eigen::VectorXd Identity() + { + return Eigen::Vector2d::Zero(); } + WOLF_STATE_BLOCK_CREATE(StatePoint2d); void transform(const VectorComposite& _transformation) override { if (transformable_) setState(_transformation.at('P') + Rotation2Dd(_transformation.at('O')(0)) * getState()); } - static StateBlockPtr create(const Eigen::VectorXd& _state, bool _fixed); }; /** @@ -97,12 +112,18 @@ class StateVector2d : public StateBlock StateVector2d(const Eigen::VectorXd& _state, bool _fixed = false, bool _transformable = true) : StateBlock(_state, _fixed, nullptr, _transformable) { + if (_state.size() != 2) + throw std::runtime_error("Wrong vector size for StateVector2d."); } + static Eigen::VectorXd Identity() + { + return Eigen::Vector2d::Zero(); + } + WOLF_STATE_BLOCK_CREATE(StateVector2d); void transform(const VectorComposite& _transformation) override { if (transformable_) setState(Rotation2Dd(_transformation.at('O')(0)) * getState()); } - static StateBlockPtr create(const Eigen::VectorXd& _state, bool _fixed); }; /** @@ -117,16 +138,24 @@ class StateVector2d : public StateBlock class StatePoint3d : public StateBlock { public: - StatePoint3d(const Eigen::VectorXd& _state, bool _fixed = false, bool _transformable = true) + StatePoint3d(const Eigen::VectorXd& _state, + bool _fixed = false, + bool _transformable = true) : StateBlock(_state, _fixed, nullptr, _transformable) { + if (_state.size() != 3) + throw std::runtime_error("Wrong vector size for StatePoint3d."); + } + static Eigen::VectorXd Identity() + { + return Eigen::Vector3d::Zero(); } + WOLF_STATE_BLOCK_CREATE(StatePoint3d); void transform(const VectorComposite& _transformation) override { if (transformable_) setState(_transformation.at('P') + Quaterniond(_transformation.at('O').data()) * getState()); } - static StateBlockPtr create(const Eigen::VectorXd& _state, bool _fixed); }; /** @@ -145,12 +174,18 @@ class StateVector3d : public StateBlock StateVector3d(const Eigen::VectorXd& _state, bool _fixed = false, bool _transformable = true) : StateBlock(_state, _fixed, nullptr, _transformable) { + if (_state.size() != 3) + throw std::runtime_error("Wrong vector size for StateVector3d."); + } + static Eigen::VectorXd Identity() + { + return Eigen::Vector3d::Zero(); } + WOLF_STATE_BLOCK_CREATE(StateVector3d); void transform(const VectorComposite& _transformation) override { if (transformable_) setState(Quaterniond(_transformation.at('O').data()) * getState()); } - static StateBlockPtr create(const Eigen::VectorXd& _state, bool _fixed); }; } // namespace wolf diff --git a/include/core/state_block/state_composite.h b/include/core/state_block/state_composite.h index 7307b8afb4e48e785a48eb9461a13ba0eda2707d..f6d851b488b1147514a3a7f76da68d1c325cae58 100644 --- a/include/core/state_block/state_composite.h +++ b/include/core/state_block/state_composite.h @@ -44,8 +44,8 @@ class MatrixComposite : public std::unordered_map < char, std::unordered_map < c std::unordered_map < char, unsigned int> size_rows_, size_cols_; unsigned int rows() const; unsigned int cols() const; - void sizeAndIndices(const StateStructure & _struct_rows, - const StateStructure & _struct_cols, + void sizeAndIndices(const StateKeys & _struct_rows, + const StateKeys & _struct_cols, std::unordered_map < char, unsigned int>& _indices_rows, std::unordered_map < char, unsigned int>& _indices_cols, unsigned int& _rows, @@ -53,11 +53,11 @@ class MatrixComposite : public std::unordered_map < char, std::unordered_map < c public: MatrixComposite() {}; - MatrixComposite(const StateStructure& _row_structure, - const StateStructure& _col_structure); - MatrixComposite(const StateStructure& _row_structure, + MatrixComposite(const StateKeys& _row_structure, + const StateKeys& _col_structure); + MatrixComposite(const StateKeys& _row_structure, const std::list<int>& _row_sizes, - const StateStructure& _col_structure, + const StateKeys& _col_structure, const std::list<int>& _col_sizes); /** @@ -75,19 +75,19 @@ class MatrixComposite : public std::unordered_map < char, std::unordered_map < c * vec_comp["b"].transpose(); // = (3,4,5); */ MatrixComposite(const MatrixXd& _m, - const StateStructure& _row_structure, + const StateKeys& _row_structure, const std::list<int>& _row_sizes, - const StateStructure& _col_structure, + const StateKeys& _col_structure, const std::list<int>& _col_sizes); ~MatrixComposite() = default; bool check() const; - static MatrixComposite zero(const StateStructure& _row_structure, + static MatrixComposite zero(const StateKeys& _row_structure, const std::list<int>& _row_sizes, - const StateStructure& _col_structure, + const StateKeys& _col_structure, const std::list<int>& _col_sizes); - static MatrixComposite identity(const StateStructure& _structure, + static MatrixComposite identity(const StateKeys& _structure, const std::list<int>& _sizes); unsigned int count(const char &_row, @@ -111,8 +111,8 @@ class MatrixComposite : public std::unordered_map < char, std::unordered_map < c using std::unordered_map < char, std::unordered_map < char, Eigen::MatrixXd > >::count; - MatrixXd matrix(const StateStructure & _struct_rows, - const StateStructure & _struct_cols) const; + MatrixXd matrix(const StateKeys & _struct_rows, + const StateKeys & _struct_cols) const; MatrixComposite operator * (double scalar_right) const; MatrixComposite operator + (const MatrixComposite & _second) const; @@ -239,7 +239,7 @@ class StateBlockComposite StateBlockPtr at(const char& _sb_type) const; void set(const char& _sb_type, const StateBlockPtr& _sb); void set(const char& _sb_type, const VectorXd& _vec); - void setVectors(const StateStructure& _structure, const std::list<VectorXd> &_vectors); + void setVectors(const StateKeys& _structure, const std::list<VectorXd> &_vectors); bool key(const StateBlockPtr& _sb, char& _key) const; char key(const StateBlockPtr& _sb) const; StateBlockMapCIter find(const StateBlockPtr& _sb) const; @@ -268,9 +268,9 @@ class StateBlockComposite // Get compact Eigen vector and related things SizeEigen stateSize() const; - SizeEigen stateSize(const StateStructure& _structure) const; - VectorXd stateVector(const StateStructure& _structure) const; - void stateVector(const StateStructure& _structure, VectorXd& _vector) const; + SizeEigen stateSize(const StateKeys& _structure) const; + VectorXd stateVector(const StateKeys& _structure) const; + void stateVector(const StateKeys& _structure, VectorXd& _vector) const; private: StateBlockMap state_block_map_; diff --git a/include/core/state_block/state_homogeneous_3d.h b/include/core/state_block/state_homogeneous_3d.h index a576fa0ea2f4beb7aec0b8c49bfa8438e674a6d2..357a976044f3574e17d9c405e0119aa338725e22 100644 --- a/include/core/state_block/state_homogeneous_3d.h +++ b/include/core/state_block/state_homogeneous_3d.h @@ -32,19 +32,20 @@ class StateHomogeneous3d : public StateBlock StateHomogeneous3d(bool _fixed = false, bool _transformable = true); StateHomogeneous3d(const Eigen::VectorXd _state, bool _fixed = false, bool _transformable = true); ~StateHomogeneous3d() override; + static Eigen::VectorXd Identity(); + WOLF_STATE_BLOCK_CREATE(StateHomogeneous3d); 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, bool _transformable) : StateBlock(_state, _fixed, nullptr, _transformable) { - assert(_state.size() == 4 && "Homogeneous 3d must be size 4."); + if(_state.size() != 4) + throw std::runtime_error("Homogeneous 3d must be size 4."); local_param_ptr_ = std::make_shared<LocalParametrizationHomogeneous>(); } @@ -60,7 +61,6 @@ inline StateHomogeneous3d::~StateHomogeneous3d() // The local_param_ptr_ pointer is already removed by the base class } - inline void StateHomogeneous3d::setIdentity(bool _notify) { setState(Eigen::Quaterniond::Identity().coeffs(), _notify); @@ -71,16 +71,15 @@ inline Eigen::VectorXd StateHomogeneous3d::identity() const return Eigen::Quaterniond::Identity().coeffs(); } -inline void StateHomogeneous3d::transform(const VectorComposite& _transformation) +inline Eigen::VectorXd StateHomogeneous3d::Identity() { - if (transformable_) - setState((Quaterniond(_transformation.at('O').data()) * Quaterniond(getState().data())).coeffs()); // TODO is this correct?????? probably not!!! + return Eigen::Quaterniond::Identity().coeffs(); } -inline StateBlockPtr StateHomogeneous3d::create (const Eigen::VectorXd& _state, bool _fixed) +inline void StateHomogeneous3d::transform(const VectorComposite& _transformation) { - MatrixSizeCheck<4,1>::check(_state); - return std::make_shared<StateHomogeneous3d>(_state, _fixed); + if (transformable_) + setState((Quaterniond(_transformation.at('O').data()) * Quaterniond(getState().data())).coeffs()); // TODO is this correct?????? probably not!!! } } // namespace wolf \ No newline at end of file diff --git a/include/core/state_block/state_quaternion.h b/include/core/state_block/state_quaternion.h index 718f3965da56e0258c0fd02f93edb3fea6dd4f68..13632c59ee2d6f1cd102a9cb7996ffcbff85d1a3 100644 --- a/include/core/state_block/state_quaternion.h +++ b/include/core/state_block/state_quaternion.h @@ -33,13 +33,13 @@ class StateQuaternion : public StateBlock StateQuaternion(const Eigen::VectorXd& _state, bool _fixed = false, bool _transformable = true); StateQuaternion(const Eigen::Quaterniond& _quaternion, bool _fixed = false, bool _transformable = true); ~StateQuaternion() override; + static Eigen::VectorXd Identity(); + WOLF_STATE_BLOCK_CREATE(StateQuaternion); 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, bool _transformable) : @@ -77,12 +77,16 @@ inline StateQuaternion::~StateQuaternion() // The local_param_ptr_ pointer is already removed by the base class } - inline void StateQuaternion::setIdentity(bool _notify) { setState(Eigen::Quaterniond::Identity().coeffs(), _notify); } +inline Eigen::VectorXd StateQuaternion::Identity() +{ + return Eigen::Quaterniond::Identity().coeffs(); +} + inline Eigen::VectorXd StateQuaternion::identity() const { return Eigen::Quaterniond::Identity().coeffs(); @@ -94,9 +98,4 @@ inline void StateQuaternion::transform(const VectorComposite& _transformation) 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); -} - } // namespace wolf \ No newline at end of file diff --git a/include/core/state_block/type_composite.h b/include/core/state_block/type_composite.h new file mode 100644 index 0000000000000000000000000000000000000000..5835e2e2b232434475249a88ee71feb8afec9079 --- /dev/null +++ b/include/core/state_block/type_composite.h @@ -0,0 +1,114 @@ +//--------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 + +#include "core/common/wolf.h" +#include "core/state_block/composite.h" + +namespace wolf +{ + +typedef Composite<std::string> TypeComposite; +void checkTypeComposite(const TypeComposite& _types, SizeEigen _dim = 0); +bool isTypeCompositeValid(const TypeComposite& _types, SizeEigen _dim = 0); + +template <> +inline Composite<std::string>::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.as<std::string>()); + } +} + +inline bool isTypeCompositeValid(const TypeComposite& _types, SizeEigen _dim) +{ + try + { + checkTypeComposite(_types, _dim); + } + catch(const std::exception& e) + { + return false; + } + return true; +} + +inline void checkTypeComposite(const TypeComposite& _types, SizeEigen _dim) +{ + if (_dim == 2) + { + if (_types.count('P') != 0 and _types.at('P') != "StatePoint2d") + { + throw std::runtime_error("Bad TypeComposite: Type for 'P' in 2D should be 'StatePoint2d'"); + } + if (_types.count('O') != 0 and _types.at('O') != "StateAngle") + { + throw std::runtime_error("Bad TypeComposite: Type for 'O' in 2D should be 'StateAngle'"); + } + if (_types.count('V') != 0 and _types.at('V') != "StateVector2d") + { + throw std::runtime_error("Bad TypeComposite: Type for 'V' in 2D should be 'StateVector2d'"); + } + } + else if (_dim == 3) + { + if (_types.count('P') != 0 and _types.at('P') != "StatePoint3d") + { + throw std::runtime_error("Bad TypeComposite: Type for 'P' in 3D should be 'StatePoint3d'"); + } + if (_types.count('O') != 0 and _types.at('O') != "StateQuaternion") + { + throw std::runtime_error("Bad TypeComposite: Type for 'O' in 3D should be 'StateQuaternion'"); + } + if (_types.count('V') != 0 and _types.at('V') != "StateVector3d") + { + throw std::runtime_error("Bad TypeComposite: Type for 'V' in 3D should be 'StateVector3d'"); + } + } + else if (_dim == 0) + { + if (_types.count('P') != 0 and _types.at('P') != "StatePoint2d" and _types.at('P') != "StatePoint3d") + { + throw std::runtime_error("Bad TypeComposite: Type for 'P' should be 'StatePoint2d' or 'StatePoint3d'"); + } + if (_types.count('O') != 0 and _types.at('O') != "StateAngle" and _types.at('O') != "StateQuaternion") + { + throw std::runtime_error("Bad TypeComposite: Type for 'O' should be 'StateAngle' or 'StateQuaternion'"); + } + if (_types.count('V') != 0 and _types.at('V') != "StateVector2d" and _types.at('V') != "StateVector3d") + { + throw std::runtime_error("Bad TypeComposite: Type for 'V' should be 'StateVector2d' or 'StateVector3d'"); + } + } + else + { + throw std::runtime_error("checkTypeComposite(_types, _dim) bad value for '_dim', should be 2 for 2D, 3 for 3D, or 0 for not defined"); + } +} + +} // namespace wolf \ No newline at end of file diff --git a/include/core/state_block/vector_composite.h b/include/core/state_block/vector_composite.h index f5a87bceae193ba31e6d203667443370d49ae451..ebed1706395b60e87650c1d621bad052a8c44b45 100644 --- a/include/core/state_block/vector_composite.h +++ b/include/core/state_block/vector_composite.h @@ -37,11 +37,9 @@ class VectorComposite : public Composite<Eigen::VectorXd> public: using Composite<Eigen::VectorXd>::Composite; - VectorComposite() {}; - VectorComposite(const StateStructure& _s); - VectorComposite(const StateStructure& _s, const std::list<int>& _sizes); + VectorComposite(); /** - * \brief Construct from Eigen::VectorXd and structure + * \brief Construct from Eigen::VectorXd and keys * * Usage: * @@ -54,13 +52,13 @@ class VectorComposite : public Composite<Eigen::VectorXd> * vec_comp["a"].transpose(); // = (1,2); * vec_comp["b"].transpose(); // = (3,4,5); */ - VectorComposite(const VectorXd& _v, const StateStructure& _structure, const std::list<int>& _sizes); - VectorComposite(const StateStructure& _structure, const std::list<VectorXd>& _vectors); + VectorComposite(const StateKeys& _keys, const VectorXd& _v, const std::list<int>& _sizes); + VectorComposite(const StateKeys& _keys, const std::list<VectorXd>& _vectors); - Eigen::VectorXd vector(const StateStructure& _structure) const; + Eigen::VectorXd vector(const StateKeys& _keys) const; /** - * \brief set from Eigen::VectorXd and structure + * \brief set from Eigen::VectorXd and keys * * Usage: * @@ -74,7 +72,7 @@ class VectorComposite : public Composite<Eigen::VectorXd> * vec_comp["a"].transpose(); // = (1,2); * vec_comp["b"].transpose(); // = (3,4,5); */ - void set(const VectorXd& _v, const StateStructure& _structure, const std::list<int>& _sizes); + void set(const VectorXd& _v, const StateKeys& _keys, const std::list<int>& _sizes); void setZero(); friend std::ostream& operator <<(std::ostream &_os, const wolf::VectorComposite &_x); diff --git a/include/core/trajectory/trajectory_base.h b/include/core/trajectory/trajectory_base.h index 194c942d76f97e88890855ab406ebb3add8306ff..4c16c1e0c094e9b822a547ae6fca5484ee0f13da 100644 --- a/include/core/trajectory/trajectory_base.h +++ b/include/core/trajectory/trajectory_base.h @@ -43,7 +43,7 @@ class TrajectoryBase : public NodeBase, public std::enable_shared_from_this<Traj FramePtrMap frame_map_; protected: - StateStructure frame_structure_; // Defines the structure of the Frames in the Trajectory. + StateKeys frame_structure_; // Defines the structure of the Frames in the Trajectory. public: TrajectoryBase(); diff --git a/include/core/tree_manager/factory_tree_manager.h b/include/core/tree_manager/factory_tree_manager.h index c4f4e4d5655c6f3519fb75e91cfab8123ac40c0c..8c3df91f865ae1136d97a495452dff1f550910ba 100644 --- a/include/core/tree_manager/factory_tree_manager.h +++ b/include/core/tree_manager/factory_tree_manager.h @@ -35,7 +35,7 @@ struct ParamsTreeManagerBase; namespace wolf { -typedef Factory<TreeManagerBase, +typedef Factory<std::shared_ptr<TreeManagerBase>, const YAML::Node&> FactoryTreeManager; template<> inline std::string FactoryTreeManager::getClass() const @@ -43,7 +43,7 @@ inline std::string FactoryTreeManager::getClass() const return "FactoryTreeManager"; } -typedef Factory<TreeManagerBase, +typedef Factory<std::shared_ptr<TreeManagerBase>, const std::string&, const std::vector<std::string>&> FactoryTreeManagerYaml; template<> diff --git a/schema/problem/Problem.schema b/schema/problem/Problem.schema index 0a7943acdbb1beca7e2946d4d0f90e1a08899d99..7b24224127a6efef1b12f83ec451026f897f2210 100644 --- a/schema/problem/Problem.schema +++ b/schema/problem/Problem.schema @@ -5,11 +5,6 @@ problem: yaml_type: scalar mandatory: true doc: Tree manager parameters - frame_structure: - yaml_type: scalar - type: string - mandatory: true - doc: The keys of the default frames in problems dimension: yaml_type: scalar type: int diff --git a/src/ceres_wrapper/solver_ceres.cpp b/src/ceres_wrapper/solver_ceres.cpp index c83686ef4100dfa4cee61e576f99fa0759a1e94a..83368de1ca47b17f139872eed0cce274d7f69f8a 100644 --- a/src/ceres_wrapper/solver_ceres.cpp +++ b/src/ceres_wrapper/solver_ceres.cpp @@ -119,7 +119,7 @@ bool SolverCeres::computeCovariancesDerived(const CovarianceBlocksToBeComputed _ std::vector<StateBlockPtr> all_state_blocks, landmark_state_blocks; //frame state blocks for(auto fr_pair : wolf_problem_->getTrajectory()->getFrameMap()) - for (auto key : fr_pair.second->getStructure()) + for (auto key : fr_pair.second->getKeys()) { const auto& sb = fr_pair.second->getStateBlock(key); all_state_blocks.push_back(sb); @@ -127,7 +127,7 @@ bool SolverCeres::computeCovariancesDerived(const CovarianceBlocksToBeComputed _ // landmark state blocks for(auto l_ptr : wolf_problem_->getMap()->getLandmarkList()) - for (auto key : l_ptr->getStructure()) + for (auto key : l_ptr->getKeys()) { const auto& sb = l_ptr->getStateBlock(key); all_state_blocks.push_back(sb); @@ -152,12 +152,12 @@ bool SolverCeres::computeCovariancesDerived(const CovarianceBlocksToBeComputed _ { // first create a vector containing all state blocks for(auto fr_pair : wolf_problem_->getTrajectory()->getFrameMap()) - for (auto key1 : fr_pair.second->getStructure()) + for (auto key1 : fr_pair.second->getKeys()) { const auto& sb1 = fr_pair.second->getStateBlock(key1); assert(isStateBlockRegisteredDerived(sb1)); - for (auto key2 : fr_pair.second->getStructure()) + for (auto key2 : fr_pair.second->getKeys()) { const auto& sb2 = fr_pair.second->getStateBlock(key2); assert(isStateBlockRegisteredDerived(sb2)); @@ -171,12 +171,12 @@ bool SolverCeres::computeCovariancesDerived(const CovarianceBlocksToBeComputed _ // landmark state blocks for(auto l_ptr : wolf_problem_->getMap()->getLandmarkList()) - for (auto key1 : l_ptr->getStructure()) + for (auto key1 : l_ptr->getKeys()) { const auto& sb1 = l_ptr->getStateBlock(key1); assert(isStateBlockRegisteredDerived(sb1)); - for (auto key2 : l_ptr->getStructure()) + for (auto key2 : l_ptr->getKeys()) { const auto& sb2 = l_ptr->getStateBlock(key2); assert(isStateBlockRegisteredDerived(sb2)); @@ -211,7 +211,7 @@ bool SolverCeres::computeCovariancesDerived(const CovarianceBlocksToBeComputed _ // landmarks for(auto l_ptr : wolf_problem_->getMap()->getLandmarkList()) - for (auto key : l_ptr->getStructure()) + for (auto key : l_ptr->getKeys()) { const auto& sb = l_ptr->getStateBlock(key); assert(isStateBlockRegisteredDerived(sb)); @@ -225,7 +225,7 @@ bool SolverCeres::computeCovariancesDerived(const CovarianceBlocksToBeComputed _ getAssociatedMemBlockPtr(sb)); // landmark marginal - for (auto key2 : l_ptr->getStructure()) + for (auto key2 : l_ptr->getKeys()) { const auto& sb2 = l_ptr->getStateBlock(key2); assert(isStateBlockRegisteredDerived(sb2)); diff --git a/src/frame/frame_base.cpp b/src/frame/frame_base.cpp index a681f73a25a76e506b151fca743cf4ef181dec43..0208dd62c388c6a9856c122b5c05ebacec1445bd 100644 --- a/src/frame/frame_base.cpp +++ b/src/frame/frame_base.cpp @@ -34,7 +34,7 @@ namespace wolf { unsigned int FrameBase::frame_id_count_ = 0; // FrameBase::FrameBase(const TimeStamp& _ts, -// const StateStructure& _frame_structure, +// const StateKeys& _frame_structure, // const VectorComposite& _state) : // NodeBase("FRAME", "FrameBase"), // HasStateBlocks(_frame_structure), @@ -44,7 +44,7 @@ unsigned int FrameBase::frame_id_count_ = 0; // { // assert(_state.has(_frame_structure) && "_state does not include all keys of _frame_structure"); -// for (auto key : getStructure()) +// for (auto key : getKeys()) // { // StateBlockPtr sb = FactoryStateBlock::create(std::string(1,key), _state.at(key), false); // false = non fixed // addStateBlock(key, sb, getProblem()); @@ -53,7 +53,7 @@ unsigned int FrameBase::frame_id_count_ = 0; // FrameBase::FrameBase(const TimeStamp& _ts, -// const StateStructure& _frame_structure, +// const StateKeys& _frame_structure, // const std::list<VectorXd>& _vectors) : // NodeBase("FRAME", "FrameBase"), // HasStateBlocks(_frame_structure), @@ -440,7 +440,7 @@ void FrameBase::setProblem(ProblemPtr _problem) void FrameBase::printHeader(int _depth, bool _constr_by, bool _metric, bool _state_blocks, std::ostream& _stream, std::string _tabs) const { _stream << _tabs << "Frm" << id() - << " " << getStructure() + << " " << getKeys() << " ts = " << std::setprecision(3) << getTimeStamp() << ((_depth < 2) ? " -- " + std::to_string(getCaptureList().size()) + "C " : ""); if (_constr_by) diff --git a/src/landmark/landmark_base.cpp b/src/landmark/landmark_base.cpp index 72b1834617074da8a53a89a1097d28d8cdfeebb3..82f91377428bb34ab7b46c29bac5b1adc5890626 100644 --- a/src/landmark/landmark_base.cpp +++ b/src/landmark/landmark_base.cpp @@ -90,7 +90,7 @@ void LandmarkBase::remove(bool viral_remove_empty_parent) // std::vector<StateBlockConstPtr> used_state_block_vec(0); // // normal state blocks in {P,O,V,W} -// for (auto key : getStructure()) +// for (auto key : getKeys()) // { // const auto sbp = getStateBlock(key); // if (sbp) @@ -110,7 +110,7 @@ void LandmarkBase::remove(bool viral_remove_empty_parent) // std::vector<StateBlockPtr> used_state_block_vec(0); // // normal state blocks in {P,O,V,W} -// for (auto key : getStructure()) +// for (auto key : getKeys()) // { // auto sbp = getStateBlock(key); // if (sbp) diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp index acab5763aad76e554d256b8a31d52ea98da1c19e..8712f0da95f856e2f21853924bc58a31e62ed1bc 100644 --- a/src/problem/problem.cpp +++ b/src/problem/problem.cpp @@ -28,6 +28,7 @@ #include "core/capture/capture_void.h" #include "core/factor/factor_block_absolute.h" #include "core/factor/factor_quaternion_absolute.h" +#include "core/state_block/factory_state_block.h" #include "core/state_block/state_quaternion.h" #include "core/state_block/state_angle.h" #include "core/tree_manager/factory_tree_manager.h" @@ -41,71 +42,52 @@ namespace wolf { -Problem::Problem(const TypeComposite& _frame_structure, SizeEigen _dim, MapBasePtr _map) : +Problem::Problem(const TypeComposite& _frame_types, SizeEigen _dim) : tree_manager_(nullptr), hardware_ptr_(std::make_shared<HardwareBase>()), trajectory_ptr_(std::make_shared<TrajectoryBase>()), - map_ptr_(_map), + map_ptr_(std::make_shared<MapBase>()), motion_provider_map_(), - frame_structure_(_frame_structure), + dim_(_dim), + frame_types_(_frame_types), prior_options_(), prior_applied_(false), transformed_(false) { + checkTypeComposite(frame_types_, dim_); } -Problem::Problem(const std::string& _frame_structure, SizeEigen _dim, MapBasePtr _map) : +Problem::Problem(const StateKeys& _frame_keys, SizeEigen _dim) : tree_manager_(nullptr), hardware_ptr_(std::make_shared<HardwareBase>()), trajectory_ptr_(std::make_shared<TrajectoryBase>()), - map_ptr_(_map), + map_ptr_(std::make_shared<MapBase>()), motion_provider_map_(), - //frame_structure_(_frame_structure), - frame_structure_(), + dim_(_dim), + frame_types_(), prior_options_(), prior_applied_(false), transformed_(false) { - dim_ = _dim; - for (auto key : _frame_structure) + for (auto key : _frame_keys) { if (key == 'P') { - appendToStructure({{'P', (dim_ == 2 ? "StatePoint2d" : "StatePoint3d")}}); + appendToFrameTypes({{'P', (dim_ == 2 ? "StatePoint2d" : "StatePoint3d")}}); } else if (key == 'O') { - appendToStructure({{'P', (dim_ == 2 ? "StateAngle" : "StateQuaternion")}}); + appendToFrameTypes({{'O', (dim_ == 2 ? "StateAngle" : "StateQuaternion")}}); } else if (key == 'V') { - appendToStructure({{'P', (dim_ == 2 ? "StateVector2d" : "StateVector3d")}}); + appendToFrameTypes({{'V', (dim_ == 2 ? "StateVector2d" : "StateVector3d")}}); } else { throw std::runtime_error("Problem::Problem: Unknown type for key " + std::string(1,key) + " use the constructor via TypeComposite"); } } - - // if (_frame_structure == "PO" and _dim == 2) - // { - // state_size_ = 3; - // state_cov_size_ = 3; - // }else if (_frame_structure == "PO" and _dim == 3) - // { - // state_size_ = 7; - // state_cov_size_ = 6; - // }else if (_frame_structure == "POV" and _dim == 3) - // { - // state_size_ = 10; - // state_cov_size_ = 9; - // }else if (_frame_structure == "POVCDL" and _dim == 3) - // { - // state_size_ = 19; - // state_cov_size_ = 18; - // } - // else std::runtime_error( - // "Problem::Problem(): Unknown frame structure. Add appropriate frame structure to the switch statement."); } void Problem::setup() @@ -116,16 +98,16 @@ void Problem::setup() map_ptr_ -> setProblem(shared_from_this()); } -ProblemPtr Problem::create(const TypeComposite& _frame_structure, SizeEigen _dim, MapBasePtr _map) +ProblemPtr Problem::create(const TypeComposite& _frame_types, SizeEigen _dim) { - ProblemPtr p(new Problem(_frame_structure, _dim, _map)); // We use `new` and not `make_shared` since the Problem constructor is private and cannot be passed to `make_shared`. + ProblemPtr p(new Problem(_frame_types, _dim)); // We use `new` and not `make_shared` since the Problem constructor is private and cannot be passed to `make_shared`. p->setup(); return p->shared_from_this(); } -ProblemPtr Problem::create(const std::string& _frame_structure, SizeEigen _dim, MapBasePtr _map) +ProblemPtr Problem::create(const std::string& _frame_structure, SizeEigen _dim) { - ProblemPtr p(new Problem(_frame_structure, _dim, _map)); // We use `new` and not `make_shared` since the Problem constructor is private and cannot be passed to `make_shared`. + ProblemPtr p(new Problem(_frame_structure, _dim)); // We use `new` and not `make_shared` since the Problem constructor is private and cannot be passed to `make_shared`. p->setup(); return p->shared_from_this(); } @@ -188,14 +170,11 @@ ProblemPtr Problem::autoSetup(const std::string& _input_yaml_file, const std::ve // structure and dimension WOLF_INFO("Loading problem parameters"); YAML::Node problem_node = param_node["problem"]; - std::string frame_structure; - if (problem_node["frame_structure"]) - { - // TODO: accept types! - frame_structure = problem_node["frame_structure"].as<std::string>(); - } int dim = problem_node["dimension"].as<int>(); - auto problem = Problem::create(frame_structure, dim, nullptr); + TypeComposite frame_types; + if (problem_node["frame_types"]) + frame_types = TypeComposite(problem_node["frame_types"]); + auto problem = Problem::create(frame_types, dim); // Tree manager WOLF_INFO("Loading tree manager parameters"); @@ -393,140 +372,119 @@ ProcessorBasePtr Problem::findProcessor(const std::string& _processor_name) return nullptr; } -// FrameBasePtr Problem::emplaceFrame(const TimeStamp& _time_stamp, // -// const StateStructure& _frame_structure, // -// const SizeEigen _dim, // -// const Eigen::VectorXd& _frame_state) -// { -// VectorComposite state; -// SizeEigen index = 0; -// SizeEigen size = 0; -// for (auto key : _frame_structure) -// { -// if (key == 'O') -// { -// if (_dim == 2) size = 1; -// else size = 4; -// } -// else -// size = _dim; - -// assert (_frame_state.size() >= index+size && "State vector incompatible with given structure and dimension! Vector too small."); - -// state.emplace(key, _frame_state.segment(index, size)); - -// // append new key to frame structure -// if (frame_structure_.find(key) == std::string::npos) // not found -// frame_structure_ += std::string(1,key); - -// index += size; -// } - -// assert (_frame_state.size() == index && "State vector incompatible with given structure and dimension! Vector too large."); - -// auto frm = FrameBase::emplace<FrameBase>(trajectory_ptr_, -// _time_stamp, -// _frame_structure, -// state); - -// return frm; -// } - -// FrameBasePtr Problem::emplaceFrame(const TimeStamp& _time_stamp, // -// const StateStructure& _frame_structure, // -// const SizeEigen _dim) -// { -// return emplaceFrame(_time_stamp, -// _frame_structure, -// getState(_time_stamp)); -// } - -FrameBasePtr Problem::emplaceFrame (const TimeStamp& _time_stamp, // - //const StateStructure& _frame_structure, // - const TypeComposite& _frame_structure, // +FrameBasePtr Problem::emplaceFrame (const TimeStamp& _time_stamp, + const TypeComposite& _frame_types, const VectorComposite& _frame_state) { + // check input _frame_specs + checkTypeComposite(_frame_types); + + // check same keys in state and types + if (_frame_types.getKeys() != _frame_state.getKeys()) + { + throw std::runtime_error("Problem::emplaceFrame: Different keys in given TypeComposite and VectorComposite. TypeComposite: " + + _frame_types.getKeys() + + "VectorComposite: " + + _frame_state.getKeys()); + } + return FrameBase::emplace<FrameBase>(getTrajectory(), _time_stamp, - _frame_structure, + _frame_types, _frame_state); } -FrameBasePtr Problem::emplaceFrame (const TimeStamp& _time_stamp, // - const VectorComposite& _frame_state) +FrameBasePtr Problem::emplaceFrame (const TimeStamp& _time_stamp, + const StateKeys& _frame_keys) { - // // append new keys to frame structure - // for (auto pair_key_vec : _frame_state) - // { - // if (not frame_structure_.has(pair_key_vec.first)) - // auto key = pair_key_vec.first; - // if (frame_structure_.find(key) == std::string::npos) // not found - // frame_structure_ += std::string(1,key); - // } - - if (not frame_structure_.has(_frame_state.getStructure())) + if (not frame_types_.has(_frame_keys)) { - throw std::runtime_error("Problem::emplaceFrame: the given VectorComposite has a state with an unknown key"); + throw std::runtime_error("Problem::emplaceFrame: any unknown key... asked for " + _frame_keys + " but only have " + frame_types_.getKeys()); } - return FrameBase::emplace<FrameBase>(getTrajectory(), - _time_stamp, - getFrameStructure(), - _frame_state); + auto state = getState(_time_stamp, _frame_keys); + + return emplaceFrame (_time_stamp, + getFrameTypes(state.getKeys()), + state); } -// FrameBasePtr Problem::emplaceFrame(const TimeStamp& _time_stamp, // -// const Eigen::VectorXd& _frame_state) -// { -// // return emplaceFrame(_time_stamp, -// // this->getFrameStructure(), -// // this->getDim(), -// // _frame_state); -// return emplaceFrame(_time_stamp, -// this->getFrameStructure(), -// this->getDim(), -// _frame_state); -// } - -// FrameBasePtr Problem::emplaceFrame(const TimeStamp& _time_stamp) -// { -// return emplaceFrame(_time_stamp, -// this->getFrameStructure(), -// this->getDim()); -// } +FrameBasePtr Problem::emplaceFrame (const TimeStamp& _time_stamp, + const VectorComposite& _frame_state) +{ + if (not frame_types_.has(_frame_state.getKeys())) + { + throw std::runtime_error("Problem::emplaceFrame: any unknown key... asked for " + _frame_state.getKeys() + " but only have " + frame_types_.getKeys()); + } + + return Problem::emplaceFrame(_time_stamp, + getFrameTypes(_frame_state.getKeys()), + _frame_state); +} FrameBasePtr Problem::emplaceFrame(const TimeStamp& _time_stamp, - const SpecComposite& _frame_spec_composite) + const StateKeys& _frame_keys, + const Eigen::VectorXd& _frame_state) { - // Checks - if (_frame_spec_composite.count('P') == 0 or _frame_spec_composite.count('O') == 0) + if (not frame_types_.has(_frame_keys)) { - WOLF_INFO(_frame_spec_composite); - throw std::runtime_error("Problem::emplaceFrame: emplacing a frame without P or O"); + throw std::runtime_error("Problem::emplaceFrame: any unknown key... asked for " + _frame_keys + " but only have " + frame_types_.getKeys()); } - if (_frame_spec_composite.at('P').getType() != "P" and - _frame_spec_composite.at('P').getType() != (dim_ == 2 ? "StatePoint2d" : "StatePoint3d")) + + VectorComposite vec_composite; + + int next_state_index = 0; + for (auto key : _frame_keys) { - throw std::runtime_error("Problem::emplaceFrame: State type of key P should be 'StatePoint2d' or 'StatePoint3d' for 2D and 3D problem, respectively"); + auto state_size = FactoryStateBlockIdentityVector::create(frame_types_.at(key)).size(); + + if (_frame_state.size() < next_state_index + state_size) + { + throw std::runtime_error("Problem::emplaceFrame: the provided vector _frame_state is too short"); + } + + vec_composite[key] = _frame_state.segment(next_state_index, state_size); + next_state_index += state_size; } - if (_frame_spec_composite.at('O').getType() != "O" and - _frame_spec_composite.at('O').getType() != (dim_ == 2 ? "StateAngle" : "StateQuaternion")) + if (_frame_state.size() != next_state_index) { - throw std::runtime_error("Problem::emplaceFrame: State type of key O should be 'StateAngle' or 'StateQuaternion' for 2D and 3D problem, respectively"); + throw std::runtime_error("Problem::emplaceFrame: the provided vector _frame_state is too long"); } - if (_frame_spec_composite.count('V') != 0 and - _frame_spec_composite.at('V').getType() != "V" and - _frame_spec_composite.at('V').getType() != (dim_ == 2 ? "StateVector2d" : "StateVector3d")) + + return Problem::emplaceFrame(_time_stamp, + getFrameTypes(vec_composite.getKeys()), + vec_composite); +} + +FrameBasePtr Problem::emplaceFrame(const TimeStamp& _time_stamp, + const SpecComposite& _frame_specs) +{ + // check input _frame_specs + checkTypeComposite(_frame_specs.getTypeComposite()); + + // Check _frame_specs are consistent with frame_types_ + auto input_types = _frame_specs.getTypeComposite(); + for (auto type_pair : frame_types_) { - throw std::runtime_error("Problem::emplaceFrame: State type of key O should be 'StateAngle' or 'StateQuaternion' for 2D and 3D problem, respectively"); + if (input_types.count(type_pair.first) and input_types.at(type_pair.first) != input_types.at(type_pair.first)) + { + throw std::runtime_error("Problem::emplaceFrame: provided _frame_specs specify the type " + + input_types.at(type_pair.first) + + " for key " + + std::string(1, type_pair.first) + + " but in problem, this key is specified as: " + + input_types.at(type_pair.first)); + } } + // add the types of the keys that are not in frame_types_ + appendToFrameTypes(_frame_specs.getTypeComposite()); + return FrameBase::emplace<FrameBase>(getTrajectory(), _time_stamp, - _frame_spec_composite); + _frame_specs); } - TimeStamp Problem::getTimeStamp ( ) const { TimeStamp ts = TimeStamp::Invalid(); @@ -552,48 +510,47 @@ TimeStamp Problem::getTimeStamp ( ) const } -VectorComposite Problem::getState(const StateStructure& _structure) const +VectorComposite Problem::getState(const StateKeys& _keys) const { - StateStructure structure = (_structure == "" ? getFrameStructure() : _structure); + StateKeys keys = (_keys == "" ? getFrameKeys() : _keys); VectorComposite state; // compose the states of all MotionProvider processors (ordered by user-defined priority) into one only state for (auto prc_pair : motion_provider_map_) { - auto prc_state = prc_pair.second->getState(structure); + auto prc_state = prc_pair.second->getState(keys); // transfer processor vector blocks to problem state for (auto pair_key_vec : prc_state) { - if (state.count(pair_key_vec.first) == 0) // Only write once. This gives priority to processors with more priority + if (not state.has(pair_key_vec.first)) // Only write once. This gives priority to processors with more priority { - state.insert(pair_key_vec); + state.insert(pair_key_vec); } } //If all keys are filled return - if (state.size() == structure.size()) + if (state.has(keys)) { return state; } } // check for empty blocks and fill them with the last KF, with the prior, or with zeros in the worst case - VectorComposite state_last; auto last_kf = trajectory_ptr_->getLastFrame(); if (last_kf) - state_last = last_kf->getState(structure); + state_last = last_kf->getState(keys); else if (not prior_options_.empty()) state_last = prior_options_.getVectorComposite(); - for (auto key : structure) + for (auto key : keys) { - if (state.count(key) == 0) + if (state.has(key) == 0) { auto state_last_it = state_last.find(key); @@ -608,9 +565,9 @@ VectorComposite Problem::getState(const StateStructure& _structure) const return state; } -VectorComposite Problem::getState (const TimeStamp& _ts, const StateStructure& _structure) const +VectorComposite Problem::getState (const TimeStamp& _ts, const StateKeys& _structure) const { - StateStructure structure = (_structure == "" ? getFrameStructure() : _structure); + StateKeys structure = (_structure == "" ? getFrameKeys() : _structure); VectorComposite state; @@ -662,9 +619,9 @@ VectorComposite Problem::getState (const TimeStamp& _ts, const StateStructure& _ return state; } -VectorComposite Problem::getOdometry(const StateStructure& _structure) const +VectorComposite Problem::getOdometry(const StateKeys& _structure) const { - StateStructure structure = (_structure == "" ? getFrameStructure() : _structure); + StateKeys structure = (_structure == "" ? getFrameKeys() : _structure); VectorComposite odom_state; @@ -710,24 +667,28 @@ SizeEigen Problem::getDim() const return dim_; } -const StateStructure& Problem::getFrameStructure() const +StateKeys Problem::getFrameKeys() const { - // return frame_structure_; - return frame_structure_.getStructure(); + return frame_types_.getKeys(); } -// void Problem::appendToStructure(const StateStructure& _structure) -// { -// for (auto key : _structure) -// if (frame_structure_.find(key) == std::string::npos) // now key not found in problem structure -> append! -// frame_structure_ += key; -// } +TypeComposite Problem::getFrameTypes(StateKeys _keys) const +{ + if (_keys == "") + return frame_types_; + + TypeComposite types; + for (auto key : _keys) + types.emplace(key, frame_types_.at(key)); + + return types; +} -void Problem::appendToStructure(const TypeComposite& _types) +void Problem::appendToFrameTypes(const TypeComposite& _types) { for (auto pair : _types) - if (not frame_structure_.has(pair.first)) // now key not found in problem structure -> append! - frame_structure_.emplace(pair.first, pair.second); + if (not frame_types_.has(pair.first)) // now key not found in problem structure -> append! + frame_types_.emplace(pair.first, pair.second); } void Problem::setTreeManager(TreeManagerBasePtr _gm) @@ -741,9 +702,6 @@ void Problem::setTreeManager(TreeManagerBasePtr _gm) void Problem::addMotionProvider(MotionProviderPtr _motion_provider_ptr) { - // Grow TypeComposite storing information about the types of the StateBlocks corresponding to each key - // TODO - // Check if is state getter if (not _motion_provider_ptr->isStateGetter()) { @@ -763,7 +721,21 @@ void Problem::addMotionProvider(MotionProviderPtr _motion_provider_ptr) // add to map ordered by priority motion_provider_map_.emplace(_motion_provider_ptr->getOrder(), _motion_provider_ptr); - appendToStructure(_motion_provider_ptr->getStateStructure()); + + // check consistent types + for (auto new_key : _motion_provider_ptr->getStateTypes().getKeys()) + if (frame_types_.has(new_key) and frame_types_[new_key] != _motion_provider_ptr->getStateTypes().at(new_key)) + { + throw std::runtime_error("Problem::addMotionProvider: inconsistent type for key " + + std::string(1,new_key) + + " previously registered as " + + frame_types_[new_key] + + " and now as " + + _motion_provider_ptr->getStateTypes().at(new_key)); + } + + // Grow TypeComposite storing information about the types of the StateBlocks corresponding to each key + appendToFrameTypes(_motion_provider_ptr->getStateTypes()); } void Problem::removeMotionProvider(MotionProviderPtr proc) @@ -773,33 +745,34 @@ void Problem::removeMotionProvider(MotionProviderPtr proc) motion_provider_map_.erase(proc->getOrder()); // rebuild frame structure with remaining motion processors - frame_structure_.clear(); + frame_types_.clear(); for (auto pm : motion_provider_map_) - appendToStructure(pm.second->getStateStructure()); + appendToFrameTypes(pm.second->getStateTypes()); } -VectorComposite Problem::stateZero ( const StateStructure& _structure ) const +VectorComposite Problem::stateZero ( TypeComposite _types ) const { - StateStructure structure = (_structure == "" ? getFrameStructure() : _structure); + if (_types.empty()) + _types = frame_types_; VectorComposite state; - for (auto key : structure) + for (auto&& type_pair : _types) { - VectorXd vec; - if (key == 'O') - { - if (dim_ == 2) vec = VectorXd::Zero(1); - else if (dim_ == 3) vec = Quaterniond::Identity().coeffs(); - } - else - { - vec = VectorXd::Zero(dim_); - } - state.emplace(key, vec); + state.emplace(type_pair.first, FactoryStateBlockIdentityVector::create(type_pair.second)); } return state; } +VectorComposite Problem::stateZero ( const StateKeys& _keys ) const +{ + if (not frame_types_.has(_keys)) + throw std::runtime_error("Problem::stateZero any unknown key... asked for " + _keys + " but only have " + frame_types_.getKeys()); + + StateKeys keys = (_keys == "" ? getFrameKeys() : _keys); + + return stateZero(getFrameTypes(_keys)); +} + bool Problem::permitKeyFrame(ProcessorBasePtr _processor_ptr) const { // This should implement a black list of processors that have forbidden keyframe creation @@ -1097,8 +1070,6 @@ MapBasePtr Problem::getMap() void Problem::setMap(MapBasePtr _map_ptr) { - assert(map_ptr_ == nullptr && "map has already been set"); - map_ptr_ = _map_ptr; } diff --git a/src/processor/motion_provider.cpp b/src/processor/motion_provider.cpp index 4cd15583000eb43bc5802aad9bd59e8e6b803fde..bd232f8d8b23af080711e953998e9042e54d7f20 100644 --- a/src/processor/motion_provider.cpp +++ b/src/processor/motion_provider.cpp @@ -26,19 +26,18 @@ using namespace wolf; void MotionProvider::addToProblem(ProblemPtr _prb_ptr, MotionProviderPtr _motion_ptr) { - setOdometry(_prb_ptr->stateZero(state_structure_)); - if (not isStateGetter()) + WOLF_DEBUG_COND(not isStateGetter(), "MotionProvider::addToProblem: MotionProvider processor with state_provider=false. Not adding this processor"); + if (isStateGetter()) { - WOLF_WARN("MotionProvider::addToProblem: MotionProvider processor with state_provider=false. Not adding this processor"); - return; + _prb_ptr->addMotionProvider(_motion_ptr); } - _prb_ptr->addMotionProvider(_motion_ptr); + setOdometry(_prb_ptr->stateZero(state_types_)); } void MotionProvider::setOdometry(const VectorComposite& _odom) { - assert(_odom.has(state_structure_) && "MotionProvider::setOdometry(): any key missing in _odom."); + assert(_odom.has(getStateKeys()) && "MotionProvider::setOdometry(): any key missing in _odom."); - for (auto key : state_structure_) - odometry_[key] = _odom.at(key); //overwrite/insert only keys of the state_structure_ + for (auto key : getStateKeys()) + odometry_[key] = _odom.at(key); //overwrite/insert only keys of the state_types_ } diff --git a/src/processor/processor_base.cpp b/src/processor/processor_base.cpp index fb2f26fea436e06e2c68f87f958d8f9209b8f586..e3dfc26a3c9856d76a96ea26efc7a04e996e7493 100644 --- a/src/processor/processor_base.cpp +++ b/src/processor/processor_base.cpp @@ -141,12 +141,13 @@ void ProcessorBase::link(SensorBasePtr _sen_ptr) void ProcessorBase::setProblem(ProblemPtr _problem) { - std::string str = "Processor works with " + std::to_string(dim_) + "D but problem is " + std::to_string(_problem->getDim()) + "D"; - assert(((dim_ == 0) or (dim_ == _problem->getDim())) && str.c_str()); - if (_problem == nullptr or _problem == this->getProblem()) return; + if (dim_ != 0 and dim_ != _problem->getDim()) + throw std::runtime_error("Processor works with " + std::to_string(dim_) + + "D but problem is " + std::to_string(_problem->getDim()) + "D"); + NodeBase::setProblem(_problem); // adding motion provider to the motion providers vector diff --git a/src/processor/processor_fixed_wing_model.cpp b/src/processor/processor_fixed_wing_model.cpp index 25ec91bc928b5c057db3bbf8b81eebb840149e1d..527f8b2c2e556ef065f4347ff1fcf412177bc59a 100644 --- a/src/processor/processor_fixed_wing_model.cpp +++ b/src/processor/processor_fixed_wing_model.cpp @@ -41,7 +41,7 @@ ProcessorFixedWingModel::~ProcessorFixedWingModel() void ProcessorFixedWingModel::configure(SensorBasePtr _sensor) { - assert(_sensor->getProblem()->getFrameStructure().find('V') != std::string::npos && "Processor only works with problems with V"); + assert(_sensor->getProblem()->getFrameKeys().find('V') != std::string::npos && "Processor only works with problems with V"); } void ProcessorFixedWingModel::processKeyFrame(FrameBasePtr _keyframe_ptr) diff --git a/src/processor/processor_motion.cpp b/src/processor/processor_motion.cpp index 2936dfc16e9be074686db29f97e060302affc1ca..d69eb4b4ead92ee67c549570e11d541c8bc09cab 100644 --- a/src/processor/processor_motion.cpp +++ b/src/processor/processor_motion.cpp @@ -19,14 +19,6 @@ // along with this program. If not, see <http://www.gnu.org/licenses/>. // //--------LICENSE_END-------- -/** - * \file processor_motion.cpp - * - * Created on: 15/03/2016 - * \author: jsola - */ - - #include "core/processor/processor_motion.h" #include "core/state_block/factory_state_block.h" @@ -35,7 +27,7 @@ namespace wolf { ProcessorMotion::ProcessorMotion(const std::string& _type, - std::string _state_structure, + TypeComposite _state_types, int _dim, SizeEigen _state_size, SizeEigen _delta_size, @@ -44,7 +36,7 @@ ProcessorMotion::ProcessorMotion(const std::string& _type, SizeEigen _calib_size, ParamsProcessorMotionPtr _params_motion) : ProcessorBase(_type, _dim, _params_motion), - MotionProvider(_state_structure, _params_motion), + MotionProvider(_state_types, _params_motion), params_motion_(_params_motion), processing_step_(RUNNING_WITHOUT_KF), bootstrapping_(false), @@ -198,13 +190,13 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr) case FIRST_TIME_WITHOUT_KF : { // There is no KF: create own origin - setOrigin(getProblem()->stateZero(getStateStructure()), _incoming_ptr->getTimeStamp()); + setOrigin(getProblem()->stateZero(getStateKeys()), _incoming_ptr->getTimeStamp()); break; } case FIRST_TIME_WITH_KF_BEFORE_INCOMING : { // cannot join to the KF: create own origin - setOrigin(getProblem()->getState(getStateStructure()), _incoming_ptr->getTimeStamp()); + setOrigin(getProblem()->getState(getStateKeys()), _incoming_ptr->getTimeStamp()); break; } case FIRST_TIME_WITH_KF_ON_INCOMING : @@ -216,7 +208,7 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr) case FIRST_TIME_WITH_KF_AFTER_INCOMING : { // cannot join to the KF: create own origin - setOrigin(getProblem()->getState(getStateStructure()), _incoming_ptr->getTimeStamp()); + setOrigin(getProblem()->getState(getStateKeys()), _incoming_ptr->getTimeStamp()); break; } case RUNNING_WITHOUT_KF : @@ -482,7 +474,7 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr) // create a new frame auto frame_new = std::make_shared<FrameBase>(getTimeStamp(), - getStateStructure(), + getStateTypes(), getProblem()->getState()); // create a new capture auto capture_new = emplaceCapture(frame_new, @@ -516,9 +508,9 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr) } -VectorComposite ProcessorMotion::getState(const StateStructure& _structure) const +VectorComposite ProcessorMotion::getState(const StateKeys& _keys) const { - const StateStructure& structure = (_structure == "" ? state_structure_ : _structure); + const StateKeys& keys = (_keys == "" ? getStateKeys() : _keys); if (origin_ptr_ == nullptr or @@ -538,7 +530,7 @@ VectorComposite ProcessorMotion::getState(const StateStructure& _structure) cons // this may happen when in the very first frame where the capture has no motion info --> empty buffer if (last_ptr_->getBuffer().empty()) { - return last_ptr_->getFrame()->getState(structure); + return last_ptr_->getFrame()->getState(keys); } /* Doing this: @@ -555,7 +547,7 @@ VectorComposite ProcessorMotion::getState(const StateStructure& _structure) cons */ // Get state of origin - auto x_origin = getOrigin()->getFrame()->getState(state_structure_); + auto x_origin = getOrigin()->getFrame()->getState(getStateKeys()); // Get most recent motion const auto& motion = last_ptr_->getBuffer().back(); @@ -591,7 +583,7 @@ VectorComposite ProcessorMotion::getState(const StateStructure& _structure) cons statePlusDelta(x_origin, delta_preint, last_ptr_->getTimeStamp() - origin_ptr_->getTimeStamp(), state); } - if (_structure == "") + if (_keys == "") return state; else @@ -600,7 +592,7 @@ VectorComposite ProcessorMotion::getState(const StateStructure& _structure) cons auto pair_key_vec_it = state.begin(); while (pair_key_vec_it != state.end()) { - if (_structure.find(pair_key_vec_it->first) == std::string::npos) + if (_keys.find(pair_key_vec_it->first) == std::string::npos) pair_key_vec_it = state.erase(pair_key_vec_it); else @@ -614,11 +606,11 @@ VectorComposite ProcessorMotion::getState(const StateStructure& _structure) cons // _x needs to have the size of the processor state -VectorComposite ProcessorMotion::getState(const TimeStamp& _ts, const StateStructure& _structure) const +VectorComposite ProcessorMotion::getState(const TimeStamp& _ts, const StateKeys& _keys) const { assert(_ts.ok()); - const StateStructure& structure = (_structure == "" ? state_structure_ : _structure); + const StateKeys& keys = (_keys == "" ? getStateKeys() : _keys); // We need to search for the capture containing a motion buffer with the queried time stamp auto capture_motion = findCaptureContainingTimeStamp(_ts); @@ -636,7 +628,7 @@ VectorComposite ProcessorMotion::getState(const TimeStamp& _ts, const StateStruc // this may happen when in the very first frame where the capture has no motion info --> empty buffer if (capture_motion->getBuffer().empty()) { - return capture_motion->getFrame()->getState(structure); + return capture_motion->getFrame()->getState(keys); } /* Doing this: @@ -654,7 +646,7 @@ VectorComposite ProcessorMotion::getState(const TimeStamp& _ts, const StateStruc // Get state of origin auto cap_orig = capture_motion->getOriginCapture(); - auto x_origin = cap_orig->getFrame()->getState(state_structure_); + auto x_origin = cap_orig->getFrame()->getState(getStateKeys()); // Get motion at time stamp const auto& motion = capture_motion->getBuffer().getMotion(_ts); @@ -689,7 +681,7 @@ VectorComposite ProcessorMotion::getState(const TimeStamp& _ts, const StateStruc statePlusDelta(x_origin, delta_preint, _ts - cap_orig->getTimeStamp(), state); } - if (_structure == "") + if (_keys == "") return state; else @@ -698,7 +690,7 @@ VectorComposite ProcessorMotion::getState(const TimeStamp& _ts, const StateStruc auto pair_key_vec_it = state.begin(); while (pair_key_vec_it != state.end()) { - if (_structure.find(pair_key_vec_it->first) == std::string::npos) + if (_keys.find(pair_key_vec_it->first) == std::string::npos) pair_key_vec_it = state.erase(pair_key_vec_it); else @@ -715,7 +707,7 @@ FrameBasePtr ProcessorMotion::setOrigin(const VectorComposite& _x_origin, const { FrameBasePtr key_frame_ptr = FrameBase::emplace<FrameBase>(getProblem()->getTrajectory(), _ts_origin, - getStateStructure(), + getStateTypes(), _x_origin); setOrigin(key_frame_ptr); @@ -744,7 +736,7 @@ void ProcessorMotion::setOrigin(FrameBasePtr _origin_frame) // ---------- LAST ---------- // Make non-key-frame for last Capture last_frame_ptr_ = std::make_shared<FrameBase>(origin_ts, - getStateStructure(), + getStateTypes(), _origin_frame->getState()); // emplace (emtpy) last Capture diff --git a/src/processor/processor_odom_2d.cpp b/src/processor/processor_odom_2d.cpp index be37af7ec5938fecdeb54c24b71c368305ed0461..c6c0e34c4ae47a507a235d990810f52745a3db04 100644 --- a/src/processor/processor_odom_2d.cpp +++ b/src/processor/processor_odom_2d.cpp @@ -29,7 +29,10 @@ namespace wolf { ProcessorOdom2d::ProcessorOdom2d(ParamsProcessorOdom2dPtr _params) : - ProcessorMotion("ProcessorOdom2d", "PO", 2, 3, 3, 3, 2, 0, _params), + ProcessorMotion("ProcessorOdom2d", + {{'P',"StatePoint2d"},{'O',"StateAngle"}}, + 2, 3, 3, 3, 2, 0, + _params), params_odom_2d_(_params) { // diff --git a/src/processor/processor_odom_3d.cpp b/src/processor/processor_odom_3d.cpp index f6eb7c2d30a340d5235938ec5c1fbe000f96344a..26a2a8d19b8c1156443638263bc92c68c9220ff4 100644 --- a/src/processor/processor_odom_3d.cpp +++ b/src/processor/processor_odom_3d.cpp @@ -27,7 +27,10 @@ namespace wolf { ProcessorOdom3d::ProcessorOdom3d(ParamsProcessorOdom3dPtr _params) : - ProcessorMotion("ProcessorOdom3d", "PO", 3, 7, 7, 6, 6, 0, _params), + ProcessorMotion("ProcessorOdom3d", + {{'P',"StatePoint3d"},{'O',"StateQuaternion"}}, + 3, 7, 7, 6, 6, 0, + _params), params_odom_3d_ (_params) { // diff --git a/src/processor/processor_tracker.cpp b/src/processor/processor_tracker.cpp index 623066614b764486e4cf568e892d8fed95c90cbf..0892276de37c33ad0390f2a01b38e14400375f29 100644 --- a/src/processor/processor_tracker.cpp +++ b/src/processor/processor_tracker.cpp @@ -36,7 +36,7 @@ namespace wolf { ProcessorTracker::ProcessorTracker(const std::string& _type, - const StateStructure& _structure, + const StateKeys& _state_keys, int _dim, ParamsProcessorTrackerPtr _params_tracker) : ProcessorBase(_type, _dim, _params_tracker), @@ -46,7 +46,7 @@ ProcessorTracker::ProcessorTracker(const std::string& _type, last_ptr_(nullptr), incoming_ptr_(nullptr), last_frame_ptr_(nullptr), - state_structure_(_structure) + state_keys_(_state_keys) { // } @@ -104,10 +104,7 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr) WOLF_DEBUG( "PT ", getName(), " FIRST_TIME_WITHOUT_KEYFRAME" ); // make a new KF at incoming - FrameBasePtr keyframe = FrameBase::emplace<FrameBase>(getProblem()->getTrajectory(), - incoming_ptr_->getTimeStamp(), - getProblem()->getFrameStructure(), - getProblem()->getState()); + FrameBasePtr keyframe = getProblem()->emplaceFrame(incoming_ptr_->getTimeStamp()); // link incoming to the new KF incoming_ptr_->link(keyframe); @@ -151,10 +148,10 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr) WOLF_DEBUG( "PT ", getName(), " SECOND_TIME_WITHOUT_KEYFRAME" ); // Make a NON KEY Frame to hold incoming capture - FrameBasePtr keyframe = std::make_shared<FrameBase>(incoming_ptr_->getTimeStamp(), - getProblem()->getFrameStructure(), - getProblem()->getState()); - incoming_ptr_->link(keyframe); + FrameBasePtr frame = std::make_shared<FrameBase>(incoming_ptr_->getTimeStamp(), + getProblem()->getFrameTypes(), + getProblem()->getState()); + incoming_ptr_->link(frame); // Process info // TrackerLandmark: If we have been given a map, all landmarks in the map are known. Process them. @@ -170,7 +167,7 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr) // Update pointers origin_ptr_ = last_ptr_; last_ptr_ = incoming_ptr_; - last_frame_ptr_ = keyframe; + last_frame_ptr_ = frame; incoming_ptr_ = nullptr; break; @@ -192,8 +189,8 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr) // Create new NON KEY frame for incoming FrameBasePtr frame = std::make_shared<FrameBase>(incoming_ptr_->getTimeStamp(), - getProblem()->getFrameStructure(), - getProblem()->getState()); + getProblem()->getFrameTypes(), + getProblem()->getState()); incoming_ptr_->link(frame); // Detect new Features, initialize Landmarks, ... @@ -237,7 +234,7 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr) // make NON KEY frame; append incoming to new frame FrameBasePtr frame = std::make_shared<FrameBase>(incoming_ptr_->getTimeStamp(), - getProblem()->getFrameStructure(), + getProblem()->getFrameTypes(), getProblem()->getState(incoming_ptr_->getTimeStamp())); incoming_ptr_ ->link(frame); @@ -256,7 +253,7 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr) // Replace last frame for a new NON KEY frame in incoming FrameBasePtr frame = std::make_shared<FrameBase>(incoming_ptr_->getTimeStamp(), - getProblem()->getFrameStructure(), + getProblem()->getFrameTypes(), getProblem()->getState(incoming_ptr_->getTimeStamp())); incoming_ptr_->link(frame); last_ptr_->unlink(); // unlink last (destroying the frame) instead of frame destruction that would implicitly destroy last diff --git a/src/processor/processor_tracker_feature.cpp b/src/processor/processor_tracker_feature.cpp index d55a2666cc00b3c8cdc53854ab43f7903f0b9d6e..3638807c5cfc769988aa54eac16e7f7048c4d668 100644 --- a/src/processor/processor_tracker_feature.cpp +++ b/src/processor/processor_tracker_feature.cpp @@ -32,7 +32,7 @@ namespace wolf { ProcessorTrackerFeature::ProcessorTrackerFeature(const std::string& _type, - const StateStructure& _structure, + const StateKeys& _structure, int _dim, ParamsProcessorTrackerFeaturePtr _params_tracker_feature) : ProcessorTracker(_type, _structure, _dim, _params_tracker_feature), diff --git a/src/processor/processor_tracker_landmark.cpp b/src/processor/processor_tracker_landmark.cpp index bab09ac8279b7c9ec770e985b953b29ed9cb82af..4bda28a91680b0f8b171c49876f5882da608b376 100644 --- a/src/processor/processor_tracker_landmark.cpp +++ b/src/processor/processor_tracker_landmark.cpp @@ -35,7 +35,7 @@ namespace wolf { ProcessorTrackerLandmark::ProcessorTrackerLandmark(const std::string& _type, - const StateStructure& _structure, + const StateKeys& _structure, ParamsProcessorTrackerLandmarkPtr _params_tracker_landmark) : ProcessorTracker(_type, _structure, diff --git a/src/sensor/sensor_base.cpp b/src/sensor/sensor_base.cpp index 725b9f9036856ac463eafd59bb926b36ef98a23b..10503f2632b25e93a4cb2137417c32d79807f8a5 100644 --- a/src/sensor/sensor_base.cpp +++ b/src/sensor/sensor_base.cpp @@ -52,12 +52,12 @@ SensorBase::SensorBase(const std::string& _type, auto prior = state_pair.second; // type - if (key == 'P' and prior.getType() != "P" and prior.getType() != "StatePoint2d" and prior.getType() != "StatePoint3d") - throw std::runtime_error("Prior type for key P has to be 'P', 'StatePoint2d' or 'StatePoint3d'"); - if (key == 'V' and prior.getType() != "V" and prior.getType() != "StateVector2d" and prior.getType() != "StateVector3d") - throw std::runtime_error("Prior type for key V has to be 'V' 'StateVector2d' or 'StateVector3d'"); - if (key == 'O' and prior.getType() != "O" and prior.getType() != "StateAngle" and prior.getType() != "StateQuaternion") - throw std::runtime_error("Prior type for key O in 3D has to be 'O', 'StateAngle' or 'StateQuaternion'"); + if (key == 'P' and prior.getType() != "StatePoint2d" and prior.getType() != "StatePoint3d") + throw std::runtime_error("Prior type for key P has to be 'StatePoint2d' or 'StatePoint3d'"); + if (key == 'V' and prior.getType() != "StateVector2d" and prior.getType() != "StateVector3d") + throw std::runtime_error("Prior type for key V has to be 'StateVector2d' or 'StateVector3d'"); + if (key == 'O' and prior.getType() != "StateAngle" and prior.getType() != "StateQuaternion") + throw std::runtime_error("Prior type for key O in 3D has to be 'StateAngle' or 'StateQuaternion'"); // Create state block auto sb = FactoryStateBlock::create(prior.getType(), prior.getState(), prior.isFixed()); @@ -73,10 +73,6 @@ SensorBase::SensorBase(const std::string& _type, if (prior.isDynamic()) setDriftStd(key, prior.getDriftStd()); } - - WOLF_INFO(_priors); - auto base_prior = _priors.cast<SpecComposite>(); - WOLF_INFO(base_prior); } SensorBase::~SensorBase() @@ -107,7 +103,7 @@ void SensorBase::unfixExtrinsics() void SensorBase::fixIntrinsics() { - for (auto key : getStructure()) + for (auto key : getKeys()) { if (key != 'P' and key != 'O') // exclude extrinsics { @@ -120,7 +116,7 @@ void SensorBase::fixIntrinsics() void SensorBase::unfixIntrinsics() { - for (auto key : getStructure()) + for (auto key : getKeys()) { if (key != 'P' and key != 'O') // exclude extrinsics { @@ -549,7 +545,7 @@ void SensorBase::printState (bool _metric, bool _state_blocks, std::ostream& _st { if (_metric && _state_blocks) { - for (auto key : getStructure()) + for (auto key : getKeys()) { auto sb = getStateBlockDynamic(key); if (sb) @@ -560,12 +556,12 @@ void SensorBase::printState (bool _metric, bool _state_blocks, std::ostream& _st else if (_metric) { _stream << _tabs << " " << (isFixed() ? "Fix" : "Est") << ",\t x = ( " << std::setprecision(3) - << getStateVector(getStructure()).transpose() << " )" << std::endl; + << getStateVector(getKeys()).transpose() << " )" << std::endl; } else if (_state_blocks) { _stream << _tabs << " " << "sb:"; - for (auto key : getStructure()) + for (auto key : getKeys()) { auto sb = getStateBlockDynamic(key); if (sb) diff --git a/src/state_block/has_state_blocks.cpp b/src/state_block/has_state_blocks.cpp index 717fc720e38d23f66f3f0d19ffc170a0cad4b149..84668cfff6d8ba7f9996255bef11d82d3bc0ad8c 100644 --- a/src/state_block/has_state_blocks.cpp +++ b/src/state_block/has_state_blocks.cpp @@ -57,7 +57,7 @@ HasStateBlocks::HasStateBlocks(const SpecComposite& _specs) HasStateBlocks::HasStateBlocks(const TypeComposite& _types, const VectorComposite& _vectors) { - if (not _types.has(_vectors.getStructure()) or not _vectors.has(_types.getStructure())) + if (not _types.has(_vectors.getKeys()) or not _vectors.has(_types.getKeys())) { throw std::runtime_error("HasStateBlocks::HasStateBlocks: provided type and vector composites don't have the same structure"); } @@ -121,7 +121,7 @@ void HasStateBlocks::registerNewStateBlocks(ProblemPtr _problem) void HasStateBlocks::removeStateBlocks(ProblemPtr _problem) { - for (const char key : getStructure()) // note: key is a char + for (const char key : getKeys()) // note: key is a char { auto sbp = getStateBlock(key); if (sbp != nullptr) @@ -156,7 +156,7 @@ void HasStateBlocks::printState (bool _metric, bool _state_blocks, std::ostream& { if (_metric && _state_blocks) { - for (auto key : getStructure()) + for (auto key : getKeys()) { auto sb = getStateBlock(key); if (sb) @@ -169,13 +169,13 @@ void HasStateBlocks::printState (bool _metric, bool _state_blocks, std::ostream& else if (_metric) { _stream << _tabs << " " << (isFixed() ? "Fix" : "Est") - << ",\t x = ( " << std::setprecision(3) << getStateVector(getStructure()).transpose() << " )" + << ",\t x = ( " << std::setprecision(3) << getStateVector(getKeys()).transpose() << " )" << std::endl; } else if (_state_blocks) { _stream << _tabs << " " << "sb:"; - for (auto key : getStructure()) + for (auto key : getKeys()) { auto sb = getStateBlock(key); if (sb) diff --git a/src/state_block/spec_composite.cpp b/src/state_block/spec_composite.cpp index f6a43c2c81d4fea91168f20c769b7033da6dd360..00234e6c75f5cf26eec05e033402a81637d6a639 100644 --- a/src/state_block/spec_composite.cpp +++ b/src/state_block/spec_composite.cpp @@ -34,7 +34,7 @@ SpecState::SpecState(const std::string& _type, const Eigen::VectorXd& _noise_std) : type_(_type), state_(_state), mode_(_mode), noise_std_(_noise_std) { - check(); + SpecState::check(); } SpecState::SpecState(const YAML::Node& _n) @@ -47,7 +47,7 @@ SpecState::SpecState(const YAML::Node& _n) else noise_std_ = Eigen::VectorXd(0); - check(); + SpecState::check(); } void SpecState::check() const diff --git a/src/state_block/state_block.cpp b/src/state_block/state_block.cpp index c061ea96beff00f3ac0a7c102c06f7e57dd4e349..4eb72cd5415d27e78ad677eec9210aedb194758f 100644 --- a/src/state_block/state_block.cpp +++ b/src/state_block/state_block.cpp @@ -77,21 +77,4 @@ namespace wolf{ WOLF_REGISTER_STATEBLOCK(StateQuaternion); WOLF_REGISTER_STATEBLOCK(StateAngle); WOLF_REGISTER_STATEBLOCK(StateHomogeneous3d); -WOLF_REGISTER_STATEBLOCK_WITH_KEY(H, StateHomogeneous3d); - -StateBlockPtr create_orientation(const Eigen::VectorXd& _state, bool _fixed) -{ - if (_state.size() == 1) - return StateAngle::create(_state, _fixed); - if (_state.size() == 4) - return StateQuaternion::create(_state, _fixed); - - throw std::runtime_error("Wrong vector size for orientation. Must be 4 for a quaternion in 3D, or 1 for an angle in 2D."); - - return nullptr; -} - -//WOLF_REGISTER_STATEBLOCK_WITH_KEY(O, wolf::create_orientation); -namespace{ const bool __attribute__((used)) create_orientationRegisteredWithO = \ - FactoryStateBlock::registerCreator("O", wolf::create_orientation); } } \ No newline at end of file diff --git a/src/state_block/state_block_derived.cpp b/src/state_block/state_block_derived.cpp index e1eca5d666cc70fbd1ed3074dbc981bddbcdd872..0b12644d84ab0320ee6ef6d7949e2449c9991c97 100644 --- a/src/state_block/state_block_derived.cpp +++ b/src/state_block/state_block_derived.cpp @@ -25,69 +25,6 @@ namespace wolf { -StateBlockPtr StatePoint2d::create(const Eigen::VectorXd& _state, bool _fixed) -{ - if (_state.size() == 2) return std::make_shared<StatePoint2d>(_state, _fixed); - - throw std::runtime_error("Wrong vector size for Point2d."); -} - -StateBlockPtr StatePoint3d::create(const Eigen::VectorXd& _state, bool _fixed) -{ - if (_state.size() == 3) return std::make_shared<StatePoint3d>(_state, _fixed); - - throw std::runtime_error("Wrong vector size for Point3d."); -} - -StateBlockPtr StateVector2d::create(const Eigen::VectorXd& _state, bool _fixed) -{ - if (_state.size() == 2) return std::make_shared<StateVector2d>(_state, _fixed); - - throw std::runtime_error("Wrong vector size for Vector2d."); -} - -StateBlockPtr StateVector3d::create(const Eigen::VectorXd& _state, bool _fixed) -{ - if (_state.size() == 3) return std::make_shared<StateVector3d>(_state, _fixed); - - throw std::runtime_error("Wrong vector size for Vector3d."); -} - -StateBlockPtr create_point(const Eigen::VectorXd& _state, bool _fixed) -{ - if (_state.size() == 2) - return std::make_shared<StatePoint2d>(_state, _fixed); - else if (_state.size() == 3) - return std::make_shared<StatePoint3d>(_state, _fixed); - - throw std::runtime_error("Wrong vector size for Point."); -} - -StateBlockPtr create_vector(const Eigen::VectorXd& _state, bool _fixed) -{ - if (_state.size() == 2) - return std::make_shared<StateVector2d>(_state, _fixed); - else if (_state.size() == 3) - return std::make_shared<StateVector3d>(_state, _fixed); - - throw std::runtime_error("Wrong vector size for Vector."); -} - -template <size_t size> -StateBlockPtr StateParams<size>::create(const Eigen::VectorXd& _state, bool _fixed) -{ - if (_state.size() == size) return std::make_shared<StateParams<size>>(_state, _fixed); - - throw std::runtime_error("Wrong vector size for Params."); -} - -namespace -{ -const bool __attribute__((used)) create_pointRegisteredWithP = - FactoryStateBlock::registerCreator("P", wolf::create_point); -const bool __attribute__((used)) create_vectorRegisteredWithP = - FactoryStateBlock::registerCreator("V", wolf::create_vector); -} // namespace WOLF_REGISTER_STATEBLOCK(StatePoint2d); WOLF_REGISTER_STATEBLOCK(StateVector2d); diff --git a/src/state_block/state_composite.cpp b/src/state_block/state_composite.cpp index a84a74f090fb125f975d29bcf0560e25c622216b..4328fe42a1719744bef044586ad88f47a5573948 100644 --- a/src/state_block/state_composite.cpp +++ b/src/state_block/state_composite.cpp @@ -317,7 +317,7 @@ MatrixComposite operator * (double scalar_left, const MatrixComposite& M) return S; } -MatrixXd MatrixComposite::matrix(const StateStructure &_struct_rows, const StateStructure &_struct_cols) const +MatrixXd MatrixComposite::matrix(const StateKeys &_struct_rows, const StateKeys &_struct_cols) const { std::unordered_map < char, unsigned int> indices_rows; @@ -367,8 +367,8 @@ unsigned int MatrixComposite::cols() const return cols; } -void MatrixComposite::sizeAndIndices(const StateStructure &_struct_rows, - const StateStructure &_struct_cols, +void MatrixComposite::sizeAndIndices(const StateKeys &_struct_rows, + const StateKeys &_struct_cols, std::unordered_map<char, unsigned int> &_indices_rows, std::unordered_map<char, unsigned int> &_indices_cols, unsigned int &_rows, @@ -388,16 +388,16 @@ void MatrixComposite::sizeAndIndices(const StateStructure &_struct_rows, } } -MatrixComposite::MatrixComposite (const StateStructure& _row_structure, const StateStructure& _col_structure) +MatrixComposite::MatrixComposite (const StateKeys& _row_structure, const StateKeys& _col_structure) { for (const auto& rkey : _row_structure) for (const auto& ckey : _col_structure) this->emplace(rkey, ckey, MatrixXd(0,0)); } -MatrixComposite::MatrixComposite (const StateStructure& _row_structure, +MatrixComposite::MatrixComposite (const StateKeys& _row_structure, const std::list<int>& _row_sizes, - const StateStructure& _col_structure, + const StateKeys& _col_structure, const std::list<int>& _col_sizes) { assert (_row_structure.size() == _row_sizes.size() && "Row structure and sizes have different number of elements!"); @@ -418,9 +418,9 @@ MatrixComposite::MatrixComposite (const StateStructure& _row_structure, } MatrixComposite::MatrixComposite (const MatrixXd& _m, - const StateStructure& _row_structure, + const StateKeys& _row_structure, const std::list<int>& _row_sizes, - const StateStructure& _col_structure, + const StateKeys& _col_structure, const std::list<int>& _col_sizes) { assert (_row_structure.size() == _row_sizes.size() && "Row structure and sizes have different number of elements!"); @@ -454,8 +454,8 @@ MatrixComposite::MatrixComposite (const MatrixXd& _m, assert(_m.rows() == rindex && "Provided matrix has too many rows"); } -MatrixComposite MatrixComposite::zero (const StateStructure& _row_structure, const std::list<int>& _row_sizes, - const StateStructure& _col_structure, const std::list<int>& _col_sizes) +MatrixComposite MatrixComposite::zero (const StateKeys& _row_structure, const std::list<int>& _row_sizes, + const StateKeys& _col_structure, const std::list<int>& _col_sizes) { MatrixComposite m; @@ -477,7 +477,7 @@ MatrixComposite MatrixComposite::zero (const StateStructure& _row_structure, con return m; } -MatrixComposite MatrixComposite::identity (const StateStructure& _structure, const std::list<int>& _sizes) +MatrixComposite MatrixComposite::identity (const StateKeys& _structure, const std::list<int>& _sizes) { MatrixComposite m; @@ -655,7 +655,7 @@ void StateBlockComposite::set(const char& _key, const VectorXd &_vec) it->second->setState(_vec); } -void StateBlockComposite::setVectors(const StateStructure& _structure, const std::list<VectorXd> &_vectors) +void StateBlockComposite::setVectors(const StateKeys& _structure, const std::list<VectorXd> &_vectors) { assert(_structure.size() == _vectors.size() && "Sizes of structure and vector list do not match"); @@ -801,7 +801,7 @@ SizeEigen StateBlockComposite::stateSize() const return size; } -SizeEigen StateBlockComposite::stateSize(const StateStructure &_structure) const +SizeEigen StateBlockComposite::stateSize(const StateKeys &_structure) const { SizeEigen size = 0; for (const auto& key : _structure) @@ -811,7 +811,7 @@ SizeEigen StateBlockComposite::stateSize(const StateStructure &_structure) const return size; } -VectorXd StateBlockComposite::stateVector(const StateStructure &_structure) const +VectorXd StateBlockComposite::stateVector(const StateKeys &_structure) const { VectorXd x(stateSize(_structure)); SizeEigen index = 0; @@ -826,7 +826,7 @@ VectorXd StateBlockComposite::stateVector(const StateStructure &_structure) cons return x; } -void StateBlockComposite::stateVector(const StateStructure &_structure, VectorXd &_vector) const +void StateBlockComposite::stateVector(const StateKeys &_structure, VectorXd &_vector) const { _vector.resize(stateSize(_structure)); SizeEigen index = 0; diff --git a/src/state_block/vector_composite.cpp b/src/state_block/vector_composite.cpp index f01efc485e1991bb7f874695e1a7c3e6d8bfb55e..1de0778e2155347244597c728ba0be5a2c0d7ae3 100644 --- a/src/state_block/vector_composite.cpp +++ b/src/state_block/vector_composite.cpp @@ -24,49 +24,41 @@ namespace wolf{ -VectorComposite::VectorComposite(const StateStructure& _structure, const std::list<int>& _sizes) +VectorComposite::VectorComposite() : + Composite<Eigen::VectorXd>() { - auto size_it = _sizes.begin(); - for ( const auto& key : _structure) - { - const auto& size = *size_it; - - this->emplace(key, VectorXd(size).setZero()); - - size_it ++; - } } -VectorComposite::VectorComposite(const VectorXd& _v, const StateStructure& _structure, const std::list<int>& _sizes) +VectorComposite::VectorComposite(const StateKeys& _keys, const VectorXd& _v, const std::list<int>& _sizes) { + assert(_keys.size() == _sizes.size() && "Keys and _sizes list size mismatch"); + int index = 0; auto size_it = _sizes.begin(); - for ( const auto& key : _structure) + for ( const auto& key : _keys) { const auto& size = *size_it; + if (_v.size() < index + size) + throw std::runtime_error("VectorComposite: provided vector _v is too short"); + (*this)[key] = _v.segment(index,size); index += size; size_it ++; } -} -VectorComposite::VectorComposite (const StateStructure& _s) -{ - for (const auto& key : _s) - { - this->emplace(key,VectorXd(0)); - } + if (_v.size() != index) + throw std::runtime_error("VectorComposite: provided vector _v is too long"); } -VectorComposite::VectorComposite (const StateStructure& _structure, const std::list<VectorXd>& _vectors) +VectorComposite::VectorComposite (const StateKeys& _keys, const std::list<VectorXd>& _vectors) { - assert(_structure.size() == _vectors.size() && "Structure and vector list size mismatch"); + assert(_keys.size() == _vectors.size() && "Keys and vector list size mismatch"); auto vector_it = _vectors.begin(); - for (const auto& key : _structure) + for (const auto& key : _keys) { this->emplace(key, *vector_it); vector_it++; @@ -74,12 +66,12 @@ VectorComposite::VectorComposite (const StateStructure& _structure, const std::l } -Eigen::VectorXd VectorComposite::vector(const StateStructure &_structure) const +Eigen::VectorXd VectorComposite::vector(const StateKeys &_keys) const { // traverse once with unordered_map access std::vector<const VectorXd*> vp; unsigned int size = 0; - for (const auto& key : _structure) + for (const auto& key : _keys) { vp.push_back(&(this->at(key))); size += vp.back()->size(); @@ -99,55 +91,18 @@ Eigen::VectorXd VectorComposite::vector(const StateStructure &_structure) const std::ostream& operator <<(std::ostream &_os, const wolf::VectorComposite &_x) { - for (const auto &pair_key_vec : _x) + for (auto&& pair : _x) { - const auto &key = pair_key_vec.first; - const auto &vec = pair_key_vec.second; - _os << key << ": (" << vec.transpose() << "); "; + _os << pair.first << ": (" << pair.second.transpose() << "); "; } return _os; } -// wolf::VectorComposite operator +(const wolf::VectorComposite &_x, const wolf::VectorComposite &_y) -// { -// wolf::VectorComposite xpy; -// for (const auto& pair_i_xi : _x) -// { -// const auto& i = pair_i_xi.first; - -// xpy.emplace(i, _x.at(i) + _y.at(i)); -// } -// return xpy; -// } - -// VectorComposite operator -(const VectorComposite &_x, const VectorComposite &_y) -// { -// VectorComposite xpy; -// for (const auto& pair_i_xi : _x) -// { -// const auto& i = pair_i_xi.first; - -// xpy.emplace(i, _x.at(i) - _y.at(i)); -// } -// return xpy; -// } - -// VectorComposite operator -(const wolf::VectorComposite &_x) -// { -// wolf::VectorComposite m; -// for (const auto& pair_i_xi : _x) -// { -// const auto& i = pair_i_xi.first; -// m.emplace(i, - _x.at(i)); -// } -// return m; -// } - -void VectorComposite::set (const VectorXd& _v, const StateStructure& _structure, const std::list<int>& _sizes) +void VectorComposite::set (const VectorXd& _v, const StateKeys& _keys, const std::list<int>& _sizes) { int index = 0; auto size_it = _sizes.begin(); - for ( const auto& key : _structure) + for ( const auto& key : _keys) { const auto& size = *size_it; diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index 0e96bf4bb0d8442a9614e690d9d586883bb97ab8..f37dc90125c15da89c96d339347b9ae5f9732b37 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -32,19 +32,18 @@ wolf_add_gtest(gtest_example gtest_example.cpp) # # BufferFrame wolf_add_gtest(gtest_buffer_frame gtest_buffer_frame.cpp) -target_link_libraries(gtest_buffer_frame PUBLIC dummy) # CaptureBase class test wolf_add_gtest(gtest_capture_base gtest_capture_base.cpp) -# Converter from String to various types used by the parameters server -wolf_add_gtest(gtest_converter gtest_converter.cpp) +# # Converter from String to various types used by the parameters server +# wolf_add_gtest(gtest_converter gtest_converter.cpp) # FactorBase class test wolf_add_gtest(gtest_factor_base gtest_factor_base.cpp) # FactorAutodiff class test -# wolf_add_gtest(gtest_factor_autodiff gtest_factor_autodiff.cpp) +wolf_add_gtest(gtest_factor_autodiff gtest_factor_autodiff.cpp) # Factory test wolf_add_gtest(gtest_factory gtest_factory.cpp) @@ -52,11 +51,10 @@ target_link_libraries(gtest_factory PUBLIC dummy) # FactoryStateBlock factory test wolf_add_gtest(gtest_factory_state_block gtest_factory_state_block.cpp) -target_link_libraries(gtest_factory_state_block PUBLIC dummy) # Node Emplace test -# wolf_add_gtest(gtest_emplace gtest_emplace.cpp) -# target_link_libraries(gtest_emplace PUBLIC dummy) +wolf_add_gtest(gtest_emplace gtest_emplace.cpp) +target_link_libraries(gtest_emplace PUBLIC dummy) # FeatureBase classes test wolf_add_gtest(gtest_feature_base gtest_feature_base.cpp) @@ -71,7 +69,7 @@ wolf_add_gtest(gtest_graph_search gtest_graph_search.cpp) wolf_add_gtest(gtest_has_state_blocks gtest_has_state_blocks.cpp) # IsMotion classes test -# wolf_add_gtest(gtest_motion_provider gtest_motion_provider.cpp) +wolf_add_gtest(gtest_motion_provider gtest_motion_provider.cpp) # LocalParametrizationXxx classes test wolf_add_gtest(gtest_local_param gtest_local_param.cpp) @@ -82,11 +80,11 @@ wolf_add_gtest(gtest_logging gtest_logging.cpp) # MotionBuffer class test wolf_add_gtest(gtest_motion_buffer gtest_motion_buffer.cpp) -# Parameters server -# wolf_add_gtest(gtest_param_server gtest_param_server.cpp) +# # Parameters server +# # wolf_add_gtest(gtest_param_server gtest_param_server.cpp) -# YAML parser -# wolf_add_gtest(gtest_parser_yaml gtest_parser_yaml.cpp) +# # YAML parser +# # wolf_add_gtest(gtest_parser_yaml gtest_parser_yaml.cpp) # Problem class test wolf_add_gtest(gtest_problem gtest_problem.cpp) @@ -108,35 +106,35 @@ wolf_add_gtest(gtest_SE3 gtest_SE3.cpp) # SE2 test wolf_add_gtest(gtest_SE2 gtest_SE2.cpp) -# SensorBase test -wolf_add_gtest(gtest_sensor_base gtest_sensor_base.cpp) +# # SensorBase test +# wolf_add_gtest(gtest_sensor_base gtest_sensor_base.cpp) -# shared_from_this test -wolf_add_gtest(gtest_shared_from_this gtest_shared_from_this.cpp) +# # shared_from_this test +# wolf_add_gtest(gtest_shared_from_this gtest_shared_from_this.cpp) -# SolverManager test -wolf_add_gtest(gtest_solver_manager gtest_solver_manager.cpp) +# # SolverManager test +# wolf_add_gtest(gtest_solver_manager gtest_solver_manager.cpp) -# SolverManagerMultithread test -wolf_add_gtest(gtest_solver_manager_multithread gtest_solver_manager_multithread.cpp) +# # SolverManagerMultithread test +# wolf_add_gtest(gtest_solver_manager_multithread gtest_solver_manager_multithread.cpp) -# StateBlock class test -wolf_add_gtest(gtest_state_block gtest_state_block.cpp) +# # StateBlock class test +# wolf_add_gtest(gtest_state_block gtest_state_block.cpp) -# StateBlock class test -wolf_add_gtest(gtest_state_composite gtest_state_composite.cpp) +# # StateBlock class test +# wolf_add_gtest(gtest_state_composite gtest_state_composite.cpp) -# TimeStamp class test -wolf_add_gtest(gtest_time_stamp gtest_time_stamp.cpp) +# # TimeStamp class test +# wolf_add_gtest(gtest_time_stamp gtest_time_stamp.cpp) -# TrackMatrix class test -wolf_add_gtest(gtest_track_matrix gtest_track_matrix.cpp) +# # TrackMatrix class test +# wolf_add_gtest(gtest_track_matrix gtest_track_matrix.cpp) -# TrajectoryBase class test -wolf_add_gtest(gtest_trajectory gtest_trajectory.cpp) +# # TrajectoryBase class test +# wolf_add_gtest(gtest_trajectory gtest_trajectory.cpp) -# TreeManager class test -wolf_add_gtest(gtest_tree_manager gtest_tree_manager.cpp) +# # TreeManager class test +# wolf_add_gtest(gtest_tree_manager gtest_tree_manager.cpp) # ------- Now Derived classes ---------- diff --git a/test/dummy/factory_dummy_object.h b/test/dummy/factory_dummy_object.h index 145ced54972a3b16cc042f519995a284df1d5e42..9b3705ab10dd98be39be717d15f5fd72f9bc2aa4 100644 --- a/test/dummy/factory_dummy_object.h +++ b/test/dummy/factory_dummy_object.h @@ -28,7 +28,7 @@ namespace wolf { class DummyObject; -typedef Factory<DummyObject, +typedef Factory<std::shared_ptr<DummyObject>, const std::string&> FactoryDummyObject; template<> inline std::string FactoryDummyObject::getClass() const diff --git a/test/dummy/processor_motion_provider_dummy.h b/test/dummy/processor_motion_provider_dummy.h index a7e64038e15c0a22130ba1f6fbddb22c08d02373..88f1465748908c2077ed6f52590c3b9c97eafc28 100644 --- a/test/dummy/processor_motion_provider_dummy.h +++ b/test/dummy/processor_motion_provider_dummy.h @@ -31,7 +31,7 @@ WOLF_STRUCT_PTR_TYPEDEFS(ParamsMotionProviderDummy); struct ParamsMotionProviderDummy : public ParamsProcessorBase, public ParamsMotionProvider { - std::string state_structure = "PO"; + TypeComposite state_types = {{'P',"StatePoint2d"},{'O',"StateAngle"}}; ParamsMotionProviderDummy() = default; ParamsMotionProviderDummy(const YAML::Node& _n): @@ -49,7 +49,7 @@ class MotionProviderDummy : public ProcessorBase, public MotionProvider public: MotionProviderDummy(ParamsMotionProviderDummyPtr _params) : ProcessorBase("MotionProviderDummy", 2, _params), - MotionProvider(_params->state_structure, _params) + MotionProvider(_params->state_types, _params) {} ~MotionProviderDummy(){}; @@ -66,12 +66,12 @@ class MotionProviderDummy : public ProcessorBase, public MotionProvider bool voteForKeyFrame() const override { return false; }; TimeStamp getTimeStamp() const override {return TimeStamp(0);}; - VectorComposite getState(const StateStructure& _structure = "") const override + VectorComposite getState(const StateKeys& _structure = "") const override { return getOdometry(); }; - VectorComposite getState(const TimeStamp& _ts, const StateStructure& _structure = "") const override + VectorComposite getState(const TimeStamp& _ts, const StateKeys& _structure = "") const override { return getOdometry(); }; diff --git a/test/gtest_SE3.cpp b/test/gtest_SE3.cpp index 5d9df91f1f67018ef774c3f3ee1eea2833f34a0f..dd1d7b0c1958c254085119a763bc2fc1bf400640 100644 --- a/test/gtest_SE3.cpp +++ b/test/gtest_SE3.cpp @@ -19,19 +19,10 @@ // along with this program. If not, see <http://www.gnu.org/licenses/>. // //--------LICENSE_END-------- -/** - * \file gtest_SE3.cpp - * - * Created on: Feb 2, 2019 - * \author: jsola - */ - #include "core/math/SE3.h" #include "core/utils/utils_gtest.h" - - using namespace Eigen; using namespace wolf; using namespace SE3; @@ -142,7 +133,7 @@ TEST(SE3, inverseComposite) Vector3d pi_true = -(q.conjugate() * p); Quaterniond qi_true = q.conjugate(); - VectorComposite pose_vc_out("PO", {3, 4}); + VectorComposite pose_vc_out("PO", Vector7d::Zero(), {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); @@ -215,7 +206,7 @@ TEST(SE3, composeVectorComposite) compose(p1, q1, p2, q2, pc, qc); // tested in composeVectorBlocks - VectorComposite x1, x2, xc("PO", {3,4}); + VectorComposite x1, x2, xc("PO", Vector7d::Zero(), {3,4}); x1.emplace('P', p1); x1.emplace('O', q1.coeffs()); @@ -246,7 +237,7 @@ TEST(SE3, composeVectorComposite_Jacobians) compose(p1, q1, p2, q2, pc, qc); // tested in composeVectorBlocks - VectorComposite x1, x2, xc("PO", {3,4}); + VectorComposite x1, x2, xc("PO", Vector7d::Zero(), {3,4}); MatrixComposite J_xc_x1, J_xc_x2; x1.emplace('P', p1); diff --git a/test/gtest_emplace.cpp b/test/gtest_emplace.cpp index c9f34bbf031ec8b09e5d50ea3ab0fcc7f6c370bb..2a2a4a55443fc7cc40fbb85aaf94a40b75104eb6 100644 --- a/test/gtest_emplace.cpp +++ b/test/gtest_emplace.cpp @@ -30,6 +30,7 @@ #include "core/state_block/state_block_derived.h" #include "core/state_block/state_angle.h" +#include "core/state_block/state_quaternion.h" #include "core/problem/problem.h" #include "core/sensor/sensor_base.h" #include "core/sensor/sensor_odom.h" @@ -58,12 +59,10 @@ TEST(Emplace, Sensor) { ProblemPtr P = Problem::create("POV", 3); - auto sen = SensorBase::emplace<SensorDummy>(P->getHardware(), // SensorBase is abstract - "dummy_name", - 2, - std::make_shared<ParamsSensorDummy>(), - SpecSensorComposite({{'P',SpecStateSensor("P",Vector2d::Zero())}, - {'O',SpecStateSensor("O",Vector1d::Zero())}})); + auto sen = SensorBase::emplace<SensorDummy<3>>(P->getHardware(), // SensorBase is abstract + std::make_shared<ParamsSensorDummy>(), + SpecSensorComposite({{'P',SpecStateSensor("StatePoint3d",Vector3d::Zero())}, + {'O',SpecStateSensor("StateQuaternion",Vector4d::Random().normalized())}})); ASSERT_EQ(P, P->getHardware()->getSensorList().front()->getHardware()->getProblem()); } @@ -72,7 +71,10 @@ TEST(Emplace, Frame) ProblemPtr P = Problem::create("POV", 3); ASSERT_NE(P->getTrajectory(), nullptr); - FrameBase::emplace<FrameBase>(P->getTrajectory(), TimeStamp(0), std::make_shared<StatePoint2d>(Vector2d::Zero(),true), std::make_shared<StateAngle>(2,true)); + FrameBase::emplace<FrameBase>(P->getTrajectory(), + TimeStamp(0), + std::make_shared<StatePoint3d>(Vector3d::Zero(),true), + std::make_shared<StateQuaternion>(Vector4d::Random().normalized(),true)); ASSERT_EQ(P, P->getTrajectory()->getFirstFrame()->getTrajectory()->getProblem()); } @@ -80,24 +82,29 @@ TEST(Emplace, Processor) { ProblemPtr P = Problem::create("PO", 2); - auto sen = SensorBase::emplace<SensorDummy>(P->getHardware(), // SensorBase is abstract - "dummy_name", - 2, - std::make_shared<ParamsSensorDummy>(), - SpecSensorComposite({{'P',SpecStateSensor("P",Vector2d::Zero())}, - {'O',SpecStateSensor("O",Vector1d::Zero())}})); + auto sen = SensorBase::emplace<SensorDummy<2>>(P->getHardware(), // SensorBase is abstract + std::make_shared<ParamsSensorDummy>(), + SpecSensorComposite({{'P',SpecStateSensor("StatePoint2d",Vector2d::Zero())}, + {'O',SpecStateSensor("StateAngle",Vector1d::Zero())}})); + WOLF_INFO("0"); auto prc = ProcessorOdom2d::emplace<ProcessorOdom2d>(sen, std::make_shared<ParamsProcessorOdom2d>()); + WOLF_INFO("1"); ASSERT_EQ(P, P->getHardware()->getSensorList().front()->getProcessorList().front()->getSensor()->getProblem()); + WOLF_INFO("2"); ASSERT_EQ(sen, sen->getProcessorList().front()->getSensor()); ASSERT_EQ(prc, sen->getProcessorList().front()); - SensorBasePtr sen2 = SensorBase::emplace<SensorDummy>(P->getHardware(), // SensorBase is abstract - "dummy_name", - 2, - std::make_shared<ParamsSensorDummy>(), - SpecSensorComposite({{'P',SpecStateSensor("P",Vector2d::Zero())}, - {'O',SpecStateSensor("O",Vector1d::Zero())}})); + WOLF_INFO("1"); + + SensorBasePtr sen2 = SensorBase::emplace<SensorDummy<2>>(P->getHardware(), // SensorBase is abstract + std::make_shared<ParamsSensorDummy>(), + SpecSensorComposite({{'P',SpecStateSensor("StatePoint2d",Vector2d::Zero())}, + {'O',SpecStateSensor("StateAngle",Vector1d::Zero())}})); + + WOLF_INFO("2"); ProcessorOdom2dPtr prc2 = ProcessorOdom2d::emplace<ProcessorOdom2d>(sen2, std::make_shared<ParamsProcessorOdom2d>()); + + WOLF_INFO("3"); ASSERT_EQ(sen2, sen2->getProcessorList().front()->getSensor()); ASSERT_EQ(prc2, sen2->getProcessorList().front()); } @@ -107,7 +114,10 @@ TEST(Emplace, Capture) ProblemPtr P = Problem::create("POV", 3); ASSERT_NE(P->getTrajectory(), nullptr); - auto frm = FrameBase::emplace<FrameBase>(P->getTrajectory(), TimeStamp(0), std::make_shared<StatePoint2d>(Vector2d::Zero(),true), std::make_shared<StateAngle>(0,true)); + auto frm = FrameBase::emplace<FrameBase>(P->getTrajectory(), + TimeStamp(0), + std::make_shared<StatePoint3d>(Vector3d::Zero(),true), + std::make_shared<StateQuaternion>(Vector4d::Random().normalized(),true)); ASSERT_EQ(P, P->getTrajectory()->getFirstFrame()->getTrajectory()->getProblem()); auto cpt = CaptureBase::emplace<CaptureBase>(frm, "Dummy", TimeStamp(0), nullptr, nullptr, nullptr, nullptr); @@ -121,7 +131,10 @@ TEST(Emplace, Feature) ProblemPtr P = Problem::create("POV", 3); ASSERT_NE(P->getTrajectory(), nullptr); - auto frm = FrameBase::emplace<FrameBase>(P->getTrajectory(), TimeStamp(0), std::make_shared<StatePoint2d>(Vector2d::Zero(),true), std::make_shared<StateAngle>(2,true)); + auto frm = FrameBase::emplace<FrameBase>(P->getTrajectory(), + TimeStamp(0), + std::make_shared<StatePoint3d>(Vector3d::Zero(),true), + std::make_shared<StateQuaternion>(Vector4d::Random().normalized(),true)); ASSERT_EQ(P, P->getTrajectory()->getFirstFrame()->getTrajectory()->getProblem()); auto cpt = CaptureBase::emplace<CaptureBase>(frm, "Dummy", TimeStamp(0), nullptr, nullptr, nullptr, nullptr); @@ -139,7 +152,11 @@ TEST(Emplace, Factor) ProblemPtr P = Problem::create("POV", 3); ASSERT_NE(P->getTrajectory(), nullptr); - auto frm = FrameBase::emplace<FrameBase>(P->getTrajectory(), TimeStamp(0), std::make_shared<StatePoint2d>(Vector2d::Zero(),true), std::make_shared<StateAngle>(2,true)); + + auto frm = FrameBase::emplace<FrameBase>(P->getTrajectory(), + TimeStamp(0), + std::make_shared<StatePoint3d>(Vector3d::Zero(),true), + std::make_shared<StateQuaternion>(Vector4d::Random().normalized(),true)); ASSERT_EQ(P, P->getTrajectory()->getFirstFrame()->getTrajectory()->getProblem()); auto cpt = CaptureBase::emplace<CaptureBase>(frm, "Dummy", TimeStamp(0), nullptr, nullptr, nullptr, nullptr); @@ -157,15 +174,13 @@ TEST(Emplace, Factor) TEST(Emplace, EmplaceDerived) { - ProblemPtr P = Problem::create("POV", 3); + ProblemPtr P = Problem::create("PO", 2); auto frm = FrameBase::emplace<FrameBase>(P->getTrajectory(), TimeStamp(0), std::make_shared<StatePoint2d>(Vector2d::Zero(),true), std::make_shared<StateAngle>(2,true)); - auto sen = SensorBase::emplace<SensorOdom>(P->getHardware(), - "dummy_name", - 2, - std::make_shared<ParamsSensorOdom>(), - SpecSensorComposite({{'P',SpecStateSensor("P",Vector2d::Zero())}, - {'O',SpecStateSensor("O",Vector1d::Zero())}})); + auto sen = SensorBase::emplace<SensorOdom2d>(P->getHardware(), + std::make_shared<ParamsSensorOdom>(), + SpecSensorComposite({{'P',SpecStateSensor("StatePoint2d",Vector2d::Zero())}, + {'O',SpecStateSensor("StateAngle",Vector1d::Zero())}})); auto cov = Eigen::MatrixXd::Identity(2,2); auto cpt = CaptureBase::emplace<CaptureOdom2d>(frm, TimeStamp(0), sen, Eigen::VectorXd(2), cov, nullptr); auto m = Eigen::Matrix<double,9,6>(); @@ -186,7 +201,10 @@ TEST(Emplace, ReturnDerived) ProblemPtr P = Problem::create("POV", 3); ASSERT_NE(P->getTrajectory(), nullptr); - auto frm = FrameBase::emplace<FrameBase>(P->getTrajectory(), TimeStamp(0), std::make_shared<StatePoint2d>(Vector2d::Zero(),true), std::make_shared<StateAngle>(2,true)); + auto frm = FrameBase::emplace<FrameBase>(P->getTrajectory(), + TimeStamp(0), + std::make_shared<StatePoint3d>(Vector3d::Zero(),true), + std::make_shared<StateQuaternion>(Vector4d::Random().normalized(),true)); auto cpt = CaptureBase::emplace<CaptureBase>(frm, "Dummy", TimeStamp(0), nullptr, nullptr, nullptr, nullptr); auto cov = Eigen::MatrixXd::Identity(2,2); auto ftr = FeatureBase::emplace<FeatureOdom2d>(cpt, Eigen::VectorXd(2), cov); diff --git a/test/gtest_factor_autodiff.cpp b/test/gtest_factor_autodiff.cpp index 718cf14ba4731b4b754de43812f0d785cfc72efa..84cddc0e0d66078976fd71a119375018d98ee77c 100644 --- a/test/gtest_factor_autodiff.cpp +++ b/test/gtest_factor_autodiff.cpp @@ -381,7 +381,7 @@ TEST(FactorAutodiff, EmplaceOdom2d) FrameBasePtr fr2_ptr = problem->emplaceFrame(TimeStamp(2)); // SENSOR - auto sensor_ptr = FactorySensorYaml::create("SensorOdom", "sensor_1", 2, wolf_root + "/test/yaml/sensor_odom_2d.yaml"); + auto sensor_ptr = FactorySensorYaml::create("SensorOdom2d", "SensorOdom2d", wolf_root + "/test/yaml/sensor_odom_2d.yaml", {wolf_root}); // CAPTURE auto capture_ptr = CaptureBase::emplace<CaptureVoid>(fr2_ptr, TimeStamp(0), sensor_ptr); @@ -407,11 +407,11 @@ TEST(FactorAutodiff, ResidualOdom2d) f1_pose << 2,1,M_PI; f2_pose << 3,5,3*M_PI/2; - FrameBasePtr fr1_ptr = problem->emplaceFrame(TimeStamp(1), f1_pose); - FrameBasePtr fr2_ptr = problem->emplaceFrame(TimeStamp(2), f2_pose); + FrameBasePtr fr1_ptr = problem->emplaceFrame(TimeStamp(1), "PO", f1_pose); + FrameBasePtr fr2_ptr = problem->emplaceFrame(TimeStamp(2), "PO", f2_pose); // SENSOR - auto sensor_ptr = FactorySensorYaml::create("SensorOdom", "sensor_1", 2, wolf_root + "/test/yaml/sensor_odom_2d.yaml"); + auto sensor_ptr = FactorySensorYaml::create("SensorOdom2d", "SensorOdom2d", wolf_root + "/test/yaml/sensor_odom_2d.yaml", {wolf_root}); // CAPTURE auto capture_ptr = CaptureBase::emplace<CaptureVoid>(fr2_ptr, TimeStamp(0), sensor_ptr); @@ -449,11 +449,11 @@ TEST(FactorAutodiff, JacobianOdom2d) f1_pose << 2,1,M_PI; f2_pose << 3,5,3*M_PI/2; - FrameBasePtr fr1_ptr = problem->emplaceFrame(TimeStamp(1), f1_pose); - FrameBasePtr fr2_ptr = problem->emplaceFrame(TimeStamp(2), f2_pose); + FrameBasePtr fr1_ptr = problem->emplaceFrame(TimeStamp(1), "PO", f1_pose); + FrameBasePtr fr2_ptr = problem->emplaceFrame(TimeStamp(2), "PO", f2_pose); // SENSOR - auto sensor_ptr = FactorySensorYaml::create("SensorOdom", "sensor_1", 2, wolf_root + "/test/yaml/sensor_odom_2d.yaml"); + auto sensor_ptr = FactorySensorYaml::create("SensorOdom2d", "SensorOdom2d", wolf_root + "/test/yaml/sensor_odom_2d.yaml", {wolf_root}); // CAPTURE auto capture_ptr = CaptureBase::emplace<CaptureVoid>(fr2_ptr, TimeStamp(0), sensor_ptr); @@ -526,11 +526,11 @@ TEST(FactorAutodiff, AutodiffVsAnalytic) f1_pose << 2,1,M_PI; f2_pose << 3,5,3*M_PI/2; - FrameBasePtr fr1_ptr = problem->emplaceFrame(TimeStamp(1), f1_pose); - FrameBasePtr fr2_ptr = problem->emplaceFrame(TimeStamp(2), f2_pose); + FrameBasePtr fr1_ptr = problem->emplaceFrame(TimeStamp(1), "PO", f1_pose); + FrameBasePtr fr2_ptr = problem->emplaceFrame(TimeStamp(2), "PO", f2_pose); // SENSOR - auto sensor_ptr = FactorySensorYaml::create("SensorOdom", "sensor_1", 2, wolf_root + "/test/yaml/sensor_odom_2d.yaml"); + auto sensor_ptr = FactorySensorYaml::create("SensorOdom2d", "SensorOdom2d", wolf_root + "/test/yaml/sensor_odom_2d.yaml", {wolf_root}); // CAPTURE auto capture_ptr = CaptureBase::emplace<CaptureVoid>(fr2_ptr, TimeStamp(0), sensor_ptr); diff --git a/test/gtest_factory_state_block.cpp b/test/gtest_factory_state_block.cpp index ea1f4be3c9258238ba927e95196e9d0e2781cfb9..1f1adb3de9d9f25dcf39b68b78e6c6de6522aa78 100644 --- a/test/gtest_factory_state_block.cpp +++ b/test/gtest_factory_state_block.cpp @@ -35,31 +35,39 @@ using namespace wolf; +using namespace Eigen; - +//////////////////////////////////////////////////////// +// FACTORY STATE BLOCK // +//////////////////////////////////////////////////////// TEST(FactoryStateBlock, creator_non_registered_key) { // non registered -> throw - ASSERT_THROW(auto sba = FactoryStateBlock::create("A", Eigen::Vector1d(6), false), std::runtime_error); + ASSERT_THROW(auto sba = FactoryStateBlock::create("A", Vector1d(6), false), std::runtime_error); } TEST(FactoryStateBlock, creator_StateBlock) { - ASSERT_THROW(auto sbp = FactoryStateBlock::create("StateBlock", Eigen::Vector3d(1,2,3), false), std::runtime_error); + ASSERT_THROW(auto sbp = FactoryStateBlock::create("StateBlock", Vector3d(1,2,3), false), std::runtime_error); } TEST(FactoryStateBlock, creator_Quaternion) { - auto sbq = FactoryStateBlock::create("StateQuaternion", Eigen::Vector4d(1,2,3,4).normalized(), false); + auto sbq = FactoryStateBlock::create("StateQuaternion", Vector4d(1,2,3,4).normalized(), false); ASSERT_EQ(sbq->getSize() , 4); ASSERT_EQ(sbq->getLocalSize(), 3); ASSERT_TRUE(sbq->hasLocalParametrization()); } +TEST(FactoryStateBlock, creator_Quaternion_not_normalized) +{ + ASSERT_THROW(auto sbq = FactoryStateBlock::create("StateQuaternion", Vector4d(1,2,3,4), false), std::runtime_error); +} + TEST(FactoryStateBlock, creator_Angle) { - auto sba = FactoryStateBlock::create("StateAngle", Eigen::Vector1d(1), false); + auto sba = FactoryStateBlock::create("StateAngle", Vector1d(1), false); ASSERT_EQ(sba->getSize() , 1); ASSERT_EQ(sba->getLocalSize(), 1); @@ -68,54 +76,22 @@ TEST(FactoryStateBlock, creator_Angle) TEST(FactoryStateBlock, creator_Homogeneous3d) { - auto sbh = FactoryStateBlock::create("StateHomogeneous3d", Eigen::Vector4d(1,2,3,4), false); + auto sbh = FactoryStateBlock::create("StateHomogeneous3d", Vector4d(1,2,3,4), false); ASSERT_EQ(sbh->getSize() , 4); ASSERT_EQ(sbh->getLocalSize(), 3); ASSERT_TRUE(sbh->hasLocalParametrization()); } -TEST(FactoryStateBlock, creator_H) -{ - auto sbh = FactoryStateBlock::create("H", Eigen::Vector4d(1,2,3,4), false); - - ASSERT_EQ(sbh->getSize() , 4); - ASSERT_EQ(sbh->getLocalSize(), 3); - ASSERT_TRUE(sbh->hasLocalParametrization()); -} - -TEST(FactoryStateBlock, creator_O_is_quaternion) -{ - auto sbq = FactoryStateBlock::create("O", Eigen::Vector4d(1,2,3,4).normalized(), false); - - ASSERT_EQ(sbq->getSize() , 4); - ASSERT_EQ(sbq->getLocalSize(), 3); - ASSERT_TRUE(sbq->hasLocalParametrization()); -} - -TEST(FactoryStateBlock, creator_O_is_angle) -{ - auto sba = FactoryStateBlock::create("O", Eigen::Vector1d(1), false); - - ASSERT_EQ(sba->getSize() , 1); - ASSERT_EQ(sba->getLocalSize(), 1); - ASSERT_TRUE(sba->hasLocalParametrization()); -} - -TEST(FactoryStateBlock, creator_O_is_wrong_size) -{ - ASSERT_THROW(auto sba = FactoryStateBlock::create("O", Eigen::Vector2d(1,2), false) , std::runtime_error); -} - TEST(FactoryStateBlock, creator_Point) { - auto sba = FactoryStateBlock::create("StatePoint2d", Eigen::Vector2d(1,2), false); + auto sba = FactoryStateBlock::create("StatePoint2d", Vector2d(1,2), false); ASSERT_EQ(sba->getSize() , 2); ASSERT_EQ(sba->getLocalSize(), 2); ASSERT_FALSE(sba->hasLocalParametrization()); - sba = FactoryStateBlock::create("StatePoint3d", Eigen::Vector3d(1,2,3), false); + sba = FactoryStateBlock::create("StatePoint3d", Vector3d(1,2,3), false); ASSERT_EQ(sba->getSize() , 3); ASSERT_EQ(sba->getLocalSize(), 3); @@ -125,72 +101,152 @@ TEST(FactoryStateBlock, creator_Point) ASSERT_THROW(sba = FactoryStateBlock::create("StatePoint2d", Vector1d(1), false) , std::runtime_error); } -TEST(FactoryStateBlock, creator_P) +TEST(FactoryStateBlock, creator_Vector) { - auto sba = FactoryStateBlock::create("P", Eigen::Vector2d(1,2), false); + auto sba = FactoryStateBlock::create("StateVector2d", Vector2d(1,2), false); ASSERT_EQ(sba->getSize() , 2); ASSERT_EQ(sba->getLocalSize(), 2); ASSERT_FALSE(sba->hasLocalParametrization()); - sba = FactoryStateBlock::create("P", Eigen::Vector3d(1,2,3), false); + sba = FactoryStateBlock::create("StateVector3d", Vector3d(1,2,3), false); ASSERT_EQ(sba->getSize() , 3); ASSERT_EQ(sba->getLocalSize(), 3); ASSERT_FALSE(sba->hasLocalParametrization()); // fails - ASSERT_THROW(sba = FactoryStateBlock::create("P", Vector1d(1), false) , std::runtime_error); + ASSERT_THROW(sba = FactoryStateBlock::create("StatePoint2d", Vector1d(1), false) , std::runtime_error); } -TEST(FactoryStateBlock, creator_Vector) +TEST(FactoryStateBlock, creator_Params) { - auto sba = FactoryStateBlock::create("StateVector2d", Eigen::Vector2d(1,2), false); + auto sb1 = FactoryStateBlock::create("StateParams1", Vector1d::Ones(), false); + auto sb2 = FactoryStateBlock::create("StateParams2", Vector2d::Ones(), false); + auto sb3 = FactoryStateBlock::create("StateParams3", Vector3d::Ones(), false); + auto sb4 = FactoryStateBlock::create("StateParams4", Vector4d::Ones(), false); + auto sb5 = FactoryStateBlock::create("StateParams5", Vector5d::Ones(), false); + auto sb6 = FactoryStateBlock::create("StateParams6", Vector6d::Ones(), false); + auto sb7 = FactoryStateBlock::create("StateParams7", Vector7d::Ones(), false); + auto sb8 = FactoryStateBlock::create("StateParams8", Vector8d::Ones(), false); + auto sb9 = FactoryStateBlock::create("StateParams9", Vector9d::Ones(), false); + auto sb10 = FactoryStateBlock::create("StateParams10", Vector10d::Ones(), false); - ASSERT_EQ(sba->getSize() , 2); - ASSERT_EQ(sba->getLocalSize(), 2); - ASSERT_FALSE(sba->hasLocalParametrization()); - - sba = FactoryStateBlock::create("StateVector3d", Eigen::Vector3d(1,2,3), false); + ASSERT_EQ(sb1->getSize(), 1); + ASSERT_EQ(sb2->getSize(), 2); + ASSERT_EQ(sb3->getSize(), 3); + ASSERT_EQ(sb4->getSize(), 4); + ASSERT_EQ(sb5->getSize(), 5); + ASSERT_EQ(sb6->getSize(), 6); + ASSERT_EQ(sb7->getSize(), 7); + ASSERT_EQ(sb8->getSize(), 8); + ASSERT_EQ(sb9->getSize(), 9); + ASSERT_EQ(sb10->getSize(), 10); - ASSERT_EQ(sba->getSize() , 3); - ASSERT_EQ(sba->getLocalSize(), 3); - ASSERT_FALSE(sba->hasLocalParametrization()); + ASSERT_EQ(sb1->getLocalSize(), 1); + ASSERT_FALSE(sb1->hasLocalParametrization()); // fails - ASSERT_THROW(sba = FactoryStateBlock::create("StatePoint2d", Vector1d(1), false) , std::runtime_error); + ASSERT_THROW(auto sba = FactoryStateBlock::create("StateParams2", Vector1d(1), false) , std::runtime_error); } -TEST(FactoryStateBlock, creator_V) +//////////////////////////////////////////////////////// +// FACTORY STATE BLOCK IDENTITY // +//////////////////////////////////////////////////////// +TEST(FactoryStateBlockIdentity, creator_idendity_non_registered_key) { - auto sba = FactoryStateBlock::create("V", Eigen::Vector2d(1,2), false); + // non registered -> throw + ASSERT_THROW(auto sba = FactoryStateBlockIdentity::create("A"), std::runtime_error); +} - ASSERT_EQ(sba->getSize() , 2); - ASSERT_EQ(sba->getLocalSize(), 2); - ASSERT_FALSE(sba->hasLocalParametrization()); +TEST(FactoryStateBlockIdentity, creator_StateBlock) +{ + ASSERT_THROW(auto sbp = FactoryStateBlockIdentity::create("StateBlock"), std::runtime_error); +} - sba = FactoryStateBlock::create("V", Eigen::Vector3d(1,2,3), false); +TEST(FactoryStateBlockIdentity, creator_Quaternion) +{ + auto sbq = FactoryStateBlockIdentity::create("StateQuaternion"); - ASSERT_EQ(sba->getSize() , 3); - ASSERT_EQ(sba->getLocalSize(), 3); - ASSERT_FALSE(sba->hasLocalParametrization()); + ASSERT_EQ(sbq->getSize() , 4); + ASSERT_EQ(sbq->getLocalSize(), 3); + ASSERT_TRUE(sbq->hasLocalParametrization()); + ASSERT_TRUE(sbq->isValid()); + ASSERT_QUATERNION_VECTOR_APPROX(Quaterniond::Identity().coeffs(), sbq->getState(), Constants::EPS_SMALL); +} - // fails - ASSERT_THROW(sba = FactoryStateBlock::create("V", Vector1d(1), false) , std::runtime_error); +TEST(FactoryStateBlockIdentity, creator_Angle) +{ + auto sba = FactoryStateBlockIdentity::create("StateAngle"); + + ASSERT_EQ(sba->getSize() , 1); + ASSERT_EQ(sba->getLocalSize(), 1); + ASSERT_TRUE(sba->hasLocalParametrization()); + ASSERT_TRUE(sba->isValid()); + ASSERT_MATRIX_APPROX(Vector1d::Zero(), sba->getState(), Constants::EPS_SMALL); } -TEST(FactoryStateBlock, creator_Params) +TEST(FactoryStateBlockIdentity, creator_Homogeneous3d) +{ + auto sbh = FactoryStateBlockIdentity::create("StateHomogeneous3d"); + + ASSERT_EQ(sbh->getSize() , 4); + ASSERT_EQ(sbh->getLocalSize(), 3); + ASSERT_TRUE(sbh->hasLocalParametrization()); + ASSERT_TRUE(sbh->isValid()); + ASSERT_MATRIX_APPROX(Quaterniond::Identity().coeffs(), sbh->getState(), Constants::EPS_SMALL); +} + +TEST(FactoryStateBlockIdentity, creator_Point) +{ + auto sbp2d = FactoryStateBlockIdentity::create("StatePoint2d"); + + ASSERT_EQ(sbp2d->getSize() , 2); + ASSERT_EQ(sbp2d->getLocalSize(), 2); + ASSERT_FALSE(sbp2d->hasLocalParametrization()); + ASSERT_TRUE(sbp2d->isValid()); + ASSERT_MATRIX_APPROX(Vector2d::Zero(), sbp2d->getState(), Constants::EPS_SMALL); + + auto sbp3d = FactoryStateBlockIdentity::create("StatePoint3d"); + + ASSERT_EQ(sbp3d->getSize() , 3); + ASSERT_EQ(sbp3d->getLocalSize(), 3); + ASSERT_FALSE(sbp3d->hasLocalParametrization()); + ASSERT_TRUE(sbp3d->isValid()); + ASSERT_MATRIX_APPROX(Vector3d::Zero(), sbp3d->getState(), Constants::EPS_SMALL); +} + +TEST(FactoryStateBlockIdentity, creator_Vector) +{ + auto sbv2d = FactoryStateBlockIdentity::create("StateVector2d"); + + ASSERT_EQ(sbv2d->getSize() , 2); + ASSERT_EQ(sbv2d->getLocalSize(), 2); + ASSERT_FALSE(sbv2d->hasLocalParametrization()); + ASSERT_TRUE(sbv2d->isValid()); + ASSERT_MATRIX_APPROX(Vector2d::Zero(), sbv2d->getState(), Constants::EPS_SMALL); + + auto sbv3d = FactoryStateBlockIdentity::create("StateVector3d"); + + ASSERT_EQ(sbv3d->getSize() , 3); + ASSERT_EQ(sbv3d->getLocalSize(), 3); + ASSERT_FALSE(sbv3d->hasLocalParametrization()); + ASSERT_TRUE(sbv3d->isValid()); + ASSERT_MATRIX_APPROX(Vector3d::Zero(), sbv3d->getState(), Constants::EPS_SMALL); +} + +TEST(FactoryStateBlockIdentity, creator_Params) { - auto sb1 = FactoryStateBlock::create("StateParams1", Eigen::Vector1d::Ones(), false); - auto sb2 = FactoryStateBlock::create("StateParams2", Eigen::Vector2d::Ones(), false); - auto sb3 = FactoryStateBlock::create("StateParams3", Eigen::Vector3d::Ones(), false); - auto sb4 = FactoryStateBlock::create("StateParams4", Eigen::Vector4d::Ones(), false); - auto sb5 = FactoryStateBlock::create("StateParams5", Eigen::Vector5d::Ones(), false); - auto sb6 = FactoryStateBlock::create("StateParams6", Eigen::Vector6d::Ones(), false); - auto sb7 = FactoryStateBlock::create("StateParams7", Eigen::Vector7d::Ones(), false); - auto sb8 = FactoryStateBlock::create("StateParams8", Eigen::Vector8d::Ones(), false); - auto sb9 = FactoryStateBlock::create("StateParams9", Eigen::Vector9d::Ones(), false); - auto sb10 = FactoryStateBlock::create("StateParams10", Eigen::Vector10d::Ones(), false); + auto sb1 = FactoryStateBlockIdentity::create("StateParams1"); + auto sb2 = FactoryStateBlockIdentity::create("StateParams2"); + auto sb3 = FactoryStateBlockIdentity::create("StateParams3"); + auto sb4 = FactoryStateBlockIdentity::create("StateParams4"); + auto sb5 = FactoryStateBlockIdentity::create("StateParams5"); + auto sb6 = FactoryStateBlockIdentity::create("StateParams6"); + auto sb7 = FactoryStateBlockIdentity::create("StateParams7"); + auto sb8 = FactoryStateBlockIdentity::create("StateParams8"); + auto sb9 = FactoryStateBlockIdentity::create("StateParams9"); + auto sb10 = FactoryStateBlockIdentity::create("StateParams10"); ASSERT_EQ(sb1->getSize(), 1); ASSERT_EQ(sb2->getSize(), 2); @@ -204,10 +260,141 @@ TEST(FactoryStateBlock, creator_Params) ASSERT_EQ(sb10->getSize(), 10); ASSERT_EQ(sb1->getLocalSize(), 1); + ASSERT_EQ(sb2->getLocalSize(), 2); + ASSERT_EQ(sb3->getLocalSize(), 3); + ASSERT_EQ(sb4->getLocalSize(), 4); + ASSERT_EQ(sb5->getLocalSize(), 5); + ASSERT_EQ(sb6->getLocalSize(), 6); + ASSERT_EQ(sb7->getLocalSize(), 7); + ASSERT_EQ(sb8->getLocalSize(), 8); + ASSERT_EQ(sb9->getLocalSize(), 9); + ASSERT_EQ(sb10->getLocalSize(), 10); + ASSERT_FALSE(sb1->hasLocalParametrization()); + ASSERT_FALSE(sb2->hasLocalParametrization()); + ASSERT_FALSE(sb3->hasLocalParametrization()); + ASSERT_FALSE(sb4->hasLocalParametrization()); + ASSERT_FALSE(sb5->hasLocalParametrization()); + ASSERT_FALSE(sb6->hasLocalParametrization()); + ASSERT_FALSE(sb7->hasLocalParametrization()); + ASSERT_FALSE(sb8->hasLocalParametrization()); + ASSERT_FALSE(sb9->hasLocalParametrization()); + ASSERT_FALSE(sb10->hasLocalParametrization()); + + ASSERT_TRUE(sb1->isValid()); + ASSERT_TRUE(sb2->isValid()); + ASSERT_TRUE(sb3->isValid()); + ASSERT_TRUE(sb4->isValid()); + ASSERT_TRUE(sb5->isValid()); + ASSERT_TRUE(sb6->isValid()); + ASSERT_TRUE(sb7->isValid()); + ASSERT_TRUE(sb8->isValid()); + ASSERT_TRUE(sb9->isValid()); + ASSERT_TRUE(sb10->isValid()); + + ASSERT_MATRIX_APPROX(Vector1d::Zero(), sb1->getState(), Constants::EPS_SMALL); + ASSERT_MATRIX_APPROX(Vector2d::Zero(), sb2->getState(), Constants::EPS_SMALL); + ASSERT_MATRIX_APPROX(Vector3d::Zero(), sb3->getState(), Constants::EPS_SMALL); + ASSERT_MATRIX_APPROX(Vector4d::Zero(), sb4->getState(), Constants::EPS_SMALL); + ASSERT_MATRIX_APPROX(Vector5d::Zero(), sb5->getState(), Constants::EPS_SMALL); + ASSERT_MATRIX_APPROX(Vector6d::Zero(), sb6->getState(), Constants::EPS_SMALL); + ASSERT_MATRIX_APPROX(Vector7d::Zero(), sb7->getState(), Constants::EPS_SMALL); + ASSERT_MATRIX_APPROX(Vector8d::Zero(), sb8->getState(), Constants::EPS_SMALL); + ASSERT_MATRIX_APPROX(Vector9d::Zero(), sb9->getState(), Constants::EPS_SMALL); + ASSERT_MATRIX_APPROX(Vector10d::Zero(), sb10->getState(), Constants::EPS_SMALL); +} - // fails - ASSERT_THROW(auto sba = FactoryStateBlock::create("StateParams2", Vector1d(1), false) , std::runtime_error); +//////////////////////////////////////////////////////// +// FACTORY STATE BLOCK IDENTITY VECTOR // +//////////////////////////////////////////////////////// +TEST(FactoryStateBlockIdentityVector, creator_idendity_non_registered_key) +{ + // non registered -> throw + ASSERT_THROW(auto vec = FactoryStateBlockIdentityVector::create("A"), std::runtime_error); +} + +TEST(FactoryStateBlockIdentityVector, creator_StateBlock) +{ + ASSERT_THROW(auto vec_p = FactoryStateBlockIdentityVector::create("StateBlock"), std::runtime_error); +} + +TEST(FactoryStateBlockIdentityVector, creator_Quaternion) +{ + auto vec_q = FactoryStateBlockIdentityVector::create("StateQuaternion"); + + ASSERT_QUATERNION_VECTOR_APPROX(Quaterniond::Identity().coeffs(), vec_q, Constants::EPS_SMALL); +} + +TEST(FactoryStateBlockIdentityVector, creator_Angle) +{ + auto vec = FactoryStateBlockIdentityVector::create("StateAngle"); + + ASSERT_MATRIX_APPROX(Vector1d::Zero(), vec, Constants::EPS_SMALL); +} + +TEST(FactoryStateBlockIdentityVector, creator_Homogeneous3d) +{ + auto vec_h = FactoryStateBlockIdentityVector::create("StateHomogeneous3d"); + + ASSERT_MATRIX_APPROX(Quaterniond::Identity().coeffs(), vec_h, Constants::EPS_SMALL); +} + +TEST(FactoryStateBlockIdentityVector, creator_Point) +{ + auto vec = FactoryStateBlockIdentityVector::create("StatePoint2d"); + + ASSERT_MATRIX_APPROX(Vector2d::Zero(), vec, Constants::EPS_SMALL); + + vec = FactoryStateBlockIdentityVector::create("StatePoint3d"); + + ASSERT_MATRIX_APPROX(Vector3d::Zero(), vec, Constants::EPS_SMALL); +} + +TEST(FactoryStateBlockIdentityVector, creator_Vector) +{ + auto vec = FactoryStateBlockIdentityVector::create("StateVector2d"); + + ASSERT_MATRIX_APPROX(Vector2d::Zero(), vec, Constants::EPS_SMALL); + + vec = FactoryStateBlockIdentityVector::create("StateVector3d"); + + ASSERT_MATRIX_APPROX(Vector3d::Zero(), vec, Constants::EPS_SMALL); +} + +TEST(FactoryStateBlockIdentityVectorVector, creator_Params) +{ + auto vec_1 = FactoryStateBlockIdentityVector::create("StateParams1"); + auto vec_2 = FactoryStateBlockIdentityVector::create("StateParams2"); + auto vec_3 = FactoryStateBlockIdentityVector::create("StateParams3"); + auto vec_4 = FactoryStateBlockIdentityVector::create("StateParams4"); + auto vec_5 = FactoryStateBlockIdentityVector::create("StateParams5"); + auto vec_6 = FactoryStateBlockIdentityVector::create("StateParams6"); + auto vec_7 = FactoryStateBlockIdentityVector::create("StateParams7"); + auto vec_8 = FactoryStateBlockIdentityVector::create("StateParams8"); + auto vec_9 = FactoryStateBlockIdentityVector::create("StateParams9"); + auto vec_10 = FactoryStateBlockIdentityVector::create("StateParams10"); + + ASSERT_EQ(vec_1.size(), 1); + ASSERT_EQ(vec_2.size(), 2); + ASSERT_EQ(vec_3.size(), 3); + ASSERT_EQ(vec_4.size(), 4); + ASSERT_EQ(vec_5.size(), 5); + ASSERT_EQ(vec_6.size(), 6); + ASSERT_EQ(vec_7.size(), 7); + ASSERT_EQ(vec_8.size(), 8); + ASSERT_EQ(vec_9.size(), 9); + ASSERT_EQ(vec_10.size(), 10); + + ASSERT_MATRIX_APPROX(Vector1d::Zero(), vec_1, Constants::EPS_SMALL); + ASSERT_MATRIX_APPROX(Vector2d::Zero(), vec_2, Constants::EPS_SMALL); + ASSERT_MATRIX_APPROX(Vector3d::Zero(), vec_3, Constants::EPS_SMALL); + ASSERT_MATRIX_APPROX(Vector4d::Zero(), vec_4, Constants::EPS_SMALL); + ASSERT_MATRIX_APPROX(Vector5d::Zero(), vec_5, Constants::EPS_SMALL); + ASSERT_MATRIX_APPROX(Vector6d::Zero(), vec_6, Constants::EPS_SMALL); + ASSERT_MATRIX_APPROX(Vector7d::Zero(), vec_7, Constants::EPS_SMALL); + ASSERT_MATRIX_APPROX(Vector8d::Zero(), vec_8, Constants::EPS_SMALL); + ASSERT_MATRIX_APPROX(Vector9d::Zero(), vec_9, Constants::EPS_SMALL); + ASSERT_MATRIX_APPROX(Vector10d::Zero(), vec_10, Constants::EPS_SMALL); } int main(int argc, char **argv) diff --git a/test/gtest_frame_base.cpp b/test/gtest_frame_base.cpp index ac3e52514df922b77473e912d7e10ef297904afd..9305205d9218ee1f0df289f2e68e833c1ca5dbb3 100644 --- a/test/gtest_frame_base.cpp +++ b/test/gtest_frame_base.cpp @@ -419,7 +419,7 @@ TEST(FrameBase, GetSetState) TEST(FrameBase, Constructor_composite) { FrameBase F = FrameBase(0.0, - "POV", + {{'P',"StatePoint3d"},{'O',"StateQuaternion"},{'V',"StateVector3d"}}, VectorComposite("POV", {Vector3d(1,2,3), Vector4d(1,2,3,4).normalized(), Vector3d(1,2,3)})); ASSERT_MATRIX_APPROX(F.getP()->getState(), Vector3d(1,2,3), 1e-15); @@ -427,18 +427,6 @@ TEST(FrameBase, Constructor_composite) ASSERT_EQ (F.getStateBlock('V')->getSize(), 3); } -TEST(FrameBase, Constructor_vectors) -{ - FrameBase F = FrameBase(0.0, - "POV", - {Vector3d(1,2,3), Vector4d(1,2,3,4).normalized(), Vector3d(1,2,3)}); - - ASSERT_MATRIX_APPROX(F.getP()->getState(), Vector3d(1,2,3), 1e-15); - ASSERT_TRUE (F.getO()->hasLocalParametrization()); - ASSERT_EQ (F.getStateBlock('V')->getSize(), 3); -} - - int main(int argc, char **argv) { diff --git a/test/gtest_graph_search.cpp b/test/gtest_graph_search.cpp index 43d4b57242a3bb7d1e4532b893d34fcb47fcfcb2..cae74f13d4f7134d855f799ce60e5b206a7fc0de 100644 --- a/test/gtest_graph_search.cpp +++ b/test/gtest_graph_search.cpp @@ -51,7 +51,7 @@ class GraphSearchTest : public testing::Test FrameBasePtr emplaceFrame(const TimeStamp& ts) { - return problem->emplaceFrame(ts, Vector3d::Zero()); + return problem->emplaceFrame(ts, "PO", Vector3d::Zero()); } FactorBasePtr createFactor(FrameBasePtr frm1, FrameBasePtr frm2) diff --git a/test/gtest_has_state_blocks.cpp b/test/gtest_has_state_blocks.cpp index 2b02be288217cca57703ab98b5b9c7a40f32d505..940d27670ead0f61139e387a1f5bc519b383a803 100644 --- a/test/gtest_has_state_blocks.cpp +++ b/test/gtest_has_state_blocks.cpp @@ -19,13 +19,6 @@ // along with this program. If not, see <http://www.gnu.org/licenses/>. // //--------LICENSE_END-------- -/** - * \file gtest_has_state_blocks.cpp - * - * Created on: Mar 24, 2020 - * \author: jsola - */ - #include "core/utils/utils_gtest.h" @@ -41,9 +34,7 @@ using namespace wolf; class HasStateBlocksTest : public testing::Test { - public: - ProblemPtr problem; SensorBasePtr S0, S1; FrameBasePtr F0, F1; @@ -52,7 +43,6 @@ class HasStateBlocksTest : public testing::Test StateBlockPtr sbp0, sbv0, sbp1, sbv1; StateQuaternionPtr sbo0, sbo1; - void SetUp() override { problem = Problem::create("POV", 3); @@ -69,10 +59,8 @@ class HasStateBlocksTest : public testing::Test } void TearDown() override{} - }; - TEST_F(HasStateBlocksTest, Notifications_add_makeKF) { Notification n; @@ -98,7 +86,6 @@ TEST_F(HasStateBlocksTest, Notifications_add_makeKF) ASSERT_TRUE(problem->getStateBlockNotification(sbv0, n)); ASSERT_EQ(n, ADD); - } TEST_F(HasStateBlocksTest, Notifications_makeKF_add) @@ -123,12 +110,10 @@ TEST_F(HasStateBlocksTest, Notifications_makeKF_add) ASSERT_TRUE(problem->getStateBlockNotification(sbv1, n)); ASSERT_EQ(n, ADD); - } TEST_F(HasStateBlocksTest, Add_solve_notify_solve_add) { - SolverCeresPtr solver = std::make_shared<SolverCeres>(problem); @@ -166,7 +151,6 @@ TEST_F(HasStateBlocksTest, Add_solve_notify_solve_add) // Add again the same SB. This should crash ASSERT_DEATH( F0->addStateBlock('V', sbv0, nullptr) , "" ); - } TEST_F(HasStateBlocksTest, hasStateBlock) @@ -189,7 +173,7 @@ TEST_F(HasStateBlocksTest, getState_structure) { F0->addStateBlock('V', sbv0, nullptr); // now KF0 is POV - WOLF_DEBUG("Retrieving state from F0 with structure ", F0->getStructure()); + WOLF_DEBUG("Retrieving state from F0 with structure ", F0->getKeys()); auto state0 = F0->getState(); WOLF_DEBUG("getState() = ", state0); @@ -219,10 +203,8 @@ TEST_F(HasStateBlocksTest, getState_structure) ASSERT_TRUE(state0.count('O')); ASSERT_FALSE(state0.count('V')); ASSERT_FALSE(state0.count('W')); - } - int main(int argc, char **argv) { testing::InitGoogleTest(&argc, argv); diff --git a/test/gtest_motion_provider.cpp b/test/gtest_motion_provider.cpp index b63823360fb0ed81441108927e5feb3d2947baf1..b8434ef925848e9203b9df3e76d14b8fd6d2ac86 100644 --- a/test/gtest_motion_provider.cpp +++ b/test/gtest_motion_provider.cpp @@ -54,41 +54,38 @@ class MotionProviderTest : public testing::Test problem = Problem::create("POV", 2); // Install odom sensor - sen = problem->installSensor("SensorOdom", - "odometer", - wolf_root + "/test/yaml/sensor_odom_2d.yaml"); + sen = problem->installSensor("SensorOdom2d", + wolf_root + "/test/yaml/sensor_odom_2d.yaml", + {wolf_root}); // Install 3 odom processors ParamsMotionProviderDummyPtr prc1_params = std::make_shared<ParamsMotionProviderDummy>(); prc1_params->time_tolerance = dt/2; - prc1_params->state_structure = "PO"; + prc1_params->state_types = {{'P',"StatePoint2d"},{'O',"StateAngle"}}; prc1_params->state_provider = false; - prc1 = problem->installProcessor("MotionProviderDummy", - "not getter processor", - sen, - prc1_params); + prc1_params->name = "not getter processor"; + + prc1 = ProcessorBase::emplace<MotionProviderDummy>(sen, prc1_params); im1 = std::dynamic_pointer_cast<MotionProvider>(prc1); ParamsMotionProviderDummyPtr prc2_params = std::make_shared<ParamsMotionProviderDummy>(); prc2_params->time_tolerance = dt/2; - prc2_params->state_structure = "PO"; + prc2_params->state_types = {{'P',"StatePoint2d"},{'O',"StateAngle"}}; prc2_params->state_provider = true; - prc2_params->state_order = 1; - prc2 = problem->installProcessor("MotionProviderDummy", - "getter processor", - sen, - prc2_params); + prc2_params->state_provider_order = 1; + prc1_params->name = "getter processor"; + + prc2 = ProcessorBase::emplace<MotionProviderDummy>(sen, prc2_params); im2 = std::dynamic_pointer_cast<MotionProvider>(prc2); ParamsMotionProviderDummyPtr prc3_params = std::make_shared<ParamsMotionProviderDummy>(); prc3_params->time_tolerance = dt/2; - prc3_params->state_structure = "POV"; + prc3_params->state_types = {{'P',"StatePoint2d"},{'O',"StateAngle"},{'V',"StateVector2d"}}; prc3_params->state_provider = true; - prc3_params->state_order = 1; - prc3 = problem->installProcessor("MotionProviderDummy", - "getter processor low priority", - sen, - prc3_params); + prc3_params->state_provider_order = 1; + prc1_params->name = "getter processor lower priority"; + + prc3 = ProcessorBase::emplace<MotionProviderDummy>(sen, prc3_params); im3 = std::dynamic_pointer_cast<MotionProvider>(prc3); } }; @@ -119,9 +116,15 @@ TEST_F(MotionProviderTest, install) ASSERT_TRUE(im3->isStateGetter()); ASSERT_EQ(im2->getOrder(), 1); ASSERT_EQ(im3->getOrder(), 2); // If duplicated priority, 2nd is changed to +1 priority. A WARN should be raised. - ASSERT_EQ(im1->getStateStructure(), "PO"); - ASSERT_EQ(im2->getStateStructure(), "PO"); - ASSERT_EQ(im3->getStateStructure(), "POV"); + ASSERT_TRUE(im1->getStateTypes().has('P')); + ASSERT_TRUE(im1->getStateTypes().has('O')); + ASSERT_FALSE(im1->getStateTypes().has('V')); + ASSERT_TRUE(im2->getStateTypes().has('P')); + ASSERT_TRUE(im2->getStateTypes().has('O')); + ASSERT_FALSE(im2->getStateTypes().has('V')); + ASSERT_TRUE(im3->getStateTypes().has('P')); + ASSERT_TRUE(im3->getStateTypes().has('O')); + ASSERT_TRUE(im3->getStateTypes().has('V')); // Only 2 and 3 in problem::motion_provider_map_ ASSERT_EQ(problem->getMotionProviderMap().size(), 2); @@ -131,12 +134,8 @@ TEST_F(MotionProviderTest, install) TEST_F(MotionProviderTest, odometry) { - VectorComposite odom_p("P"); // missing P key - odom_p['P'] = Vector2d::Zero(); - VectorComposite odom_pov("POV"); // key V not needed by prc1 and prc2 - odom_pov['P'] = Vector2d::Zero(); - odom_pov['O'] = Vector1d::Zero(); - odom_pov['V'] = Vector2d::Zero(); + VectorComposite odom_p("P",{Vector2d::Zero()}); + VectorComposite odom_pov("POV",{Vector2d::Zero(),Vector1d::Zero(),Vector2d::Zero()}); // Error: required PO keys to be added ASSERT_DEATH({im1->setOdometry(odom_p);},""); @@ -145,9 +144,7 @@ TEST_F(MotionProviderTest, odometry) im3->setOdometry(odom_pov); // im1 ->set odom = 0, 0, 0 - VectorComposite odom1("PO"); - odom1['P'] = Vector2d::Zero(); - odom1['O'] = Vector1d::Zero(); + VectorComposite odom1("PO",{Vector2d::Zero(),Vector1d::Zero()}); im1->setOdometry(odom1); auto odom1_get = im1->getOdometry(); EXPECT_TRUE(odom1_get.count('P') == 1); @@ -156,9 +153,7 @@ TEST_F(MotionProviderTest, odometry) EXPECT_MATRIX_APPROX(odom1_get.at('O'), odom1.at('O'), 1e-9); // im1 ->set odom = 1, 1, 1 - VectorComposite odom2("PO"); - odom2['P'] = Vector2d::Ones(); - odom2['O'] = Vector1d::Ones(); + VectorComposite odom2("PO",{Vector2d::Ones(),Vector1d::Ones()}); im2->setOdometry(odom2); auto odom2_get = im2->getOdometry(); EXPECT_TRUE(odom2_get.count('P') == 1); @@ -167,10 +162,7 @@ TEST_F(MotionProviderTest, odometry) EXPECT_MATRIX_APPROX(odom2_get.at('O'), odom2.at('O'), 1e-9); // im1 ->set odom = 2, 2, 2, 2, 2 - VectorComposite odom3("POV"); - odom3['P'] = 2 * Vector2d::Ones(); - odom3['O'] = 2 * Vector1d::Ones(); - odom3['V'] = 2 * Vector2d::Ones(); + VectorComposite odom3("POV",{2 * Vector2d::Ones(), 2 * Vector1d::Ones(), 2 * Vector2d::Ones()}); im3->setOdometry(odom3); auto odom3_get = im3->getOdometry(); EXPECT_TRUE(odom3_get.count('P') == 1); diff --git a/test/gtest_prior_sensor.cpp b/test/gtest_prior_sensor.cpp index 53bda0b3e1379939454150e52a6e162e4b29e29e..c6a6256990957c0cd7081144c378855f78f9c914 100644 --- a/test/gtest_prior_sensor.cpp +++ b/test/gtest_prior_sensor.cpp @@ -97,101 +97,45 @@ TEST(SpecStateSensor, P) std::vector<PriorAsStruct> setups_ok, setups_death; // Initial guess - not dynamic - setups_ok .push_back(PriorAsStruct({"P","initial_guess",vector2,vector0,false})); - setups_ok .push_back(PriorAsStruct({"P","initial_guess",vector3,vector0,false})); - setups_death.push_back(PriorAsStruct({"P","initial_guess",vector4,vector0,false})); // wrong state size + setups_ok .push_back(PriorAsStruct({"StatePoint2d","initial_guess",vector2,vector0,false})); + setups_ok .push_back(PriorAsStruct({"StatePoint3d","initial_guess",vector3,vector0,false})); + setups_death.push_back(PriorAsStruct({"StatePoint3d","initial_guess",vector4,vector0,false})); // wrong state size // Initial guess - dynamic - setups_ok .push_back(PriorAsStruct({"P","initial_guess",vector2,vector0,true})); - setups_ok .push_back(PriorAsStruct({"P","initial_guess",vector3,vector0,true,vector0})); - setups_ok .push_back(PriorAsStruct({"P","initial_guess",vector2,vector0,true,vector2})); - setups_ok .push_back(PriorAsStruct({"P","initial_guess",vector3,vector0,true,vector3})); - setups_death.push_back(PriorAsStruct({"P","initial_guess",vector4,vector0,true,vector4})); // wrong state size - setups_death.push_back(PriorAsStruct({"P","initial_guess",vector3,vector0,true,vector4})); // inconsistent size + setups_ok .push_back(PriorAsStruct({"StatePoint2d","initial_guess",vector2,vector0,true})); + setups_ok .push_back(PriorAsStruct({"StatePoint3d","initial_guess",vector3,vector0,true,vector0})); + setups_ok .push_back(PriorAsStruct({"StatePoint2d","initial_guess",vector2,vector0,true,vector2})); + setups_ok .push_back(PriorAsStruct({"StatePoint3d","initial_guess",vector3,vector0,true,vector3})); + setups_death.push_back(PriorAsStruct({"StatePoint3d","initial_guess",vector4,vector0,true,vector4})); // wrong state size + setups_death.push_back(PriorAsStruct({"StatePoint3d","initial_guess",vector3,vector0,true,vector4})); // inconsistent size // Fix - not dynamic - setups_ok .push_back(PriorAsStruct({"P","fix",vector2,vector0,false})); - setups_ok .push_back(PriorAsStruct({"P","fix",vector3,vector0,false})); - setups_death.push_back(PriorAsStruct({"P","fix",vector4,vector0,false})); // wrong size + setups_ok .push_back(PriorAsStruct({"StatePoint2d","fix",vector2,vector0,false})); + setups_ok .push_back(PriorAsStruct({"StatePoint3d","fix",vector3,vector0,false})); + setups_death.push_back(PriorAsStruct({"StatePoint2d","fix",vector4,vector0,false})); // wrong size // Fix - dynamic - setups_ok .push_back(PriorAsStruct({"P","fix",vector2,vector0,true,vector0})); - setups_ok .push_back(PriorAsStruct({"P","fix",vector3,vector0,true})); - setups_ok .push_back(PriorAsStruct({"P","fix",vector2,vector0,true,vector2})); - setups_ok .push_back(PriorAsStruct({"P","fix",vector3,vector0,true,vector3})); - setups_death.push_back(PriorAsStruct({"P","fix",vector4,vector0,true,vector4})); // wrong state size - setups_death.push_back(PriorAsStruct({"P","fix",vector3,vector0,true,vector4})); // inconsistent size + setups_ok .push_back(PriorAsStruct({"StatePoint2d","fix",vector2,vector0,true,vector0})); + setups_ok .push_back(PriorAsStruct({"StatePoint3d","fix",vector3,vector0,true})); + setups_ok .push_back(PriorAsStruct({"StatePoint2d","fix",vector2,vector0,true,vector2})); + setups_ok .push_back(PriorAsStruct({"StatePoint3d","fix",vector3,vector0,true,vector3})); + setups_death.push_back(PriorAsStruct({"StatePoint3d","fix",vector4,vector0,true,vector4})); // wrong state size + setups_death.push_back(PriorAsStruct({"StatePoint3d","fix",vector3,vector0,true,vector4})); // inconsistent size // Factor - not dynamic - setups_ok .push_back(PriorAsStruct({"P","factor",vector2,vector2,false})); - setups_ok .push_back(PriorAsStruct({"P","factor",vector3,vector3,false})); - setups_death.push_back(PriorAsStruct({"P","factor",vector4,vector4,false})); // wrong state size - setups_death.push_back(PriorAsStruct({"P","factor",vector2,vector3,false})); // inconsistent size + setups_ok .push_back(PriorAsStruct({"StatePoint2d","factor",vector2,vector2,false})); + setups_ok .push_back(PriorAsStruct({"StatePoint3d","factor",vector3,vector3,false})); + setups_death.push_back(PriorAsStruct({"StatePoint3d","factor",vector4,vector4,false})); // wrong state size + setups_death.push_back(PriorAsStruct({"StatePoint2d","factor",vector2,vector3,false})); // inconsistent size // Factor - dynamic - setups_ok .push_back(PriorAsStruct({"P","factor",vector2,vector2,true})); - setups_ok .push_back(PriorAsStruct({"P","factor",vector3,vector3,true})); - setups_ok .push_back(PriorAsStruct({"P","factor",vector2,vector2,true,vector2})); - setups_ok .push_back(PriorAsStruct({"P","factor",vector3,vector3,true,vector3})); - setups_death.push_back(PriorAsStruct({"P","factor",vector4,vector4,true,vector4})); // wrong state size - setups_death.push_back(PriorAsStruct({"P","factor",vector3,vector3,true,vector4})); // inconsistent size - setups_death.push_back(PriorAsStruct({"P","factor",vector3,vector4,true,vector3})); // inconsistent size - - // TEST SETUPS - testPriorSensorSensorMap(setups_ok, true); - testPriorSensorSensorMap(setups_death, false); -} - -TEST(SpecStateSensor, O) -{ - std::vector<PriorAsStruct> setups_ok, setups_death; - - // Initial guess - not dynamic - setups_ok .push_back(PriorAsStruct({"O","initial_guess",vector1,vector0,false})); - setups_ok .push_back(PriorAsStruct({"O","initial_guess",vectorq,vector0,false})); - setups_death.push_back(PriorAsStruct({"O","initial_guess",vector4,vector0,false})); // not normalized - setups_death.push_back(PriorAsStruct({"O","initial_guess",vector3,vector0,false})); // wrong size - - // Initial guess - dynamic - setups_ok .push_back(PriorAsStruct({"O","initial_guess",vector1,vector0,true})); - setups_ok .push_back(PriorAsStruct({"O","initial_guess",vectorq,vector0,true,vector0})); - setups_ok .push_back(PriorAsStruct({"O","initial_guess",vector1,vector0,true,vector1})); - setups_ok .push_back(PriorAsStruct({"O","initial_guess",vectorq,vector0,true,vector3})); - setups_death.push_back(PriorAsStruct({"O","initial_guess",vector4,vector0,true,vector3})); // not normalized - setups_death.push_back(PriorAsStruct({"O","initial_guess",vector3,vector0,true,vector3})); // wrong size - setups_death.push_back(PriorAsStruct({"O","initial_guess",vectorq,vector0,true,vector4})); // inconsistent size - - // Fix - not dynamic - setups_ok .push_back(PriorAsStruct({"O","fix",vector1,vector0,false})); - setups_ok .push_back(PriorAsStruct({"O","fix",vectorq,vector0,false})); - setups_death.push_back(PriorAsStruct({"O","fix",vector3,vector0,false})); // wrong size - setups_death.push_back(PriorAsStruct({"O","fix",vector4,vector0,false})); // not normalized - - // Fix - dynamic - setups_ok .push_back(PriorAsStruct({"O","fix",vector1,vector0,true,vector0})); - setups_ok .push_back(PriorAsStruct({"O","fix",vectorq,vector0,true})); - setups_ok .push_back(PriorAsStruct({"O","fix",vector1,vector0,true,vector1})); - setups_ok .push_back(PriorAsStruct({"O","fix",vectorq,vector0,true,vector3})); - setups_death.push_back(PriorAsStruct({"O","fix",vector4,vector0,true,vector3})); // not normalized - setups_death.push_back(PriorAsStruct({"O","fix",vector3,vector0,true,vector3})); // wrong size - setups_death.push_back(PriorAsStruct({"O","fix",vectorq,vector0,true,vector4})); // inconsistent size - - // Factor - not dynamic - setups_ok .push_back(PriorAsStruct({"O","factor",vector1,vector1,false})); - setups_ok .push_back(PriorAsStruct({"O","factor",vectorq,vector3,false})); - setups_death.push_back(PriorAsStruct({"O","factor",vector4,vector3,false})); // not normalized - setups_death.push_back(PriorAsStruct({"O","factor",vector3,vector3,false})); // wrong size - setups_death.push_back(PriorAsStruct({"O","factor",vectorq,vector4,false})); // inconsistent size - - // Factor - dynamic - setups_ok .push_back(PriorAsStruct({"O","factor",vector1,vector1,true})); - setups_ok .push_back(PriorAsStruct({"O","factor",vectorq,vector3,true})); - setups_ok .push_back(PriorAsStruct({"O","factor",vector1,vector1,true,vector1})); - setups_ok .push_back(PriorAsStruct({"O","factor",vectorq,vector3,true,vector3})); - setups_death.push_back(PriorAsStruct({"O","factor",vector4,vector3,true,vector3})); // not normalized - setups_death.push_back(PriorAsStruct({"O","factor",vector3,vector3,true,vector3})); // wrong size - setups_death.push_back(PriorAsStruct({"O","factor",vectorq,vector3,true,vector4})); // inconsistent size - setups_death.push_back(PriorAsStruct({"O","factor",vectorq,vector4,true,vector3})); // inconsistent size + setups_ok .push_back(PriorAsStruct({"StatePoint2d","factor",vector2,vector2,true})); + setups_ok .push_back(PriorAsStruct({"StatePoint3d","factor",vector3,vector3,true})); + setups_ok .push_back(PriorAsStruct({"StatePoint2d","factor",vector2,vector2,true,vector2})); + setups_ok .push_back(PriorAsStruct({"StatePoint3d","factor",vector3,vector3,true,vector3})); + setups_death.push_back(PriorAsStruct({"StatePoint3d","factor",vector4,vector4,true,vector4})); // wrong state size + setups_death.push_back(PriorAsStruct({"StatePoint3d","factor",vector3,vector3,true,vector4})); // inconsistent size + setups_death.push_back(PriorAsStruct({"StatePoint3d","factor",vector3,vector4,true,vector3})); // inconsistent size // TEST SETUPS testPriorSensorSensorMap(setups_ok, true); diff --git a/test/gtest_problem.cpp b/test/gtest_problem.cpp index 1b65d7c045617e2124668eb69f6717a82bedfb6c..2fbf543f033f9a2efee6959e57d1d3347fca89fa 100644 --- a/test/gtest_problem.cpp +++ b/test/gtest_problem.cpp @@ -19,12 +19,6 @@ // along with this program. If not, see <http://www.gnu.org/licenses/>. // //--------LICENSE_END-------- -/* - * gtest_problem.cpp - * - * Created on: Nov 23, 2016 - * Author: jsola - */ #include "core/utils/utils_gtest.h" @@ -68,7 +62,8 @@ TEST(Problem, create) ASSERT_EQ(P, P->getMap()->getProblem()); // check frame structure through the state size - ASSERT_EQ(P->getFrameStructure(), "POV"); + ASSERT_EQ(P->getFrameKeys().size(), 3); + ASSERT_TRUE(P->getFrameTypes().has("POV")); } TEST(Problem, Sensors) @@ -80,8 +75,8 @@ TEST(Problem, Sensors) params->name = "dummy_name"; auto S = SensorBase::emplace<SensorDummy3d>(P->getHardware(), params, - SpecSensorComposite({{'P',SpecStateSensor("P",Vector3d::Zero())}, - {'O',SpecStateSensor("O",Vector4d::Random().normalized())}})); + SpecSensorComposite({{'P',SpecStateSensor("StatePoint3d",Vector3d::Zero())}, + {'O',SpecStateSensor("StateQuaternion",Vector4d::Random().normalized())}})); // check pointers ASSERT_EQ(P, S->getProblem()); ASSERT_EQ(P->getHardware(), S->getHardware()); @@ -100,8 +95,8 @@ TEST(Problem, Processor) params->name = "dummy_name"; auto Sm = SensorBase::emplace<SensorOdom3d>(P->getHardware(), params, - SpecSensorComposite({{'P',SpecStateSensor("P",Vector3d::Zero())}, - {'O',SpecStateSensor("O",Vector4d::Random().normalized())}})); + SpecSensorComposite({{'P',SpecStateSensor("StatePoint3d",Vector3d::Zero())}, + {'O',SpecStateSensor("StateQuaternion",Vector4d::Random().normalized())}})); // add motion processor auto Pm = ProcessorBase::emplace<ProcessorOdom3d>(Sm, std::make_shared<ParamsProcessorOdom3d>()); @@ -116,16 +111,24 @@ TEST(Problem, Installers) ProblemPtr P = Problem::create("PO", 3); Eigen::Vector7d xs; xs.setRandom(); xs.tail(4).normalize(); - SensorBasePtr S = P->installSensor("SensorOdom3d", wolf_root + "/test/yaml/sensor_odom_3d.yaml", {wolf_root}); + SensorBasePtr S = P->installSensor("SensorOdom3d", + wolf_root + "/test/yaml/sensor_odom_3d.yaml", + {wolf_root}); // install processor tracker (dummy installation under an Odometry sensor -- it's OK for this test) - auto pt = P->installProcessor("ProcessorTrackerFeatureDummy", S->getName(), wolf_root + "/test/yaml/processor_tracker_feature_dummy.yaml", {wolf_root}); + auto pt = P->installProcessor("ProcessorTrackerFeatureDummy", + S->getName(), + wolf_root + "/test/yaml/processor_tracker_feature_dummy.yaml", + {wolf_root}); // check motion processor IS NOT set ASSERT_TRUE(P->getMotionProviderMap().empty()); // install processor motion - ProcessorBasePtr pm = P->installProcessor("ProcessorOdom3d", S->getName(), wolf_root + "/test/yaml/processor_odom_3d.yaml", {wolf_root}); + ProcessorBasePtr pm = P->installProcessor("ProcessorOdom3d", + S->getName(), + wolf_root + "/test/yaml/processor_odom_3d.yaml", + {wolf_root}); // check motion processor is set ASSERT_FALSE(P->getMotionProviderMap().empty()); @@ -139,8 +142,8 @@ TEST(Problem, SetOrigin_PO_2d) ProblemPtr P = Problem::create("PO", 2); TimeStamp t0(0); SpecComposite prior; - prior.emplace('P',SpecState("P",Vector2d(1,2),"factor",Vector2d(sqrt(0.1),sqrt(0.1)))); - prior.emplace('O',SpecState("O",Vector1d(3),"factor",Vector1d(sqrt(0.1)))); + prior.emplace('P',SpecState("StatePoint2d",Vector2d(1,2),"factor",Vector2d(sqrt(0.1),sqrt(0.1)))); + prior.emplace('O',SpecState("StateAngle",Vector1d(3),"factor",Vector1d(sqrt(0.1)))); P->setPrior(prior, t0); // P->setPriorFactor(x0, s0, t0); @@ -198,8 +201,8 @@ TEST(Problem, SetOrigin_PO_3d) ProblemPtr P = Problem::create("PO", 3); TimeStamp t0(0); SpecComposite prior; - prior.emplace('P',SpecState("P",Vector3d(1,2,3),"factor",Vector3d(sqrt(0.1),sqrt(0.1),sqrt(0.1)))); - prior.emplace('O',SpecState("O",Vector4d(4,5,6,7).normalized(),"factor",Vector3d(sqrt(0.1),sqrt(0.1),sqrt(0.1)))); + prior.emplace('P',SpecState("StatePoint3d",Vector3d(1,2,3),"factor",Vector3d::Constant(sqrt(0.1)))); + prior.emplace('O',SpecState("StateQuaternion",Vector4d(4,5,6,7).normalized(),"factor",Vector3d::Constant(sqrt(0.1)))); P->setPrior(prior, t0); // Eigen::VectorXd vec7(7); vec7 << 1,2,3,4,5,6,7; // vec7.tail(4).normalize(); @@ -257,35 +260,55 @@ TEST(Problem, SetOrigin_PO_3d) // P->print(4,1,1,1); } -TEST(Problem, emplaceFrame_factory) +TEST(Problem, emplaceFrame_factory_2D) { - ProblemPtr P = Problem::create("PO", 2); + Vector3d xpo; + Vector5d xpov; + + ProblemPtr P = Problem::create("POV", 2); + FrameBasePtr f1 = P->emplaceFrame(0, "PO" , xpo ); + FrameBasePtr f2 = P->emplaceFrame(1, "POV", xpov ); + + // check that all frames are effectively in the trajectory + ASSERT_EQ(P->getTrajectory()->size(), (SizeStd) 2); + + ASSERT_EQ(f1->getState().size(), 2 ); + ASSERT_EQ(f2->getState().size(), 3 ); + ASSERT_EQ(f1->getStateVector("PO").size(), 3 ); + ASSERT_EQ(f2->getStateVector("POV").size(), 5 ); + - Vector3d xpo2; - Vector7d xpo3; xpo3 << 1,2,3,.5,.5,.5,.5; - VectorXd xpov3(10); xpov3 << 1,2,3,.5,.5,.5,.5,1,2,3; + // check that all frames are linked to Problem + ASSERT_EQ(f1->getProblem(), P); + ASSERT_EQ(f2->getProblem(), P); + + ASSERT_EQ(P->getFrameKeys().size(), 3); + ASSERT_TRUE(P->getFrameTypes().has("POV")); +} - FrameBasePtr f0 = P->emplaceFrame(0, "PO" , 2, xpo2 ); - FrameBasePtr f1 = P->emplaceFrame(1, "PO" , 3, xpo3 ); - FrameBasePtr f2 = P->emplaceFrame(2, "POV", 3, xpov3 ); +TEST(Problem, emplaceFrame_factory_3D) +{ + Vector7d xpo; xpo << 1,2,3,.5,.5,.5,.5; + VectorXd xpov(10); xpov << 1,2,3,.5,.5,.5,.5,1,2,3; + ProblemPtr P = Problem::create("POV", 3); + FrameBasePtr f1 = P->emplaceFrame(0, "PO" , xpo ); + FrameBasePtr f2 = P->emplaceFrame(1, "POV", xpov ); // check that all frames are effectively in the trajectory - ASSERT_EQ(P->getTrajectory()->size(), (SizeStd) 3); + ASSERT_EQ(P->getTrajectory()->size(), (SizeStd) 2); - ASSERT_EQ(f0->getState().size(), 2 ); ASSERT_EQ(f1->getState().size(), 2 ); ASSERT_EQ(f2->getState().size(), 3 ); - ASSERT_EQ(f0->getStateVector("PO").size(), 3 ); ASSERT_EQ(f1->getStateVector("PO").size(), 7 ); ASSERT_EQ(f2->getStateVector("POV").size(), 10 ); // check that all frames are linked to Problem - ASSERT_EQ(f0->getProblem(), P); ASSERT_EQ(f1->getProblem(), P); ASSERT_EQ(f2->getProblem(), P); - ASSERT_EQ(P->getFrameStructure(), "POV"); + ASSERT_EQ(P->getFrameKeys().size(), 3); + ASSERT_TRUE(P->getFrameTypes().has("POV")); } TEST(Problem, StateBlocks) @@ -297,14 +320,22 @@ TEST(Problem, StateBlocks) Eigen::Vector3d xs2d; // 2 state blocks, fixed - SensorBasePtr Sm = P->installSensor("SensorOdom3d", wolf_root + "/test/yaml/sensor_odom_3d.yaml", {wolf_root}); + SensorBasePtr Sm = P->installSensor("SensorOdom3d", + wolf_root + "/test/yaml/sensor_odom_3d.yaml", + {wolf_root}); ASSERT_EQ(P->getStateBlockNotificationMapSize(), (SizeStd) 2); - auto pt = P->installProcessor("ProcessorTrackerFeatureDummy", Sm->getName(), wolf_root + "/test/yaml/processor_tracker_feature_dummy.yaml", {wolf_root}); - auto pm = P->installProcessor("ProcessorOdom3d", Sm->getName(), wolf_root + "/test/yaml/processor_odom_3d.yaml", {wolf_root}); + auto pt = P->installProcessor("ProcessorTrackerFeatureDummy", + Sm->getName(), + wolf_root + "/test/yaml/processor_tracker_feature_dummy.yaml", + {wolf_root}); + auto pm = P->installProcessor("ProcessorOdom3d", + Sm->getName(), + wolf_root + "/test/yaml/processor_odom_3d.yaml", + {wolf_root}); // 2 state blocks, estimated - auto KF = P->emplaceFrame(0, "PO", 3, xs3d ); + auto KF = P->emplaceFrame(0, "PO", xs3d ); ASSERT_EQ(P->getStateBlockNotificationMapSize(), (SizeStd)(2 + 2)); // Notifications @@ -319,7 +350,7 @@ TEST(Problem, StateBlocks) Sm->unfixExtrinsics(); ASSERT_EQ(P->getStateBlockNotificationMapSize(), (SizeStd)(2 + 2)); // changes in state_blocks status (fix/state/localparam) does not raise a notification in problem, only ADD/REMOVE - P->print(4,1,1,1); + P->print(4,1,1,1); // consume notifications SolverManagerDummyPtr SM = std::make_shared<SolverManagerDummy>(P); @@ -350,8 +381,12 @@ TEST(Problem, Covariances) Eigen::Vector3d xs2d; - SensorBasePtr Sm = P->installSensor("SensorOdom3d", wolf_root + "/test/yaml/sensor_odom_3d.yaml", {wolf_root}); - SensorBasePtr St = P->installSensor("SensorOdom3d", wolf_root + "/test/yaml/sensor_odom_3d_other.yaml", {wolf_root}); + SensorBasePtr Sm = P->installSensor("SensorOdom3d", + wolf_root + "/test/yaml/sensor_odom_3d.yaml", + {wolf_root}); + SensorBasePtr St = P->installSensor("SensorOdom3d", + wolf_root + "/test/yaml/sensor_odom_3d_other.yaml", + {wolf_root}); ParamsProcessorTrackerFeaturePtr params = std::make_shared<ParamsProcessorTrackerFeature>(); params->time_tolerance = 0.1; @@ -359,12 +394,18 @@ TEST(Problem, Covariances) params->min_features_for_keyframe = 10; - auto pt = P->installProcessor("ProcessorTrackerFeatureDummy", Sm->getName(), wolf_root + "/test/yaml/processor_tracker_feature_dummy.yaml", {wolf_root}); - auto pm = P->installProcessor("ProcessorOdom3d", St->getName(), wolf_root + "/test/yaml/processor_odom_3d.yaml", {wolf_root}); + auto pt = P->installProcessor("ProcessorTrackerFeatureDummy", + Sm->getName(), + wolf_root + "/test/yaml/processor_tracker_feature_dummy.yaml", + {wolf_root}); + auto pm = P->installProcessor("ProcessorOdom3d", + St->getName(), + wolf_root + "/test/yaml/processor_odom_3d.yaml", + {wolf_root}); // 4 state blocks, estimated St->unfixExtrinsics(); - FrameBasePtr F = P->emplaceFrame(0, "PO", 3, xs3d ); + FrameBasePtr F = P->emplaceFrame(0, "PO", xs3d ); // set covariance (they are not computed without a solver) P->addCovarianceBlock(F->getP(), Eigen::Matrix3d::Identity()); @@ -391,16 +432,16 @@ TEST(Problem, perturb) param->ticks_per_wheel_revolution = 100; auto sensor = SensorBase::emplace<SensorDiffDrive>(problem->getHardware(), param, - SpecSensorComposite({{'P',SpecStateSensor("P",Vector2d::Zero())}, - {'O',SpecStateSensor("O",Vector1d::Zero())}, - {'I',SpecStateSensor("StateParams3",Vector3d::Ones(),"initial_guess", VectorXd(0),true)}})); + SpecSensorComposite({{'P',SpecStateSensor("StatePoint2d",Vector2d::Zero())}, + {'O',SpecStateSensor("StateAngle",Vector1d::Zero())}, + {'I',SpecStateSensor("StateParams3",Vector3d::Ones(),"initial_guess", VectorXd(0),true)}})); Vector3d pose; pose << 0,0,0; int i = 0; for (TimeStamp t = 0.0; t < 2.0; t += 1.0) { - auto F = problem->emplaceFrame(t, pose ); + auto F = problem->emplaceFrame(t, "PO", pose ); if (i==0) F->fix(); for (int j = 0; j< 2 ; j++) @@ -477,15 +518,15 @@ TEST(Problem, check) param->ticks_per_wheel_revolution = 100; auto sensor = SensorBase::emplace<SensorDiffDrive>(problem->getHardware(), param, - SpecSensorComposite({{'P',SpecStateSensor("P",Vector2d::Zero())}, - {'O',SpecStateSensor("O",Vector1d::Zero())}, - {'I',SpecStateSensor("StateParams3",Vector3d::Ones(),"initial_guess", VectorXd(0),true)}})); + SpecSensorComposite({{'P',SpecStateSensor("StatePoint2d",Vector2d::Zero())}, + {'O',SpecStateSensor("StateAngle",Vector1d::Zero())}, + {'I',SpecStateSensor("StateParams3",Vector3d::Ones(),"initial_guess", VectorXd(0),true)}})); Vector3d pose; pose << 0,0,0; int i = 0; for (TimeStamp t = 0.0; t < 2.0; t += 1.0) { - auto F = problem->emplaceFrame(t, pose); + auto F = problem->emplaceFrame(t, "PO", pose); if (i==0) F->fix(); for (int j = 0; j< 2 ; j++) diff --git a/test/gtest_processor_base.cpp b/test/gtest_processor_base.cpp index 1c484638d4617173a7906a571cf865c2656f9d5c..859cd66471849d82fb9fcef97e1af2cbe90034a7 100644 --- a/test/gtest_processor_base.cpp +++ b/test/gtest_processor_base.cpp @@ -123,8 +123,8 @@ TEST(ProcessorBase, KeyFrameCallback) // initialize TimeStamp t(0.0); - SpecComposite prior{{'P',SpecState("P",Vector2d(0,0),"factor",Vector2d(sqrt(0.1),sqrt(0.1)))}, - {'O',SpecState("O",Vector1d(0),"factor",Vector1d(sqrt(0.1)))}}; + SpecComposite prior{{'P',SpecState("StatePoint2d",Vector2d(0,0),"factor",Vector2d(sqrt(0.1),sqrt(0.1)))}, + {'O',SpecState("StateAngle",Vector1d(0),"factor",Vector1d(sqrt(0.1)))}}; problem->setPrior(prior, t); CaptureOdom2dPtr capt_odo = make_shared<CaptureOdom2d>(t, sens_odo, Vector2d(0.5,0)); @@ -156,7 +156,9 @@ TEST(ProcessorBase, KeyFrameCallback) std::cout << "6\n"; // Only odom creating KFs - ASSERT_TRUE( problem->getLastFrame()->getStructure().compare("PO")==0 ); + ASSERT_EQ( problem->getLastFrame()->getKeys().size(),2); + ASSERT_TRUE( problem->getLastFrame()->getKeys().find('P') != std::string::npos); + ASSERT_TRUE( problem->getLastFrame()->getKeys().find('O') != std::string::npos); } } diff --git a/test/gtest_processor_motion.cpp b/test/gtest_processor_motion.cpp index 443e4f1edd4e02ea85e849476c34cc7e8ada6423..be7edb760e57dc52efd41a7ba54663add1c6c576 100644 --- a/test/gtest_processor_motion.cpp +++ b/test/gtest_processor_motion.cpp @@ -178,8 +178,8 @@ TEST_F(ProcessorMotion_test, IntegrateStraightFactorPrior) // Prior TimeStamp t(0.0); SpecComposite prior; - prior.emplace('P',SpecState("P",Vector2d(0,0),"factor",Vector2d(1,1))); - prior.emplace('O',SpecState("O",Vector1d(0),"factor",Vector1d(1))); + prior.emplace('P',SpecState("StatePoint2d",Vector2d(0,0),"factor",Vector2d(1,1))); + prior.emplace('O',SpecState("StateAngle",Vector1d(0),"factor",Vector1d(1))); auto KF_0 = problem->setPrior(prior, t); processor->setOrigin(KF_0); @@ -203,8 +203,8 @@ TEST_F(ProcessorMotion_test, IntegrateStraightFixPrior) { // Prior TimeStamp t(0.0); - SpecComposite prior{{'P',SpecState("P",Vector2d(0,0),"fix")}, - {'O',SpecState("O",Vector1d(0),"fix")}}; + SpecComposite prior{{'P',SpecState("StatePoint2d",Vector2d(0,0),"fix")}, + {'O',SpecState("StateAngle",Vector1d(0),"fix")}}; auto KF_0 = problem->setPrior(prior, t); processor->setOrigin(KF_0); @@ -251,8 +251,8 @@ TEST_F(ProcessorMotion_test, IntegrateCircleFactorPrior) { // Prior TimeStamp t(0.0); - SpecComposite prior{{'P',SpecState("P",Vector2d(0,0),"factor",Vector2d(1,1))}, - {'O',SpecState("O",Vector1d(0),"factor",Vector1d(1))}}; + SpecComposite prior{{'P',SpecState("StatePoint2d",Vector2d(0,0),"factor",Vector2d(1,1))}, + {'O',SpecState("StateAngle",Vector1d(0),"factor",Vector1d(1))}}; auto KF_0 = problem->setPrior(prior, t); processor->setOrigin(KF_0); @@ -277,8 +277,8 @@ TEST_F(ProcessorMotion_test, IntegrateCircleFixPrior) // Prior TimeStamp t(0.0); SpecComposite prior; - prior.emplace('P',SpecState("P",Vector2d(0,0),"fix")); - prior.emplace('O',SpecState("O",Vector1d(0),"fix")); + prior.emplace('P',SpecState("StatePoint2d",Vector2d(0,0),"fix")); + prior.emplace('O',SpecState("StateAngle",Vector1d(0),"fix")); auto KF_0 = problem->setPrior(prior, t); processor->setOrigin(KF_0); @@ -418,8 +418,8 @@ TEST_F(ProcessorMotion_test, splitBufferFactorPrior) { // Prior TimeStamp t(0.0); - SpecComposite prior{{'P',SpecState("P",Vector2d(0,0),"factor",Vector2d(1,1))}, - {'O',SpecState("O",Vector1d(0),"factor",Vector1d(1))}}; + SpecComposite prior{{'P',SpecState("StatePoint2d",Vector2d(0,0),"factor",Vector2d(1,1))}, + {'O',SpecState("StateAngle",Vector1d(0),"factor",Vector1d(1))}}; auto KF_0 = problem->setPrior(prior, t); processor->setOrigin(KF_0); @@ -460,8 +460,8 @@ TEST_F(ProcessorMotion_test, splitBufferFixPrior) { // Prior TimeStamp t(0.0); - SpecComposite prior{{'P',SpecState("P",Vector2d(0,0),"fix")}, - {'O',SpecState("O",Vector1d(0),"fix")}}; + SpecComposite prior{{'P',SpecState("StatePoint2d",Vector2d(0,0),"fix")}, + {'O',SpecState("StateAngle",Vector1d(0),"fix")}}; auto KF_0 = problem->setPrior(prior, t); processor->setOrigin(KF_0); diff --git a/test/gtest_trajectory.cpp b/test/gtest_trajectory.cpp index 12b8d2a1a637260a8c7bb47f34fdc682a8a6e1eb..a679c61169dda05d9a53ff096a41f9ad0e009bc1 100644 --- a/test/gtest_trajectory.cpp +++ b/test/gtest_trajectory.cpp @@ -55,10 +55,10 @@ TEST(TrajectoryBase, ClosestKeyFrame) FrameBasePtr F1 = P->emplaceFrame( 1, Eigen::Vector3d::Zero() ); FrameBasePtr F2 = P->emplaceFrame( 2, Eigen::Vector3d::Zero() ); - FrameBasePtr F3 = std::make_shared<FrameBase>(3, P->getFrameStructure(), + FrameBasePtr F3 = std::make_shared<FrameBase>(3, P->getFrameKeys(), // P->getDim(), std::list<VectorXd>({Eigen::Vector2d::Zero(),Eigen::Vector1d::Zero()}) ); - FrameBasePtr F4 = std::make_shared<FrameBase>(4, P->getFrameStructure(), + FrameBasePtr F4 = std::make_shared<FrameBase>(4, P->getFrameKeys(), // P->getDim(), std::list<VectorXd>({Eigen::Vector2d::Zero(),Eigen::Vector1d::Zero()}) ); @@ -114,7 +114,7 @@ TEST(TrajectoryBase, Add_Remove_Frame) std::cout << __LINE__ << std::endl; // add F3 - FrameBasePtr F3 = std::make_shared<FrameBase>(3, P->getFrameStructure(), + FrameBasePtr F3 = std::make_shared<FrameBase>(3, P->getFrameKeys(), // P->getDim(), std::list<VectorXd>({Eigen::Vector2d::Zero(),Eigen::Vector1d::Zero()})); if (debug) P->print(2,0,0,0); diff --git a/test/yaml/params_problem_autosetup.yaml b/test/yaml/params_problem_autosetup.yaml index 7c445e2c32fe2de43f48b4f6e36636fa38f26329..0cf74a11958598bee8058f7d4d3117ade6e98a61 100644 --- a/test/yaml/params_problem_autosetup.yaml +++ b/test/yaml/params_problem_autosetup.yaml @@ -51,5 +51,5 @@ processors: unmeasured_perturbation_std: 0.001 - state_getter: true - state_priority: 1 + state_provider: true + state_provider_order: 1 diff --git a/test/yaml/params_problem_autosetup_no_map.yaml b/test/yaml/params_problem_autosetup_no_map.yaml index c0713d0f39d04da8920000e38dfa434950b3e1d9..54d1d63c2725f4af948bc3e72d81782150b34414 100644 --- a/test/yaml/params_problem_autosetup_no_map.yaml +++ b/test/yaml/params_problem_autosetup_no_map.yaml @@ -48,5 +48,5 @@ processors: unmeasured_perturbation_std: 0.00111 - state_getter: true - state_priority: 1 + state_provider: true + state_provider_order: 1 diff --git a/test/yaml/params_problem_odom_3d.yaml b/test/yaml/params_problem_odom_3d.yaml index e038298e42bb24516caff219fd8b3a80ea976ac3..e49632f45e08386f993aed3ec454b56ccad18e86 100644 --- a/test/yaml/params_problem_odom_3d.yaml +++ b/test/yaml/params_problem_odom_3d.yaml @@ -41,5 +41,5 @@ processors: unmeasured_perturbation_std: 0.00111 - state_getter: true + state_provider: true state_priority: 1 diff --git a/test/yaml/params_tree_manager1.yaml b/test/yaml/params_tree_manager1.yaml index f7a110bf8f5ec83efeb186fd4b934c5a5817047b..3e59567cb367b45df3146b97e216df14eef84772 100644 --- a/test/yaml/params_tree_manager1.yaml +++ b/test/yaml/params_tree_manager1.yaml @@ -53,6 +53,6 @@ processors: unmeasured_perturbation_std: 0.00111 - state_getter: true + state_provider: true state_priority: 1 diff --git a/test/yaml/params_tree_manager2.yaml b/test/yaml/params_tree_manager2.yaml index edb6b17b80eda799719190d7d1b2efd6cf4d82af..fb7b93ef57e41f82ba257142873d0334bb1c8430 100644 --- a/test/yaml/params_tree_manager2.yaml +++ b/test/yaml/params_tree_manager2.yaml @@ -53,6 +53,6 @@ processors: unmeasured_perturbation_std: 0.00111 - state_getter: true + state_provider: true state_priority: 1 diff --git a/test/yaml/params_tree_manager_sliding_window1.yaml b/test/yaml/params_tree_manager_sliding_window1.yaml index 22aea41299db11cc1a6ccc326067827d14e525ec..67d349e02ad526a9c71f015337d5685950006882 100644 --- a/test/yaml/params_tree_manager_sliding_window1.yaml +++ b/test/yaml/params_tree_manager_sliding_window1.yaml @@ -48,6 +48,6 @@ processors: unmeasured_perturbation_std: 0.00111 - state_getter: true + state_provider: true state_priority: 1 diff --git a/test/yaml/params_tree_manager_sliding_window2.yaml b/test/yaml/params_tree_manager_sliding_window2.yaml index 09288cfd12247f66f2cc2b3800380d87a9cbd4e0..01a6731f1577f8a46019313b54b4c62cf07ea96a 100644 --- a/test/yaml/params_tree_manager_sliding_window2.yaml +++ b/test/yaml/params_tree_manager_sliding_window2.yaml @@ -48,6 +48,6 @@ processors: unmeasured_perturbation_std: 0.00111 - state_getter: true + state_provider: true state_priority: 1 diff --git a/test/yaml/params_tree_manager_sliding_window_dual_rate3.yaml b/test/yaml/params_tree_manager_sliding_window_dual_rate3.yaml index 0d725631260166ea8aebcc3b89d206638b6532ac..5ff6eff5eee764e306f3077014fb7e6902e5d221 100644 --- a/test/yaml/params_tree_manager_sliding_window_dual_rate3.yaml +++ b/test/yaml/params_tree_manager_sliding_window_dual_rate3.yaml @@ -56,6 +56,6 @@ processors: unmeasured_perturbation_std: 0.00111 - state_getter: true + state_provider: true state_priority: 1 diff --git a/test/yaml/params_tree_manager_sliding_window_dual_rate_baseline.yaml b/test/yaml/params_tree_manager_sliding_window_dual_rate_baseline.yaml index 47e54b3ab04136e768c7479b18d3b09adfa592f8..288f3a9c23cb93f34b3103023a46323adc3c9f7e 100644 --- a/test/yaml/params_tree_manager_sliding_window_dual_rate_baseline.yaml +++ b/test/yaml/params_tree_manager_sliding_window_dual_rate_baseline.yaml @@ -50,5 +50,5 @@ processors: unmeasured_perturbation_std: 0.00111 - state_getter: true + state_provider: true state_priority: 1 diff --git a/test/yaml/processor_odom_3d.yaml b/test/yaml/processor_odom_3d.yaml index 93ec73af0a811d65cf893c7dc9e023333db8ccc7..34ab78f54be873ab3b0b2f28918aed64ff50c00a 100644 --- a/test/yaml/processor_odom_3d.yaml +++ b/test/yaml/processor_odom_3d.yaml @@ -13,5 +13,5 @@ unmeasured_perturbation_std: 0.001 apply_loss_function: false -state_getter: true -state_priority: 1 \ No newline at end of file +state_provider: true +state_provider_order: 1 \ No newline at end of file