diff --git a/include/core/factor/factor_autodiff_distance_3D.h b/include/core/factor/factor_autodiff_distance_3D.h
index 52ba735623c49610fdf5bba6a9ff45d875a28459..d4932885e58e941ad787c52a0428aa00ea6eec5e 100644
--- a/include/core/factor/factor_autodiff_distance_3D.h
+++ b/include/core/factor/factor_autodiff_distance_3D.h
@@ -50,7 +50,12 @@ class FactorAutodiffDistance3D : public FactorAutodiff<FactorAutodiffDistance3D,
             Map<const Matrix<T,3,1>> pos2(_pos2);
             Map<Matrix<T,1,1>> res(_residual);
 
-            Matrix<T,1,1> dist_exp ( sqrt( ( pos2 - pos1 ).squaredNorm() ) );
+            // If pos2 and pos1 are the same, undefined behavior when computing the jacobian
+            T norm_squared = ( pos2 - pos1 ).squaredNorm();
+            if (norm_squared < (T)1e-8){
+                norm_squared += (T)1e-8;
+            }
+            Matrix<T,1,1> dist_exp ( sqrt(norm_squared) );
             Matrix<T,1,1> dist_meas (getMeasurement().cast<T>() );
             Matrix<T,1,1> sqrt_info_upper = getMeasurementSquareRootInformationUpper().cast<T>();