diff --git a/CMakeLists.txt b/CMakeLists.txt
index 80a9f2404d851bb255d3754777ed6c491e44052c..96f0efa8a76ebcfd5da19b986309d49324f28ebf 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -202,7 +202,7 @@ SET(HDRS_PROCESSOR
# include/core/processor/processor_fixed_wing_model.h
# include/core/processor/processor_loop_closure.h
include/core/processor/processor_motion.h
- # include/core/processor/processor_odom_2d.h
+ include/core/processor/processor_odom_2d.h
# include/core/processor/processor_odom_3d.h
# include/core/processor/processor_pose.h
include/core/processor/processor_tracker.h
@@ -311,7 +311,7 @@ SET(SRCS_PROCESSOR
# src/processor/processor_fixed_wing_model.cpp
# src/processor/processor_loop_closure.cpp
src/processor/processor_motion.cpp
- # src/processor/processor_odom_2d.cpp
+ src/processor/processor_odom_2d.cpp
# src/processor/processor_odom_3d.cpp
# src/processor/processor_pose.cpp
src/processor/processor_tracker.cpp
diff --git a/demos/hello_wolf/sensor_range_bearing.cpp b/demos/hello_wolf/sensor_range_bearing.cpp
index 24dfc14b97a20d5fc9e4c9861da8cf32f0305f45..d81740193f2ff343cbd2cd592d7c717c07c11c0a 100644
--- a/demos/hello_wolf/sensor_range_bearing.cpp
+++ b/demos/hello_wolf/sensor_range_bearing.cpp
@@ -61,5 +61,4 @@ SensorRangeBearing::~SensorRangeBearing()
namespace wolf
{
WOLF_REGISTER_SENSOR(SensorRangeBearing)
-WOLF_REGISTER_SENSOR_YAML(SensorRangeBearing)
} // namespace wolf
diff --git a/demos/hello_wolf/yaml/hello_wolf_config.yaml b/demos/hello_wolf/yaml/hello_wolf_config.yaml
index 4260a8c8efb26928a6d2242edbd43830b4467875..f01ad2b227a8824025d6d1aa0b2f415cde34c2c6 100644
--- a/demos/hello_wolf/yaml/hello_wolf_config.yaml
+++ b/demos/hello_wolf/yaml/hello_wolf_config.yaml
@@ -36,15 +36,11 @@ config:
- type: "SensorOdom2d"
plugin: "core"
name: "sen odom"
- extrinsic:
- pose: [0,0, 0]
follow: "demos/hello_wolf/yaml/sensor_odom_2d.yaml" # config parameters in this file
- type: "SensorRangeBearing"
plugin: "core"
name: "sen rb"
- extrinsic:
- pose: [1,1, 0]
noise_range_metres_std: 0.2 # parser only considers first appearence so the following file parsing will not overwrite this param
follow: demos/hello_wolf/yaml/sensor_range_bearing.yaml # config parameters in this file
diff --git a/demos/hello_wolf/yaml/sensor_odom_2d.yaml b/demos/hello_wolf/yaml/sensor_odom_2d.yaml
index 0f724c780c41c708b0b5a63a0e429cbd3319cb1e..1480abfef83b8dff429879a74ee7340a6e9ae358 100644
--- a/demos/hello_wolf/yaml/sensor_odom_2d.yaml
+++ b/demos/hello_wolf/yaml/sensor_odom_2d.yaml
@@ -3,3 +3,13 @@
k_disp_to_disp: 0.1 # m^2 / m
k_rot_to_rot: 0.1 # rad^2 / rad
apply_loss_function: true
+
+states:
+ P:
+ mode: fix
+ state: [0,0]
+ dynamic: false
+ O:
+ mode: fix
+ state: [0]
+ dynamic: false
\ No newline at end of file
diff --git a/demos/hello_wolf/yaml/sensor_range_bearing.yaml b/demos/hello_wolf/yaml/sensor_range_bearing.yaml
index 7f81091db16c74fb479684a171256a91dd425f66..bf221a21af8db1a1fcb256c85d6020981de728ea 100644
--- a/demos/hello_wolf/yaml/sensor_range_bearing.yaml
+++ b/demos/hello_wolf/yaml/sensor_range_bearing.yaml
@@ -3,3 +3,13 @@
noise_range_metres_std: 0.1
noise_bearing_degrees_std: 0.5
apply_loss_function: true
+
+states:
+ P:
+ mode: fix
+ state: [1,1]
+ dynamic: false
+ O:
+ mode: fix
+ state: [0]
+ dynamic: false
\ No newline at end of file
diff --git a/include/core/problem/problem.h b/include/core/problem/problem.h
index 9c70f894f700413c035462e449dc9be64579d760..147a04430dd803e62321b143c194cfa6ac7f0e50 100644
--- a/include/core/problem/problem.h
+++ b/include/core/problem/problem.h
@@ -39,6 +39,7 @@ struct ParamsProcessorBase;
#include "core/common/wolf.h"
#include "core/utils/params_server.h"
#include "core/frame/frame_base.h"
+#include "core/state_block/prior.h"
#include "core/state_block/state_block.h"
#include "core/state_block/state_composite.h"
#include "core/processor/motion_provider.h"
diff --git a/include/core/sensor/factory_sensor.h b/include/core/sensor/factory_sensor.h
index a1c7b77a4a4abce394410335cd16607a5fc265be..0cd4418a68d052423c03f581d99bd1075821a4aa 100644
--- a/include/core/sensor/factory_sensor.h
+++ b/include/core/sensor/factory_sensor.h
@@ -33,11 +33,14 @@ namespace wolf
{
class SensorBase;
struct ParamsSensorBase;
+class ParamsServer;
+class Prior;
}
+#include <unordered_map>
+
// wolf
#include "core/common/factory.h"
-#include "core/utils/params_server.h"
namespace wolf
{
@@ -224,14 +227,24 @@ typedef Factory<SensorBase,
typedef Factory<SensorBase,
const std::string&,
SizeEigen,
- ParamsSensorBasePtr,
- const Priors&> FactorySensorPriors;
+ std::shared_ptr<ParamsSensorBase>,
+ const std::unordered_map<char,Prior>& > FactorySensorPriors;
template<>
inline std::string FactorySensor::getClass() const
{
return "FactorySensor";
}
+template<>
+inline std::string FactorySensorYaml::getClass() const
+{
+ return "FactorySensorYaml";
+}
+template<>
+inline std::string FactorySensorPriors::getClass() const
+{
+ return "FactorySensorPriors";
+}
#define WOLF_REGISTER_SENSOR(SensorType) \
namespace{ const bool WOLF_UNUSED SensorType##Registered = \
diff --git a/include/core/sensor/sensor_base.h b/include/core/sensor/sensor_base.h
index 13c5a77d60bac6100d13f7e0d06cc4f90870b595..3059521df8e6c6a47b169372396dc8c49a6b40ff 100644
--- a/include/core/sensor/sensor_base.h
+++ b/include/core/sensor/sensor_base.h
@@ -101,18 +101,15 @@ struct ParamsSensorBase: public ParamsBase
{
std::string prefix = "sensor/";
- Eigen::VectorXd noise_std;
-
ParamsSensorBase() = default;
ParamsSensorBase(std::string _unique_name, const wolf::ParamsServer& _server)
{
- noise_std = _server.getParam<Eigen::VectorXd>(prefix + _unique_name + "/noise_std");
}
~ParamsSensorBase() override = default;
std::string print() const override
{
- return "noise_std: " + toString(noise_std) + "\n";
+ return "";
}
};
@@ -276,15 +273,17 @@ class SensorBase : public NodeBase, public HasStateBlocks, public std::enable_sh
const Eigen::MatrixXd& _cov,
unsigned int _start_idx = 0);
- void setNoiseStd(const Eigen::VectorXd & _noise_std);
- void setNoiseCov(const Eigen::MatrixXd & _noise_cov);
+ // drift
void setDriftStd(char _key, const Eigen::VectorXd & _drift_rate_std);
void setDriftCov(char _key, const Eigen::MatrixXd & _drift_rate_cov);
- Eigen::VectorXd getNoiseStd() const;
- Eigen::MatrixXd getNoiseCov() const;
Eigen::VectorXd getDriftStd(char) const;
Eigen::MatrixXd getDriftCov(char) const;
+ // noise
+ virtual void setNoiseStd(const Eigen::VectorXd & _noise_std) = 0;
+ virtual Eigen::VectorXd getNoiseStd() const = 0;
+ virtual Eigen::MatrixXd getNoiseCov() const = 0;
+
virtual void printHeader(int depth, //
bool constr_by, //
bool metric, //
diff --git a/src/sensor/sensor_odom_2d.cpp b/src/sensor/sensor_odom_2d.cpp
index b5d0946059c44d191e4e13cf244e1b443ac5f950..fae0fe9a75c6f08d57ed7604b461ca3a1c0ac455 100644
--- a/src/sensor/sensor_odom_2d.cpp
+++ b/src/sensor/sensor_odom_2d.cpp
@@ -87,5 +87,4 @@ Eigen::Matrix2d SensorOdom2d::computeCovFromMotion (const VectorXd& _data) const
#include "core/sensor/factory_sensor.h"
namespace wolf {
WOLF_REGISTER_SENSOR(SensorOdom2d);
-WOLF_REGISTER_SENSOR_YAML(SensorOdom2d);
} // namespace wolf
diff --git a/test/dummy/sensor_dummy.h b/test/dummy/sensor_dummy.h
index 8b37639c862202dab8014f3f4ad2ef472ee3ad3b..e73f7d3c7837fa6583b26f6639c6f4220468dfe5 100644
--- a/test/dummy/sensor_dummy.h
+++ b/test/dummy/sensor_dummy.h
@@ -35,20 +35,23 @@ struct ParamsSensorDummy : public ParamsSensorBase
{
~ParamsSensorDummy() override = default;
- double param1;
+ double noise_p_std;
+ double noise_o_std;
int param2;
ParamsSensorDummy(std::string _unique_name, const ParamsServer& _server):
ParamsSensorBase(_unique_name, _server)
{
- param1 = _server.getParam<double>(prefix + _unique_name + "/param1");
+ noise_p_std = _server.getParam<double>(prefix + _unique_name + "/noise_p_std");
+ noise_o_std = _server.getParam<double>(prefix + _unique_name + "/noise_o_std");
param2 = _server.getParam<int>(prefix + _unique_name + "/param2");
}
std::string print() const override
{
- return ParamsSensorBase::print() + "\n"
- + "param1: " + std::to_string(param1) + "\n"
- + "param2: " + std::to_string(param2) + "\n";
+ return ParamsSensorBase::print() + "\n"
+ + "noise_p_std: " + toString(noise_p_std) + "\n"
+ + "noise_o_std: " + toString(noise_o_std) + "\n"
+ + "param2: " + toString(param2) + "\n";
}
};
@@ -56,6 +59,10 @@ WOLF_PTR_TYPEDEFS(SensorDummy);
class SensorDummy : public SensorBase
{
+ private:
+ ParamsSensorDummyPtr params_dummy_;
+ SizeEigen dim_;
+
public:
SensorDummy(const std::string& _unique_name,
SizeEigen _dim,
@@ -66,7 +73,8 @@ class SensorDummy : public SensorBase
_dim,
_params,
_server,
- std::unordered_map<char, std::string>{{'I',"StateBlock"},{'A',"StateQuaternion"}})
+ std::unordered_map<char, std::string>{{'I',"StateBlock"},{'A',"StateQuaternion"}}),
+ params_dummy_(_params)
{
}
WOLF_SENSOR_CREATE(SensorDummy, ParamsSensorDummy);
@@ -80,11 +88,41 @@ class SensorDummy : public SensorBase
_unique_name,
_dim,
_params,
- _priors)
+ _priors),
+ params_dummy_(_params)
{
}
virtual ~SensorDummy(){};
+
+ void setNoiseStd(const Eigen::VectorXd & _noise_std) override
+ {
+ assert(_noise_std.size() == (dim_ == 2 ? 3 : 6));
+
+ params_dummy_->noise_p_std = _noise_std(1);
+ params_dummy_->noise_o_std = _noise_std((dim_ == 2 ? 2 : 3));
+ }
+
+ Eigen::VectorXd getNoiseStd() const override
+ {
+ if (dim_ == 2)
+ return (Eigen::VectorXd() << params_dummy_->noise_p_std,
+ params_dummy_->noise_p_std,
+ params_dummy_->noise_o_std).finished();
+ else
+ return (Eigen::VectorXd() << params_dummy_->noise_p_std,
+ params_dummy_->noise_p_std,
+ params_dummy_->noise_p_std,
+ params_dummy_->noise_o_std,
+ params_dummy_->noise_o_std,
+ params_dummy_->noise_o_std).finished();
+ }
+
+ Eigen::MatrixXd getNoiseCov() const override
+ {
+ return getNoiseStd().cwiseAbs2().asDiagonal();
+ }
+
};
}
@@ -93,6 +131,5 @@ class SensorDummy : public SensorBase
#include "core/sensor/factory_sensor.h"
namespace wolf {
WOLF_REGISTER_SENSOR(SensorDummy);
-WOLF_REGISTER_SENSOR_YAML(SensorDummy);
} // namespace wolf
#endif