FactorOdom2D including extrinsics

include/core/factor/factor_odom_2D.h

@@ -56,13 +56,16 @@ inline bool FactorOdom2D::operator ()(const T* const _p1, const T* const _o1, co
     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);
     T o1 = _o1[0];
     T o2 = _o2[0];
+    T so = _so[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);
+    expected_measurement.head(2) = Eigen::Rotation2D<T>(-so)*(Eigen::Rotation2D<T>(-o1)*(p2 + Eigen::Rotation2D<T>(o2)*sp - p1) - sp);
     expected_measurement(2) = o2 - o1;
 
     // Error