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

Fixed Sphere2::localCoordinates so very small difference no longer gives nan, also made it a tiny bit more efficient (avoid one cos).

release/4.3a0
dellaert 2014-02-22 15:53:54 -05:00
parent dabd3cc612
commit 37489ddba7
1 changed files with 10 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

21
gtsam/geometry/Sphere2.cpp
View File

@ -141,22 +141,21 @@ Sphere2 Sphere2::retract(const Vector& v) const {
/* ************************************************************************* */ /* ************************************************************************* */
Vector Sphere2::localCoordinates(const Sphere2& y) const { Vector Sphere2::localCoordinates(const Sphere2& y) const {
Matrix B = basis();
Vector p = Point3::Logmap(p_); Vector p = Point3::Logmap(p_);
Vector q = Point3::Logmap(y.p_); Vector q = Point3::Logmap(y.p_);
double theta = acos(p.transpose() * q); double dot = p.dot(q);
// the below will be nan if theta == 0.0 // Check for special cases
if (p == q) if (std::abs(dot - 1.0) < 1e-20)
return (Vector(2) << 0, 0); return (Vector(2) << 0, 0);
else if (p == (Vector) -q) else if (std::abs(dot + 1.0) < 1e-20)
return (Vector(2) << M_PI, 0); return (Vector(2) << M_PI, 0);
else {
Vector result_hat = (theta / sin(theta)) * (q - p * cos(theta)); // no special case
Vector result = B.transpose() * result_hat; double theta = acos(dot);
Vector result_hat = (theta / sin(theta)) * (q - p * dot);
return result; return basis().transpose() * result_hat;
}
} }
/* ************************************************************************* */ /* ************************************************************************* */