Merge branch '401-fix-factorpose3dwithextrinsics-related-classes' into 'devel'

Resolve "Fix FactorPose3dWithExtrinsics related classes"

Closes #401

See merge request !415

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);

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);

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;

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;
 };

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())

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())

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

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_
 }
 

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);
-
 }