From 8059d03624218e8f6d7fbb99d7fd022bb699002a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Joan=20Vallv=C3=A9=20Navarro?= <jvallve@iri.upc.edu> Date: Tue, 8 Nov 2022 17:35:43 +0100 Subject: [PATCH] Renamed SpecComposite to SpecStateComposite --- demos/hello_wolf/sensor_range_bearing.cpp | 2 +- demos/hello_wolf/sensor_range_bearing.h | 2 +- include/core/factor/factor_block_absolute.h | 14 +++++++--- include/core/frame/frame_base.h | 4 +-- include/core/problem/problem.h | 10 +++---- include/core/processor/processor_base.h | 2 +- include/core/sensor/factory_sensor.h | 4 +-- include/core/sensor/sensor_base.h | 8 +++--- include/core/sensor/sensor_diff_drive.h | 2 +- include/core/sensor/sensor_motion_model.h | 2 +- include/core/sensor/sensor_odom.h | 2 +- include/core/sensor/sensor_pose.h | 2 +- include/core/sensor/spec_state_sensor.h | 2 +- include/core/state_block/has_state_blocks.h | 4 +-- include/core/state_block/spec_composite.h | 2 +- include/core/state_block/type_composite.h | 2 +- include/core/state_block/vector_composite.h | 2 +- schema/landmark/LandmarkBase.schema | 27 +++++++++++++++++++ schema/map/MapBase.schema | 10 ++++++- schema/problem/Problem2d.schema | 8 +++++- schema/problem/Problem3d.schema | 8 +++++- schema/problem/SpecStateO2d.schema | 11 -------- schema/problem/SpecStateP2d.schema | 11 -------- schema/problem/SpecStateP3d.schema | 11 -------- schema/problem/SpecStateV2d.schema | 11 -------- schema/problem/SpecStateV3d.schema | 11 -------- schema/processor/MotionProvider.schema | 3 ++- schema/processor/ProcessorMotion.schema | 8 +++--- schema/processor/ProcessorOdom2d.schema | 2 +- schema/processor/ProcessorTracker.schema | 2 +- schema/sensor/SensorDiffDrive.schema | 2 +- schema/sensor/SpecStateSensor.schema | 2 ++ schema/sensor/SpecStateSensorO2d.schema | 7 +++-- schema/sensor/SpecStateSensorO3d.schema | 7 +++-- schema/sensor/SpecStateSensorP2d.schema | 7 +++-- schema/sensor/SpecStateSensorP3d.schema | 7 +++-- schema/solver/SolverCeres.schema | 19 +++++++++---- schema/solver/SolverManager.schema | 9 ++++--- schema/{problem => state}/SpecState.schema | 3 +-- schema/state/SpecStateO2d.schema | 18 +++++++++++++ schema/{problem => state}/SpecStateO3d.schema | 9 ++++++- schema/state/SpecStateP2d.schema | 18 +++++++++++++ schema/state/SpecStateP3d.schema | 18 +++++++++++++ schema/state/SpecStateV2d.schema | 18 +++++++++++++ schema/state/SpecStateV3d.schema | 18 +++++++++++++ schema/tree_manager/NONE.schema | 5 ++++ schema/tree_manager/None.schema | 6 ++++- schema/tree_manager/none.schema | 5 ++++ src/frame/frame_base.cpp | 2 +- src/landmark/landmark_base.cpp | 4 +-- src/problem/problem.cpp | 8 +++--- src/sensor/sensor_base.cpp | 2 +- src/sensor/sensor_motion_model.cpp | 2 +- src/state_block/has_state_blocks.cpp | 6 ++--- test/dummy/sensor_dummy.h | 4 +-- test/gtest_emplace.cpp | 8 +++--- test/gtest_odom_2d.cpp | 6 ++--- test/gtest_param_prior.cpp | 2 +- test/gtest_problem.cpp | 12 ++++----- ...sor_and_factor_pose_3d_with_extrinsics.cpp | 2 +- test/gtest_processor_base.cpp | 2 +- test/gtest_processor_diff_drive.cpp | 8 +++--- test/gtest_processor_motion.cpp | 12 ++++----- test/gtest_sensor_base.cpp | 22 +++++++-------- test/gtest_sensor_odom.cpp | 22 +++++++-------- test/gtest_sensor_pose.cpp | 4 +-- 66 files changed, 307 insertions(+), 188 deletions(-) create mode 100644 schema/landmark/LandmarkBase.schema delete mode 100644 schema/problem/SpecStateO2d.schema delete mode 100644 schema/problem/SpecStateP2d.schema delete mode 100644 schema/problem/SpecStateP3d.schema delete mode 100644 schema/problem/SpecStateV2d.schema delete mode 100644 schema/problem/SpecStateV3d.schema rename schema/{problem => state}/SpecState.schema (94%) create mode 100644 schema/state/SpecStateO2d.schema rename schema/{problem => state}/SpecStateO3d.schema (53%) create mode 100644 schema/state/SpecStateP2d.schema create mode 100644 schema/state/SpecStateP3d.schema create mode 100644 schema/state/SpecStateV2d.schema create mode 100644 schema/state/SpecStateV3d.schema create mode 100644 schema/tree_manager/NONE.schema create mode 100644 schema/tree_manager/none.schema diff --git a/demos/hello_wolf/sensor_range_bearing.cpp b/demos/hello_wolf/sensor_range_bearing.cpp index b7d62c95d..aff44f34d 100644 --- a/demos/hello_wolf/sensor_range_bearing.cpp +++ b/demos/hello_wolf/sensor_range_bearing.cpp @@ -26,7 +26,7 @@ namespace wolf{ WOLF_PTR_TYPEDEFS(SensorRangeBearing); SensorRangeBearing::SensorRangeBearing(ParamsSensorRangeBearingPtr _params, - const SpecSensorComposite& _priors) : + const SpecStateSensorComposite& _priors) : SensorBase("SensorRangeBearing", 2, _params, _priors("PO")), params_rb_(_params) { diff --git a/demos/hello_wolf/sensor_range_bearing.h b/demos/hello_wolf/sensor_range_bearing.h index 69e0ea53c..4b0516b0f 100644 --- a/demos/hello_wolf/sensor_range_bearing.h +++ b/demos/hello_wolf/sensor_range_bearing.h @@ -57,7 +57,7 @@ class SensorRangeBearing : public SensorBase public: SensorRangeBearing(ParamsSensorRangeBearingPtr _params, - const SpecSensorComposite& _priors); + const SpecStateSensorComposite& _priors); WOLF_SENSOR_CREATE(SensorRangeBearing, ParamsSensorRangeBearing); ~SensorRangeBearing() override; diff --git a/include/core/factor/factor_block_absolute.h b/include/core/factor/factor_block_absolute.h index 51806e360..70712879e 100644 --- a/include/core/factor/factor_block_absolute.h +++ b/include/core/factor/factor_block_absolute.h @@ -25,6 +25,7 @@ #include "core/factor/factor_analytic.h" #include "core/factor/factor_autodiff.h" #include "core/frame/frame_base.h" +#include "core/state_block/state_angle.h" namespace wolf { @@ -38,6 +39,7 @@ class FactorBlockAbsolute : public FactorAnalytic SizeEigen sb_constrained_start_; // the index of the first state element that is constrained SizeEigen sb_constrained_size_; // the size of the state segment that is constrained Eigen::MatrixXd J_; // Jacobian + bool is_angle_; // Stateblock is StateAngle public: @@ -64,7 +66,8 @@ class FactorBlockAbsolute : public FactorAnalytic _sb_ptr), sb_size_(_sb_ptr->getSize()), sb_constrained_start_(0), - sb_constrained_size_(sb_size_) + sb_constrained_size_(sb_size_), + is_angle_(std::dynamic_pointer_cast<StateAngle>(_sb_ptr)) { assert(sb_constrained_size_+sb_constrained_start_ <= sb_size_); @@ -99,7 +102,8 @@ class FactorBlockAbsolute : public FactorAnalytic _sb_ptr), sb_size_(_sb_ptr->getSize()), sb_constrained_start_(_start_idx), - sb_constrained_size_(_size == -1 ? sb_size_ : _size) + sb_constrained_size_(_size == -1 ? sb_size_ : _size), + is_angle_(std::dynamic_pointer_cast<StateAngle>(_sb_ptr)) { assert(sb_constrained_size_+sb_constrained_start_ <= sb_size_); @@ -159,8 +163,10 @@ inline Eigen::VectorXd FactorBlockAbsolute::evaluateResiduals(const std::vector< assert(_st_vector.front().size() >= getMeasurement().size() && "Wrong StateBlock size"); // residual - if (sb_constrained_size_ == _st_vector.front().size()) - return getMeasurementSquareRootInformationUpper() * (_st_vector.front() - getMeasurement()); + if (is_angle_) + return getMeasurementSquareRootInformationUpper() * Eigen::Vector1d(pi2pi(_st_vector.front()(0) - getMeasurement()(0))); + else if (sb_constrained_size_ == _st_vector.front().size()) + return getMeasurementSquareRootInformationUpper() * _st_vector.front() - getMeasurement(); else return getMeasurementSquareRootInformationUpper() * (_st_vector.front().segment(sb_constrained_start_,sb_constrained_size_) - getMeasurement()); } diff --git a/include/core/frame/frame_base.h b/include/core/frame/frame_base.h index 9a782ab6e..5ba7ef675 100644 --- a/include/core/frame/frame_base.h +++ b/include/core/frame/frame_base.h @@ -74,10 +74,10 @@ class FrameBase : public NodeBase, public HasStateBlocks, public std::enable_sha * * Constructor with time stamp and specs composite * \param _ts is the time stamp associated to this frame, provided in seconds - * \param _frame_specs SpecComposite containing all information needed to create the state blocs. + * \param _frame_specs SpecStateComposite containing all information needed to create the state blocs. **/ FrameBase(const TimeStamp& _ts, - const SpecComposite& _frame_specs); + const SpecStateComposite& _frame_specs); /** \brief Constructor time stamp and specs composite * diff --git a/include/core/problem/problem.h b/include/core/problem/problem.h index 1d2dc002a..fb077e8e9 100644 --- a/include/core/problem/problem.h +++ b/include/core/problem/problem.h @@ -76,7 +76,7 @@ class Problem : public std::enable_shared_from_this<Problem> SizeEigen dim_; TypeComposite frame_types_; - SpecComposite prior_options_; + SpecStateComposite prior_options_; bool prior_applied_; @@ -214,9 +214,9 @@ class Problem : public std::enable_shared_from_this<Problem> // Prior bool isPriorApplied() const; - void setPriorOptions(const SpecComposite& priors); + void setPriorOptions(const SpecStateComposite& priors); FrameBasePtr applyPriorOptions(const TimeStamp& _ts); - FrameBasePtr setPrior(const SpecComposite& priors, const TimeStamp& _ts); + FrameBasePtr setPrior(const SpecStateComposite& priors, const TimeStamp& _ts); /** \brief Emplace frame from timestamp, types and and state * \param _time_stamp Time stamp of the frame @@ -279,7 +279,7 @@ class Problem : public std::enable_shared_from_this<Problem> /** \brief Emplace frame from state specs composite * \param _time_stamp Time stamp of the frame - * \param _frame_spec_composite SpecComposite; each state must match in size and type with the + * \param _frame_spec_composite SpecStateComposite; each state must match in size and type with the * problem dimension and the corresponding key, for example, 'P' must be a 'StatePoint2d' in a 2D problem. * * - The structure is taken from _frame_spec_composite @@ -291,7 +291,7 @@ class Problem : public std::enable_shared_from_this<Problem> * - If it is key-frame, update state-block lists in Problem */ FrameBasePtr emplaceFrame(const TimeStamp& _time_stamp, - const SpecComposite& _frame_spec_composite); + const SpecStateComposite& _frame_spec_composite); // Frame getters FrameBaseConstPtr getLastFrame( ) const; diff --git a/include/core/processor/processor_base.h b/include/core/processor/processor_base.h index e33ff4d72..c6773d321 100644 --- a/include/core/processor/processor_base.h +++ b/include/core/processor/processor_base.h @@ -73,7 +73,7 @@ static ProcessorBasePtr create(const std::string& _yaml_filepath, \ if (not server.applySchema(#ProcessorClass)) \ { \ - WOLF_ERROR(server.getLog()); \ + WOLF_ERROR(server.getLog()); \ return nullptr; \ } \ auto params = std::make_shared<ParamsProcessorClass>(server.getNode()); \ diff --git a/include/core/sensor/factory_sensor.h b/include/core/sensor/factory_sensor.h index 4dfd6801f..58492e838 100644 --- a/include/core/sensor/factory_sensor.h +++ b/include/core/sensor/factory_sensor.h @@ -82,7 +82,7 @@ namespace wolf * auto params = std::make_shared<ParamsSensorCamera>(_server); * * // Do create the Sensor States priors - * auto priors = SpecSensorComposite(_server["states"]); + * auto priors = SpecStateSensorComposite(_server["states"]); * * // Do create the Sensor object --- example: SensorCamera * auto sensor = std::make_shared<SensorCamera>(params, priors); @@ -107,7 +107,7 @@ namespace wolf * auto params = std::make_shared<ParamsSensorCamera>(server.getNode()); * * // Do create the Sensor States priors - * auto priors = SpecSensorComposite(_server["states"]); + * auto priors = SpecStateSensorComposite(_server["states"]); * * // Do create the Sensor object --- example: SensorCamera * auto sensor = std::make_shared<SensorCamera>(params, priors); diff --git a/include/core/sensor/sensor_base.h b/include/core/sensor/sensor_base.h index 454fb8000..7e141b32a 100644 --- a/include/core/sensor/sensor_base.h +++ b/include/core/sensor/sensor_base.h @@ -51,7 +51,7 @@ namespace wolf { * must have two constructors available with the API: * * SensorClass(ParamsSensorClassPtr _params, - * const SpecSensorComposite& _priors) + * const SpecStateSensorComposite& _priors) * * Also, there should be the schema file 'SensorClass.schema' containing the specifications * of the user input yaml file. @@ -61,7 +61,7 @@ static SensorBasePtr create(const YAML::Node& _input_node) { \ auto params = std::make_shared<ParamsSensorClass>(_input_node); \ \ - auto priors = SpecSensorComposite(_input_node["states"]); \ + auto priors = SpecStateSensorComposite(_input_node["states"]); \ \ return std::make_shared<SensorClass>(params, priors); \ } \ @@ -77,7 +77,7 @@ static SensorBasePtr create(const std::string& _schema, } \ auto params = std::make_shared<ParamsSensorClass>(server.getNode()); \ \ - auto priors = SpecSensorComposite(server.getNode()["states"]); \ + auto priors = SpecStateSensorComposite(server.getNode()["states"]); \ \ return std::make_shared<SensorClass>(params, priors); \ } \ @@ -147,7 +147,7 @@ class SensorBase : public NodeBase, public HasStateBlocks, public std::enable_sh SensorBase(const std::string& _type, const int& _dim, ParamsSensorBasePtr _params, - const SpecSensorComposite& _priors); + const SpecStateSensorComposite& _priors); ~SensorBase() override; diff --git a/include/core/sensor/sensor_diff_drive.h b/include/core/sensor/sensor_diff_drive.h index 1220137da..3473bd1b5 100644 --- a/include/core/sensor/sensor_diff_drive.h +++ b/include/core/sensor/sensor_diff_drive.h @@ -55,7 +55,7 @@ class SensorDiffDrive : public SensorBase { public: SensorDiffDrive(ParamsSensorDiffDrivePtr _params, - const SpecSensorComposite& _priors); + const SpecStateSensorComposite& _priors); WOLF_SENSOR_CREATE(SensorDiffDrive, ParamsSensorDiffDrive); ~SensorDiffDrive() override = default; diff --git a/include/core/sensor/sensor_motion_model.h b/include/core/sensor/sensor_motion_model.h index 95be10c12..c8d267cb1 100644 --- a/include/core/sensor/sensor_motion_model.h +++ b/include/core/sensor/sensor_motion_model.h @@ -32,7 +32,7 @@ class SensorMotionModel : public SensorBase { public: SensorMotionModel(ParamsSensorBasePtr _params, - const SpecSensorComposite& _priors); + const SpecStateSensorComposite& _priors); WOLF_SENSOR_CREATE(SensorMotionModel, ParamsSensorBase); diff --git a/include/core/sensor/sensor_odom.h b/include/core/sensor/sensor_odom.h index 9817b4235..24771e273 100644 --- a/include/core/sensor/sensor_odom.h +++ b/include/core/sensor/sensor_odom.h @@ -68,7 +68,7 @@ class SensorOdom : public SensorBase public: SensorOdom(ParamsSensorOdomPtr _params, - const SpecSensorComposite& _priors) : + const SpecStateSensorComposite& _priors) : SensorBase("SensorOdom"+toString(DIM)+"d", DIM, _params, diff --git a/include/core/sensor/sensor_pose.h b/include/core/sensor/sensor_pose.h index 33b692296..8ebf5cc5b 100644 --- a/include/core/sensor/sensor_pose.h +++ b/include/core/sensor/sensor_pose.h @@ -55,7 +55,7 @@ class SensorPose : public SensorBase public: SensorPose(ParamsSensorPosePtr _params, - const SpecSensorComposite& _priors) : + const SpecStateSensorComposite& _priors) : SensorBase("SensorPose"+toString(DIM)+"d", DIM, _params, diff --git a/include/core/sensor/spec_state_sensor.h b/include/core/sensor/spec_state_sensor.h index 5773fbc7d..527bebe31 100644 --- a/include/core/sensor/spec_state_sensor.h +++ b/include/core/sensor/spec_state_sensor.h @@ -53,7 +53,7 @@ class SpecStateSensor : public SpecState std::string print(const std::string& _spaces = "") const override; }; -typedef Composite<SpecStateSensor> SpecSensorComposite; +typedef Composite<SpecStateSensor> SpecStateSensorComposite; inline bool SpecStateSensor::isDynamic() const { return dynamic_; } diff --git a/include/core/state_block/has_state_blocks.h b/include/core/state_block/has_state_blocks.h index 7d3473678..ed6c039e7 100644 --- a/include/core/state_block/has_state_blocks.h +++ b/include/core/state_block/has_state_blocks.h @@ -38,12 +38,12 @@ class HasStateBlocks public: HasStateBlocks(); - HasStateBlocks(const SpecComposite& _specs); + HasStateBlocks(const SpecStateComposite& _specs); HasStateBlocks(const TypeComposite& _types, const VectorComposite& _vectors); virtual ~HasStateBlocks(); StateKeys getKeys() const { return state_block_composite_.getKeys(); } - SpecComposite getSpecs() const; + SpecStateComposite getSpecs() const; bool has(const char& _sb_key) const { return state_block_composite_.has(_sb_key); } bool has(const std::string& _keys) const { return state_block_composite_.has(_keys); } diff --git a/include/core/state_block/spec_composite.h b/include/core/state_block/spec_composite.h index 81e38cd10..49222f9d7 100644 --- a/include/core/state_block/spec_composite.h +++ b/include/core/state_block/spec_composite.h @@ -78,7 +78,7 @@ class SpecBaseComposite : public Composite<T> TypeComposite getTypeComposite() const; }; -typedef SpecBaseComposite<SpecState> SpecComposite; +typedef SpecBaseComposite<SpecState> SpecStateComposite; template <> inline VectorComposite SpecBaseComposite<SpecState>::getVectorComposite() const diff --git a/include/core/state_block/type_composite.h b/include/core/state_block/type_composite.h index 392f718b5..9560642dc 100644 --- a/include/core/state_block/type_composite.h +++ b/include/core/state_block/type_composite.h @@ -36,7 +36,7 @@ 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"); + throw std::runtime_error("SpecStateComposite: constructor with a non-map yaml node"); } for (auto spec_pair : _n) diff --git a/include/core/state_block/vector_composite.h b/include/core/state_block/vector_composite.h index 5e1447f85..1f94f9706 100644 --- a/include/core/state_block/vector_composite.h +++ b/include/core/state_block/vector_composite.h @@ -88,7 +88,7 @@ inline Composite<Eigen::VectorXd>::Composite(const YAML::Node& _n) { if (not _n.IsMap()) { - throw std::runtime_error("SpecComposite: constructor with a non-map yaml node"); + throw std::runtime_error("SpecStateComposite: constructor with a non-map yaml node"); } for (auto spec_pair : _n) diff --git a/schema/landmark/LandmarkBase.schema b/schema/landmark/LandmarkBase.schema new file mode 100644 index 000000000..bccd1831d --- /dev/null +++ b/schema/landmark/LandmarkBase.schema @@ -0,0 +1,27 @@ +type: + _mandatory: true + _type: string + _doc: Type of the landmark + +plugin: + _mandatory: false + _type: string + _default: core + _doc: Name of the wolf plugin where the landmark type is implemented. + +id: + _mandatory: true + _type: int + _doc: Unique id of the landmark. + +states: + P: + _mandatory: false + _type: derived + _base: SpecState + _doc: Sequence of derived landmarks. + O: + _mandatory: false + _type: derived + _base: SpecState + _doc: Sequence of derived landmarks. \ No newline at end of file diff --git a/schema/map/MapBase.schema b/schema/map/MapBase.schema index 9fb4b5719..bf5f648e6 100644 --- a/schema/map/MapBase.schema +++ b/schema/map/MapBase.schema @@ -3,12 +3,20 @@ type: _type: string _default: MapBase _doc: Type of the Map used in the problem. + plugin: _mandatory: false _type: string _default: core _doc: Name of the wolf plugin where the map type is implemented. + filename: _mandatory: false _type: string - _doc: Optional. Absolute path of the YAML file containing the landmarks. \ No newline at end of file + _doc: Optional. Absolute path of the YAML file containing the landmarks. + +landmarks: + _mandatory: false + _type: derived[] + _base: LandmarkBase + _doc: Sequence of derived landmarks. \ No newline at end of file diff --git a/schema/problem/Problem2d.schema b/schema/problem/Problem2d.schema index 51625eeef..ee574b255 100644 --- a/schema/problem/Problem2d.schema +++ b/schema/problem/Problem2d.schema @@ -4,4 +4,10 @@ problem: P: follow: SpecStateP2d.schema O: - follow: SpecStateO2d.schema \ No newline at end of file + follow: SpecStateO2d.schema + dimension: + _type: int + _mandatory: false + _default: 2 + _options: [2] + _doc: Dimension of the problem: '2' for 2D \ No newline at end of file diff --git a/schema/problem/Problem3d.schema b/schema/problem/Problem3d.schema index 42a77ba48..f89471756 100644 --- a/schema/problem/Problem3d.schema +++ b/schema/problem/Problem3d.schema @@ -4,4 +4,10 @@ problem: P: follow: SpecStateP3d.schema O: - follow: SpecStateO3d.schema \ No newline at end of file + follow: SpecStateO3d.schema + dimension: + _type: int + _mandatory: false + _default: 3 + _options: [3] + _doc: Dimension of the problem: '3' for 3D \ No newline at end of file diff --git a/schema/problem/SpecStateO2d.schema b/schema/problem/SpecStateO2d.schema deleted file mode 100644 index 33e8a03c3..000000000 --- a/schema/problem/SpecStateO2d.schema +++ /dev/null @@ -1,11 +0,0 @@ -follow: SpecState.schema -type: - _type: string - _mandatory: false - _default: StateAngle - _options: [StateAngle] - _doc: The derived type of the StateBlock -state: - _type: Vector1d - _mandatory: true - _doc: A vector containing the state values \ No newline at end of file diff --git a/schema/problem/SpecStateP2d.schema b/schema/problem/SpecStateP2d.schema deleted file mode 100644 index 05854a626..000000000 --- a/schema/problem/SpecStateP2d.schema +++ /dev/null @@ -1,11 +0,0 @@ -follow: SpecState.schema -type: - _type: string - _mandatory: false - _default: StatePoint2d - _options: [StatePoint2d] - _doc: The derived type of the StateBlock -state: - _type: Vector2d - _mandatory: true - _doc: A vector containing the state values \ No newline at end of file diff --git a/schema/problem/SpecStateP3d.schema b/schema/problem/SpecStateP3d.schema deleted file mode 100644 index 430686bcb..000000000 --- a/schema/problem/SpecStateP3d.schema +++ /dev/null @@ -1,11 +0,0 @@ -follow: SpecState.schema -type: - _type: string - _mandatory: false - _default: StatePoint3d - _options: [StatePoint3d] - _doc: The derived type of the state in 'P' -state: - _type: Vector3d - _mandatory: true - _doc: A vector containing the state 'P' values \ No newline at end of file diff --git a/schema/problem/SpecStateV2d.schema b/schema/problem/SpecStateV2d.schema deleted file mode 100644 index 4c5e0d781..000000000 --- a/schema/problem/SpecStateV2d.schema +++ /dev/null @@ -1,11 +0,0 @@ -follow: SpecState.schema -type: - _type: string - _mandatory: false - _default: StateVector2d - _options: [StateVector2d] - _doc: The derived type of the StateBlock -state: - _type: Vector2d - _mandatory: true - _doc: A vector containing the state values \ No newline at end of file diff --git a/schema/problem/SpecStateV3d.schema b/schema/problem/SpecStateV3d.schema deleted file mode 100644 index f0e76e38a..000000000 --- a/schema/problem/SpecStateV3d.schema +++ /dev/null @@ -1,11 +0,0 @@ -follow: SpecState.schema -type: - _type: string - _mandatory: false - _default: StateVector3d - _options: [StateVector3d] - _doc: The derived type of the state in 'V' -state: - _type: Vector3d - _mandatory: true - _doc: A vector containing the state 'V' values \ No newline at end of file diff --git a/schema/processor/MotionProvider.schema b/schema/processor/MotionProvider.schema index 7ee7acbd6..3881ba712 100644 --- a/schema/processor/MotionProvider.schema +++ b/schema/processor/MotionProvider.schema @@ -2,7 +2,8 @@ state_provider: _mandatory: true _type: bool _doc: If the processor is used by the problem as provider of state. + state_provider_order: - _mandatory: false + _mandatory: $state_provider _type: double _doc: The order number of this processor when problem gets the state (only used if state_provider = true). Two processors cannot have the same priority (if so, when installing the second is increased). \ No newline at end of file diff --git a/schema/processor/ProcessorMotion.schema b/schema/processor/ProcessorMotion.schema index 3a55a522b..a3f95576d 100644 --- a/schema/processor/ProcessorMotion.schema +++ b/schema/processor/ProcessorMotion.schema @@ -2,19 +2,19 @@ follow: ProcessorBase.schema follow: MotionProvider.schema keyframe_vote: max_time_span: - _mandatory: true + _mandatory: $voting_active _type: double _doc: Time threshold to create a new frame [s]. max_buff_length: - _mandatory: true + _mandatory: $voting_active _type: unsigned int _doc: Maximum size of the buffer of motions. dist_traveled: - _mandatory: true + _mandatory: $voting_active _type: double _doc: Threshold of distance traveled to create a new frame [m]. angle_turned: - _mandatory: true + _mandatory: $voting_active _type: double _doc: Threshold of angle turned to create a new frame [rad]. unmeasured_perturbation_std: diff --git a/schema/processor/ProcessorOdom2d.schema b/schema/processor/ProcessorOdom2d.schema index ff361291a..e5358421e 100644 --- a/schema/processor/ProcessorOdom2d.schema +++ b/schema/processor/ProcessorOdom2d.schema @@ -1,6 +1,6 @@ follow: ProcessorMotion.schema keyframe_vote: cov_det: - _mandatory: true + _mandatory: $voting_active _type: double _doc: The determinant threshold of the covariance matrix of the integrated delta, to vote for a keyframe. \ No newline at end of file diff --git a/schema/processor/ProcessorTracker.schema b/schema/processor/ProcessorTracker.schema index 0c60a6c2a..4f0362c54 100644 --- a/schema/processor/ProcessorTracker.schema +++ b/schema/processor/ProcessorTracker.schema @@ -1,7 +1,7 @@ follow: ProcessorBase.schema keyframe_vote: min_features_for_keyframe: - _mandatory: true + _mandatory: $voting_active _type: unsigned int _doc: Minimum number of features to vote for keyframe. diff --git a/schema/sensor/SensorDiffDrive.schema b/schema/sensor/SensorDiffDrive.schema index 0e9c0453e..d41fa0f7d 100644 --- a/schema/sensor/SensorDiffDrive.schema +++ b/schema/sensor/SensorDiffDrive.schema @@ -43,7 +43,7 @@ states: _doc: A vector containing the intrinsic state values. 0=left wheel radius (m), 1=right wheel radius (m), 2=wheel separation (m) noise_std: _type: Vector3d - _mandatory: false + _mandatory: $mode == 'factor' _doc: A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix. drift_std: _type: Vector3d diff --git a/schema/sensor/SpecStateSensor.schema b/schema/sensor/SpecStateSensor.schema index 7db0767f6..7295f6cd7 100644 --- a/schema/sensor/SpecStateSensor.schema +++ b/schema/sensor/SpecStateSensor.schema @@ -1,8 +1,10 @@ follow: SpecState.schema + dynamic: _type: bool _mandatory: true _doc: If the state is dynamic, i.e. it changes along time. + drift_std: _type: VectorXd _mandatory: false diff --git a/schema/sensor/SpecStateSensorO2d.schema b/schema/sensor/SpecStateSensorO2d.schema index 0659b21b5..762e9b91a 100644 --- a/schema/sensor/SpecStateSensorO2d.schema +++ b/schema/sensor/SpecStateSensorO2d.schema @@ -1,18 +1,17 @@ follow: SpecStateSensor.schema + type: _type: string _mandatory: false _default: StateAngle _options: [StateAngle] _doc: The derived type of the StateBlock + state: _type: Vector1d _mandatory: true _doc: A vector containing the state values -noise_std: - _type: Vector1d - _mandatory: false - _doc: A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix. + drift_std: _type: Vector1d _mandatory: false diff --git a/schema/sensor/SpecStateSensorO3d.schema b/schema/sensor/SpecStateSensorO3d.schema index 69602d216..f9e821ac1 100644 --- a/schema/sensor/SpecStateSensorO3d.schema +++ b/schema/sensor/SpecStateSensorO3d.schema @@ -1,18 +1,17 @@ follow: SpecStateSensor.schema + type: _type: string _mandatory: false _default: StateQuaternion _options: [StateQuaternion] _doc: The derived type of the State in 'O' + state: _type: Vector4d _mandatory: true _doc: A vector containing the state values. It should be a quaternion (i.e. four values and normalized) -noise_std: - _type: Vector3d - _mandatory: false - _doc: A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix. + drift_std: _type: Vector3d _mandatory: false diff --git a/schema/sensor/SpecStateSensorP2d.schema b/schema/sensor/SpecStateSensorP2d.schema index fb5cc40a6..ca48a2101 100644 --- a/schema/sensor/SpecStateSensorP2d.schema +++ b/schema/sensor/SpecStateSensorP2d.schema @@ -1,18 +1,17 @@ follow: SpecStateSensor.schema + type: _type: string _mandatory: false _default: StatePoint2d _options: [StatePoint2d] _doc: The derived type of the StateBlock + state: _type: Vector2d _mandatory: true _doc: A vector containing the state values -noise_std: - _type: Vector2d - _mandatory: false - _doc: A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix. + drift_std: _type: Vector2d _mandatory: false diff --git a/schema/sensor/SpecStateSensorP3d.schema b/schema/sensor/SpecStateSensorP3d.schema index 36e078f68..1a43f75de 100644 --- a/schema/sensor/SpecStateSensorP3d.schema +++ b/schema/sensor/SpecStateSensorP3d.schema @@ -1,18 +1,17 @@ follow: SpecStateSensor.schema + type: _type: string _mandatory: false _default: StatePoint3d _options: [StatePoint3d] _doc: The derived type of the state in 'P' + state: _type: Vector3d _mandatory: true _doc: A vector containing the state 'P' values -noise_std: - _type: Vector3d - _mandatory: false - _doc: A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix. + drift_std: _type: Vector3d _mandatory: false diff --git a/schema/solver/SolverCeres.schema b/schema/solver/SolverCeres.schema index 019470bf3..6310da702 100644 --- a/schema/solver/SolverCeres.schema +++ b/schema/solver/SolverCeres.schema @@ -1,42 +1,51 @@ follow: SolverManager.schema + minimizer: _mandatory: true _type: string _options: [LEVENBERG_MARQUARDT, levenberg_marquardt, DOGLEG, dogleg, LBFGS, lbfgs, BFGS, bfgs] _doc: Type of minimizer. + interrupt_on_problem_change: _mandatory: true _type: bool _doc: If the solver has to interrupted each time the problem changes to rebuild the problem. + min_num_iterations: - _mandatory: false - _default: 0 + _mandatory: $interrupt_on_problem_change _type: unsigned int + _default: 0 _doc: Amount of solver iterations during which the solver cannot be interrupted (used in interrupt_on_problem_change == true). + max_num_iterations: _mandatory: true _type: unsigned int _doc: Maximum amount of solver iterations. If the solver didn't converge after this amount of iterations, it stops anyway. + function_tolerance: _mandatory: true _type: double _doc: "Function tolerance. Convergence criterion. Typical value: 1e-8" + gradient_tolerance: _mandatory: true _type: double _doc: "Gradient tolerance. Convergence criterion. Typical value: 1e-8" + n_threads: _mandatory: true _type: unsigned int _options: [1, 2, 3, 4] _doc: Amount of threads used by ceres. + use_nonmonotonic_steps: _mandatory: false - _default: false _type: bool + _default: false _doc: If the solver is allowed to update the solution with non-monotonic steps. Only used in LEVENBERG_MARQUARDT and DOGLEG minimizers. + max_consecutive_nonmonotonic_steps: - _mandatory: false - _default: 0 + _mandatory: $use_nonmonotonic_steps _type: unsigned int + _default: 0 _doc: Amount of consecutive non-monotonic steps allowed. Only used in LEVENBERG_MARQUARDT and DOGLEG minimizers. \ No newline at end of file diff --git a/schema/solver/SolverManager.schema b/schema/solver/SolverManager.schema index a2395dbae..3d490e4c7 100644 --- a/schema/solver/SolverManager.schema +++ b/schema/solver/SolverManager.schema @@ -2,22 +2,25 @@ period: _mandatory: true _type: double _doc: Period of the solver thread. + verbose: _mandatory: true _type: int _options: [0, 1, 2] _doc: "Verbosity of the solver. 0: Nothing, 1: Brief report, 2: Full report." + compute_cov: _mandatory: true _type: bool _doc: If the solver has to compute any covariance matrix block. + cov_enum: - _mandatory: false - _default: 0 + _mandatory: $compute_cov _type: int _options: [0, 1, 2, 3, 4, 5] _doc: "Which covariance matrix blocks have to be computed. 0: All blocks and all cross-covariances. 1: All marginals. 2: Marginals of landmarks and current robot pose plus cross covariances of current robot and all landmarks. 3: Last frame P and V. 4: Last frame P, O, V and T. 5: Last frame P and T." + cov_period: - _mandatory: true + _mandatory: $compute_cov _type: double _doc: Period of the covariance computation. \ No newline at end of file diff --git a/schema/problem/SpecState.schema b/schema/state/SpecState.schema similarity index 94% rename from schema/problem/SpecState.schema rename to schema/state/SpecState.schema index ac116b57a..51c1d9c42 100644 --- a/schema/problem/SpecState.schema +++ b/schema/state/SpecState.schema @@ -13,6 +13,5 @@ mode: _doc: The prior mode can be 'factor' to add an absolute factor (requires 'noise_std'), 'fix' to set the values constant or 'initial_guess' to just set the values noise_std: _type: VectorXd - _mandatory: false - _default: [] + _mandatory: $mode == 'factor' _doc: A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix. \ No newline at end of file diff --git a/schema/state/SpecStateO2d.schema b/schema/state/SpecStateO2d.schema new file mode 100644 index 000000000..6c2f7e357 --- /dev/null +++ b/schema/state/SpecStateO2d.schema @@ -0,0 +1,18 @@ +follow: SpecState.schema + +type: + _type: string + _mandatory: false + _default: StateAngle + _options: [StateAngle] + _doc: The derived type of the State in 'O' + +state: + _type: Vector1d + _mandatory: true + _doc: A vector containing the state values + +noise_std: + _type: Vector1d + _mandatory: $mode == 'factor' + _doc: A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix. \ No newline at end of file diff --git a/schema/problem/SpecStateO3d.schema b/schema/state/SpecStateO3d.schema similarity index 53% rename from schema/problem/SpecStateO3d.schema rename to schema/state/SpecStateO3d.schema index 685a4304a..584e247b3 100644 --- a/schema/problem/SpecStateO3d.schema +++ b/schema/state/SpecStateO3d.schema @@ -1,11 +1,18 @@ follow: SpecState.schema + type: _type: string _mandatory: false _default: StateQuaternion _options: [StateQuaternion] _doc: The derived type of the State in 'O' + state: _type: Vector4d _mandatory: true - _doc: A vector containing the state values. It should be a quaternion (i.e. four values and normalized) \ No newline at end of file + _doc: A vector containing the state values. It should be a quaternion (i.e. four values and normalized) + +noise_std: + _type: Vector3d + _mandatory: $mode == 'factor' + _doc: A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix. \ No newline at end of file diff --git a/schema/state/SpecStateP2d.schema b/schema/state/SpecStateP2d.schema new file mode 100644 index 000000000..97d1c8ab9 --- /dev/null +++ b/schema/state/SpecStateP2d.schema @@ -0,0 +1,18 @@ +follow: SpecState.schema + +type: + _type: string + _mandatory: false + _default: StatePoint2d + _options: [StatePoint2d] + _doc: The derived type of the state in 'P' + +state: + _type: Vector2d + _mandatory: true + _doc: A vector containing the state values + +noise_std: + _type: Vector2d + _mandatory: $mode == 'factor' + _doc: A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix. \ No newline at end of file diff --git a/schema/state/SpecStateP3d.schema b/schema/state/SpecStateP3d.schema new file mode 100644 index 000000000..efa34d61b --- /dev/null +++ b/schema/state/SpecStateP3d.schema @@ -0,0 +1,18 @@ +follow: SpecState.schema + +type: + _type: string + _mandatory: false + _default: StatePoint3d + _options: [StatePoint3d] + _doc: The derived type of the state in 'P' + +state: + _type: Vector3d + _mandatory: true + _doc: A vector containing the state 'P' values + +noise_std: + _type: Vector3d + _mandatory: $mode == 'factor' + _doc: A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix. \ No newline at end of file diff --git a/schema/state/SpecStateV2d.schema b/schema/state/SpecStateV2d.schema new file mode 100644 index 000000000..c5c165c16 --- /dev/null +++ b/schema/state/SpecStateV2d.schema @@ -0,0 +1,18 @@ +follow: SpecState.schema + +type: + _type: string + _mandatory: false + _default: StateVector2d + _options: [StateVector2d] + _doc: The derived type of the state in 'V' + +state: + _type: Vector2d + _mandatory: true + _doc: A vector containing the state values + +noise_std: + _type: Vector2d + _mandatory: $mode == 'factor' + _doc: A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix. \ No newline at end of file diff --git a/schema/state/SpecStateV3d.schema b/schema/state/SpecStateV3d.schema new file mode 100644 index 000000000..51bfc7268 --- /dev/null +++ b/schema/state/SpecStateV3d.schema @@ -0,0 +1,18 @@ +follow: SpecState.schema + +type: + _type: string + _mandatory: false + _default: StateVector3d + _options: [StateVector3d] + _doc: The derived type of the state in 'V' + +state: + _type: Vector3d + _mandatory: true + _doc: A vector containing the state 'V' values + +noise_std: + _type: Vector3d + _mandatory: $mode == 'factor' + _doc: A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix. \ No newline at end of file diff --git a/schema/tree_manager/NONE.schema b/schema/tree_manager/NONE.schema new file mode 100644 index 000000000..a351375a7 --- /dev/null +++ b/schema/tree_manager/NONE.schema @@ -0,0 +1,5 @@ +type: + _mandatory: false + _options: ["NONE"] + _type: string + _doc: Nothing to say here diff --git a/schema/tree_manager/None.schema b/schema/tree_manager/None.schema index 355f0ebec..127124113 100644 --- a/schema/tree_manager/None.schema +++ b/schema/tree_manager/None.schema @@ -1 +1,5 @@ -follow: TreeManagerBase.schema \ No newline at end of file +type: + _mandatory: false + _options: ["None"] + _type: string + _doc: Nothing to say here diff --git a/schema/tree_manager/none.schema b/schema/tree_manager/none.schema new file mode 100644 index 000000000..a392fc895 --- /dev/null +++ b/schema/tree_manager/none.schema @@ -0,0 +1,5 @@ +type: + _mandatory: false + _options: ["none"] + _type: string + _doc: Nothing to say here diff --git a/src/frame/frame_base.cpp b/src/frame/frame_base.cpp index 293aa5ad9..3a27fd90d 100644 --- a/src/frame/frame_base.cpp +++ b/src/frame/frame_base.cpp @@ -59,7 +59,7 @@ FrameBase::FrameBase(const TimeStamp& _ts, FrameBase::FrameBase(const TimeStamp& _ts, - const SpecComposite& _frame_specs) : + const SpecStateComposite& _frame_specs) : NodeBase("FRAME", "FrameBase"), HasStateBlocks(_frame_specs), trajectory_ptr_(), diff --git a/src/landmark/landmark_base.cpp b/src/landmark/landmark_base.cpp index 19c4730cb..eaa9373b1 100644 --- a/src/landmark/landmark_base.cpp +++ b/src/landmark/landmark_base.cpp @@ -53,7 +53,7 @@ LandmarkBase::LandmarkBase(const std::string& _type, StateBlockPtr _p_ptr, State LandmarkBase::LandmarkBase(const std::string& _type, const YAML::Node& _n) : NodeBase("LANDMARK", _type), - HasStateBlocks(SpecComposite(_n["states"])), + HasStateBlocks(SpecStateComposite(_n["states"])), map_ptr_(), landmark_id_(_n["id"].as<int>()) { @@ -61,7 +61,7 @@ LandmarkBase::LandmarkBase(const std::string& _type, const YAML::Node& _n) : LandmarkBase::LandmarkBase(const YAML::Node& _n) : NodeBase("LANDMARK", "LandmarkBase"), - HasStateBlocks(SpecComposite(_n["states"])), + HasStateBlocks(SpecStateComposite(_n["states"])), map_ptr_(), landmark_id_(_n["id"].as<int>()) { diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp index 1240e74e2..953748504 100644 --- a/src/problem/problem.cpp +++ b/src/problem/problem.cpp @@ -175,7 +175,7 @@ ProblemPtr Problem::autoSetup(YAML::Node _param_node) // First frame WOLF_INFO("Loading problem first frame parameters"); WOLF_INFO(problem_node["first_frame"]) - SpecComposite priors(problem_node["first_frame"]); + SpecStateComposite priors(problem_node["first_frame"]); WOLF_INFO(priors); problem->setPriorOptions(priors); @@ -461,7 +461,7 @@ FrameBasePtr Problem::emplaceFrame(const TimeStamp& _time_stamp, } FrameBasePtr Problem::emplaceFrame(const TimeStamp& _time_stamp, - const SpecComposite& _frame_specs) + const SpecStateComposite& _frame_specs) { // check input _frame_specs checkTypeComposite(_frame_specs.getTypeComposite()); @@ -1105,7 +1105,7 @@ FrameBasePtr Problem::closestFrameToTimeStamp(const TimeStamp& _ts) return trajectory_ptr_->closestFrameToTimeStamp(_ts); } -void Problem::setPriorOptions(const SpecComposite& _priors) +void Problem::setPriorOptions(const SpecStateComposite& _priors) { if (not prior_options_.empty()) { @@ -1188,7 +1188,7 @@ FrameBasePtr Problem::applyPriorOptions(const TimeStamp& _ts) return first_frame; } -FrameBasePtr Problem::setPrior(const SpecComposite& _priors, const TimeStamp& _ts) +FrameBasePtr Problem::setPrior(const SpecStateComposite& _priors, const TimeStamp& _ts) { setPriorOptions(_priors); return applyPriorOptions(_ts); diff --git a/src/sensor/sensor_base.cpp b/src/sensor/sensor_base.cpp index 7ba194fa2..483c29fd4 100644 --- a/src/sensor/sensor_base.cpp +++ b/src/sensor/sensor_base.cpp @@ -34,7 +34,7 @@ unsigned int SensorBase::sensor_id_count_ = 0; SensorBase::SensorBase(const std::string& _type, const int& _dim, ParamsSensorBasePtr _params, - const SpecSensorComposite& _priors) : + const SpecStateSensorComposite& _priors) : NodeBase("SENSOR", _type, _params->name), HasStateBlocks(), hardware_ptr_(), diff --git a/src/sensor/sensor_motion_model.cpp b/src/sensor/sensor_motion_model.cpp index c2a0ea42c..0fa8d1cc0 100644 --- a/src/sensor/sensor_motion_model.cpp +++ b/src/sensor/sensor_motion_model.cpp @@ -24,7 +24,7 @@ namespace wolf { SensorMotionModel::SensorMotionModel(ParamsSensorBasePtr _params, - const SpecSensorComposite& _priors) : + const SpecStateSensorComposite& _priors) : SensorBase("SensorMotionModel", -1, _params, diff --git a/src/state_block/has_state_blocks.cpp b/src/state_block/has_state_blocks.cpp index 082ef79a6..65353ccd7 100644 --- a/src/state_block/has_state_blocks.cpp +++ b/src/state_block/has_state_blocks.cpp @@ -26,7 +26,7 @@ namespace wolf { -HasStateBlocks::HasStateBlocks(const SpecComposite& _specs) +HasStateBlocks::HasStateBlocks(const SpecStateComposite& _specs) { for (auto spec : _specs) { @@ -93,9 +93,9 @@ HasStateBlocks::HasStateBlocks(const TypeComposite& _types, const VectorComposit } } -SpecComposite HasStateBlocks::getSpecs() const +SpecStateComposite HasStateBlocks::getSpecs() const { - SpecComposite specs; + SpecStateComposite specs; for (auto && state_pair : state_block_composite_) { specs.emplace(state_pair.first, SpecState(state_pair.second->getType(), diff --git a/test/dummy/sensor_dummy.h b/test/dummy/sensor_dummy.h index 5cb186c87..8a4992603 100644 --- a/test/dummy/sensor_dummy.h +++ b/test/dummy/sensor_dummy.h @@ -59,7 +59,7 @@ class SensorDummy : public SensorBase public: SensorDummy(ParamsSensorDummyPtr _params, - const SpecSensorComposite& _priors) : + const SpecStateSensorComposite& _priors) : SensorBase("SensorDummy"+toString(DIM)+"d", DIM, _params, @@ -87,7 +87,7 @@ class SensorDummyPoia : public SensorBase public: SensorDummyPoia(ParamsSensorDummyPtr _params, - const SpecSensorComposite& _priors) : + const SpecStateSensorComposite& _priors) : SensorBase("SensorDummyPoia"+toString(DIM)+"d", DIM, _params, diff --git a/test/gtest_emplace.cpp b/test/gtest_emplace.cpp index 2a2a4a554..0531900fa 100644 --- a/test/gtest_emplace.cpp +++ b/test/gtest_emplace.cpp @@ -61,7 +61,7 @@ TEST(Emplace, Sensor) auto sen = SensorBase::emplace<SensorDummy<3>>(P->getHardware(), // SensorBase is abstract std::make_shared<ParamsSensorDummy>(), - SpecSensorComposite({{'P',SpecStateSensor("StatePoint3d",Vector3d::Zero())}, + SpecStateSensorComposite({{'P',SpecStateSensor("StatePoint3d",Vector3d::Zero())}, {'O',SpecStateSensor("StateQuaternion",Vector4d::Random().normalized())}})); ASSERT_EQ(P, P->getHardware()->getSensorList().front()->getHardware()->getProblem()); @@ -84,7 +84,7 @@ TEST(Emplace, Processor) auto sen = SensorBase::emplace<SensorDummy<2>>(P->getHardware(), // SensorBase is abstract std::make_shared<ParamsSensorDummy>(), - SpecSensorComposite({{'P',SpecStateSensor("StatePoint2d",Vector2d::Zero())}, + SpecStateSensorComposite({{'P',SpecStateSensor("StatePoint2d",Vector2d::Zero())}, {'O',SpecStateSensor("StateAngle",Vector1d::Zero())}})); WOLF_INFO("0"); auto prc = ProcessorOdom2d::emplace<ProcessorOdom2d>(sen, std::make_shared<ParamsProcessorOdom2d>()); @@ -98,7 +98,7 @@ TEST(Emplace, Processor) SensorBasePtr sen2 = SensorBase::emplace<SensorDummy<2>>(P->getHardware(), // SensorBase is abstract std::make_shared<ParamsSensorDummy>(), - SpecSensorComposite({{'P',SpecStateSensor("StatePoint2d",Vector2d::Zero())}, + SpecStateSensorComposite({{'P',SpecStateSensor("StatePoint2d",Vector2d::Zero())}, {'O',SpecStateSensor("StateAngle",Vector1d::Zero())}})); WOLF_INFO("2"); @@ -179,7 +179,7 @@ TEST(Emplace, EmplaceDerived) 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<SensorOdom2d>(P->getHardware(), std::make_shared<ParamsSensorOdom>(), - SpecSensorComposite({{'P',SpecStateSensor("StatePoint2d",Vector2d::Zero())}, + SpecStateSensorComposite({{'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); diff --git a/test/gtest_odom_2d.cpp b/test/gtest_odom_2d.cpp index 10b60efa2..cd36b718e 100644 --- a/test/gtest_odom_2d.cpp +++ b/test/gtest_odom_2d.cpp @@ -140,7 +140,7 @@ TEST(Odom2d, FactorFix_and_FactorOdom2d) SolverCeres solver(Pr); // KF0 and absolute prior - SpecComposite prior; + SpecStateComposite prior; prior.emplace('P', SpecState("StatePoint2d", x0.at('P'), "factor", s0.at('P'))); prior.emplace('O', SpecState("StateAngle", x0.at('O'), "factor", s0.at('O'))); FrameBasePtr F0 = Pr->setPrior(prior, t0); @@ -237,7 +237,7 @@ TEST(Odom2d, VoteForKfAndSolve) SolverCeres solver(problem); // Origin Key Frame - SpecComposite prior; + SpecStateComposite prior; prior.emplace('P', SpecState("StatePoint2d", x0.at('P'), "factor", s0.at('P'))); prior.emplace('O', SpecState("StateAngle", x0.at('O'), "factor", s0.at('O'))); FrameBasePtr KF = problem->setPrior(prior, t0); @@ -380,7 +380,7 @@ TEST(Odom2d, KF_callback) SolverCeres solver(problem); // Origin Key Frame - SpecComposite prior; + SpecStateComposite prior; prior.emplace('P', SpecState("StatePoint2d", x0.at('P'), "factor", x0_cov.at('P'))); prior.emplace('O', SpecState("StateAngle", x0.at('O'), "factor", x0_cov.at('O'))); FrameBasePtr keyframe_0 = problem->setPrior(prior, t0); diff --git a/test/gtest_param_prior.cpp b/test/gtest_param_prior.cpp index caf8b9699..edc8594c7 100644 --- a/test/gtest_param_prior.cpp +++ b/test/gtest_param_prior.cpp @@ -46,7 +46,7 @@ Vector7d prior_extrinsics((Vector7d() << 10, 20, 30, 0, 0, 0, 1).finished()); Vector7d prior2_extrinsics((Vector7d() << 100, 200, 300, 0, 0, 0, 1).finished()); ParamsSensorOdomPtr params = std::make_shared<ParamsSensorOdom>(); -SpecSensorComposite priors{{'P',SpecStateSensor("StatePoint3d",initial_extrinsics_p,"initial_guess")}, +SpecStateSensorComposite priors{{'P',SpecStateSensor("StatePoint3d",initial_extrinsics_p,"initial_guess")}, {'O',SpecStateSensor("StateQuaternion",initial_extrinsics_o,"initial_guess")}}; SensorBasePtr sensor_ptr_ = SensorBase::emplace<SensorOdom3d>(problem_ptr->getHardware(), params, diff --git a/test/gtest_problem.cpp b/test/gtest_problem.cpp index 1a90de3ec..002802d7d 100644 --- a/test/gtest_problem.cpp +++ b/test/gtest_problem.cpp @@ -74,7 +74,7 @@ TEST(Problem, Sensors) params->name = "dummy_name"; auto S = SensorBase::emplace<SensorDummy3d>(P->getHardware(), params, - SpecSensorComposite({{'P',SpecStateSensor("StatePoint3d",Vector3d::Zero())}, + SpecStateSensorComposite({{'P',SpecStateSensor("StatePoint3d",Vector3d::Zero())}, {'O',SpecStateSensor("StateQuaternion",Vector4d::Random().normalized())}})); // check pointers ASSERT_EQ(P, S->getProblem()); @@ -94,7 +94,7 @@ TEST(Problem, Processor) params->name = "dummy_name"; auto Sm = SensorBase::emplace<SensorOdom3d>(P->getHardware(), params, - SpecSensorComposite({{'P',SpecStateSensor("StatePoint3d",Vector3d::Zero())}, + SpecStateSensorComposite({{'P',SpecStateSensor("StatePoint3d",Vector3d::Zero())}, {'O',SpecStateSensor("StateQuaternion",Vector4d::Random().normalized())}})); // add motion processor @@ -140,7 +140,7 @@ TEST(Problem, SetOrigin_PO_2d) { ProblemPtr P = Problem::create("PO", 2); TimeStamp t0(0); - SpecComposite prior; + SpecStateComposite prior; 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); @@ -199,7 +199,7 @@ TEST(Problem, SetOrigin_PO_3d) { ProblemPtr P = Problem::create("PO", 3); TimeStamp t0(0); - SpecComposite prior; + SpecStateComposite prior; 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); @@ -431,7 +431,7 @@ TEST(Problem, perturb) param->ticks_per_wheel_revolution = 100; auto sensor = SensorBase::emplace<SensorDiffDrive>(problem->getHardware(), param, - SpecSensorComposite({{'P',SpecStateSensor("StatePoint2d",Vector2d::Zero())}, + SpecStateSensorComposite({{'P',SpecStateSensor("StatePoint2d",Vector2d::Zero())}, {'O',SpecStateSensor("StateAngle",Vector1d::Zero())}, {'I',SpecStateSensor("StateParams3",Vector3d::Ones(),"initial_guess", VectorXd(0),true)}})); @@ -517,7 +517,7 @@ TEST(Problem, check) param->ticks_per_wheel_revolution = 100; auto sensor = SensorBase::emplace<SensorDiffDrive>(problem->getHardware(), param, - SpecSensorComposite({{'P',SpecStateSensor("StatePoint2d",Vector2d::Zero())}, + SpecStateSensorComposite({{'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; diff --git a/test/gtest_processor_and_factor_pose_3d_with_extrinsics.cpp b/test/gtest_processor_and_factor_pose_3d_with_extrinsics.cpp index de8205cd0..8c1a72191 100644 --- a/test/gtest_processor_and_factor_pose_3d_with_extrinsics.cpp +++ b/test/gtest_processor_and_factor_pose_3d_with_extrinsics.cpp @@ -123,7 +123,7 @@ class FactorPose3dWithExtrinsicsBase_Test : public testing::Test params_sp->std_noise = Vector6d::Ones(); sensor_pose_ = SensorBase::emplace<SensorPose3d>(problem_->getHardware(), params_sp, - SpecSensorComposite{{'P',SpecStateSensor("StatePoint3d",b_p_bm_,"initial_guess")}, + SpecStateSensorComposite{{'P',SpecStateSensor("StatePoint3d",b_p_bm_,"initial_guess")}, {'O',SpecStateSensor("StateQuaternion",b_q_m_.coeffs(),"initial_guess")}}); auto params_proc = std::make_shared<ParamsProcessorPose>(); params_proc->name = "pose processor"; diff --git a/test/gtest_processor_base.cpp b/test/gtest_processor_base.cpp index 859cd6647..f0011639e 100644 --- a/test/gtest_processor_base.cpp +++ b/test/gtest_processor_base.cpp @@ -123,7 +123,7 @@ TEST(ProcessorBase, KeyFrameCallback) // initialize TimeStamp t(0.0); - SpecComposite prior{{'P',SpecState("StatePoint2d",Vector2d(0,0),"factor",Vector2d(sqrt(0.1),sqrt(0.1)))}, + SpecStateComposite 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); diff --git a/test/gtest_processor_diff_drive.cpp b/test/gtest_processor_diff_drive.cpp index 345818253..1dc4267cc 100644 --- a/test/gtest_processor_diff_drive.cpp +++ b/test/gtest_processor_diff_drive.cpp @@ -139,7 +139,7 @@ class ProcessorDiffDriveTest : public testing::Test params_sen->name = "cool sensor"; params_sen->ticks_per_wheel_revolution = 100; // DO NOT MODIFY THESE VALUES !!! - SpecSensorComposite priors{{'P',SpecStateSensor("StatePoint2d", Vector2d::Zero())}, // default "fix", not dynamic + SpecStateSensorComposite priors{{'P',SpecStateSensor("StatePoint2d", Vector2d::Zero())}, // default "fix", not dynamic {'O',SpecStateSensor("StateAngle", Vector1d::Zero())}, // default "fix", not dynamic {'I',SpecStateSensor("StateParams3", Vector3d::Ones(), "initial_guess")}}; // not dynamic DO NOT MODIFY THESE VALUES !!! @@ -307,7 +307,7 @@ TEST_F(ProcessorDiffDriveTest, process) TimeStamp t = 0.0; double dt = 1.0; - SpecComposite prior; + SpecStateComposite prior; prior.emplace('P', SpecState("StatePoint2d", Vector2d::Zero(), "factor", Vector2d::Constant(1))); prior.emplace('O', SpecState("StateAngle", Vector1d::Zero(), "factor", Vector1d::Constant(1))); auto F0 = problem->setPrior(prior, t); @@ -337,7 +337,7 @@ TEST_F(ProcessorDiffDriveTest, linear) Matrix2d data_cov; data_cov . setIdentity(); TimeStamp t = 0.0; - SpecComposite prior; + SpecStateComposite prior; prior.emplace('P', SpecState("StatePoint2d", Vector2d::Zero(), "factor", Vector2d::Constant(1))); prior.emplace('O', SpecState("StateAngle", Vector1d::Zero(), "factor", Vector1d::Constant(1))); auto F0 = problem->setPrior(prior, t); @@ -366,7 +366,7 @@ TEST_F(ProcessorDiffDriveTest, angular) Matrix2d data_cov; data_cov . setIdentity(); TimeStamp t = 0.0; - SpecComposite prior; + SpecStateComposite prior; prior.emplace('P', SpecState("StatePoint2d", Vector2d::Zero(), "factor", Vector2d::Constant(1))); prior.emplace('O', SpecState("StateAngle", Vector1d::Zero(), "factor", Vector1d::Constant(1))); auto F0 = problem->setPrior(prior, t); diff --git a/test/gtest_processor_motion.cpp b/test/gtest_processor_motion.cpp index be7edb760..cf6fa784f 100644 --- a/test/gtest_processor_motion.cpp +++ b/test/gtest_processor_motion.cpp @@ -177,7 +177,7 @@ TEST_F(ProcessorMotion_test, IntegrateStraightFactorPrior) { // Prior TimeStamp t(0.0); - SpecComposite prior; + SpecStateComposite prior; 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); @@ -203,7 +203,7 @@ TEST_F(ProcessorMotion_test, IntegrateStraightFixPrior) { // Prior TimeStamp t(0.0); - SpecComposite prior{{'P',SpecState("StatePoint2d",Vector2d(0,0),"fix")}, + SpecStateComposite 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,7 +251,7 @@ TEST_F(ProcessorMotion_test, IntegrateCircleFactorPrior) { // Prior TimeStamp t(0.0); - SpecComposite prior{{'P',SpecState("StatePoint2d",Vector2d(0,0),"factor",Vector2d(1,1))}, + SpecStateComposite 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); @@ -276,7 +276,7 @@ TEST_F(ProcessorMotion_test, IntegrateCircleFixPrior) { // Prior TimeStamp t(0.0); - SpecComposite prior; + SpecStateComposite prior; prior.emplace('P',SpecState("StatePoint2d",Vector2d(0,0),"fix")); prior.emplace('O',SpecState("StateAngle",Vector1d(0),"fix")); auto KF_0 = problem->setPrior(prior, t); @@ -418,7 +418,7 @@ TEST_F(ProcessorMotion_test, splitBufferFactorPrior) { // Prior TimeStamp t(0.0); - SpecComposite prior{{'P',SpecState("StatePoint2d",Vector2d(0,0),"factor",Vector2d(1,1))}, + SpecStateComposite 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,7 +460,7 @@ TEST_F(ProcessorMotion_test, splitBufferFixPrior) { // Prior TimeStamp t(0.0); - SpecComposite prior{{'P',SpecState("StatePoint2d",Vector2d(0,0),"fix")}, + SpecStateComposite 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_sensor_base.cpp b/test/gtest_sensor_base.cpp index b0a8e731b..e42c98377 100644 --- a/test/gtest_sensor_base.cpp +++ b/test/gtest_sensor_base.cpp @@ -104,7 +104,7 @@ TEST(SensorBase, makeshared_priors_POfix2D) params->noise_o_std = noise_o_std; auto S = std::make_shared<SensorDummy2d>(params, - SpecSensorComposite({{'P',SpecStateSensor("StatePoint2d", p_state_2D)}, //default "fix", not dynamic + SpecStateSensorComposite({{'P',SpecStateSensor("StatePoint2d", p_state_2D)}, //default "fix", not dynamic {'O',SpecStateSensor("StateAngle", o_state_2D)}})); // noise @@ -124,7 +124,7 @@ TEST(SensorBase, makeshared_priors_POfix3D) params->noise_o_std = noise_o_std; auto S = std::make_shared<SensorDummy3d>(params, - SpecSensorComposite({{'P',SpecStateSensor("StatePoint3d", p_state_3D)}, //default "fix", not dynamic + SpecStateSensorComposite({{'P',SpecStateSensor("StatePoint3d", p_state_3D)}, //default "fix", not dynamic {'O',SpecStateSensor("StateQuaternion", o_state_3D)}})); // noise @@ -147,7 +147,7 @@ TEST(SensorBase, makeshared_priors_POinitial_guess2D) params->noise_o_std = noise_o_std; auto S = std::make_shared<SensorDummy2d>(params, - SpecSensorComposite({{'P',SpecStateSensor("StatePoint2d", p_state_2D, "initial_guess")}, + SpecStateSensorComposite({{'P',SpecStateSensor("StatePoint2d", p_state_2D, "initial_guess")}, {'O',SpecStateSensor("StateAngle", o_state_2D, "initial_guess")}})); // noise @@ -170,7 +170,7 @@ TEST(SensorBase, makeshared_priors_POinitial_guess3D) params->noise_o_std = noise_o_std; auto S = std::make_shared<SensorDummy3d>(params, - SpecSensorComposite({{'P',SpecStateSensor("StatePoint3d", p_state_3D, "initial_guess")}, + SpecStateSensorComposite({{'P',SpecStateSensor("StatePoint3d", p_state_3D, "initial_guess")}, {'O',SpecStateSensor("StateQuaternion", o_state_3D, "initial_guess")}})); // noise @@ -193,7 +193,7 @@ TEST(SensorBase, makeshared_priors_POfactor2D) params->noise_o_std = noise_o_std; auto S = std::make_shared<SensorDummy2d>(params, - SpecSensorComposite({{'P',SpecStateSensor("StatePoint2d", p_state_2D, "factor", p_std_2D)}, + SpecStateSensorComposite({{'P',SpecStateSensor("StatePoint2d", p_state_2D, "factor", p_std_2D)}, {'O',SpecStateSensor("StateAngle", o_state_2D, "factor", o_std_2D)}})); // noise @@ -216,7 +216,7 @@ TEST(SensorBase, makeshared_priors_POfactor3D) params->noise_o_std = noise_o_std; auto S = std::make_shared<SensorDummy3d>(params, - SpecSensorComposite({{'P',SpecStateSensor("StatePoint3d", p_state_3D, "factor", p_std_3D)}, + SpecStateSensorComposite({{'P',SpecStateSensor("StatePoint3d", p_state_3D, "factor", p_std_3D)}, {'O',SpecStateSensor("StateQuaternion", o_state_3D, "factor", o_std_3D)}})); // noise @@ -239,7 +239,7 @@ TEST(SensorBase, makeshared_priors_POinitial_guess_dynamic2D) params->noise_o_std = noise_o_std; auto S = std::make_shared<SensorDummy2d>(params, - SpecSensorComposite({{'P',SpecStateSensor("StatePoint2d", p_state_2D, "initial_guess", vector0, true)}, + SpecStateSensorComposite({{'P',SpecStateSensor("StatePoint2d", p_state_2D, "initial_guess", vector0, true)}, {'O',SpecStateSensor("StateAngle", o_state_2D, "initial_guess", vector0, true)}})); // noise @@ -262,7 +262,7 @@ TEST(SensorBase, makeshared_priors_POinitial_guess_dynamic3D) params->noise_o_std = noise_o_std; auto S = std::make_shared<SensorDummy3d>(params, - SpecSensorComposite({{'P',SpecStateSensor("StatePoint3d", p_state_3D, "initial_guess", vector0, true)}, + SpecStateSensorComposite({{'P',SpecStateSensor("StatePoint3d", p_state_3D, "initial_guess", vector0, true)}, {'O',SpecStateSensor("StateQuaternion", o_state_3D, "initial_guess", vector0, true)}})); // noise @@ -285,7 +285,7 @@ TEST(SensorBase, makeshared_priors_POinitial_guess_dynamic2D_drift) params->noise_o_std = noise_o_std; auto S = std::make_shared<SensorDummy2d>(params, - SpecSensorComposite({{'P',SpecStateSensor("StatePoint2d", p_state_2D, "initial_guess", vector0, true, p_std_2D)}, + SpecStateSensorComposite({{'P',SpecStateSensor("StatePoint2d", p_state_2D, "initial_guess", vector0, true, p_std_2D)}, {'O',SpecStateSensor("StateAngle", o_state_2D, "initial_guess", vector0, true, o_std_2D)}})); // noise @@ -308,7 +308,7 @@ TEST(SensorBase, makeshared_priors_POinitial_guess_dynamic3D_drift) params->noise_o_std = noise_o_std; auto S = std::make_shared<SensorDummy3d>(params, - SpecSensorComposite({{'P',SpecStateSensor("StatePoint3d", p_state_3D, "initial_guess", vector0, true, p_std_3D)}, + SpecStateSensorComposite({{'P',SpecStateSensor("StatePoint3d", p_state_3D, "initial_guess", vector0, true, p_std_3D)}, {'O',SpecStateSensor("StateQuaternion", o_state_3D, "initial_guess", vector0, true, o_std_3D)}})); // noise @@ -333,7 +333,7 @@ TEST(SensorBase, makeshared_priors_POIA_mixed) VectorXd i_state_3D = VectorXd::Random(5); auto S = std::make_shared<SensorDummyPoia3d>(params, - SpecSensorComposite({{'P',SpecStateSensor("StatePoint3d", p_state_3D, "fix", vector0, true)}, + SpecStateSensorComposite({{'P',SpecStateSensor("StatePoint3d", p_state_3D, "fix", vector0, true)}, {'O',SpecStateSensor("StateQuaternion", o_state_3D, "factor", o_std_3D, true, o_std_3D)}, {'I',SpecStateSensor("StateParams5", i_state_3D, "initial_guess")}, {'A',SpecStateSensor("StateQuaternion", o_state_3D, "factor", o_std_3D)}})); diff --git a/test/gtest_sensor_odom.cpp b/test/gtest_sensor_odom.cpp index e7e79f86c..9a43bcfda 100644 --- a/test/gtest_sensor_odom.cpp +++ b/test/gtest_sensor_odom.cpp @@ -111,7 +111,7 @@ TEST(SensorOdom, makeshared_priors_fix_2D) params->min_rot_var = min_rot_var; auto S = std::make_shared<SensorOdom2d>(params, - SpecSensorComposite({{'P',SpecStateSensor("StatePoint2d", p_state_2D)}, //default "fix", not dynamic + SpecStateSensorComposite({{'P',SpecStateSensor("StatePoint2d", p_state_2D)}, //default "fix", not dynamic {'O',SpecStateSensor("StateAngle", o_state_2D)}})); // checks @@ -131,7 +131,7 @@ TEST(SensorOdom, makeshared_priors_fix_3D) params->min_rot_var = min_rot_var; auto S = std::make_shared<SensorOdom3d>(params, - SpecSensorComposite({{'P',SpecStateSensor("StatePoint3d", p_state_3D)}, //default "fix", not dynamic + SpecStateSensorComposite({{'P',SpecStateSensor("StatePoint3d", p_state_3D)}, //default "fix", not dynamic {'O',SpecStateSensor("StateQuaternion", o_state_3D)}})); // factors @@ -154,7 +154,7 @@ TEST(SensorOdom, makeshared_priors_initial_guess_2D) params->min_rot_var = min_rot_var; auto S = std::make_shared<SensorOdom2d>(params, - SpecSensorComposite({{'P',SpecStateSensor("StatePoint2d", p_state_2D, "initial_guess")}, + SpecStateSensorComposite({{'P',SpecStateSensor("StatePoint2d", p_state_2D, "initial_guess")}, {'O',SpecStateSensor("StateAngle", o_state_2D, "initial_guess")}})); // factors @@ -177,7 +177,7 @@ TEST(SensorOdom, makeshared_priors_initial_guess_3D) params->min_rot_var = min_rot_var; auto S = std::make_shared<SensorOdom3d>(params, - SpecSensorComposite({{'P',SpecStateSensor("StatePoint3d", p_state_3D, "initial_guess")}, + SpecStateSensorComposite({{'P',SpecStateSensor("StatePoint3d", p_state_3D, "initial_guess")}, {'O',SpecStateSensor("StateQuaternion", o_state_3D, "initial_guess")}})); // factors @@ -200,7 +200,7 @@ TEST(SensorOdom, makeshared_priors_factor_2D) params->min_rot_var = min_rot_var; auto S = std::make_shared<SensorOdom2d>(params, - SpecSensorComposite({{'P',SpecStateSensor("StatePoint2d", p_state_2D, "factor", p_std_2D)}, + SpecStateSensorComposite({{'P',SpecStateSensor("StatePoint2d", p_state_2D, "factor", p_std_2D)}, {'O',SpecStateSensor("StateAngle", o_state_2D, "factor", o_std_2D)}})); // factors @@ -223,7 +223,7 @@ TEST(SensorOdom, makeshared_priors_factor_3D) params->min_rot_var = min_rot_var; auto S = std::make_shared<SensorOdom3d>(params, - SpecSensorComposite({{'P',SpecStateSensor("StatePoint3d", p_state_3D, "factor", p_std_3D)}, + SpecStateSensorComposite({{'P',SpecStateSensor("StatePoint3d", p_state_3D, "factor", p_std_3D)}, {'O',SpecStateSensor("StateQuaternion", o_state_3D, "factor", o_std_3D)}})); // factors @@ -246,7 +246,7 @@ TEST(SensorOdom, makeshared_priors_initial_guess_dynamic_2D) params->min_rot_var = min_rot_var; auto S = std::make_shared<SensorOdom2d>(params, - SpecSensorComposite({{'P',SpecStateSensor("StatePoint2d", p_state_2D, "initial_guess", vector0, true)}, + SpecStateSensorComposite({{'P',SpecStateSensor("StatePoint2d", p_state_2D, "initial_guess", vector0, true)}, {'O',SpecStateSensor("StateAngle", o_state_2D, "initial_guess", vector0, true)}})); // factors @@ -269,7 +269,7 @@ TEST(SensorOdom, makeshared_priors_initial_guess_dynamic_3D) params->min_rot_var = min_rot_var; auto S = std::make_shared<SensorOdom3d>(params, - SpecSensorComposite({{'P',SpecStateSensor("StatePoint3d", p_state_3D, "initial_guess", vector0, true)}, + SpecStateSensorComposite({{'P',SpecStateSensor("StatePoint3d", p_state_3D, "initial_guess", vector0, true)}, {'O',SpecStateSensor("StateQuaternion", o_state_3D, "initial_guess", vector0, true)}})); // factors @@ -292,7 +292,7 @@ TEST(SensorOdom, makeshared_priors_initial_guess_dynamic_drift_2D) params->min_rot_var = min_rot_var; auto S = std::make_shared<SensorOdom2d>(params, - SpecSensorComposite({{'P',SpecStateSensor("StatePoint2d", p_state_2D, "initial_guess", vector0, true, p_std_2D)}, + SpecStateSensorComposite({{'P',SpecStateSensor("StatePoint2d", p_state_2D, "initial_guess", vector0, true, p_std_2D)}, {'O',SpecStateSensor("StateAngle", o_state_2D, "initial_guess", vector0, true, o_std_2D)}})); // factors @@ -315,7 +315,7 @@ TEST(SensorOdom, makeshared_priors_initial_guess_dynamic_drift_3D) params->min_rot_var = min_rot_var; auto S = std::make_shared<SensorOdom3d>(params, - SpecSensorComposite({{'P',SpecStateSensor("StatePoint3d", p_state_3D, "initial_guess", vector0, true, p_std_3D)}, + SpecStateSensorComposite({{'P',SpecStateSensor("StatePoint3d", p_state_3D, "initial_guess", vector0, true, p_std_3D)}, {'O',SpecStateSensor("StateQuaternion", o_state_3D, "initial_guess", vector0, true, o_std_3D)}})); // factors @@ -477,7 +477,7 @@ TEST(SensorOdom, compute_noise_cov_3D) params->min_rot_var = min_rot_var; auto S = std::make_shared<SensorOdom3d>(params, - SpecSensorComposite({{'P',SpecStateSensor("StatePoint3d", p_state_3D)}, //default "fix", not dynamic + SpecStateSensorComposite({{'P',SpecStateSensor("StatePoint3d", p_state_3D)}, //default "fix", not dynamic {'O',SpecStateSensor("StateQuaternion", o_state_3D)}})); Vector6d disp_elements, rot_elements; diff --git a/test/gtest_sensor_pose.cpp b/test/gtest_sensor_pose.cpp index 2f57e54a5..4db18a614 100644 --- a/test/gtest_sensor_pose.cpp +++ b/test/gtest_sensor_pose.cpp @@ -47,7 +47,7 @@ TEST(Pose, constructor_priors_2D) param->name = "a not so cool sensor"; param->std_noise = std_noise_2D; - SpecSensorComposite priors{{'P',SpecStateSensor("StatePoint2d", p_state_2D)}, //default "fix", not dynamic + SpecStateSensorComposite priors{{'P',SpecStateSensor("StatePoint2d", p_state_2D)}, //default "fix", not dynamic {'O',SpecStateSensor("StateAngle", o_state_2D)}}; //default "fix", not dynamic auto sen = std::make_shared<SensorPose2d>(param, priors); @@ -69,7 +69,7 @@ TEST(Pose, constructor_priors_3D) param->name = "a not so cool sensor"; param->std_noise = std_noise_3D; - SpecSensorComposite priors{{'P',SpecStateSensor("StatePoint3d", p_state_3D)}, //default "fix", not dynamic + SpecStateSensorComposite priors{{'P',SpecStateSensor("StatePoint3d", p_state_3D)}, //default "fix", not dynamic {'O',SpecStateSensor("StateQuaternion", o_state_3D)}}; //default "fix", not dynamic auto sen = std::make_shared<SensorPose3d>(param, priors); -- GitLab