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

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

release/4.3a0
Richard Roberts 2012-01-08 20:43:17 +00:00
parent fdf7bc6dae
commit 4f59f58cd1
3 changed files with 99 additions and 57 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

72
gtsam/geometry/Pose3.cpp
View File

@ -97,52 +97,58 @@ namespace gtsam {
/* ************************************************************************* */
// Different versions of retract
Pose3 Pose3::retract(const Vector& xi) const {
#ifdef CORRECT_POSE3_EXMAP
// Lie group exponential map, traces out geodesic
return compose(Expmap(xi));
#else
Vector omega(sub(xi, 0, 3));
Point3 v(sub(xi, 3, 6));
Pose3 Pose3::retract(const Vector& xi, Pose3::CoordinatesMode mode) const {
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);
// 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());
// Incorrect version
// Retracts R and t independently
// Point3 t = t_.retract(v.vector());
// First order t approximation
Point3 t = t_ + R_ * v;
// First order t approximation
Point3 t = t_ + R_ * v;
// Second order t approximation
// Point3 t = t_ + R_ * (v+Point3(omega).cross(v)/2);
// Second order t approximation
// Point3 t = t_ + R_ * (v+Point3(omega).cross(v)/2);
return Pose3(R, t);
#endif
return Pose3(R, t);
} else if(mode == Pose3::CORRECT_EXPMAP) {
// Lie group exponential map, traces out geodesic
return compose(Expmap(xi));
} else {
assert(false);
exit(1);
}
}
/* ************************************************************************* */
// different versions of localCoordinates
Vector Pose3::localCoordinates(const Pose3& T) const {
#ifdef CORRECT_POSE3_EXMAP
// Lie group logarithm map, exact inverse of exponential map
return Logmap(between(T));
#else
// R is always done exactly in all three retract versions below
Vector omega = R_.localCoordinates(T.rotation());
Vector Pose3::localCoordinates(const Pose3& T, Pose3::CoordinatesMode mode) const {
if(mode == Pose3::FIRST_ORDER) {
// R is always done exactly in all three retract versions below
Vector omega = R_.localCoordinates(T.rotation());
// Incorrect version
// Independently computes the logmap of the translation and rotation
// Vector v = t_.localCoordinates(T.translation());
// Incorrect version
// Independently computes the logmap of the translation and rotation
// Vector v = t_.localCoordinates(T.translation());
// Correct first order t inverse
Point3 d = R_.unrotate(T.translation() - t_);
// Correct first order t inverse
Point3 d = R_.unrotate(T.translation() - t_);
// TODO: correct second order t inverse
// TODO: correct second order t inverse
return Vector_(6,omega(0),omega(1),omega(2),d.x(),d.y(),d.z());
#endif
return Vector_(6,omega(0),omega(1),omega(2),d.x(),d.y(),d.z());
} else if(mode == Pose3::CORRECT_EXPMAP) {
// Lie group logarithm map, exact inverse of exponential map
return Logmap(between(T));
} else {
assert(false);
exit(1);
}
}
/* ************************************************************************* */

16
gtsam/geometry/Pose3.h
View File

@ -18,6 +18,10 @@
#pragma once
#ifndef POSE3_DEFAULT_COORDINATES_MODE
#define POSE3_DEFAULT_COORDINATES_MODE Pose3::FIRST_ORDER
#endif
#include <gtsam/geometry/Point3.h>
#include <gtsam/geometry/Rot3.h>
@ -98,6 +102,14 @@ namespace gtsam {
/// @name Manifold
/// @{
/** Enum to indicate which method should be used in Pose3::retract() and
* Pose3::localCoordinates()
*/
enum CoordinatesMode {
FIRST_ORDER, ///< A fast first-order approximation to the exponential map.
CORRECT_EXPMAP ///< The correct exponential map, computationally expensive.
};
/// Dimensionality of tangent space = 6 DOF - used to autodetect sizes
static size_t Dim() { return dimension; }
@ -106,10 +118,10 @@ namespace gtsam {
/// Retraction from R^6 to Pose3 manifold neighborhood around current pose
Pose3 retract(const Vector& d) const;
Pose3 retract(const Vector& d, Pose3::CoordinatesMode mode = POSE3_DEFAULT_COORDINATES_MODE) const;
/// Local 6D coordinates of Pose3 manifold neighborhood around current pose
Vector localCoordinates(const Pose3& T2) const;
Vector localCoordinates(const Pose3& T2, Pose3::CoordinatesMode mode = POSE3_DEFAULT_COORDINATES_MODE) const;
/// @}
/// @name Lie Group

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

@ -44,18 +44,25 @@ TEST( Pose3, equals)
}
/* ************************************************************************* */
TEST( Pose3, retract)
TEST( Pose3, retract_first_order)
{
Pose3 id;
Vector v = zero(6);
v(0) = 0.3;
EXPECT(assert_equal(Pose3(R, Point3()), id.retract(v),1e-2));
#ifdef CORRECT_POSE3_EXMAP
v(3)=0.2;v(4)=0.394742;v(5)=-2.08998;
#else
EXPECT(assert_equal(Pose3(R, Point3()), id.retract(v, Pose3::FIRST_ORDER),1e-2));
v(3)=0.2;v(4)=0.7;v(5)=-2;
#endif
EXPECT(assert_equal(Pose3(R, P),id.retract(v),1e-2));
EXPECT(assert_equal(Pose3(R, P),id.retract(v, Pose3::FIRST_ORDER),1e-2));
}
/* ************************************************************************* */
TEST( Pose3, retract_expmap)
{
Pose3 id;
Vector v = zero(6);
v(0) = 0.3;
EXPECT(assert_equal(Pose3(R, Point3()), id.retract(v, Pose3::CORRECT_EXPMAP),1e-2));
v(3)=0.2;v(4)=0.394742;v(5)=-2.08998;
EXPECT(assert_equal(Pose3(R, P),id.retract(v, Pose3::CORRECT_EXPMAP),1e-2));
}
/* ************************************************************************* */
@ -443,31 +450,48 @@ TEST( Pose3, transformPose_to)
}
/* ************************************************************************* */
TEST(Pose3, localCoordinates)
TEST(Pose3, localCoordinates_first_order)
{
Vector d12 = repeat(6,0.1);
Pose3 t1 = T, t2 = t1.retract(d12);
EXPECT(assert_equal(d12, t1.localCoordinates(t2)));
Pose3 t1 = T, t2 = t1.retract(d12, Pose3::FIRST_ORDER);
EXPECT(assert_equal(d12, t1.localCoordinates(t2, Pose3::FIRST_ORDER)));
}
/* ************************************************************************* */
TEST(Pose3, manifold)
TEST(Pose3, localCoordinates_expmap)
{
Vector d12 = repeat(6,0.1);
Pose3 t1 = T, t2 = t1.retract(d12, Pose3::CORRECT_EXPMAP);
EXPECT(assert_equal(d12, t1.localCoordinates(t2, Pose3::CORRECT_EXPMAP)));
}
/* ************************************************************************* */
TEST(Pose3, manifold_first_order)
{
Pose3 t1 = T;
Pose3 t2 = T3;
Pose3 origin;
Vector d12 = t1.localCoordinates(t2);
EXPECT(assert_equal(t2, t1.retract(d12)));
// todo: richard - commented out because this tests for "compose-style" (new) expmap
// EXPECT(assert_equal(t2, retract(origin,d12)*t1));
Vector d21 = t2.localCoordinates(t1);
EXPECT(assert_equal(t1, t2.retract(d21)));
// todo: richard - commented out because this tests for "compose-style" (new) expmap
// EXPECT(assert_equal(t1, retract(origin,d21)*t2));
// Check that log(t1,t2)=-log(t2,t1) TODO: only holds for exp map
// EXPECT(assert_equal(d12,-d21));
Vector d12 = t1.localCoordinates(t2, Pose3::FIRST_ORDER);
EXPECT(assert_equal(t2, t1.retract(d12, Pose3::FIRST_ORDER)));
Vector d21 = t2.localCoordinates(t1, Pose3::FIRST_ORDER);
EXPECT(assert_equal(t1, t2.retract(d21, Pose3::FIRST_ORDER)));
}
/* ************************************************************************* */
TEST(Pose3, manifold_expmap)
{
Pose3 t1 = T;
Pose3 t2 = T3;
Pose3 origin;
Vector d12 = t1.localCoordinates(t2, Pose3::CORRECT_EXPMAP);
EXPECT(assert_equal(t2, t1.retract(d12, Pose3::CORRECT_EXPMAP)));
Vector d21 = t2.localCoordinates(t1, Pose3::CORRECT_EXPMAP);
EXPECT(assert_equal(t1, t2.retract(d21, Pose3::CORRECT_EXPMAP)));
// Check that log(t1,t2)=-log(t2,t1)
EXPECT(assert_equal(d12,-d21));
}
/* ************************************************************************* */
TEST(Pose3, subgroups)
{