Resolve "SensorPose with anisotropic noise specs"

Closes #477 (closed)

include/core/sensor/sensor_pose.h

@@ -39,8 +39,12 @@ WOLF_STRUCT_PTR_TYPEDEFS(ParamsSensorPose);
 
 struct ParamsSensorPose : public ParamsSensorBase
 {
-    double std_p = 1;  // m
-    double std_o = 1;  // rad
+    double std_px = 1;  // m
+    double std_py = 1;  // m
+    double std_pz = 1;  // m
+    double std_ox = 1;  // rad
+    double std_oy = 1;  // rad
+    double std_oz = 1;  // rad
     ParamsSensorPose()
     {
         //DEFINED FOR COMPATIBILITY PURPOSES. TO BE REMOVED IN THE FUTURE.
@@ -48,14 +52,56 @@ struct ParamsSensorPose : public ParamsSensorBase
     ParamsSensorPose(std::string _unique_name, const ParamsServer& _server):
         ParamsSensorBase(_unique_name, _server)
     {
-        std_p = _server.getParam<double>(prefix + _unique_name + "/std_p");
-        std_o  = _server.getParam<double>(prefix + _unique_name + "/std_o");
+        /* Here we allow isotropic specs for position and orientation
+         * and also particularizations for eaxh axis
+         * depending on how the info was ptovided in the YAML file.
+         * Combinations are possible. 
+         * 
+         * E.g. if the yaml file contains
+         * 
+         *  std_p : 1
+         * 
+         * then the result is std_px = 1, std_py = 1, std_pz = 1
+         * 
+         * If the yaml file contains (in any order)
+         * 
+         *  std_p : 1
+         *  std_pz : 0.1
+         * 
+         * then the result is std_px = 1, std_py = 1, std_pz = 0.1
+         */
+
+        // first the isotropic options
+        if (_server.hasParam(prefix + _unique_name + "/std_p"))
+            std_px = std_py = std_pz = _server.getParam<double>(prefix + _unique_name + "/std_p");
+        if (_server.hasParam(prefix + _unique_name + "/std_o"))
+            std_ox = std_oy = std_oz = _server.getParam<double>(prefix + _unique_name + "/std_o");
+
+        // then individual axes specifications
+        if (_server.hasParam(prefix + _unique_name + "/std_px"))
+            std_px = _server.getParam<double>(prefix + _unique_name + "/std_px");
+        if (_server.hasParam(prefix + _unique_name + "/std_py"))
+            std_py = _server.getParam<double>(prefix + _unique_name + "/std_py");
+        if (_server.hasParam(prefix + _unique_name + "/std_pz"))
+            std_pz = _server.getParam<double>(prefix + _unique_name + "/std_pz");
+
+        if (_server.hasParam(prefix + _unique_name + "/std_ox"))
+            std_ox = _server.getParam<double>(prefix + _unique_name + "/std_ox");
+        if (_server.hasParam(prefix + _unique_name + "/std_oy"))
+            std_oy = _server.getParam<double>(prefix + _unique_name + "/std_oy");
+        if (_server.hasParam(prefix + _unique_name + "/std_oz"))
+            std_oz = _server.getParam<double>(prefix + _unique_name + "/std_oz");
+
     }
     std::string print() const override
     {
       return ParamsSensorBase::print()           + "\n"
-        + "std_p: "    + std::to_string(std_p)   + "\n"
-        + "std_o: "    + std::to_string(std_o)  + "\n";
+        + "std_px: "    + std::to_string(std_px)   + "\n"
+        + "std_py: "    + std::to_string(std_py)   + "\n"
+        + "std_pz: "    + std::to_string(std_pz)   + "\n"
+        + "std_ox: "    + std::to_string(std_ox)  + "\n";
+        + "std_oy: "    + std::to_string(std_oy)  + "\n";
+        + "std_oz: "    + std::to_string(std_oz)  + "\n";
     }
         ~ParamsSensorPose() override = default;
 };
@@ -65,8 +111,7 @@ WOLF_PTR_TYPEDEFS(SensorPose);
 class SensorPose : public SensorBase
 {
     protected:
-        double std_p_; // standard deviation of translation measurements
-        double std_o_; // standard deviation of orientation measurements
+        ParamsSensorPosePtr params_pose_;
 
     public:
         SensorPose(const Eigen::VectorXd& _extrinsics_pq, const ParamsSensorPose& params);
@@ -75,20 +120,20 @@ class SensorPose : public SensorBase
 
         ~SensorPose() override;
 
-        double getStdP() const;
-        double getStdO() const;
+        // double getStdP() const;
+        // double getStdO() const;
 
 };
 
-inline double SensorPose::getStdP() const
-{
-    return std_p_;
-}
+// inline double SensorPose::getStdP() const
+// {
+//     return std_p_;
+// }
 
-inline double SensorPose::getStdO() const
-{
-    return std_o_;
-}
+// inline double SensorPose::getStdO() const
+// {
+//     return std_o_;
+// }
 
 // inline Matrix6d SensorPose::computeDataCov() const
 // {