WIP: Resolve "New factorRelativePose2DWithExtrinsics"

Closes #237 (closed)

include/core/factor/factor_odom_2D.h

@@ -8,25 +8,27 @@
 //#include "ceres/jet.h"
 
 namespace wolf {
-    
+
 WOLF_PTR_TYPEDEFS(FactorOdom2D);
 
 //class
-class FactorOdom2D : public FactorAutodiff<FactorOdom2D, 3, 2, 1, 2, 1>
+class FactorOdom2D : public FactorAutodiff<FactorOdom2D, 3, 2, 1, 2, 1, 2, 1>
 {
     public:
         FactorOdom2D(const FeatureBasePtr& _ftr_ptr,
                          const FrameBasePtr& _frame_other_ptr,
                          const ProcessorBasePtr& _processor_ptr = nullptr,
                          bool _apply_loss_function = false, FactorStatus _status = FAC_ACTIVE) :
-             FactorAutodiff<FactorOdom2D, 3, 2, 1, 2, 1>("ODOM 2D",
+             FactorAutodiff<FactorOdom2D, 3, 2, 1, 2, 1, 2, 1>("ODOM 2D",
                                                                  _frame_other_ptr, nullptr, nullptr, nullptr,
                                                                  _processor_ptr,
                                                                  _apply_loss_function, _status,
                                                                  _frame_other_ptr->getP(),
                                                                  _frame_other_ptr->getO(),
                                                                  _ftr_ptr->getFrame()->getP(),
-                                                                 _ftr_ptr->getFrame()->getO())
+                                                                 _ftr_ptr->getFrame()->getO(),
+                                                                 _ftr_ptr->getCapture()->getSensorP(),
+                                                                 _ftr_ptr->getCapture()->getSensorO())
         {
             //
         }
@@ -34,7 +36,7 @@ class FactorOdom2D : public FactorAutodiff<FactorOdom2D, 3, 2, 1, 2, 1>
         virtual ~FactorOdom2D() = default;
 
         template<typename T>
-        bool operator ()(const T* const _p1, const T* const _o1, const T* const _p2, const T* const _o2,
+        bool operator ()(const T* const _p1, const T* const _o1, const T* const _p2, const T* const _o2, const T* const _sp, const T* const _so,
                          T* _residuals) const;
 
     public:
@@ -47,20 +49,38 @@ class FactorOdom2D : public FactorAutodiff<FactorOdom2D, 3, 2, 1, 2, 1>
 
 template<typename T>
 inline bool FactorOdom2D::operator ()(const T* const _p1, const T* const _o1, const T* const _p2,
-                                          const T* const _o2, T* _residuals) const
+                                          const T* const _o2, const T* const _ps, const T* const _os, T* _residuals) const
 {
 
     // MAPS
     Eigen::Map<Eigen::Matrix<T,3,1> > res(_residuals);
     Eigen::Map<const Eigen::Matrix<T,2,1> > p1(_p1);
     Eigen::Map<const Eigen::Matrix<T,2,1> > p2(_p2);
+    Eigen::Map<const Eigen::Matrix<T,2,1> > ps(_ps);
     T o1 = _o1[0];
     T o2 = _o2[0];
+    T os = _os[0];
+
     Eigen::Matrix<T, 3, 1> expected_measurement;
     Eigen::Matrix<T, 3, 1> er; // error
 
     // Expected measurement
-    expected_measurement.head(2) = Eigen::Rotation2D<T>(-o1) * (p2 - p1);
+    // r1_p_r1_s1 = ps
+    // r2_p_r2_s2 = ps
+    // r1_R_s1 = R(os)
+    // r2_R_s2 = R(os)
+    // w_R_r1 = R(o1)
+    // w_R_r2 = R(o2)
+    // ----------------------------------------
+
+    // s1_p_s1_s2 = s1_R_r1*r1_p_s1_r1 +
+    //              s1_R_r1*r1_R_w*w_p_r1_w +
+    //              s1_R_r1*r1_R_w*w_p_w_r2 +
+    //              s1_R_r1*r1_R_w*w_R_r2*r2_p_r2_s2
+
+    // s1_p_s1_s2 = s1_R_r1*(r1_p_s1_r1 + r1_R_w*(w_p_r1_w + w_p_w_r2 + w_R_r2*r2_p_r2_s2))
+
+    expected_measurement.head(2) = Eigen::Rotation2D<T>(-os)*(-ps + Eigen::Rotation2D<T>(-o1)*(-p1 + p2 + Eigen::Rotation2D<T>(o2)*ps));
     expected_measurement(2) = o2 - o1;
 
     // Error