diff --git a/CMakeLists.txt b/CMakeLists.txt
index 24bac9581829cc4fadb2eb007fd15c1e88b8adec..261e7816bda8c492d043c0a3f7e26a47821cc120 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -192,7 +192,7 @@ SET(HDRS_PROCESSOR
)
SET(HDRS_SENSOR
include/${PROJECT_NAME}/sensor/factory_sensor.h
- include/${PROJECT_NAME}/sensor/prior_sensor.h
+ include/${PROJECT_NAME}/sensor/spec_state_sensor.h
include/${PROJECT_NAME}/sensor/sensor_base.h
include/${PROJECT_NAME}/sensor/sensor_diff_drive.h
include/${PROJECT_NAME}/sensor/sensor_motion_model.h
@@ -210,7 +210,7 @@ SET(HDRS_STATE_BLOCK
include/${PROJECT_NAME}/state_block/local_parametrization_base.h
include/${PROJECT_NAME}/state_block/local_parametrization_homogeneous.h
include/${PROJECT_NAME}/state_block/local_parametrization_quaternion.h
- include/${PROJECT_NAME}/state_block/prior.h
+ include/${PROJECT_NAME}/state_block/spec_composite.h
include/${PROJECT_NAME}/state_block/state_angle.h
include/${PROJECT_NAME}/state_block/state_block.h
include/${PROJECT_NAME}/state_block/state_block_derived.h
@@ -302,7 +302,7 @@ SET(SRCS_PROCESSOR
src/processor/track_matrix.cpp
)
SET(SRCS_SENSOR
- src/sensor/prior_sensor.cpp
+ src/sensor/spec_state_sensor.cpp
src/sensor/sensor_base.cpp
src/sensor/sensor_diff_drive.cpp
src/sensor/sensor_motion_model.cpp
@@ -315,7 +315,7 @@ SET(SRCS_STATE_BLOCK
src/state_block/local_parametrization_base.cpp
src/state_block/local_parametrization_homogeneous.cpp
src/state_block/local_parametrization_quaternion.cpp
- src/state_block/prior.cpp
+ src/state_block/spec_composite.cpp
src/state_block/state_block.cpp
src/state_block/state_block_derived.cpp
src/state_block/state_composite.cpp
diff --git a/demos/hello_wolf/hello_wolf.cpp b/demos/hello_wolf/hello_wolf.cpp
index 91d99685abded46079b5431c615abef6df155e9f..dde271376d13561d0055aea7f67dfe37a6fdcf4e 100644
--- a/demos/hello_wolf/hello_wolf.cpp
+++ b/demos/hello_wolf/hello_wolf.cpp
@@ -130,8 +130,8 @@ int main()
// sensor odometer 2d
ParamsSensorOdomPtr intrinsics_odo = std::make_shared<ParamsSensorOdom>();
- PriorSensorMap priors_odo = {{'P',PriorSensor("P", Vector2d::Zero())},
- {'O',PriorSensor("O", Vector1d::Zero())}};
+ SpecSensorComposite priors_odo = {{'P',SpecStateSensor("P", Vector2d::Zero())},
+ {'O',SpecStateSensor("O", Vector1d::Zero())}};
SensorBasePtr sensor_odo = problem->installSensor("SensorOdom", "sensor odo", intrinsics_odo, priors_odo);
// processor odometer 2d
@@ -147,8 +147,8 @@ int main()
// sensor Range and Bearing
ParamsSensorRangeBearingPtr intrinsics_rb = std::make_shared<ParamsSensorRangeBearing>();
- PriorSensorMap priors_rb = {{'P',PriorSensor("P", Vector2d(1,1))},
- {'O',PriorSensor("O", Vector1d::Zero())}};
+ SpecSensorComposite priors_rb = {{'P',SpecStateSensor("P", Vector2d(1,1))},
+ {'O',SpecStateSensor("O", Vector1d::Zero())}};
intrinsics_rb->noise_range_metres_std = 0.1;
intrinsics_rb->noise_bearing_degrees_std = 1.0;
SensorBasePtr sensor_rb = problem->installSensor("SensorRangeBearing", "sensor RB", intrinsics_rb, priors_rb);
diff --git a/demos/hello_wolf/sensor_range_bearing.cpp b/demos/hello_wolf/sensor_range_bearing.cpp
index 39527e6c9d024cd5972bfa9bdcf90ed4fd2d60e3..0738c4418483859406db657530110b7199afaf74 100644
--- a/demos/hello_wolf/sensor_range_bearing.cpp
+++ b/demos/hello_wolf/sensor_range_bearing.cpp
@@ -27,7 +27,7 @@ WOLF_PTR_TYPEDEFS(SensorRangeBearing);
SensorRangeBearing::SensorRangeBearing(const SizeEigen& _dim,
ParamsSensorRangeBearingPtr _params,
- const PriorSensorMap& _priors) :
+ const SpecSensorComposite& _priors) :
SensorBase("SensorRangeBearing", _dim, _params, _priors),
params_rb_(_params)
{
diff --git a/demos/hello_wolf/sensor_range_bearing.h b/demos/hello_wolf/sensor_range_bearing.h
index 5b407dd50603bd50337b42a6486aa6e6f4a644f4..75d6e773108834161c60e5601ef657a42b8cd7cc 100644
--- a/demos/hello_wolf/sensor_range_bearing.h
+++ b/demos/hello_wolf/sensor_range_bearing.h
@@ -58,7 +58,7 @@ class SensorRangeBearing : public SensorBase
public:
SensorRangeBearing(const SizeEigen& _dim,
ParamsSensorRangeBearingPtr _params,
- const PriorSensorMap& _priors);
+ const SpecSensorComposite& _priors);
WOLF_SENSOR_CREATE(SensorRangeBearing, ParamsSensorRangeBearing);
~SensorRangeBearing() override;
diff --git a/demos/hello_wolf/yaml/hello_wolf_config.yaml b/demos/hello_wolf/yaml/hello_wolf_config.yaml
index f01ad2b227a8824025d6d1aa0b2f415cde34c2c6..33889188f950760ec83fe03119313dbbe494d832 100644
--- a/demos/hello_wolf/yaml/hello_wolf_config.yaml
+++ b/demos/hello_wolf/yaml/hello_wolf_config.yaml
@@ -5,7 +5,7 @@ config:
dimension: 2 # space is 2d
frame_structure: "PO" # keyframes have position and orientation
- prior:
+ first_frame:
mode: "factor"
$state:
P: [0,0]
diff --git a/include/core/frame/frame_base.h b/include/core/frame/frame_base.h
index 97d76c632258a5ed7177c6f8230f26ce79f6aaff..2a468a08be01670400227919182bbe04f4caa917 100644
--- a/include/core/frame/frame_base.h
+++ b/include/core/frame/frame_base.h
@@ -70,13 +70,36 @@ class FrameBase : public NodeBase, public HasStateBlocks, public std::enable_sha
StateBlockPtr _o_ptr = nullptr,
StateBlockPtr _v_ptr = nullptr);
+ /** \brief Constructor with type, time stamp and state pointer
+ *
+ * Constructor with type, time stamp and state pointer
+ * \param _ts is the time stamp associated to this frame, provided in seconds
+ * \param _frame_structure StateStructure (string) with the state keys
+ * \param _state VectorComposite containing the state of each key
+ **/
FrameBase(const TimeStamp& _ts,
const StateStructure& _frame_structure,
const VectorComposite& _state);
+ /** \brief Constructor with type, time stamp and state pointer
+ *
+ * Constructor with type, time stamp and state pointer
+ * \param _ts is the time stamp associated to this frame, provided in seconds
+ * \param _frame_structure StateStructure (string) with the state keys
+ * \param _vectors std::list of VectorXd containing the state of each key
+ **/
FrameBase(const TimeStamp& _ts,
const StateStructure& _frame_structure,
const std::list<VectorXd>& _vectors);
+
+ /** \brief Constructor time stamp and specs composite
+ *
+ * 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.
+ **/
+ FrameBase(const TimeStamp& _ts,
+ const SpecComposite& _frame_specs);
~FrameBase() override;
diff --git a/include/core/problem/problem.h b/include/core/problem/problem.h
index 6bfabeb1f79dd845c3c36760bb289673b79256f8..3b2799e311f30b35105f14d56a2974b590599ebf 100644
--- a/include/core/problem/problem.h
+++ b/include/core/problem/problem.h
@@ -38,8 +38,8 @@ class Loader;
//wolf includes
#include "core/common/wolf.h"
#include "core/frame/frame_base.h"
-#include "core/state_block/prior.h"
#include "core/state_block/state_block.h"
+#include "core/state_block/spec_composite.h"
#include "core/state_block/state_composite.h"
#include "core/processor/motion_provider.h"
@@ -79,7 +79,7 @@ class Problem : public std::enable_shared_from_this<Problem>
mutable std::mutex mut_notifications_;
mutable std::mutex mut_covariances_;
StateStructure frame_structure_;
- PriorMap prior_options_;
+ SpecComposite prior_options_;
bool prior_applied_;
std::atomic_bool transformed_;
@@ -190,15 +190,9 @@ class Problem : public std::enable_shared_from_this<Problem>
// Prior
bool isPriorApplied() const;
- void setPriorOptions(const PriorMap& priors);
+ void setPriorOptions(const SpecComposite& priors);
FrameBasePtr applyPriorOptions(const TimeStamp& _ts);
- FrameBasePtr setPriorFactor(const VectorComposite &_state,
- const VectorComposite &_sigma,
- const TimeStamp &_ts);
- FrameBasePtr setPriorFix(const VectorComposite &_state,
- const TimeStamp &_ts);
- FrameBasePtr setPriorInitialGuess(const VectorComposite &_state,
- const TimeStamp &_ts);
+ FrameBasePtr setPrior(const SpecComposite& priors, const TimeStamp& _ts);
/** \brief Emplace frame from string frame_structure, dimension and vector
* \param _time_stamp Time stamp of the frame
@@ -293,6 +287,22 @@ class Problem : public std::enable_shared_from_this<Problem>
*/
FrameBasePtr emplaceFrame(const TimeStamp& _time_stamp);
+ /** \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
+ * 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
+ * - The state sizes and types are taken from _frame_spec_composite
+ *
+ * This acts as a Frame factory, but also takes care to update related lists in WolfProblem:
+ * - Create a Frame
+ * - Add it to Trajectory
+ * - If it is key-frame, update state-block lists in Problem
+ */
+ FrameBasePtr emplaceFrame(const TimeStamp& _time_stamp, //
+ const SpecComposite& _frame_spec_composite);
+
// Frame getters
FrameBaseConstPtr getLastFrame( ) const;
FrameBasePtr getLastFrame( );
diff --git a/include/core/sensor/sensor_base.h b/include/core/sensor/sensor_base.h
index 612775d7100cb624348c416ffa5c1122aae64ba0..00d8da23e2f4247f9bfa1a2adeb75b2a65fe9c01 100644
--- a/include/core/sensor/sensor_base.h
+++ b/include/core/sensor/sensor_base.h
@@ -31,7 +31,7 @@ class ParamsServer;
//Wolf includes
#include "core/common/wolf.h"
-#include "core/sensor/prior_sensor.h"
+#include "core/sensor/spec_state_sensor.h"
#include "core/common/node_base.h"
#include "core/common/time_stamp.h"
#include "core/common/params_base.h"
@@ -54,7 +54,7 @@ namespace wolf {
* SensorClass(const std::string& _unique_name,
* SizeEigen _dim,
* ParamsSensorClassPtr _params,
- * const PriorSensorMap& _priors)
+ * const SpecSensorComposite& _priors)
*
* Also, there should be the schema file 'SensorClass.schema' containing the specifications
* of the user input yaml file.
@@ -64,7 +64,7 @@ static SensorBasePtr create(const YAML::Node& _input_node)
{ \
auto params = std::make_shared<ParamsSensorClass>(_input_node); \
\
- auto priors = PriorSensorMap(_input_node["states"]); \
+ auto priors = SpecSensorComposite(_input_node["states"]); \
\
return std::make_shared<SensorClass>(params, priors); \
} \
@@ -80,7 +80,7 @@ static SensorBasePtr create(const std::string& _schema,
} \
auto params = std::make_shared<ParamsSensorClass>(server.getNode()); \
\
- auto priors = PriorSensorMap(server.getNode()["states"]); \
+ auto priors = SpecSensorComposite(server.getNode()["states"]); \
\
return std::make_shared<SensorClass>(params, priors); \
} \
@@ -135,7 +135,6 @@ class SensorBase : public NodeBase, public HasStateBlocks, public std::enable_sh
int dim_; ///< Dimension compatibility of the sensor: 2: 2D, 3: 3D, -1: both
void setProblem(ProblemPtr _problem) override final;
- virtual void loadPriors(const PriorSensorMap& _priors);
public:
@@ -151,7 +150,7 @@ class SensorBase : public NodeBase, public HasStateBlocks, public std::enable_sh
SensorBase(const std::string& _type,
const int& _dim,
ParamsSensorBasePtr _params,
- const PriorSensorMap& _priors);
+ const SpecSensorComposite& _priors);
~SensorBase() override;
diff --git a/include/core/sensor/sensor_diff_drive.h b/include/core/sensor/sensor_diff_drive.h
index db2e2c40079cdf673e7fe17a3ce783ef15c4af1a..1220137dafeb87af4df08400bae3d96a5c251589 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 PriorSensorMap& _priors);
+ const SpecSensorComposite& _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 884cf57a779d8cc81632294acbfd72605558d78f..95be10c126e69e7f54fc63c3d42206f758322fe4 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 PriorSensorMap& _priors);
+ const SpecSensorComposite& _priors);
WOLF_SENSOR_CREATE(SensorMotionModel, ParamsSensorBase);
diff --git a/include/core/sensor/sensor_odom.h b/include/core/sensor/sensor_odom.h
index 1d0610b29583ff3db99a0b090f22ffcdb28a8fa8..8efa6ae80be7acdf4def33a41ddcb99466738a3b 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 PriorSensorMap& _priors) :
+ const SpecSensorComposite& _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 5817cabc06098ce4f94501d3d7a67626ebcfd466..6a3d8adefcd3156f11be904fdd0cb5517e1a2e8f 100644
--- a/include/core/sensor/sensor_pose.h
+++ b/include/core/sensor/sensor_pose.h
@@ -57,7 +57,7 @@ class SensorPose : public SensorBase
public:
SensorPose(ParamsSensorPosePtr _params,
- const PriorSensorMap& _priors) :
+ const SpecSensorComposite& _priors) :
SensorBase("SensorPose"+toString(DIM)+"d",
DIM,
_params,
diff --git a/include/core/sensor/prior_sensor.h b/include/core/sensor/spec_state_sensor.h
similarity index 56%
rename from include/core/sensor/prior_sensor.h
rename to include/core/sensor/spec_state_sensor.h
index 496926a610aa3bf5076dbc58fe3aa01aa5efac3b..c8ea62affc3009528ea7c3b271200a4a2810f24e 100644
--- a/include/core/sensor/prior_sensor.h
+++ b/include/core/sensor/spec_state_sensor.h
@@ -21,51 +21,48 @@
//--------LICENSE_END--------
#pragma once
-#include "core/state_block/prior.h"
+#include "core/state_block/spec_composite.h"
namespace wolf
{
-class PriorSensor : public Prior
+class SpecStateSensor : public SpecState
{
protected:
bool dynamic_; // State dynamic
Eigen::VectorXd drift_std_; // OPTIONAL std of the state drift [in units/sqrt(s)] diagonal elements (ONLY IF if dynamic)
public:
- PriorSensor() = default;
- PriorSensor(const std::string& _type,
- const Eigen::VectorXd& _state,
- const std::string& _mode = "fix",
- const Eigen::VectorXd& _noise_std = Eigen::VectorXd(0),
- bool _dynamic = false,
- const Eigen::VectorXd& _drift_std = Eigen::VectorXd(0));
+ SpecStateSensor() = default;
+ SpecStateSensor(const std::string& _type,
+ const Eigen::VectorXd& _state,
+ const std::string& _mode = "fix",
+ const Eigen::VectorXd& _noise_std = Eigen::VectorXd(0),
+ bool _dynamic = false,
+ const Eigen::VectorXd& _drift_std = Eigen::VectorXd(0));
- PriorSensor(const YAML::Node& _n);
+ SpecStateSensor(const YAML::Node& _n);
- virtual ~PriorSensor() = default;
+ virtual ~SpecStateSensor() = default;
bool isDynamic() const;
const Eigen::VectorXd& getDriftStd() const;
void check() const override;
- std::string print() const override;
+ std::string print(const std::string& _spaces = "") const override;
};
-typedef std::unordered_map<char, PriorSensor> StdMapPriorSensor;
-
-class PriorSensorMap : public StdMapPriorSensor
+class SpecSensorComposite : public SpecCompositeTemplate<SpecStateSensor>
{
public:
- using StdMapPriorSensor::StdMapPriorSensor;
+ using SpecCompositeTemplate::SpecCompositeTemplate;
- PriorSensorMap(const YAML::Node& _n);
- virtual ~PriorSensorMap() = default;
+ SpecComposite getSpecComposite() const;
};
-inline bool PriorSensor::isDynamic() const { return dynamic_; }
+inline bool SpecStateSensor::isDynamic() const { return dynamic_; }
-inline const Eigen::VectorXd& PriorSensor::getDriftStd() const { return drift_std_; }
+inline const Eigen::VectorXd& SpecStateSensor::getDriftStd() const { return drift_std_; }
} // namespace wolf
\ No newline at end of file
diff --git a/include/core/state_block/has_state_blocks.h b/include/core/state_block/has_state_blocks.h
index 4857055c7392fffe063a38c574a68aaeb67e555b..a61e1605540820b1edb6d6687a582e85a0ed7a48 100644
--- a/include/core/state_block/has_state_blocks.h
+++ b/include/core/state_block/has_state_blocks.h
@@ -23,6 +23,7 @@
#include "core/common/wolf.h"
#include "core/state_block/state_composite.h"
+#include "core/state_block/spec_composite.h"
#include <map>
@@ -37,11 +38,12 @@ class HasStateBlocks
public:
HasStateBlocks();
HasStateBlocks(const StateStructure& _structure) : structure_(_structure), state_block_map_() {}
+ HasStateBlocks(const SpecComposite& _specs);
virtual ~HasStateBlocks();
const StateStructure& getStructure() const { return structure_; }
- void appendToStructure(const char& _frame_type){ structure_ += _frame_type; }
- bool isInStructure(const char& _sb_type) const { return structure_.find(_sb_type) != std::string::npos; }
+ void appendToStructure(const char& _new_key){ structure_ += _new_key; }
+ bool isInStructure(const char& _sb_key) const { return structure_.find(_sb_key) != std::string::npos; }
const std::unordered_map<char, StateBlockConstPtr>& getStateBlockMap() const;
const std::unordered_map<char, StateBlockPtr>& getStateBlockMap();
@@ -59,20 +61,20 @@ class HasStateBlocks
virtual void unfix();
virtual bool isFixed() const;
- virtual StateBlockPtr addStateBlock(const char& _sb_type, const StateBlockPtr& _sb, ProblemPtr _problem);
- virtual unsigned int removeStateBlock(const char& _sb_type);
- bool hasStateBlock(const char& _sb_type) const { return state_block_map_.count(_sb_type) > 0; }
+ virtual StateBlockPtr addStateBlock(const char& _sb_key, const StateBlockPtr& _sb, ProblemPtr _problem);
+ virtual unsigned int removeStateBlock(const char& _sb_key);
+ bool hasStateBlock(const char& _sb_key) const { return state_block_map_.count(_sb_key) > 0; }
bool hasStateBlock(const StateBlockConstPtr& _sb) const;
- bool setStateBlock(const char _sb_type, const StateBlockPtr& _sb);
+ bool setStateBlock(const char _sb_key, const StateBlockPtr& _sb);
bool stateBlockKey(const StateBlockConstPtr& _sb, char& _key) const;
- StateBlockConstPtr getStateBlock(const char& _sb_type) const;
- StateBlockPtr getStateBlock(const char& _sb_type);
+ StateBlockConstPtr getStateBlock(const char& _sb_key) const;
+ StateBlockPtr getStateBlock(const char& _sb_key);
std::unordered_map<char, StateBlockConstPtr>::const_iterator find(const StateBlockConstPtr& _sb) const;
std::unordered_map<char, StateBlockPtr>::const_iterator find(const StateBlockPtr& _sb);
// Emplace derived state blocks (angle, quaternion, etc).
template<typename SB, typename ... Args>
- std::shared_ptr<SB> emplaceStateBlock(const char& _sb_type, ProblemPtr _problem, Args&&... _args_of_derived_state_block_constructor);
+ std::shared_ptr<SB> emplaceStateBlock(const char& _sb_key, ProblemPtr _problem, Args&&... _args_of_derived_state_block_constructor);
// Register/remove state blocks to/from wolf::Problem
virtual void registerNewStateBlocks(ProblemPtr _problem);
@@ -157,44 +159,44 @@ inline std::vector<StateBlockPtr> HasStateBlocks::getStateBlockVec()
return sbv;
}
-inline unsigned int HasStateBlocks::removeStateBlock(const char& _sb_type)
+inline unsigned int HasStateBlocks::removeStateBlock(const char& _sb_key)
{
- return state_block_map_.erase(_sb_type);
- return state_block_const_map_.erase(_sb_type);
+ return state_block_map_.erase(_sb_key);
+ return state_block_const_map_.erase(_sb_key);
}
template<typename SB, typename ... Args>
-inline std::shared_ptr<SB> HasStateBlocks::emplaceStateBlock(const char& _sb_type, ProblemPtr _problem, Args&&... _args_of_derived_state_block_constructor)
+inline std::shared_ptr<SB> HasStateBlocks::emplaceStateBlock(const char& _sb_key, ProblemPtr _problem, Args&&... _args_of_derived_state_block_constructor)
{
- assert(state_block_map_.count(_sb_type) == 0 && state_block_const_map_.count(_sb_type) == 0 && "Trying to add a state block with an existing type! Use setStateBlock instead.");
+ assert(state_block_map_.count(_sb_key) == 0 && state_block_const_map_.count(_sb_key) == 0 && "Trying to add a state block with an existing type! Use setStateBlock instead.");
std::shared_ptr<SB> sb = std::make_shared<SB>(std::forward<Args>(_args_of_derived_state_block_constructor)...);
- addStateBlock(_sb_type, sb, _problem);
+ addStateBlock(_sb_key, sb, _problem);
return sb;
}
-inline bool HasStateBlocks::setStateBlock(const char _sb_type, const StateBlockPtr& _sb)
+inline bool HasStateBlocks::setStateBlock(const char _sb_key, const StateBlockPtr& _sb)
{
- assert (structure_.find(_sb_type) > 0 && "Cannot set a state block out of the state structure! Use addStateBlock instead.");
- assert ( state_block_map_.count(_sb_type) > 0 && state_block_const_map_.count(_sb_type) > 0 && "Cannot set an inexistent state block! Use addStateBlock instead.");
- state_block_map_.at(_sb_type) = _sb;
- state_block_const_map_.at(_sb_type) = _sb;
+ assert (structure_.find(_sb_key) > 0 && "Cannot set a state block out of the state structure! Use addStateBlock instead.");
+ assert ( state_block_map_.count(_sb_key) > 0 && state_block_const_map_.count(_sb_key) > 0 && "Cannot set an inexistent state block! Use addStateBlock instead.");
+ state_block_map_.at(_sb_key) = _sb;
+ state_block_const_map_.at(_sb_key) = _sb;
return true; // success
}
-inline wolf::StateBlockConstPtr HasStateBlocks::getStateBlock(const char& _sb_type) const
+inline wolf::StateBlockConstPtr HasStateBlocks::getStateBlock(const char& _sb_key) const
{
- auto it = state_block_const_map_.find(_sb_type);
+ auto it = state_block_const_map_.find(_sb_key);
if (it != state_block_const_map_.end())
return it->second;
else
return nullptr;
}
-inline wolf::StateBlockPtr HasStateBlocks::getStateBlock(const char& _sb_type)
+inline wolf::StateBlockPtr HasStateBlocks::getStateBlock(const char& _sb_key)
{
- auto it = state_block_map_.find(_sb_type);
+ auto it = state_block_map_.find(_sb_key);
if (it != state_block_map_.end())
return it->second;
else
diff --git a/include/core/state_block/prior.h b/include/core/state_block/prior.h
deleted file mode 100644
index 870614fe887592432d1c8e91d9655316af1b32e4..0000000000000000000000000000000000000000
--- a/include/core/state_block/prior.h
+++ /dev/null
@@ -1,91 +0,0 @@
-//--------LICENSE_START--------
-//
-// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
-// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
-// All rights reserved.
-//
-// This file is part of WOLF
-// WOLF is free software: you can redistribute it and/or modify
-// it under the terms of the GNU Lesser General Public License as published by
-// the Free Software Foundation, either version 3 of the License, or
-// at your option) any later version.
-//
-// This program is distributed in the hope that it will be useful,
-// but WITHOUT ANY WARRANTY; without even the implied warranty of
-// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
-// GNU Lesser General Public License for more details.
-//
-// You should have received a copy of the GNU Lesser General Public License
-// along with this program. If not, see <http://www.gnu.org/licenses/>.
-//
-//--------LICENSE_END--------
-#pragma once
-
-#include <string>
-#include <unordered_map>
-#include <eigen3/Eigen/Dense>
-#include "yaml-cpp/yaml.h"
-
-namespace wolf
-{
-
-class Prior
-{
- protected:
- std::string type_; // State type
- Eigen::VectorXd state_; // state values
- std::string mode_; // Prior mode. Can be 'initial_guess', 'fix' or 'factor'
- Eigen::VectorXd noise_std_; // factor noise std, sqrt of the diagonal of the covariance mtrix (ONLY IF mode == 'factor')
-
- public:
- Prior() = delete;
- Prior(const std::string& _type,
- const Eigen::VectorXd& _state,
- const std::string& _mode = "fix",
- const Eigen::VectorXd& _noise_std = Eigen::VectorXd(0));
-
- Prior(const YAML::Node& _n);
-
- virtual ~Prior() = default;
-
- const std::string& getType() const;
- const std::string& getMode() const;
- const Eigen::VectorXd& getState() const;
- const Eigen::VectorXd& getNoiseStd() const;
- bool isFixed() const;
- bool isInitialGuess() const;
- bool isFactor() const;
-
- virtual void check() const;
-
- virtual std::string print() const;
-};
-
-typedef std::unordered_map<char, Prior> StdMapPrior;
-
-class PriorMap : public StdMapPrior
-{
- public:
- using StdMapPrior::StdMapPrior;
-
- PriorMap(const YAML::Node& _n);
- virtual ~PriorMap() = default;
-
- VectorComposite getState() const;
-};
-
-inline const std::string& Prior::getType() const { return type_; }
-
-inline const std::string& Prior::getMode() const { return mode_; }
-
-inline const Eigen::VectorXd& Prior::getState() const { return state_; }
-
-inline const Eigen::VectorXd& Prior::getNoiseStd() const { return noise_std_; }
-
-inline bool Prior::isFixed() const { return mode_ == "fix"; }
-
-inline bool Prior::isInitialGuess() const { return mode_ == "initial_guess"; }
-
-inline bool Prior::isFactor() const { return mode_ == "factor"; }
-
-} // namespace wolf
\ No newline at end of file
diff --git a/include/core/state_block/spec_composite.h b/include/core/state_block/spec_composite.h
new file mode 100644
index 0000000000000000000000000000000000000000..f53b296fdc215ef0b088d06c0d6c2503c6c540cf
--- /dev/null
+++ b/include/core/state_block/spec_composite.h
@@ -0,0 +1,145 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+#pragma once
+
+#include "core/state_block/state_composite.h"
+
+#include <string>
+#include <unordered_map>
+#include <eigen3/Eigen/Dense>
+#include "yaml-cpp/yaml.h"
+
+namespace wolf
+{
+
+using std::unordered_map;
+
+class SpecState
+{
+ protected:
+ std::string type_; // State type
+ Eigen::VectorXd state_; // state values
+ std::string mode_; // Prior mode. Can be 'initial_guess', 'fix' or 'factor'
+ Eigen::VectorXd noise_std_; // factor noise std, sqrt of the diagonal of the covariance mtrix (ONLY IF mode == 'factor')
+
+ public:
+ SpecState() = delete;
+ SpecState(const std::string& _type,
+ const Eigen::VectorXd& _state,
+ const std::string& _mode = "fix",
+ const Eigen::VectorXd& _noise_std = Eigen::VectorXd(0));
+
+ SpecState(const YAML::Node& _n);
+
+ virtual ~SpecState() = default;
+
+ const std::string& getType() const;
+ const std::string& getMode() const;
+ const Eigen::VectorXd& getState() const;
+ const Eigen::VectorXd& getNoiseStd() const;
+ bool isFixed() const;
+ bool isInitialGuess() const;
+ bool isFactor() const;
+
+ virtual void check() const;
+
+ virtual std::string print(const std::string& _spaces = "") const;
+};
+
+template <typename SPEC>
+class SpecCompositeTemplate : public std::unordered_map<char, SPEC>
+{
+ public:
+ using unordered_map<char, SPEC>::unordered_map;
+
+ SpecCompositeTemplate(const YAML::Node& _n);
+ virtual ~SpecCompositeTemplate() = default;
+
+ VectorComposite getState() const;
+
+ void emplace(char _key, const SPEC& _spec);
+
+ std::string print() const;
+};
+
+typedef SpecCompositeTemplate<SpecState> SpecComposite;
+
+
+template <typename SPEC>
+SpecCompositeTemplate<SPEC>::SpecCompositeTemplate(const YAML::Node& _n)
+{
+ if (not _n.IsMap())
+ {
+ throw std::runtime_error("SpecComposite: constructor with a non-map yaml node");
+ }
+ for (auto spec_pair : _n)
+ {
+ this->emplace(spec_pair.first.as<char>(), SPEC(spec_pair.second));
+ }
+}
+
+template <typename SPEC>
+void SpecCompositeTemplate<SPEC>::emplace(char _key, const SPEC& _spec)
+{
+ this->insert({_key,_spec});
+}
+
+template <typename SPEC>
+VectorComposite SpecCompositeTemplate<SPEC>::getState() const
+{
+ VectorComposite states;
+ for (auto spec_pair : *this)
+ {
+ states.emplace(spec_pair.first, spec_pair.second.getState());
+ }
+ return states;
+}
+
+template <typename SPEC>
+std::string SpecCompositeTemplate<SPEC>::print() const
+{
+ std::string output;
+
+ for (auto spec_pair : *this)
+ {
+ output += std::string(1,spec_pair.first) + ":\n";
+ output += spec_pair.second.print("\t");
+ }
+
+ return output;
+}
+
+inline const std::string& SpecState::getType() const { return type_; }
+
+inline const std::string& SpecState::getMode() const { return mode_; }
+
+inline const Eigen::VectorXd& SpecState::getState() const { return state_; }
+
+inline const Eigen::VectorXd& SpecState::getNoiseStd() const { return noise_std_; }
+
+inline bool SpecState::isFixed() const { return mode_ == "fix"; }
+
+inline bool SpecState::isInitialGuess() const { return mode_ == "initial_guess"; }
+
+inline bool SpecState::isFactor() const { return mode_ == "factor"; }
+
+} // namespace wolf
\ No newline at end of file
diff --git a/schema/map/MapBase.schema b/schema/map/MapBase.schema
index 50484d21e94bea91ba327b4f94613c472daafeb5..8b2f4f7562e55b3563f6fea22762f7744f91b070 100644
--- a/schema/map/MapBase.schema
+++ b/schema/map/MapBase.schema
@@ -4,7 +4,7 @@ type:
yaml_type: scalar
default: MapBase
doc: Type of the Map used in the problem.
-type:
+plugin:
mandatory: false
type: string
yaml_type: scalar
diff --git a/schema/problem/Problem.schema b/schema/problem/Problem.schema
index 12519e7b93eb693dcc4d0eb8d5f4c91fb960d414..0a7943acdbb1beca7e2946d4d0f90e1a08899d99 100644
--- a/schema/problem/Problem.schema
+++ b/schema/problem/Problem.schema
@@ -16,10 +16,9 @@ problem:
mandatory: true
options: [2, 3]
doc: Dimension of the problem. '2' for 2D or '3' for 3D
- #prior:
map:
type: derived
- base: Map.schema
+ base: MapBase.schema
yaml_type: scalar
mandatory: false
doc: The map used in the wolf problem.
diff --git a/schema/problem/Problem2d.schema b/schema/problem/Problem2d.schema
index ff52fcd1f121e026681c44b682a97240776bd25f..51625eeefda54e05d2a4f7af004e87bdaf595309 100644
--- a/schema/problem/Problem2d.schema
+++ b/schema/problem/Problem2d.schema
@@ -1,7 +1,7 @@
follow: Problem.schema
problem:
- prior:
+ first_frame:
P:
- follow: PriorP2d.schema
+ follow: SpecStateP2d.schema
O:
- follow: PriorO2d.schema
\ No newline at end of file
+ follow: SpecStateO2d.schema
\ No newline at end of file
diff --git a/schema/problem/Problem3d.schema b/schema/problem/Problem3d.schema
index b849bec3ce39d22ca8de78d42758367399fe5e3a..42a77ba48f49234a789ddd2b8ee694b065d033a1 100644
--- a/schema/problem/Problem3d.schema
+++ b/schema/problem/Problem3d.schema
@@ -1,7 +1,7 @@
follow: Problem.schema
problem:
- prior:
+ first_frame:
P:
- follow: PriorP3d.schema
+ follow: SpecStateP3d.schema
O:
- follow: PriorO3d.schema
\ No newline at end of file
+ follow: SpecStateO3d.schema
\ No newline at end of file
diff --git a/schema/problem/Prior.schema b/schema/problem/SpecState.schema
similarity index 100%
rename from schema/problem/Prior.schema
rename to schema/problem/SpecState.schema
diff --git a/schema/problem/PriorO2d.schema b/schema/problem/SpecStateO2d.schema
similarity index 91%
rename from schema/problem/PriorO2d.schema
rename to schema/problem/SpecStateO2d.schema
index c5afbd8499317a4bfde12bf07a58e5fd3818a7b3..321c4eaf9ef1c1383de33b656aceeb34398e501d 100644
--- a/schema/problem/PriorO2d.schema
+++ b/schema/problem/SpecStateO2d.schema
@@ -1,4 +1,4 @@
-follow: Prior.schema
+follow: SpecState.schema
type:
type: string
yaml_type: scalar
diff --git a/schema/problem/PriorO3d.schema b/schema/problem/SpecStateO3d.schema
similarity index 92%
rename from schema/problem/PriorO3d.schema
rename to schema/problem/SpecStateO3d.schema
index c819039b271552bfdd085c782e161a6379233ba3..deb47e1f884354474665cd26beb53e8a85483370 100644
--- a/schema/problem/PriorO3d.schema
+++ b/schema/problem/SpecStateO3d.schema
@@ -1,4 +1,4 @@
-follow: Prior.schema
+follow: SpecState.schema
type:
type: string
yaml_type: scalar
diff --git a/schema/problem/PriorP2d.schema b/schema/problem/SpecStateP2d.schema
similarity index 91%
rename from schema/problem/PriorP2d.schema
rename to schema/problem/SpecStateP2d.schema
index ef333199230dfd8dc08847c1eb830b2a36f2eb99..44227445621cce38bdea2353e0f0c5792892131a 100644
--- a/schema/problem/PriorP2d.schema
+++ b/schema/problem/SpecStateP2d.schema
@@ -1,4 +1,4 @@
-follow: Prior.schema
+follow: SpecState.schema
type:
type: string
yaml_type: scalar
diff --git a/schema/problem/PriorP3d.schema b/schema/problem/SpecStateP3d.schema
similarity index 91%
rename from schema/problem/PriorP3d.schema
rename to schema/problem/SpecStateP3d.schema
index de70f20a0ab78f2a3a0914aca916947cde5a22a6..6f1f0c173ce87655627020a15cd1f602c7c8fe13 100644
--- a/schema/problem/PriorP3d.schema
+++ b/schema/problem/SpecStateP3d.schema
@@ -1,4 +1,4 @@
-follow: Prior.schema
+follow: SpecState.schema
type:
type: string
yaml_type: scalar
diff --git a/schema/problem/SpecStateV2d.schema b/schema/problem/SpecStateV2d.schema
new file mode 100644
index 0000000000000000000000000000000000000000..c76380cbd782d979372728bc8863f6113fe855a2
--- /dev/null
+++ b/schema/problem/SpecStateV2d.schema
@@ -0,0 +1,13 @@
+follow: SpecState.schema
+type:
+ type: string
+ yaml_type: scalar
+ mandatory: false
+ default: StateVector2d
+ options: [StateVector2d]
+ doc: The derived type of the StateBlock
+state:
+ type: Vector2d
+ yaml_type: scalar
+ 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
new file mode 100644
index 0000000000000000000000000000000000000000..2c3109ae89e77e04024c80e40b1efad483920d06
--- /dev/null
+++ b/schema/problem/SpecStateV3d.schema
@@ -0,0 +1,13 @@
+follow: SpecState.schema
+type:
+ type: string
+ yaml_type: scalar
+ mandatory: false
+ default: StateVector3d
+ options: [StateVector3d]
+ doc: The derived type of the state in 'V'
+state:
+ type: Vector3d
+ yaml_type: scalar
+ mandatory: true
+ doc: A vector containing the state 'V' values
\ No newline at end of file
diff --git a/schema/sensor/SensorDiffDrive.schema b/schema/sensor/SensorDiffDrive.schema
index ba16781b06d9492b95a46f8d3a91e8256fd596f3..eb21483483c301beccb36b248c56e71e1cbde04a 100644
--- a/schema/sensor/SensorDiffDrive.schema
+++ b/schema/sensor/SensorDiffDrive.schema
@@ -30,11 +30,11 @@ ticks_std_factor:
states:
P:
- follow: PriorSensorP2d.schema
+ follow: SpecStateSensorP2d.schema
O:
- follow: PriorSensorO2d.schema
+ follow: SpecStateSensorO2d.schema
I:
- follow: PriorSensor.schema
+ follow: SpecStateSensor.schema
type:
type: string
yaml_type: scalar
diff --git a/schema/sensor/SensorOdom2d.schema b/schema/sensor/SensorOdom2d.schema
index 08dc1f8dd6d8e5257bb8cd065c3cfbc8ec82afd1..24615eac3b6f0330de3e7865d6e5f973a66f827a 100644
--- a/schema/sensor/SensorOdom2d.schema
+++ b/schema/sensor/SensorOdom2d.schema
@@ -32,7 +32,7 @@ min_rot_var:
states:
P:
- follow: PriorSensorP2d.schema
+ follow: SpecStateSensorP2d.schema
O:
- follow: PriorSensorO2d.schema
+ follow: SpecStateSensorO2d.schema
diff --git a/schema/sensor/SensorOdom3d.schema b/schema/sensor/SensorOdom3d.schema
index 4ef58d18aaa39fef4a8d86b0eac41aef0fd6c564..02845f9af3f74ca3b77d5f564ebc2b24ff08c93f 100644
--- a/schema/sensor/SensorOdom3d.schema
+++ b/schema/sensor/SensorOdom3d.schema
@@ -49,6 +49,6 @@ min_rot_var:
states:
P:
- follow: PriorSensorP3d.schema
+ follow: SpecStateSensorP3d.schema
O:
- follow: PriorSensorO3d.schema
\ No newline at end of file
+ follow: SpecStateSensorO3d.schema
\ No newline at end of file
diff --git a/schema/sensor/PriorSensor.schema b/schema/sensor/SpecStateSensor.schema
similarity index 93%
rename from schema/sensor/PriorSensor.schema
rename to schema/sensor/SpecStateSensor.schema
index e650d3961ed1187d46735019e32380b843f064cf..75e3052c71fe8c4b9d5adc51f80a305c9e1448b2 100644
--- a/schema/sensor/PriorSensor.schema
+++ b/schema/sensor/SpecStateSensor.schema
@@ -1,4 +1,4 @@
-follow: Prior.schema
+follow: SpecState.schema
dynamic:
type: bool
yaml_type: scalar
diff --git a/schema/sensor/SpecStateSensorO2d.schema b/schema/sensor/SpecStateSensorO2d.schema
new file mode 100644
index 0000000000000000000000000000000000000000..519ca225baf3b4b15d688162dcced59d7dc1b356
--- /dev/null
+++ b/schema/sensor/SpecStateSensorO2d.schema
@@ -0,0 +1,13 @@
+follow: SpecStateSensor.schema
+type:
+ type: string
+ yaml_type: scalar
+ mandatory: false
+ default: StateAngle
+ options: [StateAngle]
+ doc: The derived type of the StateBlock
+state:
+ type: Vector1d
+ yaml_type: scalar
+ mandatory: true
+ doc: A vector containing the state values
\ No newline at end of file
diff --git a/schema/sensor/SpecStateSensorO3d.schema b/schema/sensor/SpecStateSensorO3d.schema
new file mode 100644
index 0000000000000000000000000000000000000000..c248eb442a986415e6cddc4cc745447d5b358e1f
--- /dev/null
+++ b/schema/sensor/SpecStateSensorO3d.schema
@@ -0,0 +1,13 @@
+follow: SpecStateSensor.schema
+type:
+ type: string
+ yaml_type: scalar
+ mandatory: false
+ default: StateQuaternion
+ options: [StateQuaternion]
+ doc: The derived type of the State in 'O'
+state:
+ type: Vector4d
+ yaml_type: scalar
+ 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
diff --git a/schema/sensor/SpecStateSensorP2d.schema b/schema/sensor/SpecStateSensorP2d.schema
new file mode 100644
index 0000000000000000000000000000000000000000..4d3ea92c2bad73ccf9d33c340150e8a51ed31457
--- /dev/null
+++ b/schema/sensor/SpecStateSensorP2d.schema
@@ -0,0 +1,13 @@
+follow: SpecStateSensor.schema
+type:
+ type: string
+ yaml_type: scalar
+ mandatory: false
+ default: StatePoint2d
+ options: [StatePoint2d]
+ doc: The derived type of the StateBlock
+state:
+ type: Vector2d
+ yaml_type: scalar
+ mandatory: true
+ doc: A vector containing the state values
\ No newline at end of file
diff --git a/schema/sensor/SpecStateSensorP3d.schema b/schema/sensor/SpecStateSensorP3d.schema
new file mode 100644
index 0000000000000000000000000000000000000000..c7d681d7f101c068a11d37b6df462d4b41b92255
--- /dev/null
+++ b/schema/sensor/SpecStateSensorP3d.schema
@@ -0,0 +1,13 @@
+follow: SpecStateSensor.schema
+type:
+ type: string
+ yaml_type: scalar
+ mandatory: false
+ default: StatePoint3d
+ options: [StatePoint3d]
+ doc: The derived type of the state in 'P'
+state:
+ type: Vector3d
+ yaml_type: scalar
+ mandatory: true
+ doc: A vector containing the state 'P' values
\ No newline at end of file
diff --git a/src/frame/frame_base.cpp b/src/frame/frame_base.cpp
index aae21b6eb2fa9d5552e8212259299b71e0e0c440..47da4c53dd823503ef5cd8dd5cd6a720b4a6e9df 100644
--- a/src/frame/frame_base.cpp
+++ b/src/frame/frame_base.cpp
@@ -102,6 +102,17 @@ FrameBase::FrameBase(const TimeStamp& _ts,
}
+FrameBase::FrameBase(const TimeStamp& _ts,
+ const SpecComposite& _frame_specs) :
+ NodeBase("FRAME", "FrameBase"),
+ HasStateBlocks(_frame_specs),
+ trajectory_ptr_(),
+ frame_id_(++frame_id_count_),
+ time_stamp_(_ts)
+{
+}
+
+
FrameBase::~FrameBase()
{
}
diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp
index be215e57726ee0693d590a97e804a71c24920eb0..dc7720fe56ee59e37153bdadd14cd1a88c07c532 100644
--- a/src/problem/problem.cpp
+++ b/src/problem/problem.cpp
@@ -165,7 +165,9 @@ ProblemPtr Problem::autoSetup(const std::string& _input_yaml_file, const std::ve
// First frame
WOLF_INFO("Loading problem first frame parameters");
- PriorMap priors(problem_node["first_frame"]);
+ WOLF_INFO(problem_node["first_frame"])
+ SpecComposite priors(problem_node["first_frame"]);
+ WOLF_INFO(priors.print());
problem->setPriorOptions(priors);
// SENSORS ===============================================================================
@@ -437,6 +439,38 @@ FrameBasePtr Problem::emplaceFrame(const TimeStamp& _time_stamp)
this->getDim());
}
+FrameBasePtr Problem::emplaceFrame(const TimeStamp& _time_stamp,
+ const SpecComposite& _frame_spec_composite)
+{
+ // Checks
+ if (_frame_spec_composite.count('P') == 0 or _frame_spec_composite.count('O') == 0)
+ {
+ WOLF_INFO(_frame_spec_composite.print());
+ throw std::runtime_error("Problem::emplaceFrame: emplacing a frame without P or O");
+ }
+ if (_frame_spec_composite.at('P').getType() != "P" and
+ _frame_spec_composite.at('P').getType() != (dim_ == 2 ? "StatePoint2d" : "StatePoint3d"))
+ {
+ throw std::runtime_error("Problem::emplaceFrame: State type of key P should be 'StatePoint2d' or 'StatePoint3d' for 2D and 3D problem, respectively");
+ }
+ if (_frame_spec_composite.at('O').getType() != "O" and
+ _frame_spec_composite.at('O').getType() != (dim_ == 2 ? "StateAngle" : "StateQuaternion"))
+ {
+ throw std::runtime_error("Problem::emplaceFrame: State type of key O should be 'StateAngle' or 'StateQuaternion' for 2D and 3D problem, respectively");
+ }
+ if (_frame_spec_composite.count('V') != 0 and
+ _frame_spec_composite.at('V').getType() != "V" and
+ _frame_spec_composite.at('V').getType() != (dim_ == 2 ? "StateVector2d" : "StateVector3d"))
+ {
+ throw std::runtime_error("Problem::emplaceFrame: State type of key O should be 'StateAngle' or 'StateQuaternion' for 2D and 3D problem, respectively");
+ }
+
+ return FrameBase::emplace<FrameBase>(getTrajectory(),
+ _time_stamp,
+ _frame_spec_composite);
+}
+
+
TimeStamp Problem::getTimeStamp ( ) const
{
TimeStamp ts = TimeStamp::Invalid();
@@ -1041,72 +1075,38 @@ FrameBasePtr Problem::closestFrameToTimeStamp(const TimeStamp& _ts)
return trajectory_ptr_->closestFrameToTimeStamp(_ts);
}
-void Problem::setPriorOptions(const PriorMap& priors)
+void Problem::setPriorOptions(const SpecComposite& _priors)
{
if (not prior_options_.empty())
{
throw std::runtime_error("prior options have already been applied");
}
- prior_options_ = priors;
+ prior_options_ = _priors;
}
-/*void Problem::setPriorOptions(const std::string& _mode,
- const VectorComposite& _state ,
- const VectorComposite& _sigma )
+FrameBasePtr Problem::applyPriorOptions(const TimeStamp& _ts)
{
- assert(prior_options_ != nullptr && "prior options have already been applied");
- assert(prior_options_->mode == "" && "prior options have already been set");
- assert((_mode == "nothing" || _mode == "initial_guess" || _mode == "fix" || _mode == "factor") && "wrong _mode value, it should be: 'nothing', 'initial_guess', 'fix' or 'factor'");
-
- // Store options (optionals depending on the mode)
- WOLF_TRACE("prior mode: ", _mode);
- prior_options_->mode = _mode;
-
- if (prior_options_->mode != "nothing")
+ if (isPriorApplied())
{
- assert(_state.includesStructure(frame_structure_) && "any missing key in prior state");
-
- WOLF_TRACE("prior state: ", _state);
- prior_options_->state = _state;
-
- if (prior_options_->mode == "factor")
- {
- assert(_sigma.includesStructure(frame_structure_) && "any missing key in prior sigma");
-
- bool isPositive = true;
- for(const auto& it: _sigma)
- isPositive = isPositive and (it.second.array() > Constants::EPS).all();
-
- assert(isPositive && "prior sigma is not positive");
-
- MatrixComposite Q;
- for (const auto& pair_key_sig : _sigma)
- {
- const auto& key = pair_key_sig.first;
- const auto& sig_blk = pair_key_sig.second;
-
- const auto& cov_blk = (sig_blk.array() * sig_blk.array()).matrix().asDiagonal();
- Q.emplace(key,key,cov_blk);
- }
- WOLF_TRACE("prior covariance:" , Q);
- prior_options_->cov = Q;
- }
+ throw std::runtime_error("applyPriorOptions can be called once!");
}
-}*/
-FrameBasePtr Problem::applyPriorOptions(const TimeStamp& _ts)
-{
- assert(!isPriorApplied() && "applyPriorOptions can be called once!");
+ // No prior options set
+ if (prior_options_.empty())
+ {
+ prior_applied_ = true;
+ return nullptr;
+ }
// Create first frame
- FrameBasePtr prior_keyframe = emplaceFrame(_ts, frame_structure_, prior_options_.getState());
+ FrameBasePtr first_frame = emplaceFrame(_ts, prior_options_);
CaptureBasePtr prior_cap(nullptr);
for (auto prior_pair : prior_options_)
{
auto key = prior_pair.first;
- auto sb = prior_keyframe->getStateBlock(key);
+ auto sb = first_frame->getStateBlock(key);
// Fix
if (prior_pair.second.isFixed())
@@ -1115,21 +1115,21 @@ FrameBasePtr Problem::applyPriorOptions(const TimeStamp& _ts)
}
// Factor
- if (prior_pair.second.isFactor())
+ else if (prior_pair.second.isFactor())
{
// Emplace a capture (if not done already)
if (not prior_cap)
- prior_cap = CaptureBase::emplace<CaptureVoid>(prior_keyframe, _ts, nullptr);
+ prior_cap = CaptureBase::emplace<CaptureVoid>(first_frame, _ts, nullptr);
// Emplace a feature
- // TODO: compute cov
- Eigen::MatrixXd cov;
+ Eigen::MatrixXd cov = prior_pair.second.getNoiseStd().cwiseAbs2().asDiagonal();
+ if(cov.rows() != sb->getLocalSize())
+ {
+ throw std::runtime_error("Problem::applyPriorOptions: covariance matrix has wrong size");
+ }
auto prior_fea = FeatureBase::emplace<FeatureBase>(prior_cap, "Prior " + string(1,key), prior_pair.second.getState(), cov);
- assert(sb->getSize() == state_blk.size() && "prior_options state wrong size");
- assert(sb->getLocalSize() == covar_blk.rows() && "prior_options cov. wrong size");
-
// Emplace a factor
if (sb->hasLocalParametrization())
{
@@ -1144,93 +1144,21 @@ FrameBasePtr Problem::applyPriorOptions(const TimeStamp& _ts)
{
auto prior_fac = FactorBase::emplace<FactorBlockAbsolute>(prior_fea, prior_fea, sb, nullptr, false);
}
-
- }
- }
- }
-
- if (prior_options_->mode != "nothing" and prior_options_->mode != "")
- {
- prior_keyframe = emplaceFrame(_ts, frame_structure_, prior_options_->state);
-
- // Update origin for odometry processors
- for (auto proc_pair : motion_provider_map_)
- proc_pair.second->setOdometry(prior_options_->state);
-
- if (prior_options_->mode == "fix")
- prior_keyframe->fix();
- else if (prior_options_->mode == "factor")
- {
- // ASSUMPTION: Independent measurements (non-correlated block-diagonal covariance matrix)
-
- // Emplace a capture
- auto prior_cap = CaptureBase::emplace<CaptureVoid>(prior_keyframe, _ts, nullptr);
-
- // Emplace a feature and a factor for each state block
- for (auto pair_key_sb : prior_keyframe->getStateBlockMap())
- {
- auto key = pair_key_sb.first;
- auto sb = pair_key_sb.second;
-
- const auto& state_blk = prior_options_->state.at(key);
- const auto& covar_blk = prior_options_->cov.at(key,key);
-
- assert(sb->getSize() == state_blk.size() && "prior_options state wrong size");
- assert(sb->getLocalSize() == covar_blk.rows() && "prior_options cov. wrong size");
-
- // feature
- auto prior_fea = FeatureBase::emplace<FeatureBase>(prior_cap, "Prior " + string(1,key), state_blk, covar_blk);
-
- // factor
- if (sb->hasLocalParametrization())
- {
- if (std::dynamic_pointer_cast<StateQuaternion>(sb) != nullptr)
- auto prior_fac = FactorBase::emplace<FactorQuaternionAbsolute>(prior_fea, prior_fea, sb, nullptr, false);
- else if (std::dynamic_pointer_cast<StateAngle>(sb) != nullptr)
- auto prior_fac = FactorBase::emplace<FactorBlockAbsolute>(prior_fea, prior_fea, sb, nullptr, false);
- else
- throw std::runtime_error("not implemented...!");
- }
- else
- {
- auto prior_fac = FactorBase::emplace<FactorBlockAbsolute>(prior_fea, prior_fea, sb, nullptr, false);
- }
-
- }
-
}
- else
- assert(prior_options_->mode == "initial_guess" && "wrong prior_options->mode");
-
- // notify all processors
- keyFrameCallback(prior_keyframe, nullptr);
}
- // remove prior options
- prior_options_ = nullptr;
-
- return prior_keyframe;
-}
-
-FrameBasePtr Problem::setPriorFactor(const VectorComposite &_state,
- const VectorComposite &_sigma,
- const TimeStamp &_ts)
-{
- setPriorOptions("factor", _state, _sigma);
- return applyPriorOptions(_ts);
-}
+ // notify all processors
+ keyFrameCallback(first_frame, nullptr);
+
+ // raise flag
+ prior_applied_ = true;
-FrameBasePtr Problem::setPriorFix(const VectorComposite &_state,
- const TimeStamp &_ts)
-{
- setPriorOptions("fix", _state);
- return applyPriorOptions(_ts);
+ return first_frame;
}
-FrameBasePtr Problem::setPriorInitialGuess(const VectorComposite &_state,
- const TimeStamp &_ts)
+FrameBasePtr Problem::setPrior(const SpecComposite& _priors, const TimeStamp& _ts)
{
- setPriorOptions("initial_guess", _state);
+ setPriorOptions(_priors);
return applyPriorOptions(_ts);
}
diff --git a/src/sensor/prior_sensor.cpp b/src/sensor/prior_sensor.cpp
deleted file mode 100644
index b3b67f2443168d0dde35b2ee680dc0c0910f3251..0000000000000000000000000000000000000000
--- a/src/sensor/prior_sensor.cpp
+++ /dev/null
@@ -1,97 +0,0 @@
-//--------LICENSE_START--------
-//
-// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
-// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
-// All rights reserved.
-//
-// This file is part of WOLF
-// WOLF is free software: you can redistribute it and/or modify
-// it under the terms of the GNU Lesser General Public License as published by
-// the Free Software Foundation, either version 3 of the License, or
-// at your option) any later version.
-//
-// This program is distributed in the hope that it will be useful,
-// but WITHOUT ANY WARRANTY; without even the implied warranty of
-// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
-// GNU Lesser General Public License for more details.
-//
-// You should have received a copy of the GNU Lesser General Public License
-// along with this program. If not, see <http://www.gnu.org/licenses/>.
-//
-//--------LICENSE_END--------
-
-#include "core/sensor/prior_sensor.h"
-#include "core/state_block/factory_state_block.h"
-#include "core/common/params_base.h" // toString()
-
-namespace wolf
-{
-
-PriorSensorMap::PriorSensorMap(const YAML::Node& priors_node)
-{
- if (not priors_node.IsMap())
- {
- throw std::runtime_error("PriorSensorMap: constructor with a non-map yaml node");
- }
- for (auto prior_pair : priors_node)
- {
- this->emplace(prior_pair.first.as<char>(), PriorSensor(prior_pair.second));
- }
-}
-
-PriorSensor::PriorSensor(const std::string& _type,
- const Eigen::VectorXd& _state,
- const std::string& _mode,
- const Eigen::VectorXd& _noise_std,
- bool _dynamic,
- const Eigen::VectorXd& _drift_std) :
- Prior(_type, _state, _mode, _noise_std),
- dynamic_(_dynamic),
- drift_std_(_drift_std)
-{
- check();
-}
-
-PriorSensor::PriorSensor(const YAML::Node& prior_node) :
- Prior(prior_node)
-{
- dynamic_ = prior_node["dynamic"].as<bool>();
-
- if (dynamic_ and prior_node["drift_std"])
- drift_std_ = prior_node["drift_std"].as<Eigen::VectorXd>();
- else
- drift_std_ = Eigen::VectorXd(0);
-
- check();
-}
-
-void PriorSensor::check() const
-{
- Prior::check();
-
- // try to create a state block and check for local parameterization and dimensions consistency
- StateBlockPtr sb;
- try
- {
- sb = FactoryStateBlock::create(type_, state_, mode_ == "fix");
- }
- catch (const std::exception& e)
- {
- throw std::runtime_error("Prior::check() could not create state block from prior with error: " +
- std::string(e.what()) + print());
- }
-
- // check drift sigma size
- if (dynamic_ and drift_std_.size() > 0 and drift_std_.size() != sb->getLocalSize())
- throw std::runtime_error("Prior::check() Prior " + type_ +
- " drift_std size different of StateBlock local size. " + print());
-}
-
-std::string PriorSensor::print() const
-{
- return Prior::print() +
- "dynamic: " + toString(dynamic_) + "\n" +
- (drift_std_.size() > 0 ? "drift_std: " + toString(drift_std_) + "\n" : "");
-}
-
-} // namespace wolf
\ No newline at end of file
diff --git a/src/sensor/sensor_base.cpp b/src/sensor/sensor_base.cpp
index 90d40f660ff1c5df3ba584288074ae2065a70cef..bdcfc17b0a5c94b48705d9e876ee1a469286b998 100644
--- a/src/sensor/sensor_base.cpp
+++ b/src/sensor/sensor_base.cpp
@@ -34,9 +34,9 @@ unsigned int SensorBase::sensor_id_count_ = 0;
SensorBase::SensorBase(const std::string& _type,
const int& _dim,
ParamsSensorBasePtr _params,
- const PriorSensorMap& _priors) :
+ const SpecSensorComposite& _priors) :
NodeBase("SENSOR", _type, _params->name),
- HasStateBlocks("PO"),
+ HasStateBlocks(),
hardware_ptr_(),
sensor_id_(++sensor_id_count_), // simple ID factory
params_(_params),
@@ -44,18 +44,6 @@ SensorBase::SensorBase(const std::string& _type,
features_prior_map_(),
last_capture_(nullptr),
dim_(_dim)
-{
- // load priors
- loadPriors(_priors);
-}
-
-SensorBase::~SensorBase()
-{
- // Remove State Blocks
- removeStateBlocks(getProblem());
-}
-
-void SensorBase::loadPriors(const PriorSensorMap& _priors)
{
// Iterate all keys in _priors
for (auto state_pair : _priors)
@@ -71,17 +59,13 @@ void SensorBase::loadPriors(const PriorSensorMap& _priors)
if (key == 'O' and prior.getType() != "O" and prior.getType() != "StateAngle" and prior.getType() != "StateQuaternion")
throw std::runtime_error("Prior type for key O in 3D has to be 'O', 'StateAngle' or 'StateQuaternion'");
- // --- CREATE STATE BLOCK ---
+ // Create state block
auto sb = FactoryStateBlock::create(prior.getType(), prior.getState(), prior.isFixed());
- // check local param
- if (not sb->isValid()) // always false I think, the stateblock constructor would have failed...
- throw std::runtime_error("The created stateblock " + prior.getType() + " for key " + std::string(1,key) + " is not valid (local param violation)");
-
- // --- ADD STATE BLOCK ---
+ // Add state block
addStateBlock(key, sb, prior.isDynamic());
- // Factor
+ // Add factor
if (prior.isFactor())
addPriorParameter(key, prior.getState(), prior.getNoiseStd().cwiseAbs2().asDiagonal());
@@ -91,6 +75,12 @@ void SensorBase::loadPriors(const PriorSensorMap& _priors)
}
}
+SensorBase::~SensorBase()
+{
+ // Remove State Blocks
+ removeStateBlocks(getProblem());
+}
+
void SensorBase::fixExtrinsics()
{
for (auto key : std::string("PO"))
diff --git a/src/sensor/sensor_diff_drive.cpp b/src/sensor/sensor_diff_drive.cpp
index 5e4aef093efc4cab77eb44a1ebd4a1ce179d21fc..4a3b7401b3caad2a905e6a19f67f0294534f9a16 100644
--- a/src/sensor/sensor_diff_drive.cpp
+++ b/src/sensor/sensor_diff_drive.cpp
@@ -34,7 +34,7 @@ namespace wolf
{
SensorDiffDrive::SensorDiffDrive(ParamsSensorDiffDrivePtr _params,
- const PriorSensorMap& _priors) :
+ const SpecSensorComposite& _priors) :
SensorBase("SensorDiffDrive",
2,
_params,
diff --git a/src/sensor/sensor_motion_model.cpp b/src/sensor/sensor_motion_model.cpp
index b2aa8d5d3cd7c1677874316d94460343f2396284..58a28fa17532374b8fb035c9f644a20cf5eec49d 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 PriorSensorMap& _priors) :
+ const SpecSensorComposite& _priors) :
SensorBase("SensorMotionModel",
-1,
_params,
diff --git a/src/sensor/spec_state_sensor.cpp b/src/sensor/spec_state_sensor.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..281338cc66a9f0f12bfa58839935a99762f6643a
--- /dev/null
+++ b/src/sensor/spec_state_sensor.cpp
@@ -0,0 +1,88 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+
+#include "core/sensor/spec_state_sensor.h"
+#include "core/state_block/factory_state_block.h"
+#include "core/common/params_base.h" // toString()
+
+namespace wolf
+{
+
+SpecComposite SpecSensorComposite::getSpecComposite() const
+{
+ SpecComposite spec_composite;
+ for (auto spec_sensor_pair : *this)
+ {
+ spec_composite.emplace(spec_sensor_pair.first, SpecState(spec_sensor_pair.second.getType(),
+ spec_sensor_pair.second.getState(),
+ spec_sensor_pair.second.getMode(),
+ spec_sensor_pair.second.getNoiseStd()));
+ }
+ return spec_composite;
+}
+
+SpecStateSensor::SpecStateSensor(const std::string& _type,
+ const Eigen::VectorXd& _state,
+ const std::string& _mode,
+ const Eigen::VectorXd& _noise_std,
+ bool _dynamic,
+ const Eigen::VectorXd& _drift_std) :
+ SpecState(_type, _state, _mode, _noise_std),
+ dynamic_(_dynamic),
+ drift_std_(_drift_std)
+{
+ check();
+}
+
+SpecStateSensor::SpecStateSensor(const YAML::Node& prior_node) :
+ SpecState(prior_node)
+{
+ dynamic_ = prior_node["dynamic"].as<bool>();
+
+ if (dynamic_ and prior_node["drift_std"])
+ drift_std_ = prior_node["drift_std"].as<Eigen::VectorXd>();
+ else
+ drift_std_ = Eigen::VectorXd(0);
+
+ check();
+}
+
+void SpecStateSensor::check() const
+{
+ SpecState::check();
+
+ auto sb = FactoryStateBlock::create(type_, state_, mode_ == "fix"); // already tried in SpecState::check()
+
+ // check drift sigma size
+ if (dynamic_ and drift_std_.size() > 0 and drift_std_.size() != sb->getLocalSize())
+ throw std::runtime_error("SpecStateSensor::check() Prior " + type_ +
+ " drift_std size different of StateBlock local size. " + print());
+}
+
+std::string SpecStateSensor::print(const std::string& _spaces) const
+{
+ return SpecState::print(_spaces) +
+ _spaces + "dynamic: " + toString(dynamic_) + "\n" +
+ (drift_std_.size() > 0 ? _spaces + "drift_std: " + toString(drift_std_) + "\n" : "");
+}
+
+} // namespace wolf
\ No newline at end of file
diff --git a/src/state_block/has_state_blocks.cpp b/src/state_block/has_state_blocks.cpp
index c348c5109ac0f34a5ff90705428bc6a2c1a4427d..67f6a6fa7a5204c3e827113cafea638644de6a1f 100644
--- a/src/state_block/has_state_blocks.cpp
+++ b/src/state_block/has_state_blocks.cpp
@@ -26,13 +26,42 @@
namespace wolf
{
-StateBlockPtr HasStateBlocks::addStateBlock(const char& _sb_type, const StateBlockPtr& _sb, ProblemPtr _problem)
+HasStateBlocks::HasStateBlocks(const SpecComposite& _specs)
{
- assert(state_block_map_.count(_sb_type) == 0 && state_block_const_map_.count(_sb_type) == 0 && "Trying to add a state block with an existing type! Use setStateBlock instead.");
- state_block_map_.emplace(_sb_type, _sb);
- state_block_const_map_.emplace(_sb_type, _sb);
- if (!isInStructure(_sb_type))
- appendToStructure(_sb_type);
+ for (auto spec : _specs)
+ {
+ if (spec.first == 'P' and
+ spec.second.getType() != "P" and
+ spec.second.getType() != "StatePoint2d" and
+ spec.second.getType() != "StatePoint3d" )
+ {
+ throw std::runtime_error("HasStateBlocks: in key 'P', the state block should be of type 'P', 'StatePoint2d' or 'StatePoint3d'");
+ }
+ if (spec.first == 'O' and
+ spec.second.getType() != "O" and
+ spec.second.getType() != "StateAngle" and
+ spec.second.getType() != "StateQuaternion" )
+ {
+ throw std::runtime_error("HasStateBlocks: in key 'O', the state block should be of type 'O', 'StateAngle' or 'StateQuaternion'");
+ }
+ if (spec.first == 'V' and
+ spec.second.getType() != "V" and
+ spec.second.getType() != "StateVector2d" and
+ spec.second.getType() != "StateVector3d" )
+ {
+ throw std::runtime_error("HasStateBlocks: in key 'O', the state block should be of type 'O', 'StateAngle' or 'StateQuaternion'");
+ }
+ addStateBlock(spec.first, FactoryStateBlock::create(spec.second.getType(), spec.second.getState(), spec.second.isFixed()), nullptr);
+ }
+}
+
+StateBlockPtr HasStateBlocks::addStateBlock(const char& _sb_key, const StateBlockPtr& _sb, ProblemPtr _problem)
+{
+ assert(state_block_map_.count(_sb_key) == 0 && state_block_const_map_.count(_sb_key) == 0 && "Trying to add a state block with an existing type! Use setStateBlock instead.");
+ state_block_map_.emplace(_sb_key, _sb);
+ state_block_const_map_.emplace(_sb_key, _sb);
+ if (!isInStructure(_sb_key))
+ appendToStructure(_sb_key);
// conditionally register to problem
if(_problem)
diff --git a/src/state_block/prior.cpp b/src/state_block/prior.cpp
deleted file mode 100644
index a37b084e2e4a8f4a9e997a8e0ab1c3dd644550e8..0000000000000000000000000000000000000000
--- a/src/state_block/prior.cpp
+++ /dev/null
@@ -1,117 +0,0 @@
-//--------LICENSE_START--------
-//
-// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
-// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
-// All rights reserved.
-//
-// This file is part of WOLF
-// WOLF is free software: you can redistribute it and/or modify
-// it under the terms of the GNU Lesser General Public License as published by
-// the Free Software Foundation, either version 3 of the License, or
-// at your option) any later version.
-//
-// This program is distributed in the hope that it will be useful,
-// but WITHOUT ANY WARRANTY; without even the implied warranty of
-// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
-// GNU Lesser General Public License for more details.
-//
-// You should have received a copy of the GNU Lesser General Public License
-// along with this program. If not, see <http://www.gnu.org/licenses/>.
-//
-//--------LICENSE_END--------
-
-#include "core/state_block/prior.h"
-#include "core/state_block/factory_state_block.h"
-#include "core/common/params_base.h" // toString()
-
-namespace wolf
-{
-
-PriorMap::PriorMap(const YAML::Node& priors_node)
-{
- if (not priors_node.IsMap())
- {
- throw std::runtime_error("PriorMap: constructor with a non-map yaml node");
- }
- for (auto prior_pair : priors_node)
- {
- this->emplace(prior_pair.first.as<char>(), Prior(prior_pair.second));
- }
-}
-
-VectorComposite PriorMap::getState() const
-{
- VectorComposite prior_states;
- for (auto prior_pair : *this)
- {
- prior_states.emplace(prior_pair.first, prior_pair.second.getState());
- }
-}
-
-Prior::Prior(const std::string& _type,
- const Eigen::VectorXd& _state,
- const std::string& _mode,
- const Eigen::VectorXd& _noise_std)
- : type_(_type), state_(_state), mode_(_mode), noise_std_(_noise_std)
-{
- check();
-}
-
-Prior::Prior(const YAML::Node& prior_node)
-{
- type_ = prior_node["type"].as<std::string>();
- state_ = prior_node["state"].as<Eigen::VectorXd>();
- mode_ = prior_node["mode"].as<std::string>();
- if (mode_ == "factor")
- noise_std_ = prior_node["noise_std"].as<Eigen::VectorXd>();
- else
- noise_std_ = Eigen::VectorXd(0);
-
- check();
-}
-
-void Prior::check() const
-{
- if (mode_ != "initial_guess" and mode_ != "fix" and mode_ != "factor")
- throw std::runtime_error("Prior::check() wrong mode value: " + mode_ +
- ", it should be: 'initial_guess', 'fix' or 'factor'. " + print());
-
- // check empty state
- if (state_.size() == 0)
- throw std::runtime_error("Prior::check() Prior " + type_ + " state size zero. " + print());
-
- // try to create a state block and check for local parameterization and dimensions consistency
- StateBlockPtr sb;
- try
- {
- sb = FactoryStateBlock::create(type_, state_, mode_ == "fix");
- }
- catch (const std::exception& e)
- {
- throw std::runtime_error("Prior::check() could not create state block from prior with error: " + std::string(e.what()) + print());
- }
-
- // check local param
- if (not sb->isValid())
- throw std::runtime_error("Prior::check() Prior " + type_ + " state is not valid (local param violation). " +
- print());
-
- // check state size
- if (state_.size() != sb->getSize())
- throw std::runtime_error("Prior::check() Prior " + type_ + " state size different of StateBlock size. " + print());
-
- // check factor sigma size
- if (mode_ == "factor" and noise_std_.size() != sb->getLocalSize())
- throw std::runtime_error("Prior::check() Prior " + type_ + " noise_std size different of StateBlock local size. " +
- print());
-}
-
-std::string Prior::print() const
-{
- return "Prior " + type_ + "\n" +
- "mode: " + mode_ + "\n" +
- "state: " + toString(state_) + "\n" +
- (mode_ == "factor" ? "noise_std: " + toString(noise_std_) + "\n" : "");
-}
-
-} // namespace wolf
\ No newline at end of file
diff --git a/src/state_block/spec_composite.cpp b/src/state_block/spec_composite.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..7a648badba22d1992db71d1bee684a3f720c08bb
--- /dev/null
+++ b/src/state_block/spec_composite.cpp
@@ -0,0 +1,83 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+
+#include "core/state_block/spec_composite.h"
+#include "core/state_block/factory_state_block.h"
+#include "core/common/params_base.h" // toString()
+
+namespace wolf
+{
+
+SpecState::SpecState(const std::string& _type,
+ const Eigen::VectorXd& _state,
+ const std::string& _mode,
+ const Eigen::VectorXd& _noise_std)
+ : type_(_type), state_(_state), mode_(_mode), noise_std_(_noise_std)
+{
+ check();
+}
+
+SpecState::SpecState(const YAML::Node& _n)
+{
+ type_ = _n["type"].as<std::string>();
+ state_ = _n["state"].as<Eigen::VectorXd>();
+ mode_ = _n["mode"].as<std::string>();
+ if (mode_ == "factor")
+ noise_std_ = _n["noise_std"].as<Eigen::VectorXd>();
+ else
+ noise_std_ = Eigen::VectorXd(0);
+
+ check();
+}
+
+void SpecState::check() const
+{
+ if (mode_ != "initial_guess" and mode_ != "fix" and mode_ != "factor")
+ throw std::runtime_error("SpecState::check() wrong mode value: " + mode_ +
+ ", it should be: 'initial_guess', 'fix' or 'factor'. " + print());
+
+ // try to create a state block and check for local parameterization and dimensions consistency
+ StateBlockPtr sb;
+ try
+ {
+ sb = FactoryStateBlock::create(type_, state_, mode_ == "fix");
+ }
+ catch (const std::exception& e)
+ {
+ throw std::runtime_error("SpecState::check() could not create state block from prior with error: " + std::string(e.what()) + print());
+ }
+
+ // check factor sigma size
+ if (mode_ == "factor" and noise_std_.size() != sb->getLocalSize())
+ throw std::runtime_error("SpecState::check() SpecState " + type_ + " noise_std size different of StateBlock local size. " +
+ print());
+}
+
+std::string SpecState::print(const std::string& _spaces) const
+{
+ return _spaces + "SpecState " + type_ + "\n" +
+ _spaces + "mode: " + mode_ + "\n" +
+ _spaces + "state: " + toString(state_) + "\n" +
+ (mode_ == "factor" ? _spaces + "noise_std: " + toString(noise_std_) + "\n" : "");
+}
+
+} // namespace wolf
\ No newline at end of file
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index 5736cabf66600b37b16cd9da8fc4bc56b215f0af..0e96bf4bb0d8442a9614e690d9d586883bb97ab8 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -197,7 +197,7 @@ wolf_add_gtest(gtest_tree_manager gtest_tree_manager.cpp)
# # ProcessorMotion in 2d
# wolf_add_gtest(gtest_odom_2d gtest_odom_2d.cpp)
-# PriorSensor
+# SpecStateSensor
wolf_add_gtest(gtest_prior_sensor gtest_prior_sensor.cpp)
# # ProcessorOdom3d class test
diff --git a/test/dummy/SensorDummy2d.schema b/test/dummy/SensorDummy2d.schema
index 0175bd14bd1e5b31ffca663d870f48cc901fa9b8..c45f3d5b3fd93ee906ee354e97566559d1ddde0f 100644
--- a/test/dummy/SensorDummy2d.schema
+++ b/test/dummy/SensorDummy2d.schema
@@ -1,9 +1,9 @@
follow: SensorBase.schema
states:
P:
- follow: PriorP2d.schema
+ follow: SpecStateSensorP2d.schema
O:
- follow: PriorO2d.schema
+ follow: SpecStateSensorO2d.schema
noise_p_std:
mandatory: true
type: double
diff --git a/test/dummy/SensorDummy3d.schema b/test/dummy/SensorDummy3d.schema
index 941cb53d050d8d25ebb26c8ead01a79a05bb750f..588905e62bf8e21ac07d140caa03450fc4884bff 100644
--- a/test/dummy/SensorDummy3d.schema
+++ b/test/dummy/SensorDummy3d.schema
@@ -1,9 +1,9 @@
follow: SensorBase.schema
states:
P:
- follow: PriorP3d.schema
+ follow: SpecStateSensorP3d.schema
O:
- follow: PriorO3d.schema
+ follow: SpecStateSensorO3d.schema
noise_p_std:
mandatory: true
type: double
diff --git a/test/dummy/SensorDummyPoia3d.schema b/test/dummy/SensorDummyPoia3d.schema
index 180e9e84230c364548c5d0c8aa5206db961d1135..a36247c84db2706d0d4ebfa93f036973120eed48 100644
--- a/test/dummy/SensorDummyPoia3d.schema
+++ b/test/dummy/SensorDummyPoia3d.schema
@@ -1,11 +1,11 @@
follow: SensorBase.schema
states:
P:
- follow: PriorP3d.schema
+ follow: SpecStateSensorP3d.schema
O:
- follow: PriorO3d.schema
+ follow: SpecStateSensorO3d.schema
I:
- follow: Prior.schema
+ follow: SpecStateSensor.schema
type:
type: string
yaml_type: scalar
@@ -19,7 +19,7 @@ states:
mandatory: true
doc: A vector containing the state 'I' values
A:
- follow: Prior.schema
+ follow: SpecStateSensor.schema
type:
type: string
yaml_type: scalar
diff --git a/test/dummy/sensor_dummy.h b/test/dummy/sensor_dummy.h
index 77b8c9772ffb56ab6522a2194508655fe96cfa1e..bca6fed749f1158fc4898f113a5878db01366dd5 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 PriorSensorMap& _priors) :
+ const SpecSensorComposite& _priors) :
SensorBase("SensorDummy"+toString(DIM)+"d",
DIM,
_params,
diff --git a/test/gtest_emplace.cpp b/test/gtest_emplace.cpp
index 3e9fd8f63fbab40985179670ae055288247e1f32..c9f34bbf031ec8b09e5d50ea3ab0fcc7f6c370bb 100644
--- a/test/gtest_emplace.cpp
+++ b/test/gtest_emplace.cpp
@@ -62,8 +62,8 @@ TEST(Emplace, Sensor)
"dummy_name",
2,
std::make_shared<ParamsSensorDummy>(),
- PriorSensorMap({{'P',PriorSensor("P",Vector2d::Zero())},
- {'O',PriorSensor("O",Vector1d::Zero())}}));
+ SpecSensorComposite({{'P',SpecStateSensor("P",Vector2d::Zero())},
+ {'O',SpecStateSensor("O",Vector1d::Zero())}}));
ASSERT_EQ(P, P->getHardware()->getSensorList().front()->getHardware()->getProblem());
}
@@ -84,8 +84,8 @@ TEST(Emplace, Processor)
"dummy_name",
2,
std::make_shared<ParamsSensorDummy>(),
- PriorSensorMap({{'P',PriorSensor("P",Vector2d::Zero())},
- {'O',PriorSensor("O",Vector1d::Zero())}}));
+ SpecSensorComposite({{'P',SpecStateSensor("P",Vector2d::Zero())},
+ {'O',SpecStateSensor("O",Vector1d::Zero())}}));
auto prc = ProcessorOdom2d::emplace<ProcessorOdom2d>(sen, std::make_shared<ParamsProcessorOdom2d>());
ASSERT_EQ(P, P->getHardware()->getSensorList().front()->getProcessorList().front()->getSensor()->getProblem());
ASSERT_EQ(sen, sen->getProcessorList().front()->getSensor());
@@ -95,8 +95,8 @@ TEST(Emplace, Processor)
"dummy_name",
2,
std::make_shared<ParamsSensorDummy>(),
- PriorSensorMap({{'P',PriorSensor("P",Vector2d::Zero())},
- {'O',PriorSensor("O",Vector1d::Zero())}}));
+ SpecSensorComposite({{'P',SpecStateSensor("P",Vector2d::Zero())},
+ {'O',SpecStateSensor("O",Vector1d::Zero())}}));
ProcessorOdom2dPtr prc2 = ProcessorOdom2d::emplace<ProcessorOdom2d>(sen2, std::make_shared<ParamsProcessorOdom2d>());
ASSERT_EQ(sen2, sen2->getProcessorList().front()->getSensor());
ASSERT_EQ(prc2, sen2->getProcessorList().front());
@@ -164,8 +164,8 @@ TEST(Emplace, EmplaceDerived)
"dummy_name",
2,
std::make_shared<ParamsSensorOdom>(),
- PriorSensorMap({{'P',PriorSensor("P",Vector2d::Zero())},
- {'O',PriorSensor("O",Vector1d::Zero())}}));
+ SpecSensorComposite({{'P',SpecStateSensor("P",Vector2d::Zero())},
+ {'O',SpecStateSensor("O",Vector1d::Zero())}}));
auto cov = Eigen::MatrixXd::Identity(2,2);
auto cpt = CaptureBase::emplace<CaptureOdom2d>(frm, TimeStamp(0), sen, Eigen::VectorXd(2), cov, nullptr);
auto m = Eigen::Matrix<double,9,6>();
diff --git a/test/gtest_factor_diff_drive.cpp b/test/gtest_factor_diff_drive.cpp
index e69b506e3effcaf721b12eca254d616fe312441d..78f05eb997127d4dc723f0db15f2cfd8b3de9852 100644
--- a/test/gtest_factor_diff_drive.cpp
+++ b/test/gtest_factor_diff_drive.cpp
@@ -164,9 +164,9 @@ class FactorDiffDriveTest : public testing::Test
params_sen = make_shared<ParamsSensorDiffDrive>();
params_sen->ticks_per_wheel_revolution = 100; // DO NOT MODIFY THESE VALUES !!!
- PriorSensorMap priors{{'P',PriorSensor("P", Vector2d::Zero())}, //default "fix", not dynamic
- {'O',PriorSensor("O", Vector1d::Zero())}, //default "fix", not dynamic
- {'I',PriorSensor("StateBlock", Vector3d::Ones(), "initial_guess")}}; // not dynamic
+ SpecSensorComposite priors{{'P',SpecStateSensor("P", Vector2d::Zero())}, //default "fix", not dynamic
+ {'O',SpecStateSensor("O", Vector1d::Zero())}, //default "fix", not dynamic
+ {'I',SpecStateSensor("StateBlock", Vector3d::Ones(), "initial_guess")}}; // not dynamic
sensor = static_pointer_cast<SensorDiffDrive>(problem->installSensor("SensorDiffDrive",
"sensor",
@@ -466,9 +466,9 @@ TEST(FactorDiffDrive, preintegrate_and_solve_sensor_intrinsics_gt)
ParamsSensorDiffDrivePtr params_sen = make_shared<ParamsSensorDiffDrive>();
params_sen->ticks_per_wheel_revolution = 100; // DO NOT MODIFY THESE VALUES !!!
- PriorSensorMap priors{{'P',PriorSensor("P", Vector2d::Zero())}, //default "fix", not dynamic
- {'O',PriorSensor("O", Vector1d::Zero())}, //default "fix", not dynamic
- {'I',PriorSensor("StateBlock", Vector3d::Ones(), "initial_guess")}}; // not dynamic
+ SpecSensorComposite priors{{'P',SpecStateSensor("P", Vector2d::Zero())}, //default "fix", not dynamic
+ {'O',SpecStateSensor("O", Vector1d::Zero())}, //default "fix", not dynamic
+ {'I',SpecStateSensor("StateBlock", Vector3d::Ones(), "initial_guess")}}; // not dynamic
auto sensor = static_pointer_cast<SensorDiffDrive>(problem->installSensor("SensorDiffDrive",
"sensor",
@@ -601,9 +601,9 @@ TEST(FactorDiffDrive, preintegrate_and_solve_sensor_intrinsics)
ParamsSensorDiffDrivePtr params_sen = make_shared<ParamsSensorDiffDrive>();
params_sen->ticks_per_wheel_revolution = 100; // DO NOT MODIFY THESE VALUES !!!
- PriorSensorMap priors{{'P',PriorSensor("P", Vector2d::Zero())}, //default "fix", not dynamic
- {'O',PriorSensor("O", Vector1d::Zero())}, //default "fix", not dynamic
- {'I',PriorSensor("StateBlock", Vector3d::Ones(), "initial_guess")}}; // not dynamic
+ SpecSensorComposite priors{{'P',SpecStateSensor("P", Vector2d::Zero())}, //default "fix", not dynamic
+ {'O',SpecStateSensor("O", Vector1d::Zero())}, //default "fix", not dynamic
+ {'I',SpecStateSensor("StateBlock", Vector3d::Ones(), "initial_guess")}}; // not dynamic
auto sensor = static_pointer_cast<SensorDiffDrive>(problem->installSensor("SensorDiffDrive",
"sensor",
diff --git a/test/gtest_factor_relative_pose_3d_with_extrinsics.cpp b/test/gtest_factor_relative_pose_3d_with_extrinsics.cpp
index f9b7a27b5d11c80a3fab5da79cd4aff061fe948b..d9be016dd463ede52c70e9213705a563770c387c 100644
--- a/test/gtest_factor_relative_pose_3d_with_extrinsics.cpp
+++ b/test/gtest_factor_relative_pose_3d_with_extrinsics.cpp
@@ -119,8 +119,8 @@ class FactorRelativePose3dWithExtrinsics_class : public testing::Test{
S = problem->installSensor("SensorPose",
"sensor_pose_1",
std::make_shared<ParamsSensorPose>(),
- PriorSensorMap{{'P',PriorSensor("P",pos_camera)},
- {'O',PriorSensor("O",vquat_camera)}});
+ SpecSensorComposite{{'P',SpecStateSensor("P",pos_camera)},
+ {'O',SpecStateSensor("O",vquat_camera)}});
// F1 is be origin KF
VectorComposite x0(pose_robot, "PO", {3,4});
diff --git a/test/gtest_param_prior.cpp b/test/gtest_param_prior.cpp
index 8ff7aefc9b877cbb8ff74aaf62f684f427f2d1ca..3f4146a10fe9201887f3ab5a0f9c509dd39ae689 100644
--- a/test/gtest_param_prior.cpp
+++ b/test/gtest_param_prior.cpp
@@ -46,8 +46,8 @@ 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>();
-PriorSensorMap priors{{'P',PriorSensor("P",initial_extrinsics_p,"initial_guess")},
- {'O',PriorSensor("O",initial_extrinsics_o,"initial_guess")}};
+SpecSensorComposite priors{{'P',SpecStateSensor("P",initial_extrinsics_p,"initial_guess")},
+ {'O',SpecStateSensor("O",initial_extrinsics_o,"initial_guess")}};
SensorBasePtr sensor_ptr_ = problem_ptr->installSensor("SensorOdom",
"odometer",
params,
diff --git a/test/gtest_prior_sensor.cpp b/test/gtest_prior_sensor.cpp
index a850f3a5d664ee6a8ca01c6b82ab8118844e4434..53bda0b3e1379939454150e52a6e162e4b29e29e 100644
--- a/test/gtest_prior_sensor.cpp
+++ b/test/gtest_prior_sensor.cpp
@@ -22,7 +22,7 @@
#include "core/utils/utils_gtest.h"
#include "core/common/wolf.h"
-#include "core/sensor/prior_sensor.h"
+#include "core/sensor/spec_state_sensor.h"
#include "core/common/params_base.h" // toString
using namespace wolf;
@@ -47,7 +47,7 @@ struct PriorAsStruct
VectorXd drift_std;
};
-void testPriorSensor(const PriorSensor& P,
+void testPriorSensor(const SpecStateSensor& P,
const PriorAsStruct& pstruct)
{
ASSERT_EQ(pstruct.type, P.getType());
@@ -84,15 +84,15 @@ void testPriorSensorSensorMap(const std::vector<PriorAsStruct>& setups, bool sho
"\n\tdynamic: ", setup.dynamic,
"\n\tdrift_std: ", setup.drift_std.transpose());
if (should_work)
- testPriorSensor(PriorSensor(setup.type, setup.state, setup.mode, setup.noise_std, setup.dynamic, setup.drift_std), setup);
+ testPriorSensor(SpecStateSensor(setup.type, setup.state, setup.mode, setup.noise_std, setup.dynamic, setup.drift_std), setup);
else
{
- ASSERT_THROW(testPriorSensor(PriorSensor(setup.type, setup.state, setup.mode, setup.noise_std, setup.dynamic, setup.drift_std), setup),std::runtime_error);
+ ASSERT_THROW(testPriorSensor(SpecStateSensor(setup.type, setup.state, setup.mode, setup.noise_std, setup.dynamic, setup.drift_std), setup),std::runtime_error);
}
}
}
-TEST(PriorSensor, P)
+TEST(SpecStateSensor, P)
{
std::vector<PriorAsStruct> setups_ok, setups_death;
@@ -142,7 +142,7 @@ TEST(PriorSensor, P)
testPriorSensorSensorMap(setups_death, false);
}
-TEST(PriorSensor, O)
+TEST(SpecStateSensor, O)
{
std::vector<PriorAsStruct> setups_ok, setups_death;
@@ -198,7 +198,7 @@ TEST(PriorSensor, O)
testPriorSensorSensorMap(setups_death, false);
}
-TEST(PriorSensor, StateBlock)
+TEST(SpecStateSensor, StateBlock)
{
std::vector<PriorAsStruct> setups_ok, setups_death;
@@ -247,7 +247,7 @@ TEST(PriorSensor, StateBlock)
testPriorSensorSensorMap(setups_ok, true);
testPriorSensorSensorMap(setups_death, false);
}
-TEST(PriorSensor, StateAngle)
+TEST(SpecStateSensor, StateAngle)
{
std::vector<PriorAsStruct> setups_ok, setups_death;
@@ -288,7 +288,7 @@ TEST(PriorSensor, StateAngle)
testPriorSensorSensorMap(setups_death, false);
}
-TEST(PriorSensor, StateQuaternion)
+TEST(SpecStateSensor, StateQuaternion)
{
std::vector<PriorAsStruct> setups_ok, setups_death;
@@ -335,7 +335,7 @@ TEST(PriorSensor, StateQuaternion)
testPriorSensorSensorMap(setups_death, false);
}
-TEST(PriorSensor, YamlNode)
+TEST(SpecStateSensor, YamlNode)
{
YAML::Node input_node = YAML::LoadFile(wolf_root + "/test/yaml/params_prior.yaml");
@@ -364,7 +364,7 @@ TEST(PriorSensor, YamlNode)
std::string prefix = key + "_" + toString(dim) + "D_" + mode + (dynamic ? "_dynamic" : "") + (drift ? "_drift" : "");
WOLF_INFO("Creating prior from prefix ", prefix);
- auto P = PriorSensor(input_node[prefix]);
+ auto P = SpecStateSensor(input_node[prefix]);
// Checks
ASSERT_EQ(P.getMode(), mode);
@@ -401,7 +401,7 @@ TEST(PriorSensor, YamlNode)
std::string prefix = "I_" + mode + (dynamic ? "_dynamic" : "") + (drift ? "_drift" : "");
WOLF_INFO("Creating prior from prefix ", prefix);
- auto P = PriorSensor(input_node[prefix]);
+ auto P = SpecStateSensor(input_node[prefix]);
ASSERT_EQ(P.getMode(), mode);
ASSERT_EQ(P.isDynamic(), dynamic);
ASSERT_MATRIX_APPROX(P.getState(),p_state,wolf::Constants::EPS);
@@ -416,7 +416,7 @@ TEST(PriorSensor, YamlNode)
}
}
-TEST(PriorSensor, ParamsServerWrong)
+TEST(SpecStateSensor, ParamsServerWrong)
{
YAML::Node input_node = YAML::LoadFile(wolf_root + "/test/yaml/params_prior_wrong.yaml");
@@ -440,7 +440,7 @@ TEST(PriorSensor, ParamsServerWrong)
std::string prefix = key + "_" + toString(dim) + "D_" + mode + (dynamic ? "_dynamic" : "") + (drift ? "_drift" : "");
WOLF_INFO("Creating prior from prefix ", prefix);
- ASSERT_THROW(auto prior = PriorSensor(input_node[prefix]),std::runtime_error);
+ ASSERT_THROW(auto prior = SpecStateSensor(input_node[prefix]),std::runtime_error);
}
// I
@@ -454,7 +454,7 @@ TEST(PriorSensor, ParamsServerWrong)
std::string prefix = "I_" + mode + (dynamic ? "_dynamic" : "") + (drift ? "_drift" : "");
WOLF_INFO("Creating prior from prefix ", prefix);
- ASSERT_THROW(auto prior = PriorSensor(input_node[prefix]),std::runtime_error);
+ ASSERT_THROW(auto prior = SpecStateSensor(input_node[prefix]),std::runtime_error);
}
}
diff --git a/test/gtest_problem.cpp b/test/gtest_problem.cpp
index a019989126e9904dcfc3a87ecaf1fe385f48aeb0..6c8dbf4ac368867c90052eb034125156a42f9b76 100644
--- a/test/gtest_problem.cpp
+++ b/test/gtest_problem.cpp
@@ -80,8 +80,8 @@ TEST(Problem, Sensors)
params->name = "dummy_name";
auto S = SensorBase::emplace<SensorDummy3d>(P->getHardware(),
params,
- PriorSensorMap({{'P',PriorSensor("P",Vector3d::Zero())},
- {'O',PriorSensor("O",Vector4d::Random().normalized())}}));
+ SpecSensorComposite({{'P',SpecStateSensor("P",Vector3d::Zero())},
+ {'O',SpecStateSensor("O",Vector4d::Random().normalized())}}));
// check pointers
ASSERT_EQ(P, S->getProblem());
ASSERT_EQ(P->getHardware(), S->getHardware());
@@ -100,8 +100,8 @@ TEST(Problem, Processor)
params->name = "dummy_name";
auto Sm = SensorBase::emplace<SensorOdom3d>(P->getHardware(),
params,
- PriorSensorMap({{'P',PriorSensor("P",Vector3d::Zero())},
- {'O',PriorSensor("O",Vector4d::Random().normalized())}}));
+ SpecSensorComposite({{'P',SpecStateSensor("P",Vector3d::Zero())},
+ {'O',SpecStateSensor("O",Vector4d::Random().normalized())}}));
// add motion processor
auto Pm = ProcessorBase::emplace<ProcessorOdom3d>(Sm, std::make_shared<ParamsProcessorOdom3d>());
@@ -137,13 +137,13 @@ TEST(Problem, Installers)
TEST(Problem, SetOrigin_PO_2d)
{
ProblemPtr P = Problem::create("PO", 2);
- TimeStamp t0(0);
- // Eigen::VectorXd x0(3); x0 << 1,2,3;
- VectorComposite x0(Vector3d(1,2,3), "PO", {2,1});
- // Eigen::MatrixXd P0(Eigen::MatrixXd::Identity(3,3) * 0.1); // P0 is 0.1*Id
- VectorComposite s0(Vector3d(sqrt(0.1),sqrt(0.1),sqrt(0.1)), "PO", {2,1});
+ TimeStamp t0(0);
+ SpecComposite prior;
+ prior.emplace('P',SpecState("P",Vector2d(1,2),"factor",Vector2d(sqrt(0.1),sqrt(0.1))));
+ prior.emplace('O',SpecState("O",Vector1d(3),"factor",Vector1d(sqrt(0.1))));
+ P->setPrior(prior, t0);
- P->setPriorFactor(x0, s0, t0);
+ // P->setPriorFactor(x0, s0, t0);
WOLF_INFO("printing.-..");
P->print(4,1,1,1);
@@ -151,7 +151,7 @@ TEST(Problem, SetOrigin_PO_2d)
ASSERT_EQ(P->getHardware()->getSensorList().size(), (SizeStd) 0);
// check that the state is correct
- ASSERT_POSE2d_APPROX(x0.vector("PO"), P->getState().vector("PO"), Constants::EPS_SMALL );
+ ASSERT_POSE2d_APPROX(prior.getState().vector("PO"), P->getState().vector("PO"), Constants::EPS_SMALL );
// check that we have one frame, one capture, one feature, one factor
TrajectoryBasePtr T = P->getTrajectory();
@@ -180,15 +180,15 @@ TEST(Problem, SetOrigin_PO_2d)
ASSERT_FALSE(fac->getCaptureOther());
ASSERT_FALSE(fac->getFeatureOther());
}
- auto x0_vector = x0.vector("PO");
- auto s0_vector = s0.vector("PO");
- MatrixXd P0_matrix = (s0_vector.array() * s0_vector.array()).matrix().asDiagonal();
+ auto x0_vector = prior.getState().vector("PO");
+ auto P0_p = prior.at('P').getNoiseStd().cwiseAbs2().asDiagonal();
+ auto P0_o = prior.at('O').getNoiseStd().cwiseAbs2().asDiagonal();
// check that the Feature measurement and covariance are the ones provided
ASSERT_MATRIX_APPROX(x0_vector.head<2>(), fp->getMeasurement(), Constants::EPS_SMALL );
ASSERT_MATRIX_APPROX(x0_vector.tail<1>(), fo->getMeasurement(), Constants::EPS_SMALL );
- ASSERT_MATRIX_APPROX(P0_matrix.topLeftCorner(2,2), fp->getMeasurementCovariance(), Constants::EPS_SMALL );
- ASSERT_MATRIX_APPROX(P0_matrix.bottomRightCorner(1,1), fo->getMeasurementCovariance(), Constants::EPS_SMALL );
+ ASSERT_MATRIX_APPROX(P0_p, fp->getMeasurementCovariance(), Constants::EPS_SMALL );
+ ASSERT_MATRIX_APPROX(P0_o, fo->getMeasurementCovariance(), Constants::EPS_SMALL );
// P->print(4,1,1,1);
}
@@ -197,20 +197,24 @@ TEST(Problem, SetOrigin_PO_3d)
{
ProblemPtr P = Problem::create("PO", 3);
TimeStamp t0(0);
- Eigen::VectorXd vec7(7); vec7 << 1,2,3,4,5,6,7;
- vec7.tail(4).normalize();
- VectorComposite x0(vec7, "PO", {3,4});
+ SpecComposite prior;
+ prior.emplace('P',SpecState("P",Vector3d(1,2,3),"factor",Vector3d(sqrt(0.1),sqrt(0.1),sqrt(0.1))));
+ prior.emplace('O',SpecState("O",Vector4d(4,5,6,7).normalized(),"factor",Vector3d(sqrt(0.1),sqrt(0.1),sqrt(0.1))));
+ P->setPrior(prior, t0);
+ // Eigen::VectorXd vec7(7); vec7 << 1,2,3,4,5,6,7;
+ // vec7.tail(4).normalize();
+ // VectorComposite x0(vec7, "PO", {3,4});
// Eigen::MatrixXd P0(6,6); P0.setIdentity(); P0 *= 0.1; // P0 is 0.1*Id
- Eigen::VectorXd vec6(6); vec6 << sqrt(0.1), sqrt(0.1), sqrt(0.1), sqrt(0.1), sqrt(0.1), sqrt(0.1);
- VectorComposite s0(vec6, "PO", {3,3});
+ // Eigen::VectorXd vec6(6); vec6 << sqrt(0.1), sqrt(0.1), sqrt(0.1), sqrt(0.1), sqrt(0.1), sqrt(0.1);
+ // VectorComposite s0(vec6, "PO", {3,3});
- P->setPriorFactor(x0, s0, t0);
+ // P->setPriorFactor(x0, s0, t0);
// check that no sensor has been added
ASSERT_EQ(P->getHardware()->getSensorList().size(), (SizeStd) 0);
// check that the state is correct
- ASSERT_TRUE((x0.vector("PO") - P->getState().vector("PO")).isMuchSmallerThan(1, Constants::EPS_SMALL));
+ ASSERT_TRUE((prior.getState().vector("PO") - P->getState().vector("PO")).isMuchSmallerThan(1, Constants::EPS_SMALL));
// check that we have one frame, one capture, one feature, one factor
TrajectoryBasePtr T = P->getTrajectory();
@@ -240,14 +244,15 @@ TEST(Problem, SetOrigin_PO_3d)
ASSERT_FALSE(fac->getFeatureOther());
}
- auto x0_vector = x0.vector("PO");
- auto s0_vector = s0.vector("PO");
- MatrixXd P0_matrix = (s0_vector.array() * s0_vector.array()).matrix().asDiagonal();
+ auto x0_vector = prior.getState().vector("PO");
+ auto P0_p = prior.at('P').getNoiseStd().cwiseAbs2().asDiagonal();
+ auto P0_o = prior.at('O').getNoiseStd().cwiseAbs2().asDiagonal();
+
// check that the Feature measurement and covariance are the ones provided
ASSERT_MATRIX_APPROX(x0_vector.head<3>(), fp->getMeasurement(), Constants::EPS_SMALL );
ASSERT_MATRIX_APPROX(x0_vector.tail<4>(), fo->getMeasurement(), Constants::EPS_SMALL );
- ASSERT_MATRIX_APPROX(P0_matrix.topLeftCorner(3,3), fp->getMeasurementCovariance(), Constants::EPS_SMALL );
- ASSERT_MATRIX_APPROX(P0_matrix.bottomRightCorner(3,3), fo->getMeasurementCovariance(), Constants::EPS_SMALL );
+ ASSERT_MATRIX_APPROX(P0_p, fp->getMeasurementCovariance(), Constants::EPS_SMALL );
+ ASSERT_MATRIX_APPROX(P0_o, fo->getMeasurementCovariance(), Constants::EPS_SMALL );
// P->print(4,1,1,1);
}
@@ -386,9 +391,9 @@ TEST(Problem, perturb)
param->ticks_per_wheel_revolution = 100;
auto sensor = SensorBase::emplace<SensorDiffDrive>(problem->getHardware(),
param,
- PriorSensorMap({{'P',PriorSensor("P",Vector2d::Zero())},
- {'O',PriorSensor("O",Vector1d::Zero())},
- {'I',PriorSensor("StateParams3",Vector3d::Ones(),"initial_guess", VectorXd(0),true)}}));
+ SpecSensorComposite({{'P',SpecStateSensor("P",Vector2d::Zero())},
+ {'O',SpecStateSensor("O",Vector1d::Zero())},
+ {'I',SpecStateSensor("StateParams3",Vector3d::Ones(),"initial_guess", VectorXd(0),true)}}));
Vector3d pose; pose << 0,0,0;
@@ -472,9 +477,9 @@ TEST(Problem, check)
param->ticks_per_wheel_revolution = 100;
auto sensor = SensorBase::emplace<SensorDiffDrive>(problem->getHardware(),
param,
- PriorSensorMap({{'P',PriorSensor("P",Vector2d::Zero())},
- {'O',PriorSensor("O",Vector1d::Zero())},
- {'I',PriorSensor("StateParams3",Vector3d::Ones(),"initial_guess", VectorXd(0),true)}}));
+ SpecSensorComposite({{'P',SpecStateSensor("P",Vector2d::Zero())},
+ {'O',SpecStateSensor("O",Vector1d::Zero())},
+ {'I',SpecStateSensor("StateParams3",Vector3d::Ones(),"initial_guess", VectorXd(0),true)}}));
Vector3d pose; pose << 0,0,0;
int i = 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 fb321e4295e06594e899b5f93250097f86ee9802..08578a640eec7afda29a42b3aa84b76d28424a30 100644
--- a/test/gtest_processor_and_factor_pose_3d_with_extrinsics.cpp
+++ b/test/gtest_processor_and_factor_pose_3d_with_extrinsics.cpp
@@ -130,8 +130,8 @@ class FactorPose3dWithExtrinsicsBase_Test : public testing::Test
params_sp->std_p = 1;
params_sp->std_o = 1;
- sensor_pose_ = problem_->installSensor("SensorPose", "pose", params_sp, PriorSensorMap{{'P',PriorSensor("P",b_p_bm_)},
- {'O',PriorSensor("O",b_q_m_.coeffs())}});
+ sensor_pose_ = problem_->installSensor("SensorPose", "pose", params_sp, SpecSensorComposite{{'P',SpecStateSensor("P",b_p_bm_)},
+ {'O',SpecStateSensor("O",b_q_m_.coeffs())}});
auto params_proc = std::make_shared<ParamsProcessorPose>();
params_proc->time_tolerance = 0.5;
auto proc_pose = problem_->installProcessor("ProcessorPose", "pose", sensor_pose_, params_proc);
diff --git a/test/gtest_processor_base.cpp b/test/gtest_processor_base.cpp
index 81c3ac989141fd8d65bb351baafeed15c9373be8..1c484638d4617173a7906a571cf865c2656f9d5c 100644
--- a/test/gtest_processor_base.cpp
+++ b/test/gtest_processor_base.cpp
@@ -123,11 +123,9 @@ TEST(ProcessorBase, KeyFrameCallback)
// initialize
TimeStamp t(0.0);
- // Vector3d x(0,0,0);
- VectorComposite x(Vector3d(0,0,0), "PO", {2,1});
- // Matrix3d P = Matrix3d::Identity() * 0.1;
- VectorComposite P(Vector3d(sqrt(0.1),sqrt(0.1),sqrt(0.1)), "PO", {2,1});
- problem->setPriorFactor(x, P, t); // KF1
+ SpecComposite prior{{'P',SpecState("P",Vector2d(0,0),"factor",Vector2d(sqrt(0.1),sqrt(0.1)))},
+ {'O',SpecState("O",Vector1d(0),"factor",Vector1d(sqrt(0.1)))}};
+ problem->setPrior(prior, t);
CaptureOdom2dPtr capt_odo = make_shared<CaptureOdom2d>(t, sens_odo, Vector2d(0.5,0));
diff --git a/test/gtest_processor_diff_drive.cpp b/test/gtest_processor_diff_drive.cpp
index 79a3b59cdb88f242d4c000bf5f9929dbcb462d81..c67f9294fb9bff2ac5d1e82f827642b459f54a6a 100644
--- a/test/gtest_processor_diff_drive.cpp
+++ b/test/gtest_processor_diff_drive.cpp
@@ -144,9 +144,9 @@ class ProcessorDiffDriveTest : public testing::Test
params_sen = std::make_shared<ParamsSensorDiffDrive>();
params_sen->ticks_per_wheel_revolution = 100; // DO NOT MODIFY THESE VALUES !!!
- PriorSensorMap priors{{'P',PriorSensor("P", Vector2d::Zero())}, //default "fix", not dynamic
- {'O',PriorSensor("O", Vector1d::Zero())}, //default "fix", not dynamic
- {'I',PriorSensor("StateBlock", Vector3d::Ones(), "initial_guess")}}; // not dynamic DO NOT MODIFY THESE VALUES !!!
+ SpecSensorComposite priors{{'P',SpecStateSensor("P", Vector2d::Zero())}, //default "fix", not dynamic
+ {'O',SpecStateSensor("O", Vector1d::Zero())}, //default "fix", not dynamic
+ {'I',SpecStateSensor("StateBlock", Vector3d::Ones(), "initial_guess")}}; // not dynamic DO NOT MODIFY THESE VALUES !!!
sensor = std::static_pointer_cast<SensorDiffDrive>(problem->installSensor("SensorDiffDrive",
"sensor",
diff --git a/test/gtest_processor_motion.cpp b/test/gtest_processor_motion.cpp
index 8b157d5ee90eee6ec002855695e960b5edec9bf7..443e4f1edd4e02ea85e849476c34cc7e8ada6423 100644
--- a/test/gtest_processor_motion.cpp
+++ b/test/gtest_processor_motion.cpp
@@ -177,11 +177,10 @@ TEST_F(ProcessorMotion_test, IntegrateStraightFactorPrior)
{
// Prior
TimeStamp t(0.0);
- // Vector3d x0; x0 << 0, 0, 0;
- VectorComposite x0(Vector3d(0,0,0), "PO", {2,1});
- // Matrix3d P0; P0.setIdentity();
- VectorComposite s0(Vector3d(1,1,1), "PO", {2,1});
- auto KF_0 = problem->setPriorFactor(x0, s0, t);
+ SpecComposite prior;
+ prior.emplace('P',SpecState("P",Vector2d(0,0),"factor",Vector2d(1,1)));
+ prior.emplace('O',SpecState("O",Vector1d(0),"factor",Vector1d(1)));
+ auto KF_0 = problem->setPrior(prior, t);
processor->setOrigin(KF_0);
data << 1, 0; // advance straight
@@ -204,11 +203,9 @@ TEST_F(ProcessorMotion_test, IntegrateStraightFixPrior)
{
// Prior
TimeStamp t(0.0);
- // Vector3d x0; x0 << 0, 0, 0;
- // Matrix3d P0; P0.setIdentity();
- VectorComposite x0(Vector3d(0,0,0), "PO", {2,1});
- VectorComposite s0(Vector3d(1,1,1), "PO", {2,1});
- auto KF_0 = problem->setPriorFix(x0, t);
+ SpecComposite prior{{'P',SpecState("P",Vector2d(0,0),"fix")},
+ {'O',SpecState("O",Vector1d(0),"fix")}};
+ auto KF_0 = problem->setPrior(prior, t);
processor->setOrigin(KF_0);
data << 1, 0; // advance straight
@@ -254,11 +251,9 @@ TEST_F(ProcessorMotion_test, IntegrateCircleFactorPrior)
{
// Prior
TimeStamp t(0.0);
- // Vector3d x0; x0 << 0, 0, 0;
- VectorComposite x0(Vector3d(0,0,0), "PO", {2,1});
- // Matrix3d P0; P0.setIdentity();
- VectorComposite s0(Vector3d(1,1,1), "PO", {2,1});
- auto KF_0 = problem->setPriorFactor(x0, s0, t);
+ SpecComposite prior{{'P',SpecState("P",Vector2d(0,0),"factor",Vector2d(1,1))},
+ {'O',SpecState("O",Vector1d(0),"factor",Vector1d(1))}};
+ auto KF_0 = problem->setPrior(prior, t);
processor->setOrigin(KF_0);
data << 1, 2*M_PI/10; // advance in circle
@@ -281,11 +276,10 @@ TEST_F(ProcessorMotion_test, IntegrateCircleFixPrior)
{
// Prior
TimeStamp t(0.0);
- // Vector3d x0; x0 << 0, 0, 0;
- // Matrix3d P0; P0.setIdentity();
- VectorComposite x0(Vector3d(0,0,0), "PO", {2,1});
- VectorComposite s0(Vector3d(1,1,1), "PO", {2,1});
- auto KF_0 = problem->setPriorFix(x0, t);
+ SpecComposite prior;
+ prior.emplace('P',SpecState("P",Vector2d(0,0),"fix"));
+ prior.emplace('O',SpecState("O",Vector1d(0),"fix"));
+ auto KF_0 = problem->setPrior(prior, t);
processor->setOrigin(KF_0);
data << 1, 2*M_PI/10; // advance in circle
@@ -424,11 +418,9 @@ TEST_F(ProcessorMotion_test, splitBufferFactorPrior)
{
// Prior
TimeStamp t(0.0);
- // Vector3d x0; x0 << 0, 0, 0;
- VectorComposite x0(Vector3d(0,0,0), "PO", {2,1});
- // Matrix3d P0; P0.setIdentity();
- VectorComposite s0(Vector3d(1,1,1), "PO", {2,1});
- auto KF_0 = problem->setPriorFactor(x0, s0, t);
+ SpecComposite prior{{'P',SpecState("P",Vector2d(0,0),"factor",Vector2d(1,1))},
+ {'O',SpecState("O",Vector1d(0),"factor",Vector1d(1))}};
+ auto KF_0 = problem->setPrior(prior, t);
processor->setOrigin(KF_0);
data << 1, 2*M_PI/10; // advance in circle
@@ -468,11 +460,9 @@ TEST_F(ProcessorMotion_test, splitBufferFixPrior)
{
// Prior
TimeStamp t(0.0);
- // Vector3d x0; x0 << 0, 0, 0;
- // Matrix3d P0; P0.setIdentity();
- VectorComposite x0(Vector3d(0,0,0), "PO", {2,1});
- VectorComposite s0(Vector3d(1,1,1), "PO", {2,1});
- auto KF_0 = problem->setPriorFix(x0, t);
+ SpecComposite prior{{'P',SpecState("P",Vector2d(0,0),"fix")},
+ {'O',SpecState("O",Vector1d(0),"fix")}};
+ auto KF_0 = problem->setPrior(prior, t);
processor->setOrigin(KF_0);
data << 1, 2*M_PI/10; // advance in circle
diff --git a/test/gtest_sensor_base.cpp b/test/gtest_sensor_base.cpp
index 0a12ed5723484c4a2ac7b326669bb304504ce49b..634a8b795c476185f9031a0fcf1e1bd72bb4cf33 100644
--- a/test/gtest_sensor_base.cpp
+++ b/test/gtest_sensor_base.cpp
@@ -104,8 +104,8 @@ TEST(SensorBase, makeshared_priors_POfix2D)
params->noise_o_std = noise_o_std;
auto S = std::make_shared<SensorDummy2d>(params,
- PriorSensorMap({{'P',PriorSensor("P", p_state_2D)}, //default "fix", not dynamic
- {'O',PriorSensor("O", o_state_2D)}}));
+ SpecSensorComposite({{'P',SpecStateSensor("P", p_state_2D)}, //default "fix", not dynamic
+ {'O',SpecStateSensor("O", o_state_2D)}}));
// noise
ASSERT_MATRIX_APPROX(S->computeNoiseCov(vector0), noise_cov_dummy, Constants::EPS);
@@ -124,8 +124,8 @@ TEST(SensorBase, makeshared_priors_POfix3D)
params->noise_o_std = noise_o_std;
auto S = std::make_shared<SensorDummy3d>(params,
- PriorSensorMap({{'P',PriorSensor("P", p_state_3D)}, //default "fix", not dynamic
- {'O',PriorSensor("O", o_state_3D)}}));
+ SpecSensorComposite({{'P',SpecStateSensor("P", p_state_3D)}, //default "fix", not dynamic
+ {'O',SpecStateSensor("O", o_state_3D)}}));
// noise
ASSERT_MATRIX_APPROX(S->computeNoiseCov(vector0), noise_cov_dummy, Constants::EPS);
@@ -147,8 +147,8 @@ TEST(SensorBase, makeshared_priors_POinitial_guess2D)
params->noise_o_std = noise_o_std;
auto S = std::make_shared<SensorDummy2d>(params,
- PriorSensorMap({{'P',PriorSensor("P", p_state_2D, "initial_guess")},
- {'O',PriorSensor("O", o_state_2D, "initial_guess")}}));
+ SpecSensorComposite({{'P',SpecStateSensor("P", p_state_2D, "initial_guess")},
+ {'O',SpecStateSensor("O", o_state_2D, "initial_guess")}}));
// noise
ASSERT_MATRIX_APPROX(S->computeNoiseCov(vector0), noise_cov_dummy, Constants::EPS);
@@ -170,8 +170,8 @@ TEST(SensorBase, makeshared_priors_POinitial_guess3D)
params->noise_o_std = noise_o_std;
auto S = std::make_shared<SensorDummy3d>(params,
- PriorSensorMap({{'P',PriorSensor("P", p_state_3D, "initial_guess")},
- {'O',PriorSensor("O", o_state_3D, "initial_guess")}}));
+ SpecSensorComposite({{'P',SpecStateSensor("P", p_state_3D, "initial_guess")},
+ {'O',SpecStateSensor("O", o_state_3D, "initial_guess")}}));
// noise
ASSERT_MATRIX_APPROX(S->computeNoiseCov(vector0), noise_cov_dummy, Constants::EPS);
@@ -193,8 +193,8 @@ TEST(SensorBase, makeshared_priors_POfactor2D)
params->noise_o_std = noise_o_std;
auto S = std::make_shared<SensorDummy2d>(params,
- PriorSensorMap({{'P',PriorSensor("P", p_state_2D, "factor", p_std_2D)},
- {'O',PriorSensor("O", o_state_2D, "factor", o_std_2D)}}));
+ SpecSensorComposite({{'P',SpecStateSensor("P", p_state_2D, "factor", p_std_2D)},
+ {'O',SpecStateSensor("O", o_state_2D, "factor", o_std_2D)}}));
// noise
ASSERT_MATRIX_APPROX(S->computeNoiseCov(vector0), noise_cov_dummy, Constants::EPS);
@@ -216,8 +216,8 @@ TEST(SensorBase, makeshared_priors_POfactor3D)
params->noise_o_std = noise_o_std;
auto S = std::make_shared<SensorDummy3d>(params,
- PriorSensorMap({{'P',PriorSensor("P", p_state_3D, "factor", p_std_3D)},
- {'O',PriorSensor("O", o_state_3D, "factor", o_std_3D)}}));
+ SpecSensorComposite({{'P',SpecStateSensor("P", p_state_3D, "factor", p_std_3D)},
+ {'O',SpecStateSensor("O", o_state_3D, "factor", o_std_3D)}}));
// noise
ASSERT_MATRIX_APPROX(S->computeNoiseCov(vector0), noise_cov_dummy, Constants::EPS);
@@ -239,8 +239,8 @@ TEST(SensorBase, makeshared_priors_POinitial_guess_dynamic2D)
params->noise_o_std = noise_o_std;
auto S = std::make_shared<SensorDummy2d>(params,
- PriorSensorMap({{'P',PriorSensor("P", p_state_2D, "initial_guess", vector0, true)},
- {'O',PriorSensor("O", o_state_2D, "initial_guess", vector0, true)}}));
+ SpecSensorComposite({{'P',SpecStateSensor("P", p_state_2D, "initial_guess", vector0, true)},
+ {'O',SpecStateSensor("O", o_state_2D, "initial_guess", vector0, true)}}));
// noise
ASSERT_MATRIX_APPROX(S->computeNoiseCov(vector0), noise_cov_dummy, Constants::EPS);
@@ -262,8 +262,8 @@ TEST(SensorBase, makeshared_priors_POinitial_guess_dynamic3D)
params->noise_o_std = noise_o_std;
auto S = std::make_shared<SensorDummy3d>(params,
- PriorSensorMap({{'P',PriorSensor("P", p_state_3D, "initial_guess", vector0, true)},
- {'O',PriorSensor("O", o_state_3D, "initial_guess", vector0, true)}}));
+ SpecSensorComposite({{'P',SpecStateSensor("P", p_state_3D, "initial_guess", vector0, true)},
+ {'O',SpecStateSensor("O", o_state_3D, "initial_guess", vector0, true)}}));
// noise
ASSERT_MATRIX_APPROX(S->computeNoiseCov(vector0), noise_cov_dummy, Constants::EPS);
@@ -285,8 +285,8 @@ TEST(SensorBase, makeshared_priors_POinitial_guess_dynamic2D_drift)
params->noise_o_std = noise_o_std;
auto S = std::make_shared<SensorDummy2d>(params,
- PriorSensorMap({{'P',PriorSensor("P", p_state_2D, "initial_guess", vector0, true, p_std_2D)},
- {'O',PriorSensor("O", o_state_2D, "initial_guess", vector0, true, o_std_2D)}}));
+ SpecSensorComposite({{'P',SpecStateSensor("P", p_state_2D, "initial_guess", vector0, true, p_std_2D)},
+ {'O',SpecStateSensor("O", o_state_2D, "initial_guess", vector0, true, o_std_2D)}}));
// noise
ASSERT_MATRIX_APPROX(S->computeNoiseCov(vector0), noise_cov_dummy, Constants::EPS);
@@ -308,8 +308,8 @@ TEST(SensorBase, makeshared_priors_POinitial_guess_dynamic3D_drift)
params->noise_o_std = noise_o_std;
auto S = std::make_shared<SensorDummy3d>(params,
- PriorSensorMap({{'P',PriorSensor("P", p_state_3D, "initial_guess", vector0, true, p_std_3D)},
- {'O',PriorSensor("O", o_state_3D, "initial_guess", vector0, true, o_std_3D)}}));
+ SpecSensorComposite({{'P',SpecStateSensor("P", p_state_3D, "initial_guess", vector0, true, p_std_3D)},
+ {'O',SpecStateSensor("O", o_state_3D, "initial_guess", vector0, true, o_std_3D)}}));
// noise
ASSERT_MATRIX_APPROX(S->computeNoiseCov(vector0), noise_cov_dummy, Constants::EPS);
@@ -333,10 +333,10 @@ TEST(SensorBase, makeshared_priors_POIA_mixed)
VectorXd i_state_3D = VectorXd::Random(5);
auto S = std::make_shared<SensorDummy3d>(params,
- PriorSensorMap({{'P',PriorSensor("P", p_state_3D, "fix", vector0, true)},
- {'O',PriorSensor("O", o_state_3D, "factor", o_std_3D, true, o_std_3D)},
- {'I',PriorSensor("StateParams5", i_state_3D, "initial_guess")},
- {'A',PriorSensor("StateQuaternion", o_state_3D, "factor", o_std_3D)}}));
+ SpecSensorComposite({{'P',SpecStateSensor("P", p_state_3D, "fix", vector0, true)},
+ {'O',SpecStateSensor("O", 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)}}));
// noise
ASSERT_MATRIX_APPROX(S->computeNoiseCov(vector0), noise_cov_dummy, Constants::EPS);
diff --git a/test/gtest_sensor_diff_drive.cpp b/test/gtest_sensor_diff_drive.cpp
index 7271e389f018ab8b9a4dd7d490a05817eb3742b3..1ab3e464fd1f2692a15afb04f802118085943141 100644
--- a/test/gtest_sensor_diff_drive.cpp
+++ b/test/gtest_sensor_diff_drive.cpp
@@ -45,9 +45,9 @@ TEST(SensorDiffDrive, constructor_priors)
{
auto param = std::make_shared<ParamsSensorDiffDrive>();
- PriorSensorMap priors{{'P',PriorSensor("P", p_state)}, //default "fix", not dynamic
- {'O',PriorSensor("O", o_state)}, //default "fix", not dynamic
- {'I',PriorSensor("StateBlock", i_state)}}; //default "fix", not dynamic
+ SpecSensorComposite priors{{'P',SpecStateSensor("P", p_state)}, //default "fix", not dynamic
+ {'O',SpecStateSensor("O", o_state)}, //default "fix", not dynamic
+ {'I',SpecStateSensor("StateBlock", i_state)}}; //default "fix", not dynamic
auto sen = std::make_shared<SensorDiffDrive>("sensor_1", 2, param, priors);
@@ -114,9 +114,9 @@ TEST(SensorDiffDrive, factory_priors)
{
auto param = std::make_shared<ParamsSensorDiffDrive>();
- PriorSensorMap priors{{'P',PriorSensor("P", p_state)}, //default "fix", not dynamic
- {'O',PriorSensor("O", o_state)}, //default "fix", not dynamic
- {'I',PriorSensor("StateBlock", i_state)}}; //default "fix", not dynamic
+ SpecSensorComposite priors{{'P',SpecStateSensor("P", p_state)}, //default "fix", not dynamic
+ {'O',SpecStateSensor("O", o_state)}, //default "fix", not dynamic
+ {'I',SpecStateSensor("StateBlock", i_state)}}; //default "fix", not dynamic
auto sb = FactorySensorPriors::create("SensorDiffDrive","sensor_1", 2, param, priors);
@@ -137,9 +137,9 @@ TEST(SensorDiffDrive, getParams)
param->ticks_per_wheel_revolution = 400;
param->ticks_std_factor = 2;
- PriorSensorMap priors{{'P',PriorSensor("P", p_state)}, //default "fix", not dynamic
- {'O',PriorSensor("O", o_state)}, //default "fix", not dynamic
- {'I',PriorSensor("StateBlock", i_state)}}; //default "fix", not dynamic
+ SpecSensorComposite priors{{'P',SpecStateSensor("P", p_state)}, //default "fix", not dynamic
+ {'O',SpecStateSensor("O", o_state)}, //default "fix", not dynamic
+ {'I',SpecStateSensor("StateBlock", i_state)}}; //default "fix", not dynamic
auto sen = std::make_shared<SensorDiffDrive>("sensor_1", 2, param, priors);
@@ -156,9 +156,9 @@ TEST(SensorDiffDrive, computeNoiseCov)
param->ticks_per_wheel_revolution = 400;
param->ticks_std_factor = 2;
- PriorSensorMap priors{{'P',PriorSensor("P", p_state)}, //default "fix", not dynamic
- {'O',PriorSensor("O", o_state)}, //default "fix", not dynamic
- {'I',PriorSensor("StateBlock", i_state)}}; //default "fix", not dynamic
+ SpecSensorComposite priors{{'P',SpecStateSensor("P", p_state)}, //default "fix", not dynamic
+ {'O',SpecStateSensor("O", o_state)}, //default "fix", not dynamic
+ {'I',SpecStateSensor("StateBlock", i_state)}}; //default "fix", not dynamic
auto sen = std::make_shared<SensorDiffDrive>("sensor_1", 2, param, priors);
diff --git a/test/gtest_sensor_odom.cpp b/test/gtest_sensor_odom.cpp
index de64ac89351097bb5444526f2586773962f56161..54d9a21b2e354a9c94b9acd3d84e2eed692180b4 100644
--- a/test/gtest_sensor_odom.cpp
+++ b/test/gtest_sensor_odom.cpp
@@ -110,8 +110,8 @@ TEST(SensorOdom, makeshared_priors_fix_2D)
params->min_rot_var = min_rot_var;
auto S = std::make_shared<SensorOdom>("sensor1", 2, params,
- PriorSensorMap({{'P',PriorSensor("P", p_state_2D)}, //default "fix", not dynamic
- {'O',PriorSensor("O", o_state_2D)}}));
+ SpecSensorComposite({{'P',SpecStateSensor("P", p_state_2D)}, //default "fix", not dynamic
+ {'O',SpecStateSensor("O", o_state_2D)}}));
// checks
checkSensor(S, 'P', p_state_2D, true, vector0, false, vector0);
@@ -129,8 +129,8 @@ TEST(SensorOdom, makeshared_priors_fix_3D)
params->min_rot_var = min_rot_var;
auto S = std::make_shared<SensorOdom>("sensor1", 3, params,
- PriorSensorMap({{'P',PriorSensor("P", p_state_3D)}, //default "fix", not dynamic
- {'O',PriorSensor("O", o_state_3D)}}));
+ SpecSensorComposite({{'P',SpecStateSensor("P", p_state_3D)}, //default "fix", not dynamic
+ {'O',SpecStateSensor("O", o_state_3D)}}));
// factors
ASSERT_EQ(S->getPriorFeatures().size(), 0);
@@ -151,8 +151,8 @@ TEST(SensorOdom, makeshared_priors_initial_guess_2D)
params->min_rot_var = min_rot_var;
auto S = std::make_shared<SensorOdom>("sensor1", 2, params,
- PriorSensorMap({{'P',PriorSensor("P", p_state_2D, "initial_guess")},
- {'O',PriorSensor("O", o_state_2D, "initial_guess")}}));
+ SpecSensorComposite({{'P',SpecStateSensor("P", p_state_2D, "initial_guess")},
+ {'O',SpecStateSensor("O", o_state_2D, "initial_guess")}}));
// factors
ASSERT_EQ(S->getPriorFeatures().size(), 0);
@@ -173,8 +173,8 @@ TEST(SensorOdom, makeshared_priors_initial_guess_3D)
params->min_rot_var = min_rot_var;
auto S = std::make_shared<SensorOdom>("sensor1", 3, params,
- PriorSensorMap({{'P',PriorSensor("P", p_state_3D, "initial_guess")},
- {'O',PriorSensor("O", o_state_3D, "initial_guess")}}));
+ SpecSensorComposite({{'P',SpecStateSensor("P", p_state_3D, "initial_guess")},
+ {'O',SpecStateSensor("O", o_state_3D, "initial_guess")}}));
// factors
ASSERT_EQ(S->getPriorFeatures().size(), 0);
@@ -195,8 +195,8 @@ TEST(SensorOdom, makeshared_priors_factor_2D)
params->min_rot_var = min_rot_var;
auto S = std::make_shared<SensorOdom>("sensor1", 2, params,
- PriorSensorMap({{'P',PriorSensor("P", p_state_2D, "factor", p_std_2D)},
- {'O',PriorSensor("O", o_state_2D, "factor", o_std_2D)}}));
+ SpecSensorComposite({{'P',SpecStateSensor("P", p_state_2D, "factor", p_std_2D)},
+ {'O',SpecStateSensor("O", o_state_2D, "factor", o_std_2D)}}));
// factors
ASSERT_EQ(S->getPriorFeatures().size(), 2);
@@ -217,8 +217,8 @@ TEST(SensorOdom, makeshared_priors_factor_3D)
params->min_rot_var = min_rot_var;
auto S = std::make_shared<SensorOdom>("sensor1", 3, params,
- PriorSensorMap({{'P',PriorSensor("P", p_state_3D, "factor", p_std_3D)},
- {'O',PriorSensor("O", o_state_3D, "factor", o_std_3D)}}));
+ SpecSensorComposite({{'P',SpecStateSensor("P", p_state_3D, "factor", p_std_3D)},
+ {'O',SpecStateSensor("O", o_state_3D, "factor", o_std_3D)}}));
// factors
ASSERT_EQ(S->getPriorFeatures().size(), 2);
@@ -239,8 +239,8 @@ TEST(SensorOdom, makeshared_priors_initial_guess_dynamic_2D)
params->min_rot_var = min_rot_var;
auto S = std::make_shared<SensorOdom>("sensor1", 2, params,
- PriorSensorMap({{'P',PriorSensor("P", p_state_2D, "initial_guess", vector0, true)},
- {'O',PriorSensor("O", o_state_2D, "initial_guess", vector0, true)}}));
+ SpecSensorComposite({{'P',SpecStateSensor("P", p_state_2D, "initial_guess", vector0, true)},
+ {'O',SpecStateSensor("O", o_state_2D, "initial_guess", vector0, true)}}));
// factors
ASSERT_EQ(S->getPriorFeatures().size(), 0);
@@ -261,8 +261,8 @@ TEST(SensorOdom, makeshared_priors_initial_guess_dynamic_3D)
params->min_rot_var = min_rot_var;
auto S = std::make_shared<SensorOdom>("sensor1", 3, params,
- PriorSensorMap({{'P',PriorSensor("P", p_state_3D, "initial_guess", vector0, true)},
- {'O',PriorSensor("O", o_state_3D, "initial_guess", vector0, true)}}));
+ SpecSensorComposite({{'P',SpecStateSensor("P", p_state_3D, "initial_guess", vector0, true)},
+ {'O',SpecStateSensor("O", o_state_3D, "initial_guess", vector0, true)}}));
// factors
ASSERT_EQ(S->getPriorFeatures().size(), 0);
@@ -283,8 +283,8 @@ TEST(SensorOdom, makeshared_priors_initial_guess_dynamic_drift_2D)
params->min_rot_var = min_rot_var;
auto S = std::make_shared<SensorOdom>("sensor1", 2, params,
- PriorSensorMap({{'P',PriorSensor("P", p_state_2D, "initial_guess", vector0, true, p_std_2D)},
- {'O',PriorSensor("O", o_state_2D, "initial_guess", vector0, true, o_std_2D)}}));
+ SpecSensorComposite({{'P',SpecStateSensor("P", p_state_2D, "initial_guess", vector0, true, p_std_2D)},
+ {'O',SpecStateSensor("O", o_state_2D, "initial_guess", vector0, true, o_std_2D)}}));
// factors
ASSERT_EQ(S->getPriorFeatures().size(), 0);
@@ -305,8 +305,8 @@ TEST(SensorOdom, makeshared_priors_initial_guess_dynamic_drift_3D)
params->min_rot_var = min_rot_var;
auto S = std::make_shared<SensorOdom>("sensor1", 3, params,
- PriorSensorMap({{'P',PriorSensor("P", p_state_3D, "initial_guess", vector0, true, p_std_3D)},
- {'O',PriorSensor("O", o_state_3D, "initial_guess", vector0, true, o_std_3D)}}));
+ SpecSensorComposite({{'P',SpecStateSensor("P", p_state_3D, "initial_guess", vector0, true, p_std_3D)},
+ {'O',SpecStateSensor("O", o_state_3D, "initial_guess", vector0, true, o_std_3D)}}));
// factors
ASSERT_EQ(S->getPriorFeatures().size(), 0);
@@ -463,8 +463,8 @@ TEST(SensorOdom, compute_noise_cov_3D)
params->min_rot_var = min_rot_var;
auto S = FactorySensorPriors::create("SensorOdom","sensor1", 3, params,
- PriorSensorMap({{'P',PriorSensor("P", p_state_3D)}, //default "fix", not dynamic
- {'O',PriorSensor("O", o_state_3D)}}));
+ SpecSensorComposite({{'P',SpecStateSensor("P", p_state_3D)}, //default "fix", not dynamic
+ {'O',SpecStateSensor("O", o_state_3D)}}));
Vector6d disp_elements, rot_elements;
disp_elements << 1, 1, 1, 0, 0, 0;
diff --git a/test/gtest_sensor_pose.cpp b/test/gtest_sensor_pose.cpp
index 2bf0d87ae928cf688ced83c968702cd2fbd2b914..4f905a0169af8a6aafaea99064407f86074d60d4 100644
--- a/test/gtest_sensor_pose.cpp
+++ b/test/gtest_sensor_pose.cpp
@@ -52,8 +52,8 @@ TEST(Pose, constructor_priors_2D)
param->std_p = std_p;
param->std_o = std_o;
- PriorSensorMap priors{{'P',PriorSensor("P", p_state_2D)}, //default "fix", not dynamic
- {'O',PriorSensor("O", o_state_2D)}}; //default "fix", not dynamic
+ SpecSensorComposite priors{{'P',SpecStateSensor("P", p_state_2D)}, //default "fix", not dynamic
+ {'O',SpecStateSensor("O", o_state_2D)}}; //default "fix", not dynamic
auto sen = std::make_shared<SensorPose>("sensor_1", 2, param, priors);
@@ -77,8 +77,8 @@ TEST(Pose, constructor_priors_3D)
param->std_p = std_p;
param->std_o = std_o;
- PriorSensorMap priors{{'P',PriorSensor("P", p_state_3D)}, //default "fix", not dynamic
- {'O',PriorSensor("O", o_state_3D)}}; //default "fix", not dynamic
+ SpecComposite priors{{'P',SpecStateSensor("P", p_state_3D)}, //default "fix", not dynamic
+ {'O',SpecStateSensor("O", o_state_3D)}}; //default "fix", not dynamic
auto sen = std::make_shared<SensorPose>("sensor_1", 3, param, priors);
@@ -232,8 +232,8 @@ TEST(Pose, factory_priors_2D)
param->std_p = std_p;
param->std_o = std_o;
- PriorSensorMap priors{{'P',PriorSensor("P", p_state_2D)}, //default "fix", not dynamic
- {'O',PriorSensor("O", o_state_2D)}}; //default "fix", not dynamic
+ SpecComposite priors{{'P',SpecStateSensor("P", p_state_2D)}, //default "fix", not dynamic
+ {'O',SpecStateSensor("O", o_state_2D)}}; //default "fix", not dynamic
auto sb = FactorySensorPriors::create("SensorPose","sensor_1", 2, param, priors);
@@ -259,8 +259,8 @@ TEST(Pose, factory_priors_3D)
param->std_p = std_p;
param->std_o = std_o;
- PriorSensorMap priors{{'P',PriorSensor("P", p_state_3D)}, //default "fix", not dynamic
- {'O',PriorSensor("O", o_state_3D)}}; //default "fix", not dynamic
+ SpecComposite priors{{'P',SpecStateSensor("P", p_state_3D)}, //default "fix", not dynamic
+ {'O',SpecStateSensor("O", o_state_3D)}}; //default "fix", not dynamic
auto sb = FactorySensorPriors::create("SensorPose","sensor_1", 3, param, priors);
diff --git a/test/yaml/params_problem_autosetup.yaml b/test/yaml/params_problem_autosetup.yaml
index 4f8f593f23cbc301d8a49e96bcacb44f2221530e..7c445e2c32fe2de43f48b4f6e36636fa38f26329 100644
--- a/test/yaml/params_problem_autosetup.yaml
+++ b/test/yaml/params_problem_autosetup.yaml
@@ -1,15 +1,15 @@
problem:
frame_structure: "PO"
dimension: 3
- prior:
- mode: "factor"
- $state:
- P: [0,0,0]
- O: [0,0,0,1]
- $sigma:
- P: [0.31, 0.31, 0.31]
- O: [0.31, 0.31, 0.31]
- time_tolerance: 0.1
+ first_frame:
+ P:
+ state: [0,0,0]
+ mode: "factor"
+ noise_std: [0.31, 0.31, 0.31]
+ O:
+ state: [0,0,0,1]
+ mode: "factor"
+ noise_std: [0.31, 0.31, 0.31]
tree_manager:
type: "None"
map:
diff --git a/test/yaml/params_problem_autosetup_no_map.yaml b/test/yaml/params_problem_autosetup_no_map.yaml
index e7aa7c81b9c030259fead40bb72943b369e9964d..c0713d0f39d04da8920000e38dfa434950b3e1d9 100644
--- a/test/yaml/params_problem_autosetup_no_map.yaml
+++ b/test/yaml/params_problem_autosetup_no_map.yaml
@@ -1,15 +1,15 @@
problem:
frame_structure: "PO"
dimension: 3
- prior:
- mode: "factor"
- $state:
- P: [0,0,0]
- O: [0,0,0,1]
- $sigma:
- P: [0.31, 0.31, 0.31]
- O: [0.31, 0.31, 0.31]
- time_tolerance: 0.1
+ first_frame:
+ P:
+ state: [0,0,0]
+ mode: "factor"
+ noise_std: [0.31, 0.31, 0.31]
+ O:
+ state: [0,0,0,1]
+ mode: "factor"
+ noise_std: [0.31, 0.31, 0.31]
tree_manager:
type: "None"
sensors:
diff --git a/test/yaml/params_problem_odom_3d.yaml b/test/yaml/params_problem_odom_3d.yaml
index b5e8c455bc8eafe7bc51020878240aadb42aaec5..e038298e42bb24516caff219fd8b3a80ea976ac3 100644
--- a/test/yaml/params_problem_odom_3d.yaml
+++ b/test/yaml/params_problem_odom_3d.yaml
@@ -1,7 +1,7 @@
problem:
frame_structure: "PO"
dimension: 3
- prior:
+ first_frame:
mode: "factor"
$state:
P: [0,0,0]
diff --git a/test/yaml/params_tree_manager1.yaml b/test/yaml/params_tree_manager1.yaml
index af9245339e4a0dd9ad5538a84598b6bbfcd0f240..f7a110bf8f5ec83efeb186fd4b934c5a5817047b 100644
--- a/test/yaml/params_tree_manager1.yaml
+++ b/test/yaml/params_tree_manager1.yaml
@@ -1,19 +1,20 @@
problem:
frame_structure: "POV"
dimension: 3
- prior:
- mode: "factor"
- # state: [0,0,0,0,0,0,1,0,0,0]
- # cov: [[9,9],.1,0,0,0,0,0,0,0,0, 0,.1,0,0,0,0,0,0,0, 0,0,.1,0,0,0,0,0,0, 0,0,0,.1,0,0,0,0,0, 0,0,0,0,.1,0,0,0,0, 0,0,0,0,0,.1,0,0,0, 0,0,0,0,0,0,.1,0,0, 0,0,0,0,0,0,0,.1,0, 0,0,0,0,0,0,0,0,.1]
- $state:
- P: [0,0,0]
- O: [0,0,0,1]
- V: [0,0,0]
- $sigma:
- P: [0.31, 0.31, 0.31]
- O: [0.31, 0.31, 0.31]
- V: [0.31, 0.31, 0.31]
- time_tolerance: 0.1
+ first_frame:
+ P:
+ state: [0,0,0]
+ mode: "factor"
+ noise_std: [0.31, 0.31, 0.31]
+ O:
+ state: [0,0,0,1]
+ mode: "factor"
+ noise_std: [0.31, 0.31, 0.31]
+ V:
+ state: [0,0,0]
+ mode: "factor"
+ noise_std: [0.31, 0.31, 0.31]
+ type: StateVector3d
tree_manager:
follow: "tree_manager_dummy.yaml"
sensors:
diff --git a/test/yaml/params_tree_manager2.yaml b/test/yaml/params_tree_manager2.yaml
index b8acc1b021309cfae76c86b4f96b8b059ee75cf8..edb6b17b80eda799719190d7d1b2efd6cf4d82af 100644
--- a/test/yaml/params_tree_manager2.yaml
+++ b/test/yaml/params_tree_manager2.yaml
@@ -1,19 +1,20 @@
problem:
frame_structure: "POV"
dimension: 3
- prior:
- mode: "factor"
- # state: [0,0,0,0,0,0,1,0,0,0]
- # cov: [[9,9],.1,0,0,0,0,0,0,0,0, 0,.1,0,0,0,0,0,0,0, 0,0,.1,0,0,0,0,0,0, 0,0,0,.1,0,0,0,0,0, 0,0,0,0,.1,0,0,0,0, 0,0,0,0,0,.1,0,0,0, 0,0,0,0,0,0,.1,0,0, 0,0,0,0,0,0,0,.1,0, 0,0,0,0,0,0,0,0,.1]
- $state:
- P: [0,0,0]
- O: [0,0,0,1]
- V: [0,0,0]
- $sigma:
- P: [0.31, 0.31, 0.31]
- O: [0.31, 0.31, 0.31]
- V: [0.31, 0.31, 0.31]
- time_tolerance: 0.1
+ first_frame:
+ P:
+ state: [0,0,0]
+ mode: "factor"
+ noise_std: [0.31, 0.31, 0.31]
+ O:
+ state: [0,0,0,1]
+ mode: "factor"
+ noise_std: [0.31, 0.31, 0.31]
+ V:
+ state: [0,0,0]
+ mode: "factor"
+ noise_std: [0.31, 0.31, 0.31]
+ type: StateVector3d
tree_manager:
type: "None"
sensors:
diff --git a/test/yaml/params_tree_manager_sliding_window1.yaml b/test/yaml/params_tree_manager_sliding_window1.yaml
index a317726ac748b5353a320517b2c20ac8f7e10597..22aea41299db11cc1a6ccc326067827d14e525ec 100644
--- a/test/yaml/params_tree_manager_sliding_window1.yaml
+++ b/test/yaml/params_tree_manager_sliding_window1.yaml
@@ -1,7 +1,7 @@
problem:
frame_structure: "PO"
dimension: 3
- prior:
+ first_frame:
mode: "factor"
$state:
P: [0,0,0]
diff --git a/test/yaml/params_tree_manager_sliding_window2.yaml b/test/yaml/params_tree_manager_sliding_window2.yaml
index 935f0899fe5330a382c8581954fb2166c546de72..09288cfd12247f66f2cc2b3800380d87a9cbd4e0 100644
--- a/test/yaml/params_tree_manager_sliding_window2.yaml
+++ b/test/yaml/params_tree_manager_sliding_window2.yaml
@@ -1,7 +1,7 @@
problem:
frame_structure: "PO"
dimension: 3
- prior:
+ first_frame:
mode: "factor"
$state:
P: [0,0,0]
diff --git a/test/yaml/params_tree_manager_sliding_window_dual_rate1.yaml b/test/yaml/params_tree_manager_sliding_window_dual_rate1.yaml
index 6a7b21625aac524b72da9fcb35d01573dfd655aa..4d9b7a7cb58d44b5067b5f90711796ba5e48e9cf 100644
--- a/test/yaml/params_tree_manager_sliding_window_dual_rate1.yaml
+++ b/test/yaml/params_tree_manager_sliding_window_dual_rate1.yaml
@@ -1,7 +1,7 @@
problem:
frame_structure: "PO"
dimension: 3
- prior:
+ first_frame:
mode: "factor"
$state:
P: [0,0,0]
diff --git a/test/yaml/params_tree_manager_sliding_window_dual_rate2.yaml b/test/yaml/params_tree_manager_sliding_window_dual_rate2.yaml
index 12f59ad4a91a0adb50f433ace29e4d910a3354e0..db6c4a2940bcf51e1b90cb7a53350b5bc68c8a9f 100644
--- a/test/yaml/params_tree_manager_sliding_window_dual_rate2.yaml
+++ b/test/yaml/params_tree_manager_sliding_window_dual_rate2.yaml
@@ -1,7 +1,7 @@
problem:
frame_structure: "PO"
dimension: 3
- prior:
+ first_frame:
mode: "factor"
$state:
P: [0,0,0]
diff --git a/test/yaml/params_tree_manager_sliding_window_dual_rate3.yaml b/test/yaml/params_tree_manager_sliding_window_dual_rate3.yaml
index d7c53fb130fadc88f4de1c5a971b23d2f51fdce9..0d725631260166ea8aebcc3b89d206638b6532ac 100644
--- a/test/yaml/params_tree_manager_sliding_window_dual_rate3.yaml
+++ b/test/yaml/params_tree_manager_sliding_window_dual_rate3.yaml
@@ -3,7 +3,7 @@ solver:
problem:
frame_structure: "PO"
dimension: 3
- prior:
+ first_frame:
mode: "factor"
$state:
P: [0,0,0]
diff --git a/test/yaml/params_tree_manager_sliding_window_dual_rate_baseline.yaml b/test/yaml/params_tree_manager_sliding_window_dual_rate_baseline.yaml
index e51b8a14692e17fca9ca6d6194be21a7291dd9d7..47e54b3ab04136e768c7479b18d3b09adfa592f8 100644
--- a/test/yaml/params_tree_manager_sliding_window_dual_rate_baseline.yaml
+++ b/test/yaml/params_tree_manager_sliding_window_dual_rate_baseline.yaml
@@ -3,7 +3,7 @@ solver:
problem:
frame_structure: "PO"
dimension: 3
- prior:
+ first_frame:
mode: "factor"
$state:
P: [0,0,0]