Made versions with [expmap|logmap]Full() for Pose2 and Pose3 to allow access to complete expmap functions, while also allowing for the concurrent existence of the approximate expmap for optimization speed
Alex Cunningham
parent
37ee4f6cb3
commit
8398c8a53d
|
|
@ -47,10 +47,7 @@ namespace gtsam {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
||||
#ifdef SLOW_BUT_CORRECT_EXPMAP
|
||||
|
||||
Pose2 Pose2::Expmap(const Vector& xi) {
|
||||
Pose2 Pose2::ExpmapFull(const Vector& xi) {
|
||||
assert(xi.size() == 3);
|
||||
Point2 v(xi(0),xi(1));
|
||||
double w = xi(2);
|
||||
|
|
@ -64,7 +61,8 @@ namespace gtsam {
|
|||
}
|
||||
}
|
||||
|
||||
Vector Pose2::Logmap(const Pose2& p) {
|
||||
/* ************************************************************************* */
|
||||
Vector Pose2::LogmapFull(const Pose2& p) {
|
||||
const Rot2& R = p.r();
|
||||
const Point2& t = p.t();
|
||||
double w = R.theta();
|
||||
|
|
@ -79,6 +77,20 @@ namespace gtsam {
|
|||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
#ifdef SLOW_BUT_CORRECT_EXPMAP
|
||||
/* ************************************************************************* */
|
||||
// Changes default to use the full verions of expmap/logmap
|
||||
/* ************************************************************************* */
|
||||
Pose2 Pose2::Expmap(const Vector& xi) {
|
||||
return ExpmapFull(xi);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Vector Pose2::Logmap(const Pose2& p) {
|
||||
return LogmapFull(p);
|
||||
}
|
||||
|
||||
#else
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
|
|
@ -113,10 +113,18 @@ namespace gtsam {
|
|||
*/
|
||||
static Vector Logmap(const Pose2& p);
|
||||
|
||||
/** non-approximated versions of Expmap/Logmap */
|
||||
static Pose2 ExpmapFull(const Vector& xi);
|
||||
static Vector LogmapFull(const Pose2& p);
|
||||
|
||||
/** default implementations of binary functions */
|
||||
inline Pose2 expmap(const Vector& v) const { return gtsam::expmap_default(*this, v); }
|
||||
inline Vector logmap(const Pose2& p2) const { return gtsam::logmap_default(*this, p2);}
|
||||
|
||||
/** non-approximated versions of expmap/logmap */
|
||||
inline Pose2 expmapFull(const Vector& v) const { return compose(ExpmapFull(v)); }
|
||||
inline Vector logmapFull(const Pose2& p2) const { return LogmapFull(between(p2));}
|
||||
|
||||
/**
|
||||
* Return relative pose between p1 and p2, in p1 coordinate frame
|
||||
*/
|
||||
|
|
|
|||
|
|
@ -53,11 +53,8 @@ namespace gtsam {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
||||
#ifdef CORRECT_POSE3_EXMAP
|
||||
|
||||
/** Modified from Murray94book version (which assumes w and v normalized?) */
|
||||
template<> Pose3 expmap(const Vector& xi) {
|
||||
Pose3 Pose3::ExpmapFull(const Vector& xi) {
|
||||
|
||||
// get angular velocity omega and translational velocity v from twist xi
|
||||
Point3 w(xi(0),xi(1),xi(2)), v(xi(3),xi(4),xi(5));
|
||||
|
|
@ -77,8 +74,9 @@ namespace gtsam {
|
|||
}
|
||||
}
|
||||
|
||||
Vector logmap(const Pose3& p) {
|
||||
Vector w = logmap(p.rotation()), T = p.translation().vector();
|
||||
/* ************************************************************************* */
|
||||
Vector Pose3::LogmapFull(const Pose3& p) {
|
||||
Vector w = Rot3::Logmap(p.rotation()), T = p.translation().vector();
|
||||
double t = norm_2(w);
|
||||
if (t < 1e-5)
|
||||
return concatVectors(2, &w, &T);
|
||||
|
|
@ -90,12 +88,27 @@ namespace gtsam {
|
|||
}
|
||||
}
|
||||
|
||||
Pose3 expmap(const Pose3& T, const Vector& d) {
|
||||
return compose(T,Pose3::Expmap(d));
|
||||
#ifdef CORRECT_POSE3_EXMAP
|
||||
/* ************************************************************************* */
|
||||
// Changes default to use the full verions of expmap/logmap
|
||||
/* ************************************************************************* */
|
||||
Pose3 Expmap(const Vector& xi) {
|
||||
return Pose3::ExpmapFull(xi);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Vector Logmap(const Pose3& p) {
|
||||
return Pose3::LogmapFull(p);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Pose3 expmap(const Vector& d) {
|
||||
return expmapFull(d);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Vector logmap(const Pose3& T1, const Pose3& T2) {
|
||||
return Pose3::logmap(between(T1,T2));
|
||||
return logmapFull(T2);
|
||||
}
|
||||
|
||||
#else
|
||||
|
|
|
|||
|
|
@ -126,6 +126,14 @@ namespace gtsam {
|
|||
/** Logarithm map around another pose T1 */
|
||||
Vector logmap(const Pose3& T2) const;
|
||||
|
||||
/** non-approximated versions of Expmap/Logmap */
|
||||
static Pose3 ExpmapFull(const Vector& xi);
|
||||
static Vector LogmapFull(const Pose3& p);
|
||||
|
||||
/** non-approximated versions of expmap/logmap */
|
||||
inline Pose3 expmapFull(const Vector& v) const { return compose(Pose3::ExpmapFull(v)); }
|
||||
inline Vector logmapFull(const Pose3& p2) const { return Pose3::LogmapFull(between(p2));}
|
||||
|
||||
/**
|
||||
* Return relative pose between p1 and p2, in p1 coordinate frame
|
||||
* as well as optionally the derivatives
|
||||
|
|
|
|||
|
|
@ -36,7 +36,6 @@ using namespace std;
|
|||
|
||||
/* ************************************************************************* */
|
||||
TEST(Pose2, constructors) {
|
||||
//cout << "constructors" << endl;
|
||||
Point2 p;
|
||||
Pose2 pose(0,p);
|
||||
Pose2 origin;
|
||||
|
|
@ -47,7 +46,6 @@ TEST(Pose2, constructors) {
|
|||
|
||||
/* ************************************************************************* */
|
||||
TEST(Pose2, manifold) {
|
||||
//cout << "manifold" << endl;
|
||||
Pose2 t1(M_PI_2, Point2(1, 2));
|
||||
Pose2 t2(M_PI_2+0.018, Point2(1.015, 2.01));
|
||||
Pose2 origin;
|
||||
|
|
@ -61,7 +59,6 @@ TEST(Pose2, manifold) {
|
|||
|
||||
/* ************************************************************************* */
|
||||
TEST(Pose2, expmap) {
|
||||
//cout << "expmap" << endl;
|
||||
Pose2 pose(M_PI_2, Point2(1, 2));
|
||||
#ifdef SLOW_BUT_CORRECT_EXPMAP
|
||||
Pose2 expected(1.00811, 2.01528, 2.5608);
|
||||
|
|
@ -72,6 +69,14 @@ TEST(Pose2, expmap) {
|
|||
EXPECT(assert_equal(expected, actual, 1e-5));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(Pose2, expmap_full) {
|
||||
Pose2 pose(M_PI_2, Point2(1, 2));
|
||||
Pose2 expected(1.00811, 2.01528, 2.5608);
|
||||
Pose2 actual = pose.expmapFull(Vector_(3, 0.01, -0.015, 0.99));
|
||||
EXPECT(assert_equal(expected, actual, 1e-5));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(Pose2, expmap2) {
|
||||
// do an actual series exponential map
|
||||
|
|
@ -90,7 +95,6 @@ TEST(Pose2, expmap2) {
|
|||
|
||||
/* ************************************************************************* */
|
||||
TEST(Pose2, expmap0) {
|
||||
//cout << "expmap0" << endl;
|
||||
Pose2 pose(M_PI_2, Point2(1, 2));
|
||||
#ifdef SLOW_BUT_CORRECT_EXPMAP
|
||||
Pose2 expected(1.01491, 2.01013, 1.5888);
|
||||
|
|
@ -101,6 +105,14 @@ TEST(Pose2, expmap0) {
|
|||
EXPECT(assert_equal(expected, actual, 1e-5));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(Pose2, expmap0_full) {
|
||||
Pose2 pose(M_PI_2, Point2(1, 2));
|
||||
Pose2 expected(1.01491, 2.01013, 1.5888);
|
||||
Pose2 actual = pose * Pose2::ExpmapFull(Vector_(3, 0.01, -0.015, 0.018));
|
||||
EXPECT(assert_equal(expected, actual, 1e-5));
|
||||
}
|
||||
|
||||
#ifdef SLOW_BUT_CORRECT_EXPMAP
|
||||
/* ************************************************************************* */
|
||||
// test case for screw motion in the plane
|
||||
|
|
@ -120,9 +132,21 @@ TEST(Pose3, expmap_c)
|
|||
}
|
||||
#endif
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(Pose3, expmap_c_full)
|
||||
{
|
||||
double w=0.3;
|
||||
Vector xi = Vector_(3, 0.0, w, w);
|
||||
Rot2 expectedR = Rot2::fromAngle(w);
|
||||
Point2 expectedT(-0.0446635, 0.29552);
|
||||
Pose2 expected(expectedR, expectedT);
|
||||
EXPECT(assert_equal(expected, expm<Pose2>(xi),1e-6));
|
||||
EXPECT(assert_equal(expected, Pose2::ExpmapFull(xi),1e-6));
|
||||
EXPECT(assert_equal(xi, Pose2::LogmapFull(expected),1e-6));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(Pose2, logmap) {
|
||||
//cout << "logmap" << endl;
|
||||
Pose2 pose0(M_PI_2, Point2(1, 2));
|
||||
Pose2 pose(M_PI_2+0.018, Point2(1.015, 2.01));
|
||||
#ifdef SLOW_BUT_CORRECT_EXPMAP
|
||||
|
|
@ -134,6 +158,15 @@ TEST(Pose2, logmap) {
|
|||
EXPECT(assert_equal(expected, actual, 1e-5));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(Pose2, logmap_full) {
|
||||
Pose2 pose0(M_PI_2, Point2(1, 2));
|
||||
Pose2 pose(M_PI_2+0.018, Point2(1.015, 2.01));
|
||||
Vector expected = Vector_(3, 0.00986473, -0.0150896, 0.018);
|
||||
Vector actual = pose0.logmapFull(pose);
|
||||
EXPECT(assert_equal(expected, actual, 1e-5));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Point2 transform_to_proxy(const Pose2& pose, const Point2& point) {
|
||||
return pose.transform_to(point);
|
||||
|
|
@ -193,7 +226,6 @@ TEST (Pose2, transform_from)
|
|||
/* ************************************************************************* */
|
||||
TEST(Pose2, compose_a)
|
||||
{
|
||||
//cout << "compose_a" << endl;
|
||||
Pose2 pose1(M_PI/4.0, Point2(sqrt(0.5), sqrt(0.5)));
|
||||
Pose2 pose2(M_PI/2.0, Point2(0.0, 2.0));
|
||||
|
||||
|
|
@ -228,7 +260,6 @@ TEST(Pose2, compose_a)
|
|||
/* ************************************************************************* */
|
||||
TEST(Pose2, compose_b)
|
||||
{
|
||||
//cout << "compose_b" << endl;
|
||||
Pose2 pose1(Rot2::fromAngle(M_PI/10.0), Point2(.75, .5));
|
||||
Pose2 pose2(Rot2::fromAngle(M_PI/4.0-M_PI/10.0), Point2(0.701289620636, 1.34933052585));
|
||||
|
||||
|
|
@ -250,7 +281,6 @@ TEST(Pose2, compose_b)
|
|||
/* ************************************************************************* */
|
||||
TEST(Pose2, compose_c)
|
||||
{
|
||||
//cout << "compose_c" << endl;
|
||||
Pose2 pose1(Rot2::fromAngle(M_PI/4.0), Point2(1.0, 1.0));
|
||||
Pose2 pose2(Rot2::fromAngle(M_PI/4.0), Point2(sqrt(.5), sqrt(.5)));
|
||||
|
||||
|
|
|
|||
|
|
@ -56,6 +56,18 @@ TEST( Pose3, expmap_a)
|
|||
CHECK(assert_equal(Pose3(R, P),id.expmap(v),1e-5));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( Pose3, expmap_a_full)
|
||||
{
|
||||
Pose3 id;
|
||||
Vector v(6);
|
||||
fill(v.begin(), v.end(), 0);
|
||||
v(0) = 0.3;
|
||||
CHECK(assert_equal(id.expmap(v), Pose3(R, Point3())));
|
||||
v(3)=0.2;v(4)=0.394742;v(5)=-2.08998;
|
||||
CHECK(assert_equal(Pose3(R, P),id.expmapFull(v),1e-5));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(Pose3, expmap_b)
|
||||
{
|
||||
|
|
@ -65,8 +77,6 @@ TEST(Pose3, expmap_b)
|
|||
CHECK(assert_equal(expected, p2));
|
||||
}
|
||||
|
||||
#ifdef CORRECT_POSE3_EXMAP
|
||||
|
||||
/* ************************************************************************* */
|
||||
// test case for screw motion in the plane
|
||||
namespace screw {
|
||||
|
|
@ -77,6 +87,8 @@ namespace screw {
|
|||
Pose3 expected(expectedR, expectedT);
|
||||
}
|
||||
|
||||
#ifdef CORRECT_POSE3_EXMAP
|
||||
/* ************************************************************************* */
|
||||
TEST(Pose3, expmap_c)
|
||||
{
|
||||
CHECK(assert_equal(screw::expected, expm<Pose3>(screw::xi),1e-6));
|
||||
|
|
@ -159,6 +171,88 @@ TEST(Pose3, Adjoint_compose)
|
|||
}
|
||||
#endif // SLOW_BUT_CORRECT_EXMAP
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(Pose3, expmap_c_full)
|
||||
{
|
||||
CHECK(assert_equal(screw::expected, expm<Pose3>(screw::xi),1e-6));
|
||||
CHECK(assert_equal(screw::expected, Pose3::ExpmapFull(screw::xi),1e-6));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// assert that T*exp(xi)*T^-1 is equal to exp(Ad_T(xi))
|
||||
TEST(Pose3, Adjoint_full)
|
||||
{
|
||||
Pose3 expected = T * Pose3::ExpmapFull(screw::xi) * T.inverse();
|
||||
Vector xiprime = T.Adjoint(screw::xi);
|
||||
CHECK(assert_equal(expected, Pose3::ExpmapFull(xiprime), 1e-6));
|
||||
|
||||
Pose3 expected2 = T2 * Pose3::ExpmapFull(screw::xi) * T2.inverse();
|
||||
Vector xiprime2 = T2.Adjoint(screw::xi);
|
||||
CHECK(assert_equal(expected2, Pose3::ExpmapFull(xiprime2), 1e-6));
|
||||
|
||||
Pose3 expected3 = T3 * Pose3::ExpmapFull(screw::xi) * T3.inverse();
|
||||
Vector xiprime3 = T3.Adjoint(screw::xi);
|
||||
CHECK(assert_equal(expected3, Pose3::ExpmapFull(xiprime3), 1e-6));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
/** Agrawal06iros version */
|
||||
using namespace boost::numeric::ublas;
|
||||
Pose3 Agrawal06iros(const Vector& xi) {
|
||||
Vector w = vector_range<const Vector>(xi, range(0,3));
|
||||
Vector v = vector_range<const Vector>(xi, range(3,6));
|
||||
double t = norm_2(w);
|
||||
if (t < 1e-5)
|
||||
return Pose3(Rot3(), Point3::Expmap(v));
|
||||
else {
|
||||
Matrix W = skewSymmetric(w/t);
|
||||
Matrix A = eye(3) + ((1 - cos(t)) / t) * W + ((t - sin(t)) / t) * (W * W);
|
||||
return Pose3(Rot3::Expmap (w), Point3::Expmap(A * v));
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(Pose3, expmaps_galore_full)
|
||||
{
|
||||
Vector xi; Pose3 actual;
|
||||
xi = Vector_(6,0.1,0.2,0.3,0.4,0.5,0.6);
|
||||
actual = Pose3::ExpmapFull(xi);
|
||||
CHECK(assert_equal(expm<Pose3>(xi), actual,1e-6));
|
||||
CHECK(assert_equal(Agrawal06iros(xi), actual,1e-6));
|
||||
CHECK(assert_equal(xi, Pose3::LogmapFull(actual),1e-6));
|
||||
|
||||
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::ExpmapFull(txi);
|
||||
CHECK(assert_equal(expm<Pose3>(txi,30), actual,1e-6));
|
||||
CHECK(assert_equal(Agrawal06iros(txi), actual,1e-6));
|
||||
Vector log = Pose3::LogmapFull(actual);
|
||||
CHECK(assert_equal(actual, Pose3::ExpmapFull(log),1e-6));
|
||||
CHECK(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::ExpmapFull(xi);
|
||||
CHECK(assert_equal(expm<Pose3>(xi,10), actual,1e-5));
|
||||
CHECK(assert_equal(Agrawal06iros(xi), actual,1e-6));
|
||||
CHECK(assert_equal(xi, Pose3::LogmapFull(actual),1e-6));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(Pose3, Adjoint_compose_full)
|
||||
{
|
||||
// To debug derivatives of compose, assert that
|
||||
// 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::ExpmapFull(x) * T2;
|
||||
Vector y = T2.inverse().Adjoint(x);
|
||||
Pose3 actual = T1 * T2 * Pose3::ExpmapFull(y);
|
||||
CHECK(assert_equal(expected, actual, 1e-6));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( Pose3, compose )
|
||||
{
|
||||
|
|
@ -510,10 +604,9 @@ TEST( Pose3, unicycle )
|
|||
{
|
||||
// velocity in X should be X in inertial frame, rather than global frame
|
||||
Vector x_step = delta(6,3,1.0);
|
||||
EXPECT(assert_equal(Pose3(Rot3::ypr(0,0,0), l1), x1.expmap(x_step), tol));
|
||||
EXPECT(assert_equal(Pose3(Rot3::ypr(0,0,0), Point3(2,1,0)), x2.expmap(x_step), tol));
|
||||
// FAILS: moves in global X, not inertial X
|
||||
// EXPECT(assert_equal(Pose3(Rot3::ypr(M_PI_4,0,0), Point3(3,2,0)), x3.expmap(sqrt(2) * x_step), tol));
|
||||
EXPECT(assert_equal(Pose3(Rot3::ypr(0,0,0), l1), x1.expmapFull(x_step), tol));
|
||||
EXPECT(assert_equal(Pose3(Rot3::ypr(0,0,0), Point3(2,1,0)), x2.expmapFull(x_step), tol));
|
||||
EXPECT(assert_equal(Pose3(Rot3::ypr(M_PI_4,0,0), Point3(2,2,0)), x3.expmapFull(sqrt(2) * x_step), tol));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
Loading…
Reference in New Issue