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

Fixed bug on retraction of zero in Sphere2 expmap method.

release/4.3a0
Alex Trevor 2014-01-08 00:18:58 -05:00
parent 401fede2e9
commit 7484b31bc3
1 changed files with 5 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
gtsam/geometry/Sphere2.cpp
View File

@ -110,6 +110,11 @@ Sphere2 Sphere2::retract(const Vector& v, Sphere2::CoordinatesMode mode) const {
if (mode == Sphere2::EXPMAP) { if (mode == Sphere2::EXPMAP) {
double xi_hat_norm = xi_hat.norm(); double xi_hat_norm = xi_hat.norm();
Vector exp_p_xi_hat = cos (xi_hat_norm) * p + sin(xi_hat_norm) * (xi_hat / xi_hat_norm); Vector exp_p_xi_hat = cos (xi_hat_norm) * p + sin(xi_hat_norm) * (xi_hat / xi_hat_norm);
// Avoid nan
if (xi_hat_norm == 0.0)
return Sphere2 (point3 ());
return Sphere2(exp_p_xi_hat); return Sphere2(exp_p_xi_hat);
} else if (mode == Sphere2::RENORM) { } else if (mode == Sphere2::RENORM) {
// Project onto the manifold, i.e. the closest point on the circle to the new location; // Project onto the manifold, i.e. the closest point on the circle to the new location;