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

retract and localCoordinates optional argument in Rot3 to switch between different math versions, and unit testing all versions

release/4.3a0
Richard Roberts 2012-01-08 19:40:32 +00:00
parent 95587fd2d3
commit fdf7bc6dae
4 changed files with 131 additions and 60 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
gtsam/geometry/Rot3.h
View File

@ -21,6 +21,10 @@
#pragma once #pragma once
#ifndef ROT3_DEFAULT_COORDINATES_MODE
#define ROT3_DEFAULT_COORDINATES_MODE Rot3::FIRST_ORDER
#endif
#include <gtsam/geometry/Point3.h> #include <gtsam/geometry/Point3.h>
#include <gtsam/3rdparty/Eigen/Eigen/Geometry> #include <gtsam/3rdparty/Eigen/Eigen/Geometry>
@ -220,6 +224,15 @@ namespace gtsam {
/// @name Manifold /// @name Manifold
/// @{ /// @{
/** Enum to indicate which method should be used in Rot3::retract() and
* Rot3::localCoordinates()
*/
enum CoordinatesMode {
FIRST_ORDER, ///< A fast first-order approximation to the exponential map.
SLOW_CALEY, ///< Retract and localCoordinates using the Caley transform.
CORRECT_EXPMAP ///< The correct exponential map, computationally expensive.
};
/// dimension of the variable - used to autodetect sizes /// dimension of the variable - used to autodetect sizes
static size_t Dim() { return dimension; } static size_t Dim() { return dimension; }
@ -227,10 +240,10 @@ namespace gtsam {
size_t dim() const { return dimension; } size_t dim() const { return dimension; }
/// Retraction from R^3 to Pose2 manifold neighborhood around current pose /// Retraction from R^3 to Pose2 manifold neighborhood around current pose
Rot3 retract(const Vector& omega) const; Rot3 retract(const Vector& omega, CoordinatesMode mode = ROT3_DEFAULT_COORDINATES_MODE) const;
/// Returns inverse retraction /// Returns inverse retraction
Vector localCoordinates(const Rot3& t2) const; Vector localCoordinates(const Rot3& t2, CoordinatesMode mode = ROT3_DEFAULT_COORDINATES_MODE) const;
/// @} /// @}
/// @name Lie Group /// @name Lie Group

90
gtsam/geometry/Rot3M.cpp
View File

@ -246,52 +246,56 @@ Vector Rot3::Logmap(const Rot3& R) {
} }
/* ************************************************************************* */ /* ************************************************************************* */
Rot3 Rot3::retract(const Vector& omega) const { Rot3 Rot3::retract(const Vector& omega, CoordinatesMode mode) const {
#ifdef CORRECT_ROT3_EXMAP if(mode == Rot3::FIRST_ORDER) {
return (*this)*Expmap(omega); const double x = omega(0), y = omega(1), z = omega(2);
#else const double x2 = x*x, y2 = y*y, z2 = z*z;
#ifdef SLOW_CAYLEY const double xy = x*y, xz = x*z, yz = y*z;
Matrix Omega = skewSymmetric(omega); const double f = 1.0 / (4.0 + x2 + y2 + z2), _2f = 2.0*f;
return (*this)*Cayley<3>(-Omega/2); return (*this)* Rot3(
#else (4+x2-y2-z2)*f, (xy - 2*z)*_2f, (xz + 2*y)*_2f,
const double x = omega(0), y = omega(1), z = omega(2); (xy + 2*z)*_2f, (4-x2+y2-z2)*f, (yz - 2*x)*_2f,
const double x2 = x*x, y2 = y*y, z2 = z*z; (xz - 2*y)*_2f, (yz + 2*x)*_2f, (4-x2-y2+z2)*f
const double xy = x*y, xz = x*z, yz = y*z; );
const double f = 1.0 / (4.0 + x2 + y2 + z2), _2f = 2.0*f; } else if(mode == Rot3::SLOW_CALEY) {
return (*this)* Rot3( Matrix Omega = skewSymmetric(omega);
(4+x2-y2-z2)*f, (xy - 2*z)*_2f, (xz + 2*y)*_2f, return (*this)*Cayley<3>(-Omega/2);
(xy + 2*z)*_2f, (4-x2+y2-z2)*f, (yz - 2*x)*_2f, } else if(mode == Rot3::CORRECT_EXPMAP) {
(xz - 2*y)*_2f, (yz + 2*x)*_2f, (4-x2-y2+z2)*f return (*this)*Expmap(omega);
); } else {
#endif assert(false);
#endif exit(1);
}
} }
/* ************************************************************************* */ /* ************************************************************************* */
Vector Rot3::localCoordinates(const Rot3& T) const { Vector Rot3::localCoordinates(const Rot3& T, CoordinatesMode mode) const {
#ifdef CORRECT_ROT3_EXMAP if(mode == Rot3::FIRST_ORDER) {
return Logmap(between(T)); // Create a fixed-size matrix
#else Eigen::Matrix3d A(between(T).matrix());
// Create a fixed-size matrix // Mathematica closed form optimization (procrastination?) gone wild:
Eigen::Matrix3d A(between(T).matrix()); const double a=A(0,0),b=A(0,1),c=A(0,2);
#ifdef SLOW_CAYLEY const double d=A(1,0),e=A(1,1),f=A(1,2);
// using templated version of Cayley const double g=A(2,0),h=A(2,1),i=A(2,2);
Matrix Omega = Cayley<3>(A); const double di = d*i, ce = c*e, cd = c*d, fg=f*g;
return -2*Vector_(3,Omega(2,1),Omega(0,2),Omega(1,0)); const double M = 1 + e - f*h + i + e*i;
#else const double K = 2.0 / (cd*h + M + a*M -g*(c + ce) - b*(d + di - fg));
// Mathematica closed form optimization (procrastination?) gone wild: const double x = (a * f - cd + f) * K;
const double a=A(0,0),b=A(0,1),c=A(0,2); const double y = (b * f - ce - c) * K;
const double d=A(1,0),e=A(1,1),f=A(1,2); const double z = (fg - di - d) * K;
const double g=A(2,0),h=A(2,1),i=A(2,2); return -2 * Vector_(3, x, y, z);
const double di = d*i, ce = c*e, cd = c*d, fg=f*g; } else if(mode == Rot3::SLOW_CALEY) {
const double M = 1 + e - f*h + i + e*i; // Create a fixed-size matrix
const double K = 2.0 / (cd*h + M + a*M -g*(c + ce) - b*(d + di - fg)); Eigen::Matrix3d A(between(T).matrix());
const double x = (a * f - cd + f) * K; // using templated version of Cayley
const double y = (b * f - ce - c) * K; Matrix Omega = Cayley<3>(A);
const double z = (fg - di - d) * K; return -2*Vector_(3,Omega(2,1),Omega(0,2),Omega(1,0));
return -2 * Vector_(3, x, y, z); } else if(mode == Rot3::CORRECT_EXPMAP) {
#endif return Logmap(between(T));
#endif } else {
assert(false);
exit(1);
}
} }
/* ************************************************************************* */ /* ************************************************************************* */

4
gtsam/geometry/Rot3Q.cpp
View File

@ -150,12 +150,12 @@ namespace gtsam {
} }
/* ************************************************************************* */ /* ************************************************************************* */
Rot3 Rot3::retract(const Vector& omega) const { Rot3 Rot3::retract(const Vector& omega, CoordinatesMode mode) const {
return compose(Expmap(omega)); return compose(Expmap(omega));
} }
/* ************************************************************************* */ /* ************************************************************************* */
Vector Rot3::localCoordinates(const Rot3& t2) const { Vector Rot3::localCoordinates(const Rot3& t2, CoordinatesMode mode) const {
return Logmap(between(t2)); return Logmap(between(t2));
} }

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

@ -201,23 +201,17 @@ TEST(Rot3, log)
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST(Rot3, manifold) TEST(Rot3, manifold_first_order)
{ {
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); Vector d12 = gR1.localCoordinates(gR2, Rot3::FIRST_ORDER);
CHECK(assert_equal(gR2, gR1.retract(d12))); CHECK(assert_equal(gR2, gR1.retract(d12, Rot3::FIRST_ORDER)));
Vector d21 = gR2.localCoordinates(gR1); Vector d21 = gR2.localCoordinates(gR1, Rot3::FIRST_ORDER);
CHECK(assert_equal(gR1, gR2.retract(d21))); CHECK(assert_equal(gR1, gR2.retract(d21, Rot3::FIRST_ORDER)));
#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 that log(t1,t2)=-log(t2,t1)
CHECK(assert_equal(d12,-d21)); CHECK(assert_equal(d12,-d21));
@ -234,6 +228,66 @@ TEST(Rot3, manifold)
CHECK(assert_equal(R5,R3*R2)); CHECK(assert_equal(R5,R3*R2));
} }
/* ************************************************************************* */
TEST(Rot3, manifold_caley)
{
Rot3 gR1 = Rot3::rodriguez(0.1, 0.4, 0.2);
Rot3 gR2 = Rot3::rodriguez(0.3, 0.1, 0.7);
Rot3 origin;
// log behaves correctly
Vector d12 = gR1.localCoordinates(gR2, Rot3::SLOW_CALEY);
CHECK(assert_equal(gR2, gR1.retract(d12, Rot3::SLOW_CALEY)));
Vector d21 = gR2.localCoordinates(gR1, Rot3::SLOW_CALEY);
CHECK(assert_equal(gR1, gR2.retract(d21, Rot3::SLOW_CALEY)));
// Check that log(t1,t2)=-log(t2,t1)
CHECK(assert_equal(d12,-d21));
// lines in canonical coordinates correspond to Abelian subgroups in SO(3)
Vector d = Vector_(3, 0.1, 0.2, 0.3);
// exp(-d)=inverse(exp(d))
CHECK(assert_equal(Rot3::Expmap(-d),Rot3::Expmap(d).inverse()));
// exp(5d)=exp(2*d+3*d)=exp(2*d)exp(3*d)=exp(3*d)exp(2*d)
Rot3 R2 = Rot3::Expmap (2 * d);
Rot3 R3 = Rot3::Expmap (3 * d);
Rot3 R5 = Rot3::Expmap (5 * d);
CHECK(assert_equal(R5,R2*R3));
CHECK(assert_equal(R5,R3*R2));
}
/* ************************************************************************* */
TEST(Rot3, manifold_expmap)
{
Rot3 gR1 = Rot3::rodriguez(0.1, 0.4, 0.2);
Rot3 gR2 = Rot3::rodriguez(0.3, 0.1, 0.7);
Rot3 origin;
// log behaves correctly
Vector d12 = gR1.localCoordinates(gR2, Rot3::CORRECT_EXPMAP);
CHECK(assert_equal(gR2, gR1.retract(d12, Rot3::CORRECT_EXPMAP)));
Vector d21 = gR2.localCoordinates(gR1, Rot3::CORRECT_EXPMAP);
CHECK(assert_equal(gR1, gR2.retract(d21, Rot3::CORRECT_EXPMAP)));
// Check that it is expmap
CHECK(assert_equal(gR2, gR1*Rot3::Expmap(d12)));
CHECK(assert_equal(gR1, gR2*Rot3::Expmap(d21)));
// Check that log(t1,t2)=-log(t2,t1)
CHECK(assert_equal(d12,-d21));
// lines in canonical coordinates correspond to Abelian subgroups in SO(3)
Vector d = Vector_(3, 0.1, 0.2, 0.3);
// exp(-d)=inverse(exp(d))
CHECK(assert_equal(Rot3::Expmap(-d),Rot3::Expmap(d).inverse()));
// exp(5d)=exp(2*d+3*d)=exp(2*d)exp(3*d)=exp(3*d)exp(2*d)
Rot3 R2 = Rot3::Expmap (2 * d);
Rot3 R3 = Rot3::Expmap (3 * d);
Rot3 R5 = Rot3::Expmap (5 * d);
CHECK(assert_equal(R5,R2*R3));
CHECK(assert_equal(R5,R3*R2));
}
/* ************************************************************************* */ /* ************************************************************************* */
class AngularVelocity: public Point3 { class AngularVelocity: public Point3 {
public: public:
@ -498,8 +552,6 @@ TEST(Rot3, quaternion) {
EXPECT(assert_equal(expected2, actual2)); EXPECT(assert_equal(expected2, actual2));
} }
#endif
/* ************************************************************************* */ /* ************************************************************************* */
TEST( Rot3, Cayley ) { TEST( Rot3, Cayley ) {
Matrix A = skewSymmetric(1,2,-3); Matrix A = skewSymmetric(1,2,-3);
@ -508,6 +560,8 @@ TEST( Rot3, Cayley ) {
EXPECT(assert_equal(A, Cayley(Q))); EXPECT(assert_equal(A, Cayley(Q)));
} }
#endif
/* ************************************************************************* */ /* ************************************************************************* */
int main() { int main() {
TestResult tr; TestResult tr;