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

Merge remote-tracking branch 'origin/develop' into feature/sam_sfm_directories

release/4.3a0
dellaert 2015-07-14 22:25:38 -07:00
parent df1886c56c de72aa371a
commit 3d04fa0ed3
2 changed files with 13 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
gtsam/geometry/Unit3.cpp
View File

@ -153,14 +153,14 @@ Unit3 Unit3::retract(const Vector2& v) const {
/* ************************************************************************* */ /* ************************************************************************* */
Vector2 Unit3::localCoordinates(const Unit3& other) const { Vector2 Unit3::localCoordinates(const Unit3& other) const {
const double x = p_.dot(other.p_); const double x = p_.dot(other.p_);
// Crucial quantitity here is y = theta/sin(theta) with theta=acos(x) // Crucial quantity here is y = theta/sin(theta) with theta=acos(x)
// Now, y = acos(x) / sin(acos(x)) = acos(x)/sqrt(1-x^2) // Now, y = acos(x) / sin(acos(x)) = acos(x)/sqrt(1-x^2)
// We treat the special caes 1 and -1 below // We treat the special case 1 and -1 below
const double x2 = x * x; const double x2 = x * x;
const double z = 1 - x2; const double z = 1 - x2;
double y; double y;
if (z < std::numeric_limits<double>::epsilon()) { if (z < std::numeric_limits<double>::epsilon()) {
if (x > 0) // expand at x=1 if (x > 0) // first order expansion at x=1
y = 1.0 - (x - 1.0) / 3.0; y = 1.0 - (x - 1.0) / 3.0;
else // cop out else // cop out
return Vector2(M_PI, 0.0); return Vector2(M_PI, 0.0);

10
gtsam/geometry/tests/testUnit3.cpp
View File

@ -192,6 +192,16 @@ TEST(Unit3, localCoordinates) {
EXPECT(assert_equal(expected, actual, 1e-8)); EXPECT(assert_equal(expected, actual, 1e-8));
EXPECT(assert_equal(q, p.retract(expected), 1e-8)); EXPECT(assert_equal(q, p.retract(expected), 1e-8));
} }
{
Unit3 p(0,1,0), q(0,-1,0);
Vector2 actual = p.localCoordinates(q);
EXPECT(assert_equal(q, p.retract(actual), 1e-8));
}
{
Unit3 p(0,0,1), q(0,0,-1);
Vector2 actual = p.localCoordinates(q);
EXPECT(assert_equal(q, p.retract(actual), 1e-8));
}
double twist = 1e-4; double twist = 1e-4;
{ {