diff --git a/CMakeLists.txt b/CMakeLists.txt index 5c48da8725a57fefa0538e16d60e312faa9b1658..5d2e28e42b7ca16e7b62eb9df9bbce21d286c37b 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -270,21 +270,21 @@ SET(HDRS_CAPTURE SET(HDRS_FACTOR include/base/factor/factor_analytic.h include/base/factor/factor_autodiff.h - include/base/factor/factor_autodiff_distance_3d.h + include/base/factor/factor_autodiff_distance_3D.h include/base/factor/factor_base.h include/base/factor/factor_block_absolute.h include/base/factor/factor_container.h - include/base/factor/factor_corner_2d.h + include/base/factor/factor_corner_2D.h include/base/factor/factor_diff_drive.h include/base/factor/factor_epipolar.h include/base/factor/factor_fix_bias.h - include/base/factor/factor_odom_2d.h - include/base/factor/factor_odom_2d_analytic.h - include/base/factor/factor_odom_3d.h - include/base/factor/factor_pose_2d.h - include/base/factor/factor_pose_3d.h + include/base/factor/factor_odom_2D.h + include/base/factor/factor_odom_2D_analytic.h + include/base/factor/factor_odom_3D.h + include/base/factor/factor_pose_2D.h + include/base/factor/factor_pose_3D.h include/base/factor/factor_quaternion_absolute.h - include/base/factor/factor_relative_2d_analytic.h + include/base/factor/factor_relative_2D_analytic.h ) SET(HDRS_FEATURE include/base/feature/feature_base.h @@ -335,6 +335,7 @@ SET(HDRS_DTASSC include/base/association/association_node.h include/base/association/association_solver.h include/base/association/association_tree.h + include/base/association/matrix.h include/base/track_matrix.h ) diff --git a/include/base/factor/factor_autodiff_distance_3D.h b/include/base/factor/factor_autodiff_distance_3D.h index aaf3872c3d879ad5cb628f1d455331d28e301202..77eb08e2ae049978c7f2978eee3106fcec66ba9c 100644 --- a/include/base/factor/factor_autodiff_distance_3D.h +++ b/include/base/factor/factor_autodiff_distance_3D.h @@ -50,12 +50,7 @@ class FactorAutodiffDistance3D : public FactorAutodiff<FactorAutodiffDistance3D, Map<const Matrix<T,3,1>> pos2(_pos2); Map<Matrix<T,1,1>> res(_residual); - // 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_exp ( sqrt( ( pos2 - pos1 ).squaredNorm() ) ); Matrix<T,1,1> dist_meas (getMeasurement().cast<T>() ); Matrix<T,1,1> sqrt_info_upper = getMeasurementSquareRootInformationUpper().cast<T>();