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 {
Matrix B = basis();
Vector p = Point3::Logmap(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
if (p == q)
// Check for special cases
if (std::abs(dot - 1.0) < 1e-20)
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);
Vector result_hat = (theta / sin(theta)) * (q - p * cos(theta));
Vector result = B.transpose() * result_hat;
return result;
else {
// no special case
double theta = acos(dot);
Vector result_hat = (theta / sin(theta)) * (q - p * dot);
return basis().transpose() * result_hat;
}
}
/* ************************************************************************* */