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

Speeding up localCoordinates

release/4.3a0
dellaert 2014-10-22 13:45:57 +02:00
parent 0f53c8d5ec
commit 1061a66fc1
2 changed files with 15 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
gtsam/geometry/Rot3M.cpp
View File

@ -270,23 +270,23 @@ Vector3 Rot3::localCoordinates(const Rot3& T, Rot3::CoordinatesMode mode) const
return Logmap(between(T)); return Logmap(between(T));
} else if(mode == Rot3::CAYLEY) { } else if(mode == Rot3::CAYLEY) {
// Create a fixed-size matrix // Create a fixed-size matrix
Eigen::Matrix3d A(between(T).matrix()); Matrix3 A = rot_.transpose() * T.matrix();
// Mathematica closed form optimization (procrastination?) gone wild: // Mathematica closed form optimization (procrastination?) gone wild:
const double a=A(0,0),b=A(0,1),c=A(0,2); const double a=A(0,0),b=A(0,1),c=A(0,2);
const double d=A(1,0),e=A(1,1),f=A(1,2); const double d=A(1,0),e=A(1,1),f=A(1,2);
const double g=A(2,0),h=A(2,1),i=A(2,2); const double g=A(2,0),h=A(2,1),i=A(2,2);
const double di = d*i, ce = c*e, cd = c*d, fg=f*g; const double di = d*i, ce = c*e, cd = c*d, fg=f*g;
const double M = 1 + e - f*h + i + e*i; const double M = 1 + e - f*h + i + e*i;
const double K = 2.0 / (cd*h + M + a*M -g*(c + ce) - b*(d + di - fg)); const double K = - 4.0 / (cd*h + M + a*M -g*(c + ce) - b*(d + di - fg));
const double x = (a * f - cd + f) * K; const double x = a * f - cd + f;
const double y = (b * f - ce - c) * K; const double y = b * f - ce - c;
const double z = (fg - di - d) * K; const double z = fg - di - d;
return -2 * Vector3(x, y, z); return K * Vector3(x, y, z);
} else if(mode == Rot3::SLOW_CAYLEY) { } else if(mode == Rot3::SLOW_CAYLEY) {
// Create a fixed-size matrix // Create a fixed-size matrix
Eigen::Matrix3d A(between(T).matrix()); Matrix3 A(between(T).matrix());
// using templated version of Cayley // using templated version of Cayley
Eigen::Matrix3d Omega = CayleyFixed<3>(A); Matrix3 Omega = CayleyFixed<3>(A);
return -2*Vector3(Omega(2,1),Omega(0,2),Omega(1,0)); return -2*Vector3(Omega(2,1),Omega(0,2),Omega(1,0));
} else { } else {
assert(false); assert(false);

9
gtsam/geometry/tests/testRot3M.cpp
View File

@ -145,10 +145,15 @@ TEST( Rot3, rodriguez4)
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( Rot3, expmap) TEST( Rot3, retract)
{ {
Vector v = zero(3); Vector v = zero(3);
CHECK(assert_equal(R.retract(v), R)); CHECK(assert_equal(R, R.retract(v)));
// test Canonical coordinates
Canonical<Rot3> chart;
Vector v2 = chart.apply(R);
CHECK(assert_equal(R, chart.retract(v2)));
} }
/* ************************************************************************* */ /* ************************************************************************* */