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

Implemented and tested Rot3.retract based on the Cayley Transform (about three times faster)

release/4.3a0
Frank Dellaert 2012-01-06 01:30:10 +00:00
parent dfa395529d
commit dae02c387f
6 changed files with 88 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
gtsam/geometry/Rot3.h
View File

@ -227,10 +227,10 @@ namespace gtsam {
size_t dim() const { return dimension; }
/// Retraction from R^3 to Pose2 manifold neighborhood around current pose
Rot3 retract(const Vector& v) const { return compose(Expmap(v)); }
Rot3 retract(const Vector& omega) const;
/// Returns inverse retraction
Vector localCoordinates(const Rot3& t2) const { return Logmap(between(t2)); }
Vector localCoordinates(const Rot3& t2) const;
/// @}
/// @name Lie Group

32
gtsam/geometry/Rot3M.cpp
View File

@ -236,6 +236,38 @@ Vector Rot3::Logmap(const Rot3& R) {
}
}
/* ************************************************************************* */
Rot3 Rot3::retract(const Vector& omega) const {
#ifdef CORRECT_ROT3_EXMAP
return (*this)*Expmap(omega);
#else
#ifdef SLOW_CAYLEY
Matrix Omega = skewSymmetric(omega);
return (*this)*Cayley(-Omega/2);
#else
double x = omega(0), y = omega(1), z = omega(2);
double x2 = x*x, y2 = y*y, z2 = z*z;
double xy = x*y, xz = x*z, yz = y*z;
double f = 1.0 / (4.0 + x2 + y2 + z2), _2f = 2.0*f;
return (*this)* Rot3(
(4+x2-y2-z2)*f, (xy - 2*z)*_2f, (xz + 2*y)*_2f,
(xy + 2*z)*_2f, (4-x2+y2-z2)*f, (yz - 2*x)*_2f,
(xz - 2*y)*_2f, (yz + 2*x)*_2f, (4-x2-y2+z2)*f
);
#endif
#endif
}
/* ************************************************************************* */
Vector Rot3::localCoordinates(const Rot3& T) const {
#ifdef CORRECT_ROT3_EXMAP
return Logmap(between(T));
#else
Matrix Omega = Cayley(between(T).matrix());
return -2*Vector_(3,Omega(2,1),Omega(0,2),Omega(1,0));
#endif
}
/* ************************************************************************* */
Matrix Rot3::matrix() const {
Matrix R(3,3);

10
gtsam/geometry/Rot3Q.cpp
View File

@ -149,6 +149,16 @@ namespace gtsam {
return angleAxis.axis() * angleAxis.angle();
}
/* ************************************************************************* */
Rot3 Rot3::retract(const Vector& omega) const {
return compose(Expmap(omega));
}
/* ************************************************************************* */
Vector Rot3::localCoordinates(const Rot3& t2) const {
return Logmap(between(t2));
}
/* ************************************************************************* */
Matrix Rot3::matrix() const { return quaternion_.toRotationMatrix(); }

6
gtsam/geometry/tests/testPose3.cpp
View File

@ -49,13 +49,13 @@ TEST( Pose3, retract)
Pose3 id;
Vector v = zero(6);
v(0) = 0.3;
EXPECT(assert_equal(Pose3(R, Point3()), id.retract(v)));
EXPECT(assert_equal(Pose3(R, Point3()), id.retract(v),1e-2));
#ifdef CORRECT_POSE3_EXMAP
v(3)=0.2;v(4)=0.394742;v(5)=-2.08998;
#else
v(3)=0.2;v(4)=0.7;v(5)=-2;
#endif
EXPECT(assert_equal(Pose3(R, P),id.retract(v),1e-5));
EXPECT(assert_equal(Pose3(R, P),id.retract(v),1e-2));
}
/* ************************************************************************* */
@ -86,7 +86,7 @@ TEST(Pose3, expmap_b)
Pose3 p1(Rot3(), Point3(100, 0, 0));
Pose3 p2 = p1.retract(Vector_(6,0.0, 0.0, 0.1, 0.0, 0.0, 0.0));
Pose3 expected(Rot3::rodriguez(0.0, 0.0, 0.1), Point3(100.0, 0.0, 0.0));
EXPECT(assert_equal(expected, p2));
EXPECT(assert_equal(expected, p2,1e-2));
}
/* ************************************************************************* */

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

@ -30,11 +30,12 @@ using namespace gtsam;
Rot3 R = Rot3::rodriguez(0.1, 0.4, 0.2);
Point3 P(0.2, 0.7, -2.0);
double error = 1e-9, epsilon = 0.001;
static const Matrix I3 = eye(3);
/* ************************************************************************* */
TEST( Rot3, constructor)
{
Rot3 expected(eye(3, 3));
Rot3 expected(I3);
Vector r1(3), r2(3), r3(3);
r1(0) = 1;
r1(1) = 0;
@ -88,7 +89,7 @@ Rot3 slow_but_correct_rodriguez(const Vector& w) {
double t = norm_2(w);
Matrix J = skewSymmetric(w / t);
if (t < 1e-5) return Rot3();
Matrix R = eye(3) + sin(t) * J + (1.0 - cos(t)) * (J * J);
Matrix R = I3 + sin(t) * J + (1.0 - cos(t)) * (J * J);
return R;
}
@ -189,10 +190,14 @@ TEST(Rot3, manifold)
// log behaves correctly
Vector d12 = gR1.localCoordinates(gR2);
CHECK(assert_equal(gR2, gR1.retract(d12)));
CHECK(assert_equal(gR2, gR1*Rot3::Expmap(d12)));
Vector d21 = gR2.localCoordinates(gR1);
CHECK(assert_equal(gR1, gR2.retract(d21)));
#ifdef CORRECT_ROT3_EXMAP
// Check that it is expmap
CHECK(assert_equal(gR2, gR1*Rot3::Expmap(d12)));
CHECK(assert_equal(gR1, gR2*Rot3::Expmap(d21)));
#endif
// Check that log(t1,t2)=-log(t2,t1)
CHECK(assert_equal(d12,-d21));
@ -381,7 +386,7 @@ TEST( Rot3, RQ)
Vector actual;
boost::tie(actualK, actual) = RQ(R.matrix());
Vector expected = Vector_(3, 0.14715, 0.385821, 0.231671);
CHECK(assert_equal(eye(3),actualK));
CHECK(assert_equal(I3,actualK));
CHECK(assert_equal(expected,actual,1e-6));
// Try using xyz call, asserting that Rot3::RzRyRx(x,y,z).xyz()==[x;y;z]
@ -415,7 +420,7 @@ TEST( Rot3, expmapStability ) {
w(2), 0.0, -w(0),
-w(1), w(0), 0.0 );
Matrix W2 = W*W;
Matrix Rmat = eye(3) + (1.0-theta2/6.0 + theta2*theta2/120.0
Matrix Rmat = I3 + (1.0-theta2/6.0 + theta2*theta2/120.0
- theta2*theta2*theta2/5040.0)*W + (0.5 - theta2/24.0 + theta2*theta2/720.0)*W2 ;
Rot3 expectedR( Rmat );
CHECK(assert_equal(expectedR, actualR, 1e-10));
@ -475,6 +480,14 @@ TEST(Rot3, quaternion) {
#endif
/* ************************************************************************* */
TEST( Rot3, Cayley ) {
Matrix A = skewSymmetric(1,2,-3);
Matrix Q = Cayley(A);
EXPECT(assert_equal(I3, trans(Q)*Q));
EXPECT(assert_equal(A, Cayley(Q)));
}
/* ************************************************************************* */
int main() {
TestResult tr;

27
gtsam/geometry/tests/timeRot3.cpp
View File

@ -27,8 +27,9 @@ int main()
{
int n = 300000;
Vector v = Vector_(3,1.,0.,0.);
Rot3 R = Rot3::rodriguez(0.1, 0.4, 0.2);
// Rodriguez formula given axis angle
cout << "Rodriguez formula given axis angle" << endl;
long timeLog = clock();
for(int i = 0; i < n; i++)
Rot3::rodriguez(v,0.001);
@ -37,7 +38,7 @@ int main()
cout << seconds << " seconds" << endl;
cout << ((double)n/seconds) << " calls/second" << endl;
// Rodriguez formula given canonical coordinates
cout << "Rodriguez formula given canonical coordinates" << endl;
timeLog = clock();
for(int i = 0; i < n; i++)
Rot3::rodriguez(v);
@ -46,7 +47,25 @@ int main()
cout << seconds << " seconds" << endl;
cout << ((double)n/seconds) << " calls/second" << endl;
// Slow rotation matrix
cout << "Exmpap" << endl;
timeLog = clock();
for(int i = 0; i < n; i++)
R*Rot3::Expmap(v);
timeLog2 = clock();
seconds = (double)(timeLog2-timeLog)/CLOCKS_PER_SEC;
cout << seconds << " seconds" << endl;
cout << ((double)n/seconds) << " calls/second" << endl;
cout << "Retract" << endl;
timeLog = clock();
for(int i = 0; i < n; i++)
R.retract(v);
timeLog2 = clock();
seconds = (double)(timeLog2-timeLog)/CLOCKS_PER_SEC;
cout << seconds << " seconds" << endl;
cout << ((double)n/seconds) << " calls/second" << endl;
cout << "Slow rotation matrix" << endl;
timeLog = clock();
for(int i = 0; i < n; i++)
Rot3::Rz(0.3)*Rot3::Ry(0.2)*Rot3::Rx(0.1);
@ -55,7 +74,7 @@ int main()
cout << seconds << " seconds" << endl;
cout << ((double)n/seconds) << " calls/second" << endl;
// Fast Rotation matrix
cout << "Fast Rotation matrix" << endl;
timeLog = clock();
for(int i = 0; i < n; i++)
Rot3::RzRyRx(0.1,0.2,0.3);