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

Cleaned up commented code

release/4.3a0
Alex Cunningham 2011-11-10 15:19:04 +00:00
parent f77c479dec
commit af9320e468
3 changed files with 28 additions and 50 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

40
gtsam/geometry/Pose3.cpp
View File

@ -95,46 +95,23 @@ namespace gtsam {
}
#ifdef CORRECT_POSE3_EXMAP
// /* ************************************************************************* */
// // Changes default to use the full verions of expmap/logmap
// /* ************************************************************************* */
// Pose3 Retract(const Vector& xi) {
// return Pose3::Expmap(xi);
// }
//
// /* ************************************************************************* */
// Vector Unretract(const Pose3& p) {
// return Pose3::Logmap(p);
// }
/* ************************************************************************* */
// Changes default to use the full verions of expmap/logmap
/* ************************************************************************* */
/* ************************************************************************* */
Pose3 retract(const Vector& d) {
return retract(d);
Pose3 Pose3::retract(const Vector& d) {
return compose(Expmap(d));
}
/* ************************************************************************* */
Vector localCoordinates(const Pose3& T1, const Pose3& T2) {
return localCoordinates(T2);
Vector Pose3::localCoordinates(const Pose3& T1, const Pose3& T2) {
return Logmap(T1.between(T2));
}
#else
// /* ************************************************************************* */
// /* incorrect versions for which we know how to compute derivatives */
// Pose3 Pose3::Retract(const Vector& d) {
// Vector w = sub(d, 0,3);
// Vector u = sub(d, 3,6);
// return Pose3(Rot3::Retract(w), Point3::Retract(u));
// }
//
// /* ************************************************************************* */
// // Log map at identity - return the translation and canonical rotation
// // coordinates of a pose.
// Vector Pose3::Unretract(const Pose3& p) {
// const Vector w = Rot3::Unretract(p.rotation()), u = Point3::Unretract(p.translation());
// return concatVectors(2, &w, &u);
// }
/* ************************************************************************* */
/** These are the "old-style" expmap and logmap about the specified
* pose. Increments the offset and rotation independently given a translation and
* canonical rotation coordinates. Created to match ML derivatives, but
@ -144,6 +121,7 @@ namespace gtsam {
t_.retract(sub(d, 3, 6)));
}
/* ************************************************************************* */
/** Independently computes the logmap of the translation and rotation. */
Vector Pose3::localCoordinates(const Pose3& pp) const {
const Vector r(R_.localCoordinates(pp.rotation())),

4
gtsam/geometry/tests/testPose2.cpp
View File

@ -150,8 +150,8 @@ namespace screw {
TEST(Pose3, expmap_c)
{
EXPECT(assert_equal(screw::expected, expm<Pose2>(screw::xi),1e-6));
EXPECT(assert_equal(screw::expected, Pose2::Retract(screw::xi),1e-6));
EXPECT(assert_equal(screw::xi, Pose2::Unretract(screw::expected),1e-6));
EXPECT(assert_equal(screw::expected, Pose2::Expmap(screw::xi),1e-6));
EXPECT(assert_equal(screw::xi, Pose2::Logmap(screw::expected),1e-6));
}
#endif

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

@ -105,24 +105,24 @@ namespace screw {
TEST(Pose3, expmap_c)
{
EXPECT(assert_equal(screw::expected, expm<Pose3>(screw::xi),1e-6));
EXPECT(assert_equal(screw::expected, Pose3::Retract(screw::xi),1e-6));
EXPECT(assert_equal(screw::expected, Pose3::Expmap(screw::xi),1e-6));
}
/* ************************************************************************* */
// assert that T*exp(xi)*T^-1 is equal to exp(Ad_T(xi))
TEST(Pose3, Adjoint)
{
Pose3 expected = T * Pose3::Retract(screw::xi) * inverse(T);
Pose3 expected = T * Pose3::Expmap(screw::xi) * inverse(T);
Vector xiprime = Adjoint(T, screw::xi);
EXPECT(assert_equal(expected, Pose3::Retract(xiprime), 1e-6));
EXPECT(assert_equal(expected, Pose3::Expmap(xiprime), 1e-6));
Pose3 expected2 = T2 * Pose3::Retract(screw::xi) * inverse(T2);
Pose3 expected2 = T2 * Pose3::Expmap(screw::xi) * inverse(T2);
Vector xiprime2 = Adjoint(T2, screw::xi);
EXPECT(assert_equal(expected2, Pose3::Retract(xiprime2), 1e-6));
EXPECT(assert_equal(expected2, Pose3::Expmap(xiprime2), 1e-6));
Pose3 expected3 = T3 * Pose3::Retract(screw::xi) * inverse(T3);
Pose3 expected3 = T3 * Pose3::Expmap(screw::xi) * inverse(T3);
Vector xiprime3 = Adjoint(T3, screw::xi);
EXPECT(assert_equal(expected3, Pose3::Retract(xiprime3), 1e-6));
EXPECT(assert_equal(expected3, Pose3::Expmap(xiprime3), 1e-6));
}
/* ************************************************************************* */
@ -145,7 +145,7 @@ TEST(Pose3, expmaps_galore)
{
Vector xi; Pose3 actual;
xi = Vector_(6,0.1,0.2,0.3,0.4,0.5,0.6);
actual = Pose3::Retract(xi);
actual = Pose3::Expmap(xi);
EXPECT(assert_equal(expm<Pose3>(xi), actual,1e-6));
EXPECT(assert_equal(Agrawal06iros(xi), actual,1e-6));
EXPECT(assert_equal(xi, localCoordinates(actual),1e-6));
@ -153,17 +153,17 @@ TEST(Pose3, expmaps_galore)
xi = Vector_(6,0.1,-0.2,0.3,-0.4,0.5,-0.6);
for (double theta=1.0;0.3*theta<=M_PI;theta*=2) {
Vector txi = xi*theta;
actual = Pose3::Retract(txi);
actual = Pose3::Expmap(txi);
EXPECT(assert_equal(expm<Pose3>(txi,30), actual,1e-6));
EXPECT(assert_equal(Agrawal06iros(txi), actual,1e-6));
Vector log = localCoordinates(actual);
EXPECT(assert_equal(actual, Pose3::Retract(log),1e-6));
EXPECT(assert_equal(actual, Pose3::Expmap(log),1e-6));
EXPECT(assert_equal(txi,log,1e-6)); // not true once wraps
}
// Works with large v as well, but expm needs 10 iterations!
xi = Vector_(6,0.2,0.3,-0.8,100.0,120.0,-60.0);
actual = Pose3::Retract(xi);
actual = Pose3::Expmap(xi);
EXPECT(assert_equal(expm<Pose3>(xi,10), actual,1e-5));
EXPECT(assert_equal(Agrawal06iros(xi), actual,1e-6));
EXPECT(assert_equal(xi, localCoordinates(actual),1e-6));
@ -176,9 +176,9 @@ TEST(Pose3, Adjoint_compose)
// T1*T2*exp(Adjoint(inv(T2),x) = T1*exp(x)*T2
const Pose3& T1 = T;
Vector x = Vector_(6,0.1,0.1,0.1,0.4,0.2,0.8);
Pose3 expected = T1 * Pose3::Retract(x) * T2;
Pose3 expected = T1 * Pose3::Expmap(x) * T2;
Vector y = Adjoint(inverse(T2), x);
Pose3 actual = T1 * T2 * Pose3::Retract(y);
Pose3 actual = T1 * T2 * Pose3::Expmap(y);
EXPECT(assert_equal(expected, actual, 1e-6));
}
#endif // SLOW_BUT_CORRECT_EXMAP
@ -544,11 +544,11 @@ TEST(Pose3, manifold)
// lines in canonical coordinates correspond to Abelian subgroups in SE(3)
Vector d = Vector_(6,0.1,0.2,0.3,0.4,0.5,0.6);
// exp(-d)=inverse(exp(d))
EXPECT(assert_equal(Pose3::Retract(-d),inverse(Pose3::Retract(d))));
EXPECT(assert_equal(Pose3::Expmap(-d),inverse(Pose3::Expmap(d))));
// exp(5d)=exp(2*d+3*d)=exp(2*d)exp(3*d)=exp(3*d)exp(2*d)
Pose3 T2 = Pose3::Retract(2*d);
Pose3 T3 = Pose3::Retract(3*d);
Pose3 T5 = Pose3::Retract(5*d);
Pose3 T2 = Pose3::Expmap(2*d);
Pose3 T3 = Pose3::Expmap(3*d);
Pose3 T5 = Pose3::Expmap(5*d);
EXPECT(assert_equal(T5,T2*T3));
EXPECT(assert_equal(T5,T3*T2));