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
#ifndef ROT3_DEFAULT_COORDINATES_MODE
#define ROT3_DEFAULT_COORDINATES_MODE Rot3::FIRST_ORDER
#endif
#include <gtsam/geometry/Point3.h>
#include <gtsam/3rdparty/Eigen/Eigen/Geometry>
@ -220,6 +224,15 @@ namespace gtsam {
/// @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
static size_t Dim() { return dimension; }
@ -227,10 +240,10 @@ namespace gtsam {
size_t dim() const { return dimension; }
/// 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
Vector localCoordinates(const Rot3& t2) const;
Vector localCoordinates(const Rot3& t2, CoordinatesMode mode = ROT3_DEFAULT_COORDINATES_MODE) const;
/// @}
/// @name Lie Group

46
gtsam/geometry/Rot3M.cpp
View File

@ -246,14 +246,8 @@ 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<3>(-Omega/2);
#else
Rot3 Rot3::retract(const Vector& omega, CoordinatesMode mode) const {
if(mode == Rot3::FIRST_ORDER) {
const double x = omega(0), y = omega(1), z = omega(2);
const double x2 = x*x, y2 = y*y, z2 = z*z;
const double xy = x*y, xz = x*z, yz = y*z;
@ -263,22 +257,22 @@ Rot3 Rot3::retract(const Vector& omega) const {
(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
} else if(mode == Rot3::SLOW_CALEY) {
Matrix Omega = skewSymmetric(omega);
return (*this)*Cayley<3>(-Omega/2);
} else if(mode == Rot3::CORRECT_EXPMAP) {
return (*this)*Expmap(omega);
} else {
assert(false);
exit(1);
}
}
/* ************************************************************************* */
Vector Rot3::localCoordinates(const Rot3& T) const {
#ifdef CORRECT_ROT3_EXMAP
return Logmap(between(T));
#else
Vector Rot3::localCoordinates(const Rot3& T, CoordinatesMode mode) const {
if(mode == Rot3::FIRST_ORDER) {
// Create a fixed-size matrix
Eigen::Matrix3d A(between(T).matrix());
#ifdef SLOW_CAYLEY
// using templated version of Cayley
Matrix Omega = Cayley<3>(A);
return -2*Vector_(3,Omega(2,1),Omega(0,2),Omega(1,0));
#else
// Mathematica closed form optimization (procrastination?) gone wild:
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);
@ -290,8 +284,18 @@ Vector Rot3::localCoordinates(const Rot3& T) const {
const double y = (b * f - ce - c) * K;
const double z = (fg - di - d) * K;
return -2 * Vector_(3, x, y, z);
#endif
#endif
} else if(mode == Rot3::SLOW_CALEY) {
// Create a fixed-size matrix
Eigen::Matrix3d A(between(T).matrix());
// using templated version of Cayley
Matrix Omega = Cayley<3>(A);
return -2*Vector_(3,Omega(2,1),Omega(0,2),Omega(1,0));
} else if(mode == Rot3::CORRECT_EXPMAP) {
return Logmap(between(T));
} 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));
}
/* ************************************************************************* */
Vector Rot3::localCoordinates(const Rot3& t2) const {
Vector Rot3::localCoordinates(const Rot3& t2, CoordinatesMode mode) const {
return Logmap(between(t2));
}

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

@ -201,23 +201,77 @@ TEST(Rot3, log)
}
/* ************************************************************************* */
TEST(Rot3, manifold)
TEST(Rot3, manifold_first_order)
{
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);
CHECK(assert_equal(gR2, gR1.retract(d12)));
Vector d21 = gR2.localCoordinates(gR1);
CHECK(assert_equal(gR1, gR2.retract(d21)));
Vector d12 = gR1.localCoordinates(gR2, Rot3::FIRST_ORDER);
CHECK(assert_equal(gR2, gR1.retract(d12, Rot3::FIRST_ORDER)));
Vector d21 = gR2.localCoordinates(gR1, Rot3::FIRST_ORDER);
CHECK(assert_equal(gR1, gR2.retract(d21, Rot3::FIRST_ORDER)));
// 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_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)));
#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));
@ -498,8 +552,6 @@ TEST(Rot3, quaternion) {
EXPECT(assert_equal(expected2, actual2));
}
#endif
/* ************************************************************************* */
TEST( Rot3, Cayley ) {
Matrix A = skewSymmetric(1,2,-3);
@ -508,6 +560,8 @@ TEST( Rot3, Cayley ) {
EXPECT(assert_equal(A, Cayley(Q)));
}
#endif
/* ************************************************************************* */
int main() {
TestResult tr;