From 739a52e67faeb1f78229407707241a59d009a0f7 Mon Sep 17 00:00:00 2001
From: mfourmy <mederic.fourmy@laas.fr>
Date: Wed, 15 May 2019 16:05:26 +0200
Subject: [PATCH] Fixed a bug in constraint dist 3D
---
include/core/factor/factor_autodiff_distance_3D.h | 7 ++++++-
1 file changed, 6 insertions(+), 1 deletion(-)
diff --git a/include/core/factor/factor_autodiff_distance_3D.h b/include/core/factor/factor_autodiff_distance_3D.h
index 52ba73562..d4932885e 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>();
--
GitLab