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

explicit retract versions for calling in MATLAB

release/4.3a0
Frank Dellaert 2012-01-10 20:23:48 +00:00
parent c792a73e9f
commit c75f7ead65
5 changed files with 36 additions and 27 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
gtsam.h
View File

@ -139,6 +139,7 @@ class Rot3 {
bool equals(const Rot3& rot, double tol) const;
Vector localCoordinates(const Rot3& p);
Rot3 retract(Vector v);
Rot3 retractCayley(Vector v);
Rot3 compose(const Rot3& p2);
Rot3 between(const Rot3& p2);
};
@ -185,6 +186,7 @@ class Pose3 {
Pose3 compose(const Pose3& p2);
Pose3 between(const Pose3& p2);
Pose3 retract(Vector v);
Pose3 retractFirstOrder(Vector v);
Point3 translation() const;
Rot3 rotation() const;
};

32
gtsam/geometry/Pose3.cpp
View File

@ -102,31 +102,27 @@ namespace gtsam {
}
}
/* ************************************************************************* */
Pose3 Pose3::retractFirstOrder(const Vector& xi) const {
Vector omega(sub(xi, 0, 3));
Point3 v(sub(xi, 3, 6));
Rot3 R = R_.retract(omega); // R is done exactly
Point3 t = t_ + R_ * v; // First order t approximation
return Pose3(R, t);
}
/* ************************************************************************* */
// Different versions of retract
Pose3 Pose3::retract(const Vector& xi, Pose3::CoordinatesMode mode) const {
if(mode == Pose3::EXPMAP) {
// Lie group exponential map, traces out geodesic
// Lie group exponential map, traces out geodesic
return compose(Expmap(xi));
} else if(mode == Pose3::FIRST_ORDER) {
Vector omega(sub(xi, 0, 3));
Point3 v(sub(xi, 3, 6));
// R is always done exactly in all three retract versions below
Rot3 R = R_.retract(omega);
// Incorrect version
// Retracts R and t independently
// Point3 t = t_.retract(v.vector());
// First order t approximation
Point3 t = t_ + R_ * v;
// Second order t approximation
// Point3 t = t_ + R_ * (v+Point3(omega).cross(v)/2);
return Pose3(R, t);
// First order
return retractFirstOrder(xi);
} else {
// Point3 t = t_.retract(v.vector()); // Incorrect version retracts t independently
// Point3 t = t_ + R_ * (v+Point3(omega).cross(v)/2); // Second order t approximation
assert(false);
exit(1);
}

2
gtsam/geometry/Pose3.h
View File

@ -121,6 +121,8 @@ namespace gtsam {
/// Dimensionality of the tangent space = 6 DOF
size_t dim() const { return dimension; }
/// Retraction with fast first-order approximation to the exponential map
Pose3 retractFirstOrder(const Vector& d) const;
/// Retraction from R^6 to Pose3 manifold neighborhood around current pose
Pose3 retract(const Vector& d, Pose3::CoordinatesMode mode = POSE3_DEFAULT_COORDINATES_MODE) const;

5
gtsam/geometry/Rot3.h
View File

@ -251,6 +251,11 @@ namespace gtsam {
#endif
};
#ifndef GTSAM_DEFAULT_QUATERNIONS
/// Retraction from R^3 to Rot3 manifold using the Cayley transform
Rot3 retractCayley(const Vector& omega) const;
#endif
/// Retraction from R^3 to Rot3 manifold neighborhood around current rotation
Rot3 retract(const Vector& omega, Rot3::CoordinatesMode mode = ROT3_DEFAULT_COORDINATES_MODE) const;

22
gtsam/geometry/Rot3M.cpp
View File

@ -245,20 +245,24 @@ Vector Rot3::Logmap(const Rot3& R) {
}
}
/* ************************************************************************* */
Rot3 Rot3::retractCayley(const Vector& omega) const {
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;
const 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);
}
/* ************************************************************************* */
Rot3 Rot3::retract(const Vector& omega, Rot3::CoordinatesMode mode) const {
if(mode == Rot3::EXPMAP) {
return (*this)*Expmap(omega);
} 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;
const 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
);
return retractCayley(omega);
} else if(mode == Rot3::SLOW_CAYLEY) {
Matrix Omega = skewSymmetric(omega);
return (*this)*Cayley<3>(-Omega/2);