12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

change: small value approximation for retract

release/4.3a0
zhaoyang 2015-06-29 18:29:13 -04:00
parent 4e9f365a7e
commit 3f5f0e852d
1 changed files with 3 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
gtsam/geometry/Unit3.cpp
View File

@ -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