diff --git a/demos/hello_wolf/sensor_range_bearing.cpp b/demos/hello_wolf/sensor_range_bearing.cpp
index b7d62c95d6392ec0e9a3b7bbf87b21a166e5f274..aff44f34dbf697a4476cf0b7d8499409353f0037 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 69e0ea53c7b915c723cde0d95d54c64ac5846677..4b0516b0f8b350a668435353f752498b4717670b 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 51806e360fa78d15d0bc3af4bf2e420a4efbcf9f..70712879e117b2c54285499756eee5f1c9f322b8 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 9a782ab6e936f6dd79746061c5d68fb7bb1ffa72..5ba7ef6757528a9901ceb43b0416ef2ac8f3126c 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 1d2dc002ab9d5aa47f0cf6aebf9891937a727732..fb077e8e9b0bd55924e2998f426da9bb14e07a0c 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 e33ff4d726e6eb3071f04f1bba9c4846c0934b07..c6773d321645c134fd15e88833b177663e6bda79 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 4dfd6801fc182fb009fbacefea1d8ea9b5751d35..58492e838259e3bac14c68f874740f92ef6a51c2 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 454fb800063780f68b64f3e2564e63a0f59e4c87..7e141b32af1f34954994d5aeb0abd0b0fb4d8e52 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 1220137dafeb87af4df08400bae3d96a5c251589..3473bd1b5502d3c494b5ee8912129b8a665539a9 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 95be10c126e69e7f54fc63c3d42206f758322fe4..c8d267cb18674e0dd227a109192f14118efe108e 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 9817b4235e1476741d068b9d14e743eb08d8d602..24771e2731bb0a69670aa67381d5f71eceb98a5f 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 33b692296cb947b5c71e4571c41f5a996ad76375..8ebf5cc5bfaa299d03749445febeb720bcb5b07a 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 5773fbc7dc16b878704e21ebca370a2f37693d74..527bebe31a0b773ce6a7069a7ce9c0a4eaa4d043 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 7d3473678c65126015cab891c67382eca0eb1193..ed6c039e71cd14c71167768b14550dcc5e61bf94 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 81e38cd10165e352ef431ad7c636fb1666c7492f..49222f9d72a10cecab72fa8bfb597488b36b07b6 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 392f718b57b47dd7b677d10b823e3d352a9c3820..9560642dcc62cc2d162e18f6c530e9b4b58e91c3 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 5e1447f858b63a10610b764c2e3e5bdced073b58..1f94f9706ab0f1d17eb180a588220be13148a5f5 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 0000000000000000000000000000000000000000..bccd1831dbe20bb3a280aba55108469c3fc4b0ed
--- /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 9fb4b5719b73e9a0fbb6a66b66550b722c94619e..bf5f648e68b1b63eb0a4d67ac26ed99c7aa8dc0e 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 51625eeefda54e05d2a4f7af004e87bdaf595309..ee574b2556c28567e118e26c987a95cf1874c5f7 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 42a77ba48f49234a789ddd2b8ee694b065d033a1..f89471756640123a7fe325a3776ce096a5e8735d 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 33e8a03c3ccfd6a3d51802e5bfdf5ee364b71571..0000000000000000000000000000000000000000
--- 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 05854a626af92246e6a6e31a98fad13ee80da97f..0000000000000000000000000000000000000000
--- 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 430686bcb332719e4ba1d644fa9a9c6e707f2f0e..0000000000000000000000000000000000000000
--- 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 4c5e0d781e7c51e70bbbbc88a9a0560ed5a9351d..0000000000000000000000000000000000000000
--- 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 f0e76e38a7fecbe1820e62ed7979fde13e18163e..0000000000000000000000000000000000000000
--- 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 7ee7acbd63ee0956439479c418d65e6f1b447f9e..3881ba7120d2c2f21d4ccf002e8cdf5ef38342a4 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 3a55a522ba7bbd5d2e3f7a38c9133813be03c077..a3f95576dad9761ff4e152c9073c4d71b78c8c58 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 ff361291ac3d45d95ae17c73af094f7f0a64554e..e5358421e4b6ad792e9de9560e66208dfe8eda55 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 0c60a6c2ac417d0df0eaa68594aa30adfe355677..4f0362c5485df56b97fcdc20fa714ac718d5c01f 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 0e9c0453ef61818f4014e5ab0bae76be04d7f5c3..d41fa0f7dbec493f9f555c639b4f383cae78c199 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 7db0767f6b3df1166d76807da8388b485d6dd12b..7295f6cd73ae49b20b1c3b86b74562967f2991d5 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 0659b21b5d0ae3b138e98c9c169457af69f47fb0..762e9b91a6f75383ead4b5327ca1cf09d9354352 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 69602d216bde84a81825a37d7819c259f5722875..f9e821ac191e6ef7e4ca9b20e3433bd7774b9df4 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 fb5cc40a69820c8f572f8c8cdf5212c922b4d7e0..ca48a2101cb49ffae4f762da99c63374c8dd35c8 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 36e078f6885b725b84ba6e98255f71a4a9e93ac8..1a43f75de766a03a6105df0d5523de42a148ded3 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 019470bf39210972744602c5ad9992cbcfa9bc0a..6310da70265e5dc2fe8beb4e43b0299ff06ddb08 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 a2395dbaeeb348fe0c01fb2bc44235867f79d215..3d490e4c716737dd9a3ba00b5b3ecd82e7d3eb5d 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 ac116b57aba90c51afc7f7282953b6e2232e384c..51c1d9c425fabbeb9f8a884b8c5577a9cc397878 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 0000000000000000000000000000000000000000..6c2f7e357cb51545509109862211001eec9a6d5a
--- /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 685a4304aab0e896cef09f15d6251ca7f12fdf0b..584e247b3d6c93dae680a37492c671eaea85527d 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 0000000000000000000000000000000000000000..97d1c8ab9627df18c2f3d7ec77a62d745bdaef33
--- /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 0000000000000000000000000000000000000000..efa34d61b0ec166a5040c28cb9ef042c2c1f65c5
--- /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 0000000000000000000000000000000000000000..c5c165c16178564bc6b690b83fac47f194b03263
--- /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 0000000000000000000000000000000000000000..51bfc7268ff81a82629d8780bf6aa29d5e8efabb
--- /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 0000000000000000000000000000000000000000..a351375a7ef1e673fd58f20b06c2377dd8d82c89
--- /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 355f0ebec57d032df08c916caf2f4e9cece9631b..127124113fcdb2aab7e3373682bd920665d42f8d 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 0000000000000000000000000000000000000000..a392fc8955c3f5699684332f7c52e29199e8bf28
--- /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 293aa5ad97e2c82714f9d13cde47aa99db1be256..3a27fd90d3ad5cad30cc59e3a7609bdce6abbd95 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 19c4730cb1577fb293b1086dbbbeb6e559645d75..eaa9373b1efce14ed4e44f5e8b27f6cdf3c5c387 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 1240e74e2b0d1e49d82fe2ac071858dfc75b00e8..953748504650a14f91ec12577a454c368cc5a826 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 7ba194fa29eabcecf4f204f951336c25ffb0bcfb..483c29fd4a9721a7263ec030093874151b6a5de8 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 c2a0ea42c635cfc8f2dcd51c9c17cb39a32f5318..0fa8d1cc01b73eeacbceb4f35533496f0eae1d86 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 082ef79a609e59d06a2133c93149ce2f34576065..65353ccd74d689e5f60f390ebdaef601180bff92 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 5cb186c8706002cc7fa469981fd21bde19121e2f..8a4992603d993c2d3975977104d495860877013c 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 2a2a4a55443fc7cc40fbb85aaf94a40b75104eb6..0531900fa376af83dd7701ec23e4fe1a5e13b8f8 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 10b60efa228ef6d243368dba2235728048998c84..cd36b718e5cc07829a9968a22514eb9ecd56b710 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 caf8b96994445d0b1dede7299d45dcbfdbb9adce..edc8594c7b6f91d5e1734fed628abaaa88a840ba 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 1a90de3ecd612b4b38b67db426f7ab74e3d4cfef..002802d7dfcbecba70ed78dada1967be5519c740 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 de8205cd0b1dd9084bbdbf2fa1677038a81823ef..8c1a72191402a7ebe72ed9866a174145e9c49f03 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 859cd66471849d82fb9fcef97e1af2cbe90034a7..f0011639ea12a9e23a745e51d3283ce8adf50e67 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 345818253dfeede026481d1665f0dcef680c35e2..1dc4267cc1773965049b2d41d38237c567c79cf5 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 be7edb760e57dc52efd41a7ba54663add1c6c576..cf6fa784feca7ff20a423073e37a96dddd5a8ed3 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 b0a8e731bafc6083423b7ea33c201ebbcd109344..e42c98377e9df96e5045556d658e5bb72cd394df 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 e7e79f86c3e3a2c9b4956b0b5dedf9397902aca7..9a43bcfda1eb9de5d54797f8f4ddf19aca3c7422 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 2f57e54a57d3311d1f4fce3386525650b92ead2c..4db18a614f6dc00fe939c8f846e83a47803917c7 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);