From 9a3acdd141975c37d7ae6b3ca9d10a84f1a75670 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?M=C3=A9d=C3=A9ric=20Fourmy?= <mfourmy@laas.fr>
Date: Tue, 27 Apr 2021 16:31:21 +0200
Subject: [PATCH] Fix sensor pose covariance setting in constructor

---
 include/core/capture/capture_base.h           |  1 +
 .../factor/factor_pose_3d_with_extrinsics.h   | 68 ++++++++++++++++---
 include/core/frame/frame_base.h               |  1 +
 include/core/sensor/sensor_pose.h             |  2 +-
 src/capture/capture_base.cpp                  |  7 ++
 src/frame/frame_base.cpp                      |  7 ++
 src/processor/processor_pose.cpp              |  2 +-
 src/sensor/sensor_pose.cpp                    |  3 +-
 ...sor_and_factor_pose_3d_with_extrinsics.cpp | 15 +++-
 9 files changed, 92 insertions(+), 14 deletions(-)

diff --git a/include/core/capture/capture_base.h b/include/core/capture/capture_base.h
index 18a09ac3e..68d34c20b 100644
--- a/include/core/capture/capture_base.h
+++ b/include/core/capture/capture_base.h
@@ -67,6 +67,7 @@ class CaptureBase : public NodeBase, public HasStateBlocks, public std::enable_s
         const FeatureBasePtrList& getFeatureList() const;
 
         void getFactorList(FactorBasePtrList& _fac_list) const;
+        FactorBasePtrList getFactorList() const;
 
         SensorBasePtr getSensor() const;
         virtual void setSensor(const SensorBasePtr sensor_ptr);
diff --git a/include/core/factor/factor_pose_3d_with_extrinsics.h b/include/core/factor/factor_pose_3d_with_extrinsics.h
index f726291a9..ad35fb6c2 100644
--- a/include/core/factor/factor_pose_3d_with_extrinsics.h
+++ b/include/core/factor/factor_pose_3d_with_extrinsics.h
@@ -40,6 +40,18 @@ class FactorPose3dWithExtrinsics : public FactorAutodiff<FactorPose3dWithExtrins
 
         ~FactorPose3dWithExtrinsics() override = default;
 
+        template<typename D1, typename D2, typename D3, typename D4, typename D5, typename D6, typename D7>
+        inline void error(const Eigen::MatrixBase<D1> &        w_p_wb,
+                          const Eigen::QuaternionBase<D2> &    w_q_b,
+                          const Eigen::MatrixBase<D3> &        b_p_bm,
+                          const Eigen::QuaternionBase<D4> &    b_q_m,
+                          const Eigen::MatrixBase<D5> &        w_p_wm,
+                          const Eigen::QuaternionBase<D6> &    w_q_m,
+                          Eigen::MatrixBase<D7> &              p_err,
+                          Eigen::MatrixBase<D7> &              o_err) const;
+
+        inline Eigen::Vector6d error() const;
+
         template<typename T>
         bool operator ()(const T* const _p,
                          const T* const _q,
@@ -48,6 +60,50 @@ class FactorPose3dWithExtrinsics : public FactorAutodiff<FactorPose3dWithExtrins
                          T* _residuals) const;
 };
 
+template<typename D1, typename D2, typename D3, typename D4, typename D5, typename D6, typename D7>
+inline void FactorPose3dWithExtrinsics::error(const Eigen::MatrixBase<D1> &        w_p_wb,
+                                              const Eigen::QuaternionBase<D2> &    w_q_b,
+                                              const Eigen::MatrixBase<D3> &        b_p_bm,
+                                              const Eigen::QuaternionBase<D4> &    b_q_m,
+                                              const Eigen::MatrixBase<D5> &        w_p_wm,
+                                              const Eigen::QuaternionBase<D6> &    w_q_m,
+                                              Eigen::MatrixBase<D7> &              p_err,
+                                              Eigen::MatrixBase<D7> &              o_err) const
+{
+    // Transformation definitions:
+    // w_p_wm, w_q_m: world to measurement frame (robot attached mocap frame) 
+    // w_p_wb, w_q_b: world to robot base frame 
+    // b_p_bm, b_q_m: base to measurement frame (extrinsics)
+    // Error function:
+    // err = w_T_m |-| (w_T_b*b_T_m)
+
+    p_err = w_p_wm - (w_p_wb + w_q_b*b_p_bm);
+    o_err = q2v((w_q_b * b_q_m).conjugate() * w_q_m);
+}
+
+
+inline Eigen::Vector6d FactorPose3dWithExtrinsics::error() const
+{
+    // get current frame and extrinsics estimates
+    const Eigen::Vector3d w_p_wb = (getFeature()->getFrame()->getP()->getState());
+    const Eigen::Quaterniond w_q_b (getFeature()->getFrame()->getO()->getState().data());
+    const Eigen::Vector3d b_p_bm = (getFeature()->getCapture()->getSensorP()->getState());
+    const Eigen::Quaterniond b_q_m (getFeature()->getCapture()->getSensorO()->getState().data());
+
+    // measurements
+    Eigen::Vector3d    w_p_wm(getMeasurement().data() + 0);  // measurements
+    Eigen::Quaterniond w_q_m (getMeasurement().data() + 3);  // measurements
+
+    Vector6d err;
+    Eigen::Map<Vector3d> p_err(err.data() + 0);
+    Eigen::Map<Vector3d> o_err(err.data() + 3);
+
+    error(w_p_wb, w_q_b, b_p_bm, b_q_m, w_p_wm, w_q_m, p_err, o_err);
+
+    return err;
+}
+
+
 template<typename T>
 inline bool FactorPose3dWithExtrinsics::operator ()(const T* const _p,
                                                     const T* const _q,
@@ -65,16 +121,10 @@ inline bool FactorPose3dWithExtrinsics::operator ()(const T* const _p,
     Eigen::Vector3d    w_p_wm(getMeasurement().data() + 0);  // measurements
     Eigen::Quaterniond w_q_m (getMeasurement().data() + 3);  // measurements
 
-    // Transformation definitions:
-    // w_p_wm, w_q_m: world to measurement frame (robot attached mocap frame) 
-    // w_p_wb, w_q_b: world to robot base frame 
-    // b_p_bm, b_q_m: base to measurement frame (extrinsics)
-    // Error function:
-    // err = w_T_m |-| (w_T_b*b_T_m)
-
     Eigen::Matrix<T, 6, 1> err; // error
-    err.head(3) = w_p_wm - (w_p_wb + w_q_b*b_p_bm);
-    err.tail(3) = q2v((w_q_b * b_q_m).conjugate() * w_q_m.cast<T>());
+    Eigen::Map<Matrix<T, 3, 1> > p_err(err.data() + 0);
+    Eigen::Map<Matrix<T, 3, 1> > o_err(err.data() + 3);
+    error(w_p_wb, w_q_b, b_p_bm, b_q_m, w_p_wm.cast<T>(), w_q_m.cast<T>(), p_err, o_err);
 
     // Residuals
     Eigen::Map<Eigen::Matrix<T,6,1> > res(_residuals);
diff --git a/include/core/frame/frame_base.h b/include/core/frame/frame_base.h
index 2f790d633..848fa3f12 100644
--- a/include/core/frame/frame_base.h
+++ b/include/core/frame/frame_base.h
@@ -108,6 +108,7 @@ class FrameBase : public NodeBase, public HasStateBlocks, public std::enable_sha
 
         FactorBasePtr getFactorOf(const ProcessorBasePtr _processor_ptr) const;
         FactorBasePtr getFactorOf(const ProcessorBasePtr _processor_ptr, const std::string& type) const;
+        FactorBasePtrList getFactorList() const;
         void getFactorList(FactorBasePtrList& _fac_list) const;
 
         unsigned int getHits() const;
diff --git a/include/core/sensor/sensor_pose.h b/include/core/sensor/sensor_pose.h
index 63f16d1e4..4df621546 100644
--- a/include/core/sensor/sensor_pose.h
+++ b/include/core/sensor/sensor_pose.h
@@ -34,7 +34,7 @@ struct ParamsSensorPose : public ParamsSensorBase
     {
       return ParamsSensorBase::print()           + "\n"
         + "std_p: "    + std::to_string(std_p)   + "\n"
-        + "std_o: "     + std::to_string(std_o)  + "\n";
+        + "std_o: "    + std::to_string(std_o)  + "\n";
     }
         ~ParamsSensorPose() override = default;
 };
diff --git a/src/capture/capture_base.cpp b/src/capture/capture_base.cpp
index f29109d18..546fafeec 100644
--- a/src/capture/capture_base.cpp
+++ b/src/capture/capture_base.cpp
@@ -119,6 +119,13 @@ void CaptureBase::removeFeature(FeatureBasePtr _ft_ptr)
     feature_list_.remove(_ft_ptr);
 }
 
+FactorBasePtrList CaptureBase::getFactorList() const
+{
+    FactorBasePtrList fac_list;
+    getFactorList(fac_list);
+    return fac_list;
+}
+
 void CaptureBase::getFactorList(FactorBasePtrList& _fac_list) const
 {
     for (auto f_ptr : getFeatureList())
diff --git a/src/frame/frame_base.cpp b/src/frame/frame_base.cpp
index 4b278bf8b..7df23b32b 100644
--- a/src/frame/frame_base.cpp
+++ b/src/frame/frame_base.cpp
@@ -216,6 +216,13 @@ FactorBasePtr FrameBase::getFactorOf(const ProcessorBasePtr _processor_ptr) cons
     return nullptr;
 }
 
+FactorBasePtrList FrameBase::getFactorList() const
+{
+    FactorBasePtrList fac_list;
+    getFactorList(fac_list);
+    return fac_list;
+}
+
 void FrameBase::getFactorList(FactorBasePtrList& _fac_list) const
 {
     for (auto c_ptr : getCaptureList())
diff --git a/src/processor/processor_pose.cpp b/src/processor/processor_pose.cpp
index e80abe716..70d9c6c97 100644
--- a/src/processor/processor_pose.cpp
+++ b/src/processor/processor_pose.cpp
@@ -36,7 +36,7 @@ void ProcessorPose::createFactorIfNecessary(){
         }
         auto cap_it = buffer_capture_.selectIterator(t, time_tolerance);
 
-        // if capture with corresponding timestamp is not found, stop and assume you will get it later
+        // if capture with corresponding timestamp is not found, assume you will get it later
         if (cap_it != buffer_capture_.getContainer().end())
         {
             // if a corresponding capture exists, link it to the KF and create a factor
diff --git a/src/sensor/sensor_pose.cpp b/src/sensor/sensor_pose.cpp
index 9cd65ccc8..c308bed71 100644
--- a/src/sensor/sensor_pose.cpp
+++ b/src/sensor/sensor_pose.cpp
@@ -20,8 +20,7 @@ SensorPose::SensorPose(const Eigen::VectorXd& _extrinsics_pq, const ParamsSensor
 {
     assert(_extrinsics_pq.size() == 7 && "Bad extrinsics vector size! Should be 7 for 3d.");
 
-    noise_cov_ = Matrix6d::Identity();
-    // 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();
+    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_
 }
 
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 07450aa2e..ec7700ae2 100644
--- a/test/gtest_processor_and_factor_pose_3d_with_extrinsics.cpp
+++ b/test/gtest_processor_and_factor_pose_3d_with_extrinsics.cpp
@@ -254,6 +254,20 @@ TEST_F(FactorPose3dWithExtrinsicsMANUAL_Test, check_tree)
     ASSERT_TRUE(problem_->check(0));
 }
 
+TEST_F(FactorPose3dWithExtrinsicsMANUAL_Test, error)
+{
+    // Since initialized at the right state, error function should return 0
+    std::vector<FrameBasePtr> KF_vec = {KF1_, KF2_, KF3_};
+    for (auto KF: KF_vec){
+        for (auto fac: KF->getFactorList()){
+            auto f = std::dynamic_pointer_cast<FactorPose3dWithExtrinsics>(fac);
+            if (f){
+                ASSERT_MATRIX_APPROX(f->error(), Vector6d::Zero(), 1e-6);
+            }
+        }
+    }
+}
+
 TEST_F(FactorPose3dWithExtrinsicsMANUAL_Test, solve)
 {
     problem_->perturb();
@@ -264,7 +278,6 @@ TEST_F(FactorPose3dWithExtrinsicsMANUAL_Test, solve)
     ASSERT_MATRIX_APPROX(KF2_->getStateVector(), pose2_, 1e-6);
     ASSERT_MATRIX_APPROX(sp->getP()->getState(), b_p_bm_, 1e-6);
     ASSERT_MATRIX_APPROX(sp->getO()->getState(), b_q_m_.coeffs(), 1e-6);
-
 }
 
 
-- 
GitLab