diff --git a/CMakeLists.txt b/CMakeLists.txt
index 6c1ea91516a75e5a5408335fa7c2bcbebf0c72cd..24bac9581829cc4fadb2eb007fd15c1e88b8adec 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -171,6 +171,7 @@ SET(HDRS_MAP
)
SET(HDRS_PROBLEM
include/${PROJECT_NAME}/problem/problem.h
+ # include/${PROJECT_NAME}/problem/prior_problem.h
)
SET(HDRS_PROCESSOR
include/${PROJECT_NAME}/processor/factory_processor.h
@@ -191,6 +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/sensor_base.h
include/${PROJECT_NAME}/sensor/sensor_diff_drive.h
include/${PROJECT_NAME}/sensor/sensor_motion_model.h
@@ -280,6 +282,7 @@ SET(SRCS_MAP
src/map/map_base.cpp
)
SET(SRCS_PROBLEM
+ # src/problem/prior_problem.cpp
src/problem/problem.cpp
)
SET(SRCS_PROCESSOR
@@ -299,6 +302,7 @@ SET(SRCS_PROCESSOR
src/processor/track_matrix.cpp
)
SET(SRCS_SENSOR
+ src/sensor/prior_sensor.cpp
src/sensor/sensor_base.cpp
src/sensor/sensor_diff_drive.cpp
src/sensor/sensor_motion_model.cpp
diff --git a/demos/hello_wolf/hello_wolf.cpp b/demos/hello_wolf/hello_wolf.cpp
index 366d68f47c5f675fba20ad28ee1ccbe6162c3e8c..91d99685abded46079b5431c615abef6df155e9f 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>();
- Priors priors_odo = {{'P',Prior("P", Vector2d::Zero())},
- {'O',Prior("O", Vector1d::Zero())}};
+ PriorSensorMap priors_odo = {{'P',PriorSensor("P", Vector2d::Zero())},
+ {'O',PriorSensor("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>();
- Priors priors_rb = {{'P',Prior("P", Vector2d(1,1))},
- {'O',Prior("O", Vector1d::Zero())}};
+ PriorSensorMap priors_rb = {{'P',PriorSensor("P", Vector2d(1,1))},
+ {'O',PriorSensor("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 07dc0c31e2238542af54c6507984263dbec6c3db..39527e6c9d024cd5972bfa9bdcf90ed4fd2d60e3 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 Priors& _priors) :
+ const PriorSensorMap& _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 f111fff790358225eedd080fd032d8e8f2fab287..5b407dd50603bd50337b42a6486aa6e6f4a644f4 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 Priors& _priors);
+ const PriorSensorMap& _priors);
WOLF_SENSOR_CREATE(SensorRangeBearing, ParamsSensorRangeBearing);
~SensorRangeBearing() override;
diff --git a/include/core/problem/prior_problem.h b/include/core/problem/prior_problem.h
new file mode 100644
index 0000000000000000000000000000000000000000..a3fcbf94260e5ff1c734ffcd01ff90a6e039af86
--- /dev/null
+++ b/include/core/problem/prior_problem.h
@@ -0,0 +1,66 @@
+//--------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/prior.h"
+
+namespace wolf
+{
+
+class PriorProblem : public Prior
+{
+ protected:
+ double time_tolerance_; // time tolerance of the first frame
+
+ public:
+ PriorProblem() = default;
+ PriorProblem(const std::string& _type,
+ const Eigen::VectorXd& _state,
+ const double& _time_tolerance,
+ const std::string& _mode = "fix",
+ const Eigen::VectorXd& _noise_std = Eigen::VectorXd(0));
+
+ PriorProblem(const YAML::Node& _n);
+
+ virtual ~PriorProblem() = default;
+
+ double getTimeTolerance() const;
+
+ using Prior::check;
+
+ std::string print() const override;
+};
+
+typedef std::unordered_map<char, PriorProblem> StdMapPriorProblem;
+
+class PriorProblemMap : public StdMapPriorProblem
+{
+ public:
+ using StdMapPriorProblem::StdMapPriorProblem;
+
+ PriorProblemMap(const YAML::Node& _n);
+ virtual ~PriorProblemMap() = default;
+};
+
+inline double PriorProblem::getTimeTolerance() const { return time_tolerance_; }
+
+} // namespace wolf
\ No newline at end of file
diff --git a/include/core/problem/problem.h b/include/core/problem/problem.h
index bb3cb379ae80a582ec3ba90db10cc013487a5ce9..6bfabeb1f79dd845c3c36760bb289673b79256f8 100644
--- a/include/core/problem/problem.h
+++ b/include/core/problem/problem.h
@@ -57,14 +57,6 @@ enum Notification
REMOVE
};
-struct PriorOptions
-{
- std::string mode = "";
- VectorComposite state;
- MatrixComposite cov;
-};
-WOLF_STRUCT_PTR_TYPEDEFS(PriorOptions);
-
/** \brief Wolf problem node element in the Wolf Tree
*/
class Problem : public std::enable_shared_from_this<Problem>
@@ -87,7 +79,8 @@ class Problem : public std::enable_shared_from_this<Problem>
mutable std::mutex mut_notifications_;
mutable std::mutex mut_covariances_;
StateStructure frame_structure_;
- PriorOptionsPtr prior_options_;
+ PriorMap prior_options_;
+ bool prior_applied_;
std::atomic_bool transformed_;
VectorComposite transformation_;
@@ -196,10 +189,8 @@ class Problem : public std::enable_shared_from_this<Problem>
TrajectoryBasePtr getTrajectory();
// Prior
- bool isPriorSet() const;
- void setPriorOptions(const std::string& _mode,
- const VectorComposite& _state = VectorComposite(),
- const VectorComposite& _cov = VectorComposite());
+ bool isPriorApplied() const;
+ void setPriorOptions(const PriorMap& priors);
FrameBasePtr applyPriorOptions(const TimeStamp& _ts);
FrameBasePtr setPriorFactor(const VectorComposite &_state,
const VectorComposite &_sigma,
@@ -443,9 +434,9 @@ inline TreeManagerBasePtr Problem::getTreeManager()
return tree_manager_;
}
-inline bool Problem::isPriorSet() const
+inline bool Problem::isPriorApplied() const
{
- return prior_options_ == nullptr;
+ return prior_applied_;
}
inline std::map<int,MotionProviderConstPtr> Problem::getMotionProviderMap() const
diff --git a/include/core/sensor/prior_sensor.h b/include/core/sensor/prior_sensor.h
new file mode 100644
index 0000000000000000000000000000000000000000..496926a610aa3bf5076dbc58fe3aa01aa5efac3b
--- /dev/null
+++ b/include/core/sensor/prior_sensor.h
@@ -0,0 +1,71 @@
+//--------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/prior.h"
+
+namespace wolf
+{
+
+class PriorSensor : public Prior
+{
+ 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));
+
+ PriorSensor(const YAML::Node& _n);
+
+ virtual ~PriorSensor() = default;
+
+ bool isDynamic() const;
+ const Eigen::VectorXd& getDriftStd() const;
+
+ void check() const override;
+
+ std::string print() const override;
+};
+
+typedef std::unordered_map<char, PriorSensor> StdMapPriorSensor;
+
+class PriorSensorMap : public StdMapPriorSensor
+{
+ public:
+ using StdMapPriorSensor::StdMapPriorSensor;
+
+ PriorSensorMap(const YAML::Node& _n);
+ virtual ~PriorSensorMap() = default;
+};
+
+inline bool PriorSensor::isDynamic() const { return dynamic_; }
+
+inline const Eigen::VectorXd& PriorSensor::getDriftStd() const { return drift_std_; }
+
+} // namespace wolf
\ No newline at end of file
diff --git a/include/core/sensor/sensor_base.h b/include/core/sensor/sensor_base.h
index d849b3fae49bf6a150546cdda7be6eba1a5229ae..612775d7100cb624348c416ffa5c1122aae64ba0 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/state_block/prior.h"
+#include "core/sensor/prior_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 Priors& _priors)
+ * const PriorSensorMap& _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 = Priors(_input_node["states"]); \
+ auto priors = PriorSensorMap(_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 = Priors(server.getNode()["states"]); \
+ auto priors = PriorSensorMap(server.getNode()["states"]); \
\
return std::make_shared<SensorClass>(params, priors); \
} \
@@ -125,7 +125,7 @@ class SensorBase : public NodeBase, public HasStateBlocks, public std::enable_sh
std::map<char, bool> state_block_dynamic_; // mark dynamic state blocks
- std::map<char, FeatureBasePtr> params_prior_map_; // Priors (value and covariance) of extrinsic & intrinsic state blocks (by key in state_block_map_)
+ std::map<char, FeatureBasePtr> features_prior_map_; // Priors (value and covariance) of extrinsic & intrinsic state blocks (by key in state_block_map_)
CaptureBasePtr last_capture_; // last capture of the sensor (in the WOLF tree)
@@ -135,7 +135,7 @@ 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 Priors& _priors);
+ virtual void loadPriors(const PriorSensorMap& _priors);
public:
@@ -151,7 +151,7 @@ class SensorBase : public NodeBase, public HasStateBlocks, public std::enable_sh
SensorBase(const std::string& _type,
const int& _dim,
ParamsSensorBasePtr _params,
- const Priors& _priors);
+ const PriorSensorMap& _priors);
~SensorBase() override;
@@ -333,7 +333,7 @@ inline CaptureBasePtr SensorBase::getLastCapture()
inline StateBlockPtr SensorBase::addStateBlock(const char& _key, const StateBlockPtr& _sb_ptr, const bool _dynamic)
{
- assert((params_prior_map_.find(_key) == params_prior_map_.end() || _sb_ptr == nullptr) && "overwriting a state block that has an absolute factor");
+ assert((features_prior_map_.find(_key) == features_prior_map_.end() || _sb_ptr == nullptr) && "overwriting a state block that has an absolute factor");
HasStateBlocks::addStateBlock(_key, _sb_ptr, getProblem());
setStateBlockDynamic(_key, _dynamic);
return _sb_ptr;
@@ -400,29 +400,29 @@ inline void SensorBase::addPriorIntrinsics(const Eigen::VectorXd& _x, const Eige
inline std::map<char, FeatureBaseConstPtr> SensorBase::getPriorFeatures() const
{
std::map<char, FeatureBaseConstPtr> map_const;
- for (auto&& pair : params_prior_map_)
+ for (auto&& pair : features_prior_map_)
map_const[pair.first] = pair.second;
return map_const;
}
inline std::map<char, FeatureBasePtr> SensorBase::getPriorFeatures()
{
- return params_prior_map_;
+ return features_prior_map_;
}
inline FeatureBaseConstPtr SensorBase::getPriorFeature(char key) const
{
- if (params_prior_map_.count(key) == 0)
+ if (features_prior_map_.count(key) == 0)
return nullptr;
- return params_prior_map_.at(key);
+ return features_prior_map_.at(key);
}
inline FeatureBasePtr SensorBase::getPriorFeature(char key)
{
- if (params_prior_map_.count(key) == 0)
+ if (features_prior_map_.count(key) == 0)
return nullptr;
- return params_prior_map_.at(key);
+ return features_prior_map_.at(key);
}
} // namespace wolf
\ No newline at end of file
diff --git a/include/core/sensor/sensor_diff_drive.h b/include/core/sensor/sensor_diff_drive.h
index c516eaf012c0783fc1d9fd091b91868ca63a80dc..db2e2c40079cdf673e7fe17a3ce783ef15c4af1a 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 Priors& _priors);
+ const PriorSensorMap& _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 4d68fb6df0619e6b9a8f8989af635be37d68767e..884cf57a779d8cc81632294acbfd72605558d78f 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 Priors& _priors);
+ const PriorSensorMap& _priors);
WOLF_SENSOR_CREATE(SensorMotionModel, ParamsSensorBase);
diff --git a/include/core/sensor/sensor_odom.h b/include/core/sensor/sensor_odom.h
index 1ed6e3c8799cc37469df7f80a4ded49822363c6d..1d0610b29583ff3db99a0b090f22ffcdb28a8fa8 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 Priors& _priors) :
+ const PriorSensorMap& _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 abf7e9e1108b81b92ba3974ff6fe51a99ef24702..5817cabc06098ce4f94501d3d7a67626ebcfd466 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 Priors& _priors) :
+ const PriorSensorMap& _priors) :
SensorBase("SensorPose"+toString(DIM)+"d",
DIM,
_params,
diff --git a/include/core/state_block/prior.h b/include/core/state_block/prior.h
index 67da9837d8fa5bc8743404d26e6c046f2d0de914..870614fe887592432d1c8e91d9655316af1b32e4 100644
--- a/include/core/state_block/prior.h
+++ b/include/core/state_block/prior.h
@@ -31,24 +31,20 @@ namespace wolf
class Prior
{
- private:
+ 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')
- 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:
- Prior() = default;
+ Prior() = delete;
Prior(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));
+ const Eigen::VectorXd& _state,
+ const std::string& _mode = "fix",
+ const Eigen::VectorXd& _noise_std = Eigen::VectorXd(0));
- Prior(const YAML::Node& prior_node);
+ Prior(const YAML::Node& _n);
virtual ~Prior() = default;
@@ -56,26 +52,26 @@ class Prior
const std::string& getMode() const;
const Eigen::VectorXd& getState() const;
const Eigen::VectorXd& getNoiseStd() const;
- bool isDynamic() const;
bool isFixed() const;
bool isInitialGuess() const;
bool isFactor() const;
- const Eigen::VectorXd& getDriftStd() const;
- void check() const;
+ virtual void check() const;
- virtual std::string print() const final;
+ virtual std::string print() const;
};
-typedef std::unordered_map<char, Prior> PriorMap;
+typedef std::unordered_map<char, Prior> StdMapPrior;
-class Priors : public PriorMap
+class PriorMap : public StdMapPrior
{
public:
- using PriorMap::PriorMap;
+ using StdMapPrior::StdMapPrior;
- Priors(const YAML::Node& priors_node);
- virtual ~Priors() = default;
+ PriorMap(const YAML::Node& _n);
+ virtual ~PriorMap() = default;
+
+ VectorComposite getState() const;
};
inline const std::string& Prior::getType() const { return type_; }
@@ -86,14 +82,10 @@ inline const Eigen::VectorXd& Prior::getState() const { return state_; }
inline const Eigen::VectorXd& Prior::getNoiseStd() const { return noise_std_; }
-inline bool Prior::isDynamic() const { return dynamic_; }
-
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"; }
-inline const Eigen::VectorXd& Prior::getDriftStd() const { return drift_std_; }
-
} // namespace wolf
\ No newline at end of file
diff --git a/schema/Prior.schema b/schema/problem/Prior.schema
similarity index 64%
rename from schema/Prior.schema
rename to schema/problem/Prior.schema
index 5556861da585a34130950e32939dcb797b192344..da8eaad3a865ba5091cf91356b03520b36c500f2 100644
--- a/schema/Prior.schema
+++ b/schema/problem/Prior.schema
@@ -14,20 +14,9 @@ mode:
mandatory: true
options: ["fix", "factor", "initial_guess"]
doc: The prior mode can be 'factor' to add an absolute factor (requires 'noise_std'), 'fix' to set the values constant or 'initial_guess' to just set the values
-dynamic:
- type: bool
- yaml_type: scalar
- mandatory: true
- doc: If the state is dynamic, i.e. it changes along time.
noise_std:
type: VectorXd
yaml_type: scalar
mandatory: false
default: []
- doc: A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix.
-drift_std:
- type: VectorXd
- yaml_type: scalar
- mandatory: false
- default: []
- doc: A vector containing the stdev values of the noise of the drift factor (only if dynamic==true), i.e. the sqrt of the diagonal elements of the covariance matrix.
\ No newline at end of file
+ doc: A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix.
\ No newline at end of file
diff --git a/schema/PriorO2d.schema b/schema/problem/PriorO2d.schema
similarity index 100%
rename from schema/PriorO2d.schema
rename to schema/problem/PriorO2d.schema
diff --git a/schema/PriorO3d.schema b/schema/problem/PriorO3d.schema
similarity index 100%
rename from schema/PriorO3d.schema
rename to schema/problem/PriorO3d.schema
diff --git a/schema/PriorP2d.schema b/schema/problem/PriorP2d.schema
similarity index 100%
rename from schema/PriorP2d.schema
rename to schema/problem/PriorP2d.schema
diff --git a/schema/PriorP3d.schema b/schema/problem/PriorP3d.schema
similarity index 100%
rename from schema/PriorP3d.schema
rename to schema/problem/PriorP3d.schema
diff --git a/schema/problem/Problem2d.schema b/schema/problem/Problem2d.schema
new file mode 100644
index 0000000000000000000000000000000000000000..ff52fcd1f121e026681c44b682a97240776bd25f
--- /dev/null
+++ b/schema/problem/Problem2d.schema
@@ -0,0 +1,7 @@
+follow: Problem.schema
+problem:
+ prior:
+ P:
+ follow: PriorP2d.schema
+ O:
+ follow: PriorO2d.schema
\ No newline at end of file
diff --git a/schema/problem/Problem3d.schema b/schema/problem/Problem3d.schema
new file mode 100644
index 0000000000000000000000000000000000000000..b849bec3ce39d22ca8de78d42758367399fe5e3a
--- /dev/null
+++ b/schema/problem/Problem3d.schema
@@ -0,0 +1,7 @@
+follow: Problem.schema
+problem:
+ prior:
+ P:
+ follow: PriorP3d.schema
+ O:
+ follow: PriorO3d.schema
\ No newline at end of file
diff --git a/schema/sensor/PriorSensor.schema b/schema/sensor/PriorSensor.schema
new file mode 100644
index 0000000000000000000000000000000000000000..e650d3961ed1187d46735019e32380b843f064cf
--- /dev/null
+++ b/schema/sensor/PriorSensor.schema
@@ -0,0 +1,12 @@
+follow: Prior.schema
+dynamic:
+ type: bool
+ yaml_type: scalar
+ mandatory: true
+ doc: If the state is dynamic, i.e. it changes along time.
+drift_std:
+ type: VectorXd
+ yaml_type: scalar
+ mandatory: false
+ default: []
+ doc: A vector containing the stdev values of the noise of the drift factor (only if dynamic==true), i.e. the sqrt of the diagonal elements of the covariance matrix.
\ No newline at end of file
diff --git a/schema/sensor/SensorDiffDrive.schema b/schema/sensor/SensorDiffDrive.schema
index ca425eed8c618fd1623407e8a67382f5c645f07a..ba16781b06d9492b95a46f8d3a91e8256fd596f3 100644
--- a/schema/sensor/SensorDiffDrive.schema
+++ b/schema/sensor/SensorDiffDrive.schema
@@ -30,11 +30,11 @@ ticks_std_factor:
states:
P:
- follow: PriorP2d.schema
+ follow: PriorSensorP2d.schema
O:
- follow: PriorO2d.schema
+ follow: PriorSensorO2d.schema
I:
- follow: Prior.schema
+ follow: PriorSensor.schema
type:
type: string
yaml_type: scalar
diff --git a/schema/sensor/SensorOdom2d.schema b/schema/sensor/SensorOdom2d.schema
index 4983ec87a098bc87e8c429338099d88202db8859..08dc1f8dd6d8e5257bb8cd065c3cfbc8ec82afd1 100644
--- a/schema/sensor/SensorOdom2d.schema
+++ b/schema/sensor/SensorOdom2d.schema
@@ -32,7 +32,7 @@ min_rot_var:
states:
P:
- follow: PriorP2d.schema
+ follow: PriorSensorP2d.schema
O:
- follow: PriorO2d.schema
+ follow: PriorSensorO2d.schema
diff --git a/schema/sensor/SensorOdom3d.schema b/schema/sensor/SensorOdom3d.schema
index 0866f2278f7ec47dfee156e620e29ee487cca813..4ef58d18aaa39fef4a8d86b0eac41aef0fd6c564 100644
--- a/schema/sensor/SensorOdom3d.schema
+++ b/schema/sensor/SensorOdom3d.schema
@@ -49,6 +49,6 @@ min_rot_var:
states:
P:
- follow: PriorP3d.schema
+ follow: PriorSensorP3d.schema
O:
- follow: PriorO3d.schema
\ No newline at end of file
+ follow: PriorSensorO3d.schema
\ No newline at end of file
diff --git a/src/problem/prior_problem.cpp b/src/problem/prior_problem.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..3fe7d416b2e20a34e966f2835178c068508bc0b8
--- /dev/null
+++ b/src/problem/prior_problem.cpp
@@ -0,0 +1,67 @@
+//--------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/problem/prior_problem.h"
+#include "core/state_block/factory_state_block.h"
+#include "core/common/params_base.h" // toString()
+
+namespace wolf
+{
+
+PriorProblemMap::PriorProblemMap(const YAML::Node& priors_node)
+{
+ if (not priors_node.IsMap())
+ {
+ throw std::runtime_error("PriorProblemMap: constructor with a non-map yaml node");
+ }
+ for (auto prior_pair : priors_node)
+ {
+ this->emplace(prior_pair.first.as<char>(), PriorProblem(prior_pair.second));
+ }
+}
+
+PriorProblem::PriorProblem(const std::string& _type,
+ const Eigen::VectorXd& _state,
+ const double& _time_tolerance,
+ const std::string& _mode,
+ const Eigen::VectorXd& _noise_std) :
+ Prior(_type, _state, _mode, _noise_std),
+ time_tolerance_(_time_tolerance)
+{
+ check();
+}
+
+PriorProblem::PriorProblem(const YAML::Node& prior_node) :
+ Prior(prior_node)
+{
+ time_tolerance_ = prior_node["time_tolerance"].as<double>();
+
+ check();
+}
+
+std::string PriorProblem::print() const
+{
+ return Prior::print() +
+ "time_tolerance: " + toString(time_tolerance_) + "\n";
+}
+
+} // namespace wolf
\ No newline at end of file
diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp
index d6f838d4fcf18bf385208c9bbf1c840667da6380..be215e57726ee0693d590a97e804a71c24920eb0 100644
--- a/src/problem/problem.cpp
+++ b/src/problem/problem.cpp
@@ -48,7 +48,8 @@ Problem::Problem(const std::string& _frame_structure, SizeEigen _dim, MapBasePtr
map_ptr_(_map),
motion_provider_map_(),
frame_structure_(_frame_structure),
- prior_options_(std::make_shared<PriorOptions>()),
+ prior_options_(),
+ prior_applied_(false),
transformed_(false)
{
dim_ = _dim;
@@ -124,14 +125,20 @@ ProblemPtr Problem::autoSetup(const std::string& _input_yaml_file, const std::ve
auto server = yaml_schema_cpp::YamlServer(schema_folders, _input_yaml_file);
WOLF_INFO("loaded node:\n", server.getNode());
- // Validate agains schema files
+ // Validate against schema
WOLF_INFO("Applying schema");
if (not server.applySchema("Problem.schema"))
{
WOLF_ERROR(server.getLog().str());
return nullptr;
- }
-
+ }
+ // validate against schema of specific dimension
+ bool is2D = server.getNode()["problem"]["dimension"].as<int>() == 2;
+ if (not server.applySchema(is2D ? "Problem2d.schema" : "Problem3d.schema"))
+ {
+ WOLF_ERROR(server.getLog().str());
+ return nullptr;
+ }
WOLF_INFO("schema applied");
// Get param node
@@ -155,26 +162,11 @@ ProblemPtr Problem::autoSetup(const std::string& _input_yaml_file, const std::ve
{
problem->setTreeManager(FactoryTreeManager::create(tree_manager_type, tree_manager_node));
}
- // TODO: Prior -- first keyframe
- // std::string prior_mode = _server.getParam<std::string>("problem/prior/mode");
- // assert((prior_mode == "nothing" || prior_mode == "initial_guess" || prior_mode == "fix" || prior_mode == "factor") && "wrong _mode value, it should be: 'nothing', 'initial_guess', 'fix' or 'factor'");
- // WOLF_TRACE("Prior mode: ", prior_mode);
- // if (prior_mode == "nothing")
- // {
- // problem->setPriorOptions(prior_mode);
- // }
- // else if (prior_mode == "factor")
- // {
- // problem->setPriorOptions(prior_mode,
- // _server.getParam<VectorComposite>("problem/prior/state"),
- // _server.getParam<VectorComposite>("problem/prior/sigma"));
- // }
- // else
- // {
- // problem->setPriorOptions(prior_mode,
- // _server.getParam<VectorComposite>("problem/prior/state"));
- // }
- // WOLF_TRACE("Setting up problem with frame structure {" + frame_structure + "} and dimension " + std::to_string(dim) + "D");
+
+ // First frame
+ WOLF_INFO("Loading problem first frame parameters");
+ PriorMap priors(problem_node["first_frame"]);
+ problem->setPriorOptions(priors);
// SENSORS ===============================================================================
// load plugin if sensor is not registered
@@ -394,8 +386,8 @@ FrameBasePtr Problem::emplaceFrame(const TimeStamp& _time_stamp, //
}
FrameBasePtr Problem::emplaceFrame(const TimeStamp& _time_stamp, //
- const StateStructure& _frame_structure, //
- const SizeEigen _dim)
+ const StateStructure& _frame_structure, //
+ const SizeEigen _dim)
{
return emplaceFrame(_time_stamp,
_frame_structure,
@@ -413,7 +405,7 @@ FrameBasePtr Problem::emplaceFrame (const TimeStamp& _time_stamp, //
}
FrameBasePtr Problem::emplaceFrame (const TimeStamp& _time_stamp, //
- const VectorComposite& _frame_state)
+ const VectorComposite& _frame_state)
{
// append new keys to frame structure
for (auto pair_key_vec : _frame_state)
@@ -506,8 +498,8 @@ VectorComposite Problem::getState(const StateStructure& _structure) const
if (last_kf)
state_last = last_kf->getState(structure);
- else if (prior_options_ and prior_options_->mode != "nothing")
- state_last = prior_options_->state;
+ else if (not prior_options_.empty())
+ state_last = prior_options_.getState();
for (auto key : structure)
{
@@ -560,8 +552,8 @@ VectorComposite Problem::getState (const TimeStamp& _ts, const StateStructure& _
if (last_kf)
state_last = last_kf->getState(structure);
- else if (prior_options_ and prior_options_->mode != "nothing")
- state_last = prior_options_->state;
+ else if (not prior_options_.empty())
+ state_last = prior_options_.getState();
for (auto key : structure)
{
@@ -603,8 +595,8 @@ VectorComposite Problem::getOdometry(const StateStructure& _structure) const
VectorComposite state_last;
- if (prior_options_ and prior_options_->mode != "nothing")
- state_last = prior_options_->state;
+ if (not prior_options_.empty())
+ state_last = prior_options_.getState();
for (auto key : structure)
{
@@ -1049,7 +1041,17 @@ FrameBasePtr Problem::closestFrameToTimeStamp(const TimeStamp& _ts)
return trajectory_ptr_->closestFrameToTimeStamp(_ts);
}
-void Problem::setPriorOptions(const std::string& _mode,
+void Problem::setPriorOptions(const PriorMap& priors)
+{
+ if (not prior_options_.empty())
+ {
+ throw std::runtime_error("prior options have already been applied");
+ }
+
+ prior_options_ = priors;
+}
+
+/*void Problem::setPriorOptions(const std::string& _mode,
const VectorComposite& _state ,
const VectorComposite& _sigma )
{
@@ -1091,13 +1093,61 @@ void Problem::setPriorOptions(const std::string& _mode,
prior_options_->cov = Q;
}
}
-}
+}*/
FrameBasePtr Problem::applyPriorOptions(const TimeStamp& _ts)
{
- assert(!isPriorSet() && "applyPriorOptions can be called once!");
+ assert(!isPriorApplied() && "applyPriorOptions can be called once!");
- FrameBasePtr prior_keyframe(nullptr);
+ // Create first frame
+ FrameBasePtr prior_keyframe = emplaceFrame(_ts, frame_structure_, prior_options_.getState());
+ CaptureBasePtr prior_cap(nullptr);
+
+ for (auto prior_pair : prior_options_)
+ {
+ auto key = prior_pair.first;
+ auto sb = prior_keyframe->getStateBlock(key);
+
+ // Fix
+ if (prior_pair.second.isFixed())
+ {
+ sb->fix();
+ }
+
+ // Factor
+ 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);
+
+ // Emplace a feature
+ // TODO: compute cov
+ Eigen::MatrixXd cov;
+ 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())
+ {
+ 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("Absolute factor not implemented for state of type " + prior_pair.second.getType());
+ }
+ else
+ {
+ auto prior_fac = FactorBase::emplace<FactorBlockAbsolute>(prior_fea, prior_fea, sb, nullptr, false);
+ }
+
+ }
+ }
+ }
if (prior_options_->mode != "nothing" and prior_options_->mode != "")
{
diff --git a/src/processor/processor_base.cpp b/src/processor/processor_base.cpp
index a4a448aa610ebc9890f90b9060005c1d54f7881f..fb2f26fea436e06e2c68f87f958d8f9209b8f586 100644
--- a/src/processor/processor_base.cpp
+++ b/src/processor/processor_base.cpp
@@ -85,7 +85,7 @@ void ProcessorBase::captureCallback(CaptureBasePtr _capture)
startCaptureProfiling();
// apply prior in problem if not done (very first capture)
- if (getProblem() && !getProblem()->isPriorSet())
+ if (getProblem() and not getProblem()->isPriorApplied())
getProblem()->applyPriorOptions(_capture->getTimeStamp());
// asking if capture should be stored
diff --git a/src/sensor/prior_sensor.cpp b/src/sensor/prior_sensor.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..b3b67f2443168d0dde35b2ee680dc0c0910f3251
--- /dev/null
+++ b/src/sensor/prior_sensor.cpp
@@ -0,0 +1,97 @@
+//--------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 db7c52199433dc72922a008725d2b1785e365e58..90d40f660ff1c5df3ba584288074ae2065a70cef 100644
--- a/src/sensor/sensor_base.cpp
+++ b/src/sensor/sensor_base.cpp
@@ -20,7 +20,6 @@
//
//--------LICENSE_END--------
#include "core/sensor/sensor_base.h"
-#include "core/state_block/prior.h"
#include "core/state_block/factory_state_block.h"
#include "core/state_block/state_block.h"
#include "core/state_block/state_quaternion.h"
@@ -35,14 +34,14 @@ unsigned int SensorBase::sensor_id_count_ = 0;
SensorBase::SensorBase(const std::string& _type,
const int& _dim,
ParamsSensorBasePtr _params,
- const Priors& _priors) :
+ const PriorSensorMap& _priors) :
NodeBase("SENSOR", _type, _params->name),
HasStateBlocks("PO"),
hardware_ptr_(),
sensor_id_(++sensor_id_count_), // simple ID factory
params_(_params),
state_block_dynamic_(),
- params_prior_map_(),
+ features_prior_map_(),
last_capture_(nullptr),
dim_(_dim)
{
@@ -56,7 +55,7 @@ SensorBase::~SensorBase()
removeStateBlocks(getProblem());
}
-void SensorBase::loadPriors(const Priors& _priors)
+void SensorBase::loadPriors(const PriorSensorMap& _priors)
{
// Iterate all keys in _priors
for (auto state_pair : _priors)
@@ -190,8 +189,8 @@ void SensorBase::addPriorParameter(const char& _key, const Eigen::VectorXd& _x,
}
// remove previous prior (if any)
- if (params_prior_map_.find(_key) != params_prior_map_.end())
- params_prior_map_[_key]->remove();
+ if (features_prior_map_.find(_key) != features_prior_map_.end())
+ features_prior_map_[_key]->remove();
// create feature
FeatureBasePtr ftr_prior = FeatureBase::emplace<FeatureBase>(nullptr, "ABSOLUTE",_x,_cov);
@@ -207,7 +206,7 @@ void SensorBase::addPriorParameter(const char& _key, const Eigen::VectorXd& _x,
FactorBase::emplace<FactorBlockAbsolute>(ftr_prior, ftr_prior, _sb, _start_idx, _x.size(), nullptr, false);
// store feature in params_prior_map_
- params_prior_map_[_key] = ftr_prior;
+ features_prior_map_[_key] = ftr_prior;
}
void SensorBase::registerNewStateBlocks(ProblemPtr _problem)
@@ -521,7 +520,7 @@ void SensorBase::setProblem(ProblemPtr _problem)
NodeBase::setProblem(_problem);
for (auto prc : processor_list_)
prc->setProblem(_problem);
- for(auto ftr_prior : params_prior_map_)
+ for(auto ftr_prior : features_prior_map_)
ftr_prior.second->setProblem(_problem);
}
diff --git a/src/sensor/sensor_diff_drive.cpp b/src/sensor/sensor_diff_drive.cpp
index 1819213b7cf468cd764092b8bf3db18339bf6033..5e4aef093efc4cab77eb44a1ebd4a1ce179d21fc 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 Priors& _priors) :
+ const PriorSensorMap& _priors) :
SensorBase("SensorDiffDrive",
2,
_params,
diff --git a/src/sensor/sensor_motion_model.cpp b/src/sensor/sensor_motion_model.cpp
index a567d2c4fe9ccaa0e8545955f3c3f8cc4c0d6a0d..b2aa8d5d3cd7c1677874316d94460343f2396284 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 Priors& _priors) :
+ const PriorSensorMap& _priors) :
SensorBase("SensorMotionModel",
-1,
_params,
diff --git a/src/state_block/prior.cpp b/src/state_block/prior.cpp
index eabd2ef1835434e72032c496def94dc9b857f5e8..a37b084e2e4a8f4a9e997a8e0ab1c3dd644550e8 100644
--- a/src/state_block/prior.cpp
+++ b/src/state_block/prior.cpp
@@ -27,11 +27,11 @@
namespace wolf
{
-Priors::Priors(const YAML::Node& priors_node)
+PriorMap::PriorMap(const YAML::Node& priors_node)
{
if (not priors_node.IsMap())
{
- throw std::runtime_error("Priors: constructor with a non-map yaml node");
+ throw std::runtime_error("PriorMap: constructor with a non-map yaml node");
}
for (auto prior_pair : priors_node)
{
@@ -39,13 +39,20 @@ Priors::Priors(const YAML::Node& priors_node)
}
}
+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,
- bool _dynamic,
- const Eigen::VectorXd& _drift_std)
- : type_(_type), state_(_state), mode_(_mode), noise_std_(_noise_std), dynamic_(_dynamic), drift_std_(_drift_std)
+ const Eigen::VectorXd& _noise_std)
+ : type_(_type), state_(_state), mode_(_mode), noise_std_(_noise_std)
{
check();
}
@@ -60,13 +67,6 @@ Prior::Prior(const YAML::Node& prior_node)
else
noise_std_ = Eigen::VectorXd(0);
- 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();
}
@@ -82,11 +82,13 @@ void Prior::check() const
// try to create a state block and check for local parameterization and dimensions consistency
StateBlockPtr sb;
- try {
+ 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());
+ }
+ 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
@@ -102,11 +104,6 @@ void Prior::check() const
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());
-
- // 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 Prior::print() const
@@ -114,8 +111,7 @@ std::string Prior::print() const
return "Prior " + type_ + "\n" +
"mode: " + mode_ + "\n" +
"state: " + toString(state_) + "\n" +
- (mode_ == "factor" ? "noise_std: " + toString(noise_std_) + "\n" : "") + "dynamic: " + toString(dynamic_) +
- "\n" + (dynamic_ ? "drift_std: " + toString(drift_std_) + "\n" : "");
+ (mode_ == "factor" ? "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 c2c6857f74bf9f266b1fe143530c67ef0d1fb404..5736cabf66600b37b16cd9da8fc4bc56b215f0af 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -85,9 +85,6 @@ wolf_add_gtest(gtest_motion_buffer gtest_motion_buffer.cpp)
# Parameters server
# wolf_add_gtest(gtest_param_server gtest_param_server.cpp)
-# Parameters server
-wolf_add_gtest(gtest_prior gtest_prior.cpp)
-
# YAML parser
# wolf_add_gtest(gtest_parser_yaml gtest_parser_yaml.cpp)
@@ -200,6 +197,9 @@ wolf_add_gtest(gtest_tree_manager gtest_tree_manager.cpp)
# # ProcessorMotion in 2d
# wolf_add_gtest(gtest_odom_2d gtest_odom_2d.cpp)
+# PriorSensor
+wolf_add_gtest(gtest_prior_sensor gtest_prior_sensor.cpp)
+
# # ProcessorOdom3d class test
# wolf_add_gtest(gtest_processor_odom_3d gtest_processor_odom_3d.cpp)
diff --git a/test/dummy/sensor_dummy.h b/test/dummy/sensor_dummy.h
index 5f82b090dc2327e4cda05f3d0dd07abbc4c50e85..77b8c9772ffb56ab6522a2194508655fe96cfa1e 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 Priors& _priors) :
+ const PriorSensorMap& _priors) :
SensorBase("SensorDummy"+toString(DIM)+"d",
DIM,
_params,
diff --git a/test/gtest_emplace.cpp b/test/gtest_emplace.cpp
index a18e266ccc164878cb100a90e0fcfae3dbb5a3d9..3e9fd8f63fbab40985179670ae055288247e1f32 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>(),
- Priors({{'P',Prior("P",Vector2d::Zero())},
- {'O',Prior("O",Vector1d::Zero())}}));
+ PriorSensorMap({{'P',PriorSensor("P",Vector2d::Zero())},
+ {'O',PriorSensor("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>(),
- Priors({{'P',Prior("P",Vector2d::Zero())},
- {'O',Prior("O",Vector1d::Zero())}}));
+ PriorSensorMap({{'P',PriorSensor("P",Vector2d::Zero())},
+ {'O',PriorSensor("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>(),
- Priors({{'P',Prior("P",Vector2d::Zero())},
- {'O',Prior("O",Vector1d::Zero())}}));
+ PriorSensorMap({{'P',PriorSensor("P",Vector2d::Zero())},
+ {'O',PriorSensor("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>(),
- Priors({{'P',Prior("P",Vector2d::Zero())},
- {'O',Prior("O",Vector1d::Zero())}}));
+ PriorSensorMap({{'P',PriorSensor("P",Vector2d::Zero())},
+ {'O',PriorSensor("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 2cca034dd35daa2b368b9a0e1bc1c347fb0a0ec7..e69b506e3effcaf721b12eca254d616fe312441d 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 !!!
- Priors priors{{'P',Prior("P", Vector2d::Zero())}, //default "fix", not dynamic
- {'O',Prior("O", Vector1d::Zero())}, //default "fix", not dynamic
- {'I',Prior("StateBlock", Vector3d::Ones(), "initial_guess")}}; // not dynamic
+ 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
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 !!!
- Priors priors{{'P',Prior("P", Vector2d::Zero())}, //default "fix", not dynamic
- {'O',Prior("O", Vector1d::Zero())}, //default "fix", not dynamic
- {'I',Prior("StateBlock", Vector3d::Ones(), "initial_guess")}}; // not dynamic
+ 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
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 !!!
- Priors priors{{'P',Prior("P", Vector2d::Zero())}, //default "fix", not dynamic
- {'O',Prior("O", Vector1d::Zero())}, //default "fix", not dynamic
- {'I',Prior("StateBlock", Vector3d::Ones(), "initial_guess")}}; // not dynamic
+ 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
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 3f6c88ac226b0fab73b3ab8b4f5c21e649056dbe..f9b7a27b5d11c80a3fab5da79cd4aff061fe948b 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>(),
- Priors{{'P',Prior("P",pos_camera)},
- {'O',Prior("O",vquat_camera)}});
+ PriorSensorMap{{'P',PriorSensor("P",pos_camera)},
+ {'O',PriorSensor("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 9c90d2ce427328577eb57ac41d221ecd05f680d8..8ff7aefc9b877cbb8ff74aaf62f684f427f2d1ca 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>();
-Priors priors{{'P',Prior("P",initial_extrinsics_p,"initial_guess")},
- {'O',Prior("O",initial_extrinsics_o,"initial_guess")}};
+PriorSensorMap priors{{'P',PriorSensor("P",initial_extrinsics_p,"initial_guess")},
+ {'O',PriorSensor("O",initial_extrinsics_o,"initial_guess")}};
SensorBasePtr sensor_ptr_ = problem_ptr->installSensor("SensorOdom",
"odometer",
params,
diff --git a/test/gtest_prior.cpp b/test/gtest_prior_sensor.cpp
similarity index 93%
rename from test/gtest_prior.cpp
rename to test/gtest_prior_sensor.cpp
index c8abf4fa57bf34dbbbeee8546d025bff062b5650..a850f3a5d664ee6a8ca01c6b82ab8118844e4434 100644
--- a/test/gtest_prior.cpp
+++ b/test/gtest_prior_sensor.cpp
@@ -22,7 +22,7 @@
#include "core/utils/utils_gtest.h"
#include "core/common/wolf.h"
-#include "core/state_block/prior.h"
+#include "core/sensor/prior_sensor.h"
#include "core/common/params_base.h" // toString
using namespace wolf;
@@ -47,8 +47,8 @@ struct PriorAsStruct
VectorXd drift_std;
};
-void testPrior(const Prior& P,
- const PriorAsStruct& pstruct)
+void testPriorSensor(const PriorSensor& P,
+ const PriorAsStruct& pstruct)
{
ASSERT_EQ(pstruct.type, P.getType());
ASSERT_EQ(pstruct.mode, P.getMode());
@@ -71,7 +71,7 @@ void testPrior(const Prior& P,
}
};
-void testPriors(const std::vector<PriorAsStruct>& setups, bool should_work)
+void testPriorSensorSensorMap(const std::vector<PriorAsStruct>& setups, bool should_work)
{
WOLF_INFO("Testing setups that should ", (should_work ? "" : "NOT"), " work...");
for (auto setup : setups)
@@ -84,15 +84,15 @@ void testPriors(const std::vector<PriorAsStruct>& setups, bool should_work)
"\n\tdynamic: ", setup.dynamic,
"\n\tdrift_std: ", setup.drift_std.transpose());
if (should_work)
- testPrior(Prior(setup.type, setup.state, setup.mode, setup.noise_std, setup.dynamic, setup.drift_std), setup);
+ testPriorSensor(PriorSensor(setup.type, setup.state, setup.mode, setup.noise_std, setup.dynamic, setup.drift_std), setup);
else
{
- ASSERT_THROW(testPrior(Prior(setup.type, setup.state, setup.mode, setup.noise_std, setup.dynamic, setup.drift_std), setup),std::runtime_error);
+ ASSERT_THROW(testPriorSensor(PriorSensor(setup.type, setup.state, setup.mode, setup.noise_std, setup.dynamic, setup.drift_std), setup),std::runtime_error);
}
}
}
-TEST(Prior, P)
+TEST(PriorSensor, P)
{
std::vector<PriorAsStruct> setups_ok, setups_death;
@@ -138,11 +138,11 @@ TEST(Prior, P)
setups_death.push_back(PriorAsStruct({"P","factor",vector3,vector4,true,vector3})); // inconsistent size
// TEST SETUPS
- testPriors(setups_ok, true);
- testPriors(setups_death, false);
+ testPriorSensorSensorMap(setups_ok, true);
+ testPriorSensorSensorMap(setups_death, false);
}
-TEST(Prior, O)
+TEST(PriorSensor, O)
{
std::vector<PriorAsStruct> setups_ok, setups_death;
@@ -194,11 +194,11 @@ TEST(Prior, O)
setups_death.push_back(PriorAsStruct({"O","factor",vectorq,vector4,true,vector3})); // inconsistent size
// TEST SETUPS
- testPriors(setups_ok, true);
- testPriors(setups_death, false);
+ testPriorSensorSensorMap(setups_ok, true);
+ testPriorSensorSensorMap(setups_death, false);
}
-TEST(Prior, StateBlock)
+TEST(PriorSensor, StateBlock)
{
std::vector<PriorAsStruct> setups_ok, setups_death;
@@ -244,10 +244,10 @@ TEST(Prior, StateBlock)
setups_death.push_back(PriorAsStruct({"StateParams3","factor",vector3,vector4,true,vector3})); // inconsistent size
// TEST SETUPS
- testPriors(setups_ok, true);
- testPriors(setups_death, false);
+ testPriorSensorSensorMap(setups_ok, true);
+ testPriorSensorSensorMap(setups_death, false);
}
-TEST(Prior, StateAngle)
+TEST(PriorSensor, StateAngle)
{
std::vector<PriorAsStruct> setups_ok, setups_death;
@@ -284,11 +284,11 @@ TEST(Prior, StateAngle)
setups_death.push_back(PriorAsStruct({"StateAngle","factor",vector1,vector1,true,vector3})); // inconsistent size
// TEST SETUPS
- testPriors(setups_ok, true);
- testPriors(setups_death, false);
+ testPriorSensorSensorMap(setups_ok, true);
+ testPriorSensorSensorMap(setups_death, false);
}
-TEST(Prior, StateQuaternion)
+TEST(PriorSensor, StateQuaternion)
{
std::vector<PriorAsStruct> setups_ok, setups_death;
@@ -331,11 +331,11 @@ TEST(Prior, StateQuaternion)
setups_death.push_back(PriorAsStruct({"StateQuaternion","factor",vectorq,vector4,true,vector3})); // inconsistent size
// TEST SETUPS
- testPriors(setups_ok, true);
- testPriors(setups_death, false);
+ testPriorSensorSensorMap(setups_ok, true);
+ testPriorSensorSensorMap(setups_death, false);
}
-TEST(Prior, YamlNode)
+TEST(PriorSensor, YamlNode)
{
YAML::Node input_node = YAML::LoadFile(wolf_root + "/test/yaml/params_prior.yaml");
@@ -364,7 +364,7 @@ TEST(Prior, YamlNode)
std::string prefix = key + "_" + toString(dim) + "D_" + mode + (dynamic ? "_dynamic" : "") + (drift ? "_drift" : "");
WOLF_INFO("Creating prior from prefix ", prefix);
- auto P = Prior(input_node[prefix]);
+ auto P = PriorSensor(input_node[prefix]);
// Checks
ASSERT_EQ(P.getMode(), mode);
@@ -401,7 +401,7 @@ TEST(Prior, YamlNode)
std::string prefix = "I_" + mode + (dynamic ? "_dynamic" : "") + (drift ? "_drift" : "");
WOLF_INFO("Creating prior from prefix ", prefix);
- auto P = Prior(input_node[prefix]);
+ auto P = PriorSensor(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(Prior, YamlNode)
}
}
-TEST(Prior, ParamsServerWrong)
+TEST(PriorSensor, ParamsServerWrong)
{
YAML::Node input_node = YAML::LoadFile(wolf_root + "/test/yaml/params_prior_wrong.yaml");
@@ -440,7 +440,7 @@ TEST(Prior, ParamsServerWrong)
std::string prefix = key + "_" + toString(dim) + "D_" + mode + (dynamic ? "_dynamic" : "") + (drift ? "_drift" : "");
WOLF_INFO("Creating prior from prefix ", prefix);
- ASSERT_THROW(auto prior = Prior(input_node[prefix]),std::runtime_error);
+ ASSERT_THROW(auto prior = PriorSensor(input_node[prefix]),std::runtime_error);
}
// I
@@ -454,7 +454,7 @@ TEST(Prior, ParamsServerWrong)
std::string prefix = "I_" + mode + (dynamic ? "_dynamic" : "") + (drift ? "_drift" : "");
WOLF_INFO("Creating prior from prefix ", prefix);
- ASSERT_THROW(auto prior = Prior(input_node[prefix]),std::runtime_error);
+ ASSERT_THROW(auto prior = PriorSensor(input_node[prefix]),std::runtime_error);
}
}
diff --git a/test/gtest_problem.cpp b/test/gtest_problem.cpp
index b73e36e1262465f06ea1c9d3e965e75829e1ce55..a019989126e9904dcfc3a87ecaf1fe385f48aeb0 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,
- Priors({{'P',Prior("P",Vector3d::Zero())},
- {'O',Prior("O",Vector4d::Random().normalized())}}));
+ PriorSensorMap({{'P',PriorSensor("P",Vector3d::Zero())},
+ {'O',PriorSensor("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,
- Priors({{'P',Prior("P",Vector3d::Zero())},
- {'O',Prior("O",Vector4d::Random().normalized())}}));
+ PriorSensorMap({{'P',PriorSensor("P",Vector3d::Zero())},
+ {'O',PriorSensor("O",Vector4d::Random().normalized())}}));
// add motion processor
auto Pm = ProcessorBase::emplace<ProcessorOdom3d>(Sm, std::make_shared<ParamsProcessorOdom3d>());
@@ -386,9 +386,9 @@ TEST(Problem, perturb)
param->ticks_per_wheel_revolution = 100;
auto sensor = SensorBase::emplace<SensorDiffDrive>(problem->getHardware(),
param,
- Priors({{'P',Prior("P",Vector2d::Zero())},
- {'O',Prior("O",Vector1d::Zero())},
- {'I',Prior("StateParams3",Vector3d::Ones(),"initial_guess", VectorXd(0),true)}}));
+ PriorSensorMap({{'P',PriorSensor("P",Vector2d::Zero())},
+ {'O',PriorSensor("O",Vector1d::Zero())},
+ {'I',PriorSensor("StateParams3",Vector3d::Ones(),"initial_guess", VectorXd(0),true)}}));
Vector3d pose; pose << 0,0,0;
@@ -472,9 +472,9 @@ TEST(Problem, check)
param->ticks_per_wheel_revolution = 100;
auto sensor = SensorBase::emplace<SensorDiffDrive>(problem->getHardware(),
param,
- Priors({{'P',Prior("P",Vector2d::Zero())},
- {'O',Prior("O",Vector1d::Zero())},
- {'I',Prior("StateParams3",Vector3d::Ones(),"initial_guess", VectorXd(0),true)}}));
+ PriorSensorMap({{'P',PriorSensor("P",Vector2d::Zero())},
+ {'O',PriorSensor("O",Vector1d::Zero())},
+ {'I',PriorSensor("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 87eb9db92716ea4ad4ddefec788bb60499780d33..fb321e4295e06594e899b5f93250097f86ee9802 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, Priors{{'P',Prior("P",b_p_bm_)},
- {'O',Prior("O",b_q_m_.coeffs())}});
+ sensor_pose_ = problem_->installSensor("SensorPose", "pose", params_sp, PriorSensorMap{{'P',PriorSensor("P",b_p_bm_)},
+ {'O',PriorSensor("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_diff_drive.cpp b/test/gtest_processor_diff_drive.cpp
index 04fa9ddb439181ed6013e02546c7e2a72838366e..79a3b59cdb88f242d4c000bf5f9929dbcb462d81 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 !!!
- Priors priors{{'P',Prior("P", Vector2d::Zero())}, //default "fix", not dynamic
- {'O',Prior("O", Vector1d::Zero())}, //default "fix", not dynamic
- {'I',Prior("StateBlock", Vector3d::Ones(), "initial_guess")}}; // not dynamic 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 !!!
sensor = std::static_pointer_cast<SensorDiffDrive>(problem->installSensor("SensorDiffDrive",
"sensor",
diff --git a/test/gtest_sensor_base.cpp b/test/gtest_sensor_base.cpp
index 1ec8217d3e920ecf944eb457f615faca7dbf351b..0a12ed5723484c4a2ac7b326669bb304504ce49b 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,
- Priors({{'P',Prior("P", p_state_2D)}, //default "fix", not dynamic
- {'O',Prior("O", o_state_2D)}}));
+ PriorSensorMap({{'P',PriorSensor("P", p_state_2D)}, //default "fix", not dynamic
+ {'O',PriorSensor("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,
- Priors({{'P',Prior("P", p_state_3D)}, //default "fix", not dynamic
- {'O',Prior("O", o_state_3D)}}));
+ PriorSensorMap({{'P',PriorSensor("P", p_state_3D)}, //default "fix", not dynamic
+ {'O',PriorSensor("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,
- Priors({{'P',Prior("P", p_state_2D, "initial_guess")},
- {'O',Prior("O", o_state_2D, "initial_guess")}}));
+ PriorSensorMap({{'P',PriorSensor("P", p_state_2D, "initial_guess")},
+ {'O',PriorSensor("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,
- Priors({{'P',Prior("P", p_state_3D, "initial_guess")},
- {'O',Prior("O", o_state_3D, "initial_guess")}}));
+ PriorSensorMap({{'P',PriorSensor("P", p_state_3D, "initial_guess")},
+ {'O',PriorSensor("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,
- Priors({{'P',Prior("P", p_state_2D, "factor", p_std_2D)},
- {'O',Prior("O", o_state_2D, "factor", o_std_2D)}}));
+ PriorSensorMap({{'P',PriorSensor("P", p_state_2D, "factor", p_std_2D)},
+ {'O',PriorSensor("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,
- Priors({{'P',Prior("P", p_state_3D, "factor", p_std_3D)},
- {'O',Prior("O", o_state_3D, "factor", o_std_3D)}}));
+ PriorSensorMap({{'P',PriorSensor("P", p_state_3D, "factor", p_std_3D)},
+ {'O',PriorSensor("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,
- Priors({{'P',Prior("P", p_state_2D, "initial_guess", vector0, true)},
- {'O',Prior("O", o_state_2D, "initial_guess", vector0, true)}}));
+ PriorSensorMap({{'P',PriorSensor("P", p_state_2D, "initial_guess", vector0, true)},
+ {'O',PriorSensor("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,
- Priors({{'P',Prior("P", p_state_3D, "initial_guess", vector0, true)},
- {'O',Prior("O", o_state_3D, "initial_guess", vector0, true)}}));
+ PriorSensorMap({{'P',PriorSensor("P", p_state_3D, "initial_guess", vector0, true)},
+ {'O',PriorSensor("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,
- Priors({{'P',Prior("P", p_state_2D, "initial_guess", vector0, true, p_std_2D)},
- {'O',Prior("O", o_state_2D, "initial_guess", vector0, true, o_std_2D)}}));
+ 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)}}));
// 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,
- Priors({{'P',Prior("P", p_state_3D, "initial_guess", vector0, true, p_std_3D)},
- {'O',Prior("O", o_state_3D, "initial_guess", vector0, true, o_std_3D)}}));
+ 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)}}));
// 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,
- Priors({{'P',Prior("P", p_state_3D, "fix", vector0, true)},
- {'O',Prior("O", o_state_3D, "factor", o_std_3D, true, o_std_3D)},
- {'I',Prior("StateParams5", i_state_3D, "initial_guess")},
- {'A',Prior("StateQuaternion", o_state_3D, "factor", o_std_3D)}}));
+ 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)}}));
// 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 2339b5026489b26a40c0985421782b791d048d0b..7271e389f018ab8b9a4dd7d490a05817eb3742b3 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>();
- Priors priors{{'P',Prior("P", p_state)}, //default "fix", not dynamic
- {'O',Prior("O", o_state)}, //default "fix", not dynamic
- {'I',Prior("StateBlock", i_state)}}; //default "fix", not dynamic
+ 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
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>();
- Priors priors{{'P',Prior("P", p_state)}, //default "fix", not dynamic
- {'O',Prior("O", o_state)}, //default "fix", not dynamic
- {'I',Prior("StateBlock", i_state)}}; //default "fix", not dynamic
+ 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
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;
- Priors priors{{'P',Prior("P", p_state)}, //default "fix", not dynamic
- {'O',Prior("O", o_state)}, //default "fix", not dynamic
- {'I',Prior("StateBlock", i_state)}}; //default "fix", not dynamic
+ 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
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;
- Priors priors{{'P',Prior("P", p_state)}, //default "fix", not dynamic
- {'O',Prior("O", o_state)}, //default "fix", not dynamic
- {'I',Prior("StateBlock", i_state)}}; //default "fix", not dynamic
+ 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
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 23e6b46046628a022cf4fc80c7d950675a6b9927..de64ac89351097bb5444526f2586773962f56161 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,
- Priors({{'P',Prior("P", p_state_2D)}, //default "fix", not dynamic
- {'O',Prior("O", o_state_2D)}}));
+ PriorSensorMap({{'P',PriorSensor("P", p_state_2D)}, //default "fix", not dynamic
+ {'O',PriorSensor("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,
- Priors({{'P',Prior("P", p_state_3D)}, //default "fix", not dynamic
- {'O',Prior("O", o_state_3D)}}));
+ PriorSensorMap({{'P',PriorSensor("P", p_state_3D)}, //default "fix", not dynamic
+ {'O',PriorSensor("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,
- Priors({{'P',Prior("P", p_state_2D, "initial_guess")},
- {'O',Prior("O", o_state_2D, "initial_guess")}}));
+ PriorSensorMap({{'P',PriorSensor("P", p_state_2D, "initial_guess")},
+ {'O',PriorSensor("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,
- Priors({{'P',Prior("P", p_state_3D, "initial_guess")},
- {'O',Prior("O", o_state_3D, "initial_guess")}}));
+ PriorSensorMap({{'P',PriorSensor("P", p_state_3D, "initial_guess")},
+ {'O',PriorSensor("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,
- Priors({{'P',Prior("P", p_state_2D, "factor", p_std_2D)},
- {'O',Prior("O", o_state_2D, "factor", o_std_2D)}}));
+ PriorSensorMap({{'P',PriorSensor("P", p_state_2D, "factor", p_std_2D)},
+ {'O',PriorSensor("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,
- Priors({{'P',Prior("P", p_state_3D, "factor", p_std_3D)},
- {'O',Prior("O", o_state_3D, "factor", o_std_3D)}}));
+ PriorSensorMap({{'P',PriorSensor("P", p_state_3D, "factor", p_std_3D)},
+ {'O',PriorSensor("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,
- Priors({{'P',Prior("P", p_state_2D, "initial_guess", vector0, true)},
- {'O',Prior("O", o_state_2D, "initial_guess", vector0, true)}}));
+ PriorSensorMap({{'P',PriorSensor("P", p_state_2D, "initial_guess", vector0, true)},
+ {'O',PriorSensor("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,
- Priors({{'P',Prior("P", p_state_3D, "initial_guess", vector0, true)},
- {'O',Prior("O", o_state_3D, "initial_guess", vector0, true)}}));
+ PriorSensorMap({{'P',PriorSensor("P", p_state_3D, "initial_guess", vector0, true)},
+ {'O',PriorSensor("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,
- Priors({{'P',Prior("P", p_state_2D, "initial_guess", vector0, true, p_std_2D)},
- {'O',Prior("O", o_state_2D, "initial_guess", vector0, true, o_std_2D)}}));
+ 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)}}));
// 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,
- Priors({{'P',Prior("P", p_state_3D, "initial_guess", vector0, true, p_std_3D)},
- {'O',Prior("O", o_state_3D, "initial_guess", vector0, true, o_std_3D)}}));
+ 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)}}));
// 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,
- Priors({{'P',Prior("P", p_state_3D)}, //default "fix", not dynamic
- {'O',Prior("O", o_state_3D)}}));
+ PriorSensorMap({{'P',PriorSensor("P", p_state_3D)}, //default "fix", not dynamic
+ {'O',PriorSensor("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 85f0c0668c7519c23c0f5a0437c37348b16fe6d0..2bf0d87ae928cf688ced83c968702cd2fbd2b914 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;
- Priors priors{{'P',Prior("P", p_state_2D)}, //default "fix", not dynamic
- {'O',Prior("O", o_state_2D)}}; //default "fix", not dynamic
+ PriorSensorMap priors{{'P',PriorSensor("P", p_state_2D)}, //default "fix", not dynamic
+ {'O',PriorSensor("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;
- Priors priors{{'P',Prior("P", p_state_3D)}, //default "fix", not dynamic
- {'O',Prior("O", o_state_3D)}}; //default "fix", not dynamic
+ PriorSensorMap priors{{'P',PriorSensor("P", p_state_3D)}, //default "fix", not dynamic
+ {'O',PriorSensor("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;
- Priors priors{{'P',Prior("P", p_state_2D)}, //default "fix", not dynamic
- {'O',Prior("O", o_state_2D)}}; //default "fix", not dynamic
+ PriorSensorMap priors{{'P',PriorSensor("P", p_state_2D)}, //default "fix", not dynamic
+ {'O',PriorSensor("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;
- Priors priors{{'P',Prior("P", p_state_3D)}, //default "fix", not dynamic
- {'O',Prior("O", o_state_3D)}}; //default "fix", not dynamic
+ PriorSensorMap priors{{'P',PriorSensor("P", p_state_3D)}, //default "fix", not dynamic
+ {'O',PriorSensor("O", o_state_3D)}}; //default "fix", not dynamic
auto sb = FactorySensorPriors::create("SensorPose","sensor_1", 3, param, priors);
diff --git a/test/yaml/params_tree_manager1.yaml b/test/yaml/params_tree_manager1.yaml
index a51cf0355f324260a077161b44c583767b8f246e..af9245339e4a0dd9ad5538a84598b6bbfcd0f240 100644
--- a/test/yaml/params_tree_manager1.yaml
+++ b/test/yaml/params_tree_manager1.yaml
@@ -18,7 +18,7 @@ problem:
follow: "tree_manager_dummy.yaml"
sensors:
-
- type: "SensorOdom"
+ type: "SensorOdom3d"
name: "odom"
plugin: "core"
states:
diff --git a/test/yaml/params_tree_manager2.yaml b/test/yaml/params_tree_manager2.yaml
index b38cefd896c77a257001ef9d6746d7ce6eaa7367..b8acc1b021309cfae76c86b4f96b8b059ee75cf8 100644
--- a/test/yaml/params_tree_manager2.yaml
+++ b/test/yaml/params_tree_manager2.yaml
@@ -18,7 +18,7 @@ problem:
type: "None"
sensors:
-
- type: "SensorOdom"
+ type: "SensorOdom3d"
name: "odom"
plugin: "core"
states: