diff --git a/include/core/factor/factor_relative_pose_2d_with_extrinsics.h b/include/core/factor/factor_relative_pose_2d_with_extrinsics.h
index 4455b0960e7fb5b22f1b88c1106d2e07aa036550..f0f70a5c15341c48f03e1b36ce86eb6e1e1415db 100644
--- a/include/core/factor/factor_relative_pose_2d_with_extrinsics.h
+++ b/include/core/factor/factor_relative_pose_2d_with_extrinsics.h
@@ -175,7 +175,7 @@ inline bool FactorRelativePose2dWithExtrinsics::operator ()(const T* const _p_re
         // s1_p_s2 = s1_R_r1*(r1_R_w*(w_p_r2 - w_p_r1) - r1_p_s1)
 
         expected_measurement.head(2) = Eigen::Rotation2D<T>(-o_sensor)*(-p_sensor + Eigen::Rotation2D<T>(-o_ref)*(-p_ref + p_target));
-        expected_measurement(2) = o_target - o_ref;
+        expected_measurement(2) = o_target - o_ref - o_sensor;
     }
 
     // Error
diff --git a/test/gtest_factor_relative_pose_2d_with_extrinsics.cpp b/test/gtest_factor_relative_pose_2d_with_extrinsics.cpp
index a10190e0fceb62db586e3a8d87b844a067d72969..b5b4c9d0f36318408b0e056a7bdc3b17def589e7 100644
--- a/test/gtest_factor_relative_pose_2d_with_extrinsics.cpp
+++ b/test/gtest_factor_relative_pose_2d_with_extrinsics.cpp
@@ -113,7 +113,7 @@ void generateRandomProblemLandmark()
     x_s(2) = pi2pi(x_s(2));
 
     // landmark pose
-    x_l(2) = pi2pi(x_f(2) + delta(2));
+    x_l(2) = pi2pi(x_f(2) + x_s(2) + delta(2));
     x_l.head<2>() = x_f.head<2>() + Rotation2Dd(x_f(2)) * (x_s.head<2>() + Rotation2Dd(x_s(2)) * delta.head<2>());
 
     // Set states