Use 6 specifiers for noise in SensorPose

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

src/sensor/sensor_pose.cpp

@@ -35,14 +35,19 @@
 namespace wolf {
 
 SensorPose::SensorPose(const Eigen::VectorXd& _extrinsics_pq, const ParamsSensorPose& _intrinsics) :
-                        SensorBase("SensorPose", std::make_shared<StatePoint3d>(_extrinsics_pq.head(3), true), std::make_shared<StateQuaternion>(_extrinsics_pq.tail(4), true), nullptr, 6),
-                        std_p_(_intrinsics.std_p),
-                        std_o_(_intrinsics.std_o)
+                        SensorBase("SensorPose", 
+                                   std::make_shared<StatePoint3d>(_extrinsics_pq.head(3), true), 
+                                   std::make_shared<StateQuaternion>(_extrinsics_pq.tail(4), true), 
+                                   nullptr, 
+                                   6)
 {
     assert(_extrinsics_pq.size() == 7 && "Bad extrinsics vector size! Should be 7 for 3d.");
 
-    noise_cov_ = (Eigen::Vector6d() << std_p_*std_p_, std_p_*std_p_, std_p_*std_p_, std_o_*std_o_, std_o_*std_o_, std_o_*std_o_).finished().asDiagonal();
-    setNoiseCov(noise_cov_); // sets also noise_std_
+    params_pose_ = std::make_shared<ParamsSensorPose>(_intrinsics);
+
+    noise_std_ << params_pose_->std_px, params_pose_->std_py, params_pose_->std_pz, params_pose_->std_ox, params_pose_->std_oy, params_pose_->std_oz;
+
+    setNoiseStd(noise_std_); // sets also noise_cov_
 }
 
 SensorPose::SensorPose(const Eigen::VectorXd& _extrinsics_pq, ParamsSensorPosePtr _intrinsics) :

src/yaml/sensor_pose_yaml.cpp

@@ -49,8 +49,12 @@ static ParamsSensorBasePtr createParamsSensorPose(const std::string & _filename_
     {
         ParamsSensorPosePtr params = std::make_shared<ParamsSensorPose>();
 
-        params->std_p = config["std_p"].as<double>();
-        params->std_o = config["std_o"].as<double>();
+        params->std_px = config["std_p"].as<double>();
+        params->std_py = config["std_p"].as<double>();
+        params->std_pz = config["std_p"].as<double>();
+        params->std_ox = config["std_o"].as<double>();
+        params->std_oy = config["std_o"].as<double>();
+        params->std_oz = config["std_o"].as<double>();
 
         return params;
     }

test/gtest_processor_and_factor_pose_3d_with_extrinsics.cpp

@@ -127,8 +127,12 @@ class FactorPose3dWithExtrinsicsBase_Test : public testing::Test
 
         // pose sensor and proc (to get extrinsics in the prb)
         auto intr_sp = std::make_shared<ParamsSensorPose>();
-        intr_sp->std_p = 1;
-        intr_sp->std_o = 1;
+        intr_sp->std_px = 1;
+        intr_sp->std_py = 1;
+        intr_sp->std_pz = 1;
+        intr_sp->std_ox = 1;
+        intr_sp->std_oy = 1;
+        intr_sp->std_oz = 1;
         Vector7d extr; extr << b_p_bm_, b_q_m_.coeffs();
         sensor_pose_ = problem_->installSensor("SensorPose", "pose", extr, intr_sp);
         auto params_proc = std::make_shared<ParamsProcessorPose>();

test/gtest_sensor_pose.cpp

@@ -51,24 +51,32 @@ TEST(Pose, constructor)
 TEST(Pose, getParams)
 {
     auto intr = std::make_shared<ParamsSensorPose>();
-    intr->std_p = 2;
-    intr->std_o = 3;
+    intr->std_px = 2;
+    intr->std_py = 2;
+    intr->std_pz = 2;
+    intr->std_ox = 3;
+    intr->std_oy = 3;
+    intr->std_oz = 3;
 
     Vector7d extr; extr << 1,2,3,.5,.5,.5,.5;
 
     auto sen = std::make_shared<SensorPose>(extr, intr);
 
-    ASSERT_EQ(sen->getStdP(), intr->std_p); 
-    ASSERT_EQ(sen->getStdO(), intr->std_o); 
-    // ASSERT_EQ(sen->getNoiseCov()(0,0), (Vector1d() << 4).finished());
-    // ASSERT_EQ(sen->getNoiseCov()(3,3), (Vector1d() << 9).finished());
+    // ASSERT_EQ(sen->getStdP(), intr->std_p); 
+    // ASSERT_EQ(sen->getStdO(), intr->std_o); 
+    ASSERT_EQ(sen->getNoiseCov()(0,0), 4);
+    ASSERT_EQ(sen->getNoiseCov()(3,3), 9);
 }
 
 TEST(Pose, create)
 {
     auto intr = std::make_shared<ParamsSensorPose>();
-    intr->std_p = 2;
-    intr->std_o = 3;
+    intr->std_px = 2;
+    intr->std_py = 2;
+    intr->std_pz = 2;
+    intr->std_ox = 3;
+    intr->std_oy = 3;
+    intr->std_oz = 3;
 
     Vector7d extr; extr << 1,2,3,.5,.5,.5,.5;
 
@@ -76,10 +84,10 @@ TEST(Pose, create)
 
     auto sen = std::static_pointer_cast<SensorPose>(sen_base);
 
-    ASSERT_EQ(sen->getStdP(), intr->std_p); 
-    ASSERT_EQ(sen->getStdO(), intr->std_o);
-    // ASSERT_EQ(sen->getNoiseCov()(0,0), (Vector1d() << 4).finished());
-    // ASSERT_EQ(sen->getNoiseCov()(3,3), (Vector1d() << 9).finished());
+    // ASSERT_EQ(sen->getStdP(), intr->std_p); 
+    // ASSERT_EQ(sen->getStdO(), intr->std_o);
+    ASSERT_EQ(sen->getNoiseCov()(0,0), 4);
+    ASSERT_EQ(sen->getNoiseCov()(3,3), 9);
 }
 
 int main(int argc, char **argv)