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

Fixed spelling of coordinates modes in Rot3 and Pose3

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

6
gtsam/geometry/Rot3.h
View File

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

8
gtsam/geometry/Rot3M.cpp
View File

@ -249,7 +249,7 @@ Vector Rot3::Logmap(const Rot3& R) {
Rot3 Rot3::retract(const Vector& omega, Rot3::CoordinatesMode mode) const {
if(mode == Rot3::EXPMAP) {
return (*this)*Expmap(omega);
} else if(mode == Rot3::CALEY) {
} else if(mode == Rot3::CAYLEY) {
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;
@ -259,7 +259,7 @@ Rot3 Rot3::retract(const Vector& omega, Rot3::CoordinatesMode mode) 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
);
} else if(mode == Rot3::SLOW_CALEY) {
} else if(mode == Rot3::SLOW_CAYLEY) {
Matrix Omega = skewSymmetric(omega);
return (*this)*Cayley<3>(-Omega/2);
} else {
@ -272,7 +272,7 @@ Rot3 Rot3::retract(const Vector& omega, Rot3::CoordinatesMode mode) const {
Vector Rot3::localCoordinates(const Rot3& T, Rot3::CoordinatesMode mode) const {
if(mode == Rot3::EXPMAP) {
return Logmap(between(T));
} else if(mode == Rot3::CALEY) {
} else if(mode == Rot3::CAYLEY) {
// Create a fixed-size matrix
Eigen::Matrix3d A(between(T).matrix());
// Mathematica closed form optimization (procrastination?) gone wild:
@ -286,7 +286,7 @@ Vector Rot3::localCoordinates(const Rot3& T, Rot3::CoordinatesMode mode) const {
const double y = (b * f - ce - c) * K;
const double z = (fg - di - d) * K;
return -2 * Vector_(3, x, y, z);
} else if(mode == Rot3::SLOW_CALEY) {
} else if(mode == Rot3::SLOW_CAYLEY) {
// Create a fixed-size matrix
Eigen::Matrix3d A(between(T).matrix());
// using templated version of Cayley

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

@ -208,10 +208,10 @@ TEST(Rot3, manifold_caley)
Rot3 origin;
// log behaves correctly
Vector d12 = gR1.localCoordinates(gR2, Rot3::CALEY);
CHECK(assert_equal(gR2, gR1.retract(d12, Rot3::CALEY)));
Vector d21 = gR2.localCoordinates(gR1, Rot3::CALEY);
CHECK(assert_equal(gR1, gR2.retract(d21, Rot3::CALEY)));
Vector d12 = gR1.localCoordinates(gR2, Rot3::CAYLEY);
CHECK(assert_equal(gR2, gR1.retract(d12, Rot3::CAYLEY)));
Vector d21 = gR2.localCoordinates(gR1, Rot3::CAYLEY);
CHECK(assert_equal(gR1, gR2.retract(d21, Rot3::CAYLEY)));
// Check that log(t1,t2)=-log(t2,t1)
CHECK(assert_equal(d12,-d21));
@ -236,10 +236,10 @@ TEST(Rot3, manifold_slow_caley)
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)));
Vector d12 = gR1.localCoordinates(gR2, Rot3::SLOW_CAYLEY);
CHECK(assert_equal(gR2, gR1.retract(d12, Rot3::SLOW_CAYLEY)));
Vector d21 = gR2.localCoordinates(gR1, Rot3::SLOW_CAYLEY);
CHECK(assert_equal(gR1, gR2.retract(d21, Rot3::SLOW_CAYLEY)));
// Check that log(t1,t2)=-log(t2,t1)
CHECK(assert_equal(d12,-d21));