[skip ci] wip

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

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

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
           

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

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

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"

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 =                 \

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, //

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

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