From 3f5f0e852d1e7b1d5bc21c64174a884ca09fd9a2 Mon Sep 17 00:00:00 2001 From: zhaoyang Date: Mon, 29 Jun 2015 18:29:13 -0400 Subject: [PATCH] change: small value approximation for retract --- gtsam/geometry/Unit3.cpp | 9 +++------ 1 file changed, 3 insertions(+), 6 deletions(-) diff --git a/gtsam/geometry/Unit3.cpp b/gtsam/geometry/Unit3.cpp index 5196b9477..1a08b7fcc 100644 --- a/gtsam/geometry/Unit3.cpp +++ b/gtsam/geometry/Unit3.cpp @@ -150,12 +150,9 @@ Unit3 Unit3::retract(const Vector2& v) const { Vector3 xi_hat = v(0) * B.col(0) + v(1) * B.col(1); double xi_hat_norm = xi_hat.norm(); - // Avoid nan - if (xi_hat_norm < 1e-16) { - if (v.norm() < 1e-16) - return Unit3(point3()); - else - return Unit3(-point3()); + // When v is the so small and approximate as a direction + if (xi_hat_norm < 1e-8) { + return Unit3(cos(xi_hat_norm) * p + xi_hat); } Vector3 exp_p_xi_hat = cos(xi_hat_norm) * p