Resolve "New factorRelativePose2DWithExtrinsics"

Closes #237 (closed)

include/core/factor/factor_diff_drive.h

@@ -14,6 +14,7 @@
 #include "core/factor/factor_autodiff.h"
 #include "core/frame/frame_base.h"
 #include "core/feature/feature_motion.h"
+#include "core/math/rotations.h"
 
 namespace
 {
@@ -126,10 +127,7 @@ inline bool FactorDiffDrive::operator ()(const T* const _p1, const T* const _o1,
     residuals = delta_corrected - delta_predicted;
 
     // angle remapping
-    while (residuals(2) > T(M_PI))
-        residuals(2) = residuals(2) - T(2. * M_PI);
-    while (residuals(2) <= T(-M_PI))
-        residuals(2) = residuals(2) + T(2. * M_PI);
+    pi2pi(residuals(2));
 
     // weighted residual
     residuals = getMeasurementSquareRootInformationUpper() * residuals;