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

Fixed names of coordinates modes in Rot3 and Pose3

release/4.3a0
Richard Roberts 2012-01-08 21:25:29 +00:00
parent ca3c78a5bc
commit f2638b2c0c
6 changed files with 38 additions and 38 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
gtsam/geometry/Pose3.cpp
View File

@ -98,7 +98,10 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
// Different versions of retract // Different versions of retract
Pose3 Pose3::retract(const Vector& xi, Pose3::CoordinatesMode mode) const { Pose3 Pose3::retract(const Vector& xi, Pose3::CoordinatesMode mode) const {
if(mode == Pose3::FIRST_ORDER) { if(mode == Pose3::EXPMAP) {
// Lie group exponential map, traces out geodesic
return compose(Expmap(xi));
} else if(mode == Pose3::FIRST_ORDER) {
Vector omega(sub(xi, 0, 3)); Vector omega(sub(xi, 0, 3));
Point3 v(sub(xi, 3, 6)); Point3 v(sub(xi, 3, 6));
@ -116,9 +119,6 @@ namespace gtsam {
// Point3 t = t_ + R_ * (v+Point3(omega).cross(v)/2); // Point3 t = t_ + R_ * (v+Point3(omega).cross(v)/2);
return Pose3(R, t); return Pose3(R, t);
} else if(mode == Pose3::CORRECT_EXPMAP) {
// Lie group exponential map, traces out geodesic
return compose(Expmap(xi));
} else { } else {
assert(false); assert(false);
exit(1); exit(1);
@ -128,7 +128,10 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
// different versions of localCoordinates // different versions of localCoordinates
Vector Pose3::localCoordinates(const Pose3& T, Pose3::CoordinatesMode mode) const { Vector Pose3::localCoordinates(const Pose3& T, Pose3::CoordinatesMode mode) const {
if(mode == Pose3::FIRST_ORDER) { if(mode == Pose3::EXPMAP) {
// Lie group logarithm map, exact inverse of exponential map
return Logmap(between(T));
} else if(mode == Pose3::FIRST_ORDER) {
// R is always done exactly in all three retract versions below // R is always done exactly in all three retract versions below
Vector omega = R_.localCoordinates(T.rotation()); Vector omega = R_.localCoordinates(T.rotation());
@ -142,9 +145,6 @@ namespace gtsam {
// TODO: correct second order t inverse // TODO: correct second order t inverse
return Vector_(6,omega(0),omega(1),omega(2),d.x(),d.y(),d.z()); return Vector_(6,omega(0),omega(1),omega(2),d.x(),d.y(),d.z());
} else if(mode == Pose3::CORRECT_EXPMAP) {
// Lie group logarithm map, exact inverse of exponential map
return Logmap(between(T));
} else { } else {
assert(false); assert(false);
exit(1); exit(1);

4
gtsam/geometry/Pose3.h
View File

@ -106,8 +106,8 @@ namespace gtsam {
* Pose3::localCoordinates() * Pose3::localCoordinates()
*/ */
enum CoordinatesMode { enum CoordinatesMode {
FIRST_ORDER, ///< A fast first-order approximation to the exponential map. EXPMAP, ///< The correct exponential map, computationally expensive.
CORRECT_EXPMAP ///< The correct exponential map, computationally expensive. FIRST_ORDER ///< A fast first-order approximation to the exponential map.
}; };
/// Dimensionality of tangent space = 6 DOF - used to autodetect sizes /// Dimensionality of tangent space = 6 DOF - used to autodetect sizes

8
gtsam/geometry/Rot3.h
View File

@ -22,7 +22,7 @@
#pragma once #pragma once
#ifndef ROT3_DEFAULT_COORDINATES_MODE #ifndef ROT3_DEFAULT_COORDINATES_MODE
#define ROT3_DEFAULT_COORDINATES_MODE Rot3::FIRST_ORDER #define ROT3_DEFAULT_COORDINATES_MODE Rot3::CALEY
#endif #endif
#include <gtsam/geometry/Point3.h> #include <gtsam/geometry/Point3.h>
@ -228,9 +228,9 @@ namespace gtsam {
* Rot3::localCoordinates() * Rot3::localCoordinates()
*/ */
enum CoordinatesMode { enum CoordinatesMode {
FIRST_ORDER, ///< A fast first-order approximation to the exponential map. EXPMAP, ///< The exponential map, computationally expensive.
SLOW_CALEY, ///< Retract and localCoordinates using the Caley transform. CALEY, ///< Retract and localCoordinates using the Caley transform.
CORRECT_EXPMAP ///< The correct exponential map, computationally expensive. SLOW_CALEY ///< Slow matrix implementation of Cayley transform (for tests only).
}; };
/// dimension of the variable - used to autodetect sizes /// dimension of the variable - used to autodetect sizes

12
gtsam/geometry/Rot3M.cpp
View File

@ -247,7 +247,9 @@ Vector Rot3::Logmap(const Rot3& R) {
/* ************************************************************************* */ /* ************************************************************************* */
Rot3 Rot3::retract(const Vector& omega, Rot3::CoordinatesMode mode) const { Rot3 Rot3::retract(const Vector& omega, Rot3::CoordinatesMode mode) const {
if(mode == Rot3::FIRST_ORDER) { if(mode == Rot3::EXPMAP) {
return (*this)*Expmap(omega);
} else if(mode == Rot3::CALEY) {
const double x = omega(0), y = omega(1), z = omega(2); const double x = omega(0), y = omega(1), z = omega(2);
const double x2 = x*x, y2 = y*y, z2 = z*z; const double x2 = x*x, y2 = y*y, z2 = z*z;
const double xy = x*y, xz = x*z, yz = y*z; const double xy = x*y, xz = x*z, yz = y*z;
@ -260,8 +262,6 @@ Rot3 Rot3::retract(const Vector& omega, Rot3::CoordinatesMode mode) const {
} else if(mode == Rot3::SLOW_CALEY) { } else if(mode == Rot3::SLOW_CALEY) {
Matrix Omega = skewSymmetric(omega); Matrix Omega = skewSymmetric(omega);
return (*this)*Cayley<3>(-Omega/2); return (*this)*Cayley<3>(-Omega/2);
} else if(mode == Rot3::CORRECT_EXPMAP) {
return (*this)*Expmap(omega);
} else { } else {
assert(false); assert(false);
exit(1); exit(1);
@ -270,7 +270,9 @@ Rot3 Rot3::retract(const Vector& omega, Rot3::CoordinatesMode mode) const {
/* ************************************************************************* */ /* ************************************************************************* */
Vector Rot3::localCoordinates(const Rot3& T, Rot3::CoordinatesMode mode) const { Vector Rot3::localCoordinates(const Rot3& T, Rot3::CoordinatesMode mode) const {
if(mode == Rot3::FIRST_ORDER) { if(mode == Rot3::EXPMAP) {
return Logmap(between(T));
} else if(mode == Rot3::CALEY) {
// Create a fixed-size matrix // Create a fixed-size matrix
Eigen::Matrix3d A(between(T).matrix()); Eigen::Matrix3d A(between(T).matrix());
// Mathematica closed form optimization (procrastination?) gone wild: // Mathematica closed form optimization (procrastination?) gone wild:
@ -290,8 +292,6 @@ Vector Rot3::localCoordinates(const Rot3& T, Rot3::CoordinatesMode mode) const {
// using templated version of Cayley // using templated version of Cayley
Matrix Omega = Cayley<3>(A); Matrix Omega = Cayley<3>(A);
return -2*Vector_(3,Omega(2,1),Omega(0,2),Omega(1,0)); return -2*Vector_(3,Omega(2,1),Omega(0,2),Omega(1,0));
} else if(mode == Rot3::CORRECT_EXPMAP) {
return Logmap(between(T));
} else { } else {
assert(false); assert(false);
exit(1); exit(1);

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

@ -60,9 +60,9 @@ TEST( Pose3, retract_expmap)
Pose3 id; Pose3 id;
Vector v = zero(6); Vector v = zero(6);
v(0) = 0.3; v(0) = 0.3;
EXPECT(assert_equal(Pose3(R, Point3()), id.retract(v, Pose3::CORRECT_EXPMAP),1e-2)); EXPECT(assert_equal(Pose3(R, Point3()), id.retract(v, Pose3::EXPMAP),1e-2));
v(3)=0.2;v(4)=0.394742;v(5)=-2.08998; v(3)=0.2;v(4)=0.394742;v(5)=-2.08998;
EXPECT(assert_equal(Pose3(R, P),id.retract(v, Pose3::CORRECT_EXPMAP),1e-2)); EXPECT(assert_equal(Pose3(R, P),id.retract(v, Pose3::EXPMAP),1e-2));
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -461,8 +461,8 @@ TEST(Pose3, localCoordinates_first_order)
TEST(Pose3, localCoordinates_expmap) TEST(Pose3, localCoordinates_expmap)
{ {
Vector d12 = repeat(6,0.1); Vector d12 = repeat(6,0.1);
Pose3 t1 = T, t2 = t1.retract(d12, Pose3::CORRECT_EXPMAP); Pose3 t1 = T, t2 = t1.retract(d12, Pose3::EXPMAP);
EXPECT(assert_equal(d12, t1.localCoordinates(t2, Pose3::CORRECT_EXPMAP))); EXPECT(assert_equal(d12, t1.localCoordinates(t2, Pose3::EXPMAP)));
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -483,10 +483,10 @@ TEST(Pose3, manifold_expmap)
Pose3 t1 = T; Pose3 t1 = T;
Pose3 t2 = T3; Pose3 t2 = T3;
Pose3 origin; Pose3 origin;
Vector d12 = t1.localCoordinates(t2, Pose3::CORRECT_EXPMAP); Vector d12 = t1.localCoordinates(t2, Pose3::EXPMAP);
EXPECT(assert_equal(t2, t1.retract(d12, Pose3::CORRECT_EXPMAP))); EXPECT(assert_equal(t2, t1.retract(d12, Pose3::EXPMAP)));
Vector d21 = t2.localCoordinates(t1, Pose3::CORRECT_EXPMAP); Vector d21 = t2.localCoordinates(t1, Pose3::EXPMAP);
EXPECT(assert_equal(t1, t2.retract(d21, Pose3::CORRECT_EXPMAP))); EXPECT(assert_equal(t1, t2.retract(d21, Pose3::EXPMAP)));
// Check that log(t1,t2)=-log(t2,t1) // Check that log(t1,t2)=-log(t2,t1)
EXPECT(assert_equal(d12,-d21)); EXPECT(assert_equal(d12,-d21));

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

@ -201,17 +201,17 @@ TEST(Rot3, log)
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST(Rot3, manifold_first_order) TEST(Rot3, manifold_caley)
{ {
Rot3 gR1 = Rot3::rodriguez(0.1, 0.4, 0.2); Rot3 gR1 = Rot3::rodriguez(0.1, 0.4, 0.2);
Rot3 gR2 = Rot3::rodriguez(0.3, 0.1, 0.7); Rot3 gR2 = Rot3::rodriguez(0.3, 0.1, 0.7);
Rot3 origin; Rot3 origin;
// log behaves correctly // log behaves correctly
Vector d12 = gR1.localCoordinates(gR2, Rot3::FIRST_ORDER); Vector d12 = gR1.localCoordinates(gR2, Rot3::CALEY);
CHECK(assert_equal(gR2, gR1.retract(d12, Rot3::FIRST_ORDER))); CHECK(assert_equal(gR2, gR1.retract(d12, Rot3::CALEY)));
Vector d21 = gR2.localCoordinates(gR1, Rot3::FIRST_ORDER); Vector d21 = gR2.localCoordinates(gR1, Rot3::CALEY);
CHECK(assert_equal(gR1, gR2.retract(d21, Rot3::FIRST_ORDER))); CHECK(assert_equal(gR1, gR2.retract(d21, Rot3::CALEY)));
// Check that log(t1,t2)=-log(t2,t1) // Check that log(t1,t2)=-log(t2,t1)
CHECK(assert_equal(d12,-d21)); CHECK(assert_equal(d12,-d21));
@ -229,7 +229,7 @@ TEST(Rot3, manifold_first_order)
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST(Rot3, manifold_caley) TEST(Rot3, manifold_slow_caley)
{ {
Rot3 gR1 = Rot3::rodriguez(0.1, 0.4, 0.2); Rot3 gR1 = Rot3::rodriguez(0.1, 0.4, 0.2);
Rot3 gR2 = Rot3::rodriguez(0.3, 0.1, 0.7); Rot3 gR2 = Rot3::rodriguez(0.3, 0.1, 0.7);
@ -264,10 +264,10 @@ TEST(Rot3, manifold_expmap)
Rot3 origin; Rot3 origin;
// log behaves correctly // log behaves correctly
Vector d12 = gR1.localCoordinates(gR2, Rot3::CORRECT_EXPMAP); Vector d12 = gR1.localCoordinates(gR2, Rot3::EXPMAP);
CHECK(assert_equal(gR2, gR1.retract(d12, Rot3::CORRECT_EXPMAP))); CHECK(assert_equal(gR2, gR1.retract(d12, Rot3::EXPMAP)));
Vector d21 = gR2.localCoordinates(gR1, Rot3::CORRECT_EXPMAP); Vector d21 = gR2.localCoordinates(gR1, Rot3::EXPMAP);
CHECK(assert_equal(gR1, gR2.retract(d21, Rot3::CORRECT_EXPMAP))); CHECK(assert_equal(gR1, gR2.retract(d21, Rot3::EXPMAP)));
// Check that it is expmap // Check that it is expmap
CHECK(assert_equal(gR2, gR1*Rot3::Expmap(d12))); CHECK(assert_equal(gR2, gR1*Rot3::Expmap(d12)));