diff --git a/include/core/factor/factor_odom_2D.h b/include/core/factor/factor_odom_2D.h
index ed3d0da6918e42b073c4ba49dd73387e9470c855..0589fbcd4bf71c1c8e16abb69deb0c98422c4a6a 100644
--- a/include/core/factor/factor_odom_2D.h
+++ b/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