WIP: Resolve "New factorRelativePose2DWithExtrinsics"

Closes #237 (closed)

include/core/factor/factor_odom_2D.h

@@ -49,23 +49,38 @@ class FactorOdom2D : public FactorAutodiff<FactorOdom2D, 3, 2, 1, 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, const T* const _sp, const T* const _so, 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> > sp(_sp);
+    Eigen::Map<const Eigen::Matrix<T,2,1> > ps(_ps);
     T o1 = _o1[0];
     T o2 = _o2[0];
-    T so = _so[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>(-so)*(Eigen::Rotation2D<T>(-o1)*(p2 + Eigen::Rotation2D<T>(o2)*sp - p1) - sp);
+    // 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