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

Pose3 lost its mojo! But there are errors in Expmap/Logmap derivatives around identity...

release/4.3a0
dellaert 2014-12-25 19:43:32 +01:00
parent 2624e9876d
commit 47ff09f6c8
5 changed files with 299 additions and 374 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
gtsam/base/Lie.h
View File

@ -80,6 +80,22 @@ struct LieGroup {
return Class::Logmap(between(g)); return Class::Logmap(between(g));
} }
static Class Retract(const TangentVector& v) {
return Class::ChartAtOrigin::Retract(v);
}
static TangentVector LocalCoordinates(const Class& g) {
return Class::ChartAtOrigin::Local(g);
}
static Class Retract(const TangentVector& v, ChartJacobian H) {
return Class::ChartAtOrigin::Retract(v,H);
}
static TangentVector LocalCoordinates(const Class& g, ChartJacobian H) {
return Class::ChartAtOrigin::Local(g,H);
}
Class retract(const TangentVector& v) const { Class retract(const TangentVector& v) const {
return compose(Class::ChartAtOrigin::Retract(v)); return compose(Class::ChartAtOrigin::Retract(v));
} }

117
gtsam/geometry/Pose3.cpp
View File

@ -23,6 +23,8 @@
#include <iostream> #include <iostream>
#include <cmath> #include <cmath>
#define GTSAM_POSE3_EXPMAP
using namespace std; using namespace std;
namespace gtsam { namespace gtsam {
@ -36,6 +38,12 @@ Pose3::Pose3(const Pose2& pose2) :
Point3(pose2.x(), pose2.y(), 0)) { Point3(pose2.x(), pose2.y(), 0)) {
} }
/* ************************************************************************* */
Pose3 Pose3::inverse() const {
Rot3 Rt = R_.inverse();
return Pose3(Rt, Rt * (-t_));
}
/* ************************************************************************* */ /* ************************************************************************* */
// Calculate Adjoint map // Calculate Adjoint map
// Ad_pose is 6*6 matrix that when applied to twist xi, returns Ad_pose(xi) // Ad_pose is 6*6 matrix that when applied to twist xi, returns Ad_pose(xi)
@ -127,8 +135,7 @@ Pose3 Pose3::Expmap(const Vector& xi, OptionalJacobian<6, 6> H) {
/* ************************************************************************* */ /* ************************************************************************* */
Vector6 Pose3::Logmap(const Pose3& p, OptionalJacobian<6, 6> H) { Vector6 Pose3::Logmap(const Pose3& p, OptionalJacobian<6, 6> H) {
if (H) CONCEPT_NOT_IMPLEMENTED; if (H) *H = LogmapDerivative(p);
Vector3 w = Rot3::Logmap(p.rotation()), T = p.translation().vector(); Vector3 w = Rot3::Logmap(p.rotation()), T = p.translation().vector();
double t = w.norm(); double t = w.norm();
if (t < 1e-10) { if (t < 1e-10) {
@ -149,57 +156,25 @@ Vector6 Pose3::Logmap(const Pose3& p, OptionalJacobian<6, 6> H) {
} }
/* ************************************************************************* */ /* ************************************************************************* */
Pose3 Pose3::retractFirstOrder(const Vector& xi) const { Pose3 Pose3::ChartAtOrigin::Retract(const Vector6& xi, ChartJacobian H) {
Vector3 omega(sub(xi, 0, 3)); #ifdef GTSAM_POSE3_EXPMAP
Point3 v(sub(xi, 3, 6)); return Expmap(xi, H);
Rot3 R = R_.retract(omega); // R is done exactly #else
Point3 t = t_ + R_ * v; // First order t approximation if (H) CONCEPT_NOT_IMPLEMENTED;
return Pose3(R, t); return Pose3(Rot3::Retract(xi.head<3>()), Point3(xi.tail<3>()));
#endif
} }
/* ************************************************************************* */ /* ************************************************************************* */
// Different versions of retract Vector6 Pose3::ChartAtOrigin::Local(const Pose3& T, ChartJacobian H) {
Pose3 Pose3::retract(const Vector& xi, Pose3::CoordinatesMode mode) const { #ifdef GTSAM_POSE3_EXPMAP
if (mode == Pose3::EXPMAP) { return Logmap(T, H);
// Lie group exponential map, traces out geodesic #else
return compose(Expmap(xi)); if (H) CONCEPT_NOT_IMPLEMENTED;
} else if (mode == Pose3::FIRST_ORDER) { Vector6 xi;
// First order xi << Rot3::Logmap(T.rotation()), T.translation().vector();
return retractFirstOrder(xi); return xi;
} else { #endif
// 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);
}
}
/* ************************************************************************* */
// different versions of localCoordinates
Vector6 Pose3::localCoordinates(const Pose3& T,
Pose3::CoordinatesMode mode) const {
if (mode == Pose3::EXPMAP) {
// Lie group logarithm map, exact inverse of exponential map
return Logmap(between(T));
} else if (mode == Pose3::FIRST_ORDER) {
// R is always done exactly in all three retract versions below
Vector3 omega = R_.localCoordinates(T.rotation());
// 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_);
// TODO: correct second order t inverse
Vector6 local;
local << omega(0), omega(1), omega(2), d.x(), d.y(), d.z();
return local;
} else {
assert(false);
exit(1);
}
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -253,7 +228,8 @@ Matrix6 Pose3::ExpmapDerivative(const Vector6& xi) {
} }
/* ************************************************************************* */ /* ************************************************************************* */
Matrix6 Pose3::LogmapDerivative(const Vector6& xi) { Matrix6 Pose3::LogmapDerivative(const Pose3& pose) {
Vector6 xi = Logmap(pose);
Vector3 w(sub(xi, 0, 3)); Vector3 w(sub(xi, 0, 3));
Matrix3 Jw = Rot3::LogmapDerivative(w); Matrix3 Jw = Rot3::LogmapDerivative(w);
Matrix3 Q = computeQforExpmapDerivative(xi); Matrix3 Q = computeQforExpmapDerivative(xi);
@ -262,20 +238,6 @@ Matrix6 Pose3::LogmapDerivative(const Vector6& xi) {
return J; return J;
} }
/* ************************************************************************* */
Pose3 Pose3::retract(const Vector& d, OptionalJacobian<6, 6> Hthis,
OptionalJacobian<6, 6> Hd, Pose3::CoordinatesMode mode) const {
if (Hthis || Hd) CONCEPT_NOT_IMPLEMENTED;
return retract(d, mode);
}
/* ************************************************************************* */
Vector6 Pose3::localCoordinates(const Pose3& T2, OptionalJacobian<6, 6> Horigin,
OptionalJacobian<6, 6> Hother, Pose3::CoordinatesMode mode) const {
if (Horigin || Hother) CONCEPT_NOT_IMPLEMENTED;
return localCoordinates(T2, mode);
}
/* ************************************************************************* */ /* ************************************************************************* */
Matrix4 Pose3::matrix() const { Matrix4 Pose3::matrix() const {
const Matrix3 R = R_.matrix(); const Matrix3 R = R_.matrix();
@ -326,31 +288,6 @@ Point3 Pose3::transform_to(const Point3& p, OptionalJacobian<3,6> Dpose,
return q; return q;
} }
/* ************************************************************************* */
Pose3 Pose3::compose(const Pose3& p2, OptionalJacobian<6,6> H1,
OptionalJacobian<6,6> H2) const {
if (H1) *H1 = p2.inverse().AdjointMap();
if (H2) *H2 = I_6x6;
return (*this) * p2;
}
/* ************************************************************************* */
Pose3 Pose3::inverse(OptionalJacobian<6,6> H1) const {
if (H1) *H1 = -AdjointMap();
Rot3 Rt = R_.inverse();
return Pose3(Rt, Rt * (-t_));
}
/* ************************************************************************* */
// between = compose(p2,inverse(p1));
Pose3 Pose3::between(const Pose3& p2, OptionalJacobian<6,6> H1,
OptionalJacobian<6,6> H2) const {
Pose3 result = inverse() * p2;
if (H1) *H1 = -result.inverse().AdjointMap();
if (H2) *H2 = I_6x6;
return result;
}
/* ************************************************************************* */ /* ************************************************************************* */
double Pose3::range(const Point3& point, OptionalJacobian<1, 6> H1, double Pose3::range(const Point3& point, OptionalJacobian<1, 6> H1,
OptionalJacobian<1, 3> H2) const { OptionalJacobian<1, 3> H2) const {

163
gtsam/geometry/Pose3.h
View File

@ -19,12 +19,6 @@
#include <gtsam/config.h> #include <gtsam/config.h>
#ifndef GTSAM_POSE3_EXPMAP
#define POSE3_DEFAULT_COORDINATES_MODE Pose3::FIRST_ORDER
#else
#define POSE3_DEFAULT_COORDINATES_MODE Pose3::EXPMAP
#endif
#include <gtsam/geometry/Point3.h> #include <gtsam/geometry/Point3.h>
#include <gtsam/geometry/Rot3.h> #include <gtsam/geometry/Rot3.h>
#include <gtsam/base/Lie.h> #include <gtsam/base/Lie.h>
@ -39,7 +33,7 @@ class Pose2;
* @addtogroup geometry * @addtogroup geometry
* \nosubgrouping * \nosubgrouping
*/ */
class GTSAM_EXPORT Pose3{ class GTSAM_EXPORT Pose3: public LieGroup<Pose3, 6> {
public: public:
/** Pose Concept requirements */ /** Pose Concept requirements */
@ -53,8 +47,6 @@ private:
public: public:
enum { dimension = 6 };
/// @name Standard Constructors /// @name Standard Constructors
/// @{ /// @{
@ -77,7 +69,6 @@ public:
R_(R), t_(t) { R_(R), t_(t) {
} }
/** Construct from Pose2 */ /** Construct from Pose2 */
explicit Pose3(const Pose2& pose2); explicit Pose3(const Pose2& pose2);
@ -107,59 +98,14 @@ public:
} }
/// inverse transformation with derivatives /// inverse transformation with derivatives
Pose3 inverse(OptionalJacobian<6, 6> H1 = boost::none) const; Pose3 inverse() const;
///compose this transformation onto another (first *this and then p2)
Pose3 compose(const Pose3& p2, OptionalJacobian<6, 6> H1 = boost::none,
OptionalJacobian<6, 6> H2 = boost::none) const;
/// compose syntactic sugar /// compose syntactic sugar
Pose3 operator*(const Pose3& T) const { Pose3 operator*(const Pose3& T) const {
return Pose3(R_ * T.R_, t_ + R_ * T.t_); return Pose3(R_ * T.R_, t_ + R_ * T.t_);
} }
/**
* Return relative pose between p1 and p2, in p1 coordinate frame
* as well as optionally the derivatives
*/
Pose3 between(const Pose3& p2, OptionalJacobian<6, 6> H1 = boost::none,
OptionalJacobian<6, 6> H2 = boost::none) const;
/// @} /// @}
/// @name Manifold
/// @{
/** Enum to indicate which method should be used in Pose3::retract() and
* Pose3::localCoordinates()
*/
enum CoordinatesMode {
EXPMAP, ///< The correct exponential map, computationally expensive.
FIRST_ORDER ///< A fast first-order approximation to the exponential map.
};
/// Dimensionality of tangent space = 6 DOF - used to autodetect sizes
static size_t Dim() {
return 6;
}
/// Dimensionality of the tangent space = 6 DOF
size_t dim() const {
return 6;
}
/// Retraction from R^6 \f$ [R_x,R_y,R_z,T_x,T_y,T_z] \f$ from R^ with fast first-order approximation to the exponential map
Pose3 retractFirstOrder(const Vector& d) const;
/// Retraction from R^6 \f$ [R_x,R_y,R_z,T_x,T_y,T_z] \f$ to Pose3 manifold neighborhood around current pose
Pose3 retract(const Vector& d, Pose3::CoordinatesMode mode =
POSE3_DEFAULT_COORDINATES_MODE) const;
/// Local 6D coordinates \f$ [R_x,R_y,R_z,T_x,T_y,T_z] \f$ of Pose3 manifold neighborhood around current pose
Vector6 localCoordinates(const Pose3& T2, Pose3::CoordinatesMode mode =
POSE3_DEFAULT_COORDINATES_MODE) const;
/// @}
/// @name Lie Group /// @name Lie Group
/// @{ /// @{
@ -169,16 +115,6 @@ public:
/// Log map at identity - return the canonical coordinates \f$ [R_x,R_y,R_z,T_x,T_y,T_z] \f$ of this rotation /// Log map at identity - return the canonical coordinates \f$ [R_x,R_y,R_z,T_x,T_y,T_z] \f$ of this rotation
static Vector6 Logmap(const Pose3& p, OptionalJacobian<6, 6> H = boost::none); static Vector6 Logmap(const Pose3& p, OptionalJacobian<6, 6> H = boost::none);
/// Local 6D coordinates \f$ [R_x,R_y,R_z,T_x,T_y,T_z] \f$ of Pose3 manifold neighborhood around current pose
Pose3 retract(const Vector& d, OptionalJacobian<6, 6> Hthis,
OptionalJacobian<6, 6> Hd = boost::none, Pose3::CoordinatesMode mode =
POSE3_DEFAULT_COORDINATES_MODE) const;
/// Local 6D coordinates \f$ [R_x,R_y,R_z,T_x,T_y,T_z] \f$ of Pose3 manifold neighborhood around current pose
Vector6 localCoordinates(const Pose3& T2, OptionalJacobian<6, 6> Horigin,
OptionalJacobian<6, 6> Hother = boost::none, Pose3::CoordinatesMode mode =
POSE3_DEFAULT_COORDINATES_MODE) const;
/** /**
* Calculate Adjoint map, transforming a twist in the this pose's (i.e, body) frame to the world spatial frame * Calculate Adjoint map, transforming a twist in the this pose's (i.e, body) frame to the world spatial frame
* Ad_pose is 6*6 matrix that when applied to twist xi \f$ [R_x,R_y,R_z,T_x,T_y,T_z] \f$, returns Ad_pose(xi) * Ad_pose is 6*6 matrix that when applied to twist xi \f$ [R_x,R_y,R_z,T_x,T_y,T_z] \f$, returns Ad_pose(xi)
@ -189,7 +125,9 @@ public:
* Apply this pose's AdjointMap Ad_g to a twist \f$ \xi_b \f$, i.e. a body-fixed velocity, transforming it to the spatial frame * Apply this pose's AdjointMap Ad_g to a twist \f$ \xi_b \f$, i.e. a body-fixed velocity, transforming it to the spatial frame
* \f$ \xi^s = g*\xi^b*g^{-1} = Ad_g * \xi^b \f$ * \f$ \xi^s = g*\xi^b*g^{-1} = Ad_g * \xi^b \f$
*/ */
Vector Adjoint(const Vector& xi_b) const {return AdjointMap()*xi_b; } /// FIXME Not tested - marked as incorrect Vector Adjoint(const Vector& xi_b) const {
return AdjointMap() * xi_b;
} /// FIXME Not tested - marked as incorrect
/** /**
* Compute the [ad(w,v)] operator as defined in [Kobilarov09siggraph], pg 11 * Compute the [ad(w,v)] operator as defined in [Kobilarov09siggraph], pg 11
@ -211,20 +149,28 @@ public:
/** /**
* Action of the adjointMap on a Lie-algebra vector y, with optional derivatives * Action of the adjointMap on a Lie-algebra vector y, with optional derivatives
*/ */
static Vector6 adjoint(const Vector6 &xi, const Vector6 &y, OptionalJacobian<6,6> = boost::none); static Vector6 adjoint(const Vector6 &xi, const Vector6 &y,
OptionalJacobian<6, 6> = boost::none);
/** /**
* The dual version of adjoint action, acting on the dual space of the Lie-algebra vector space. * The dual version of adjoint action, acting on the dual space of the Lie-algebra vector space.
*/ */
static Vector6 adjointTranspose(const Vector6& xi, const Vector6& y, OptionalJacobian<6, 6> H = boost::none); static Vector6 adjointTranspose(const Vector6& xi, const Vector6& y,
OptionalJacobian<6, 6> H = boost::none);
public:
/// Derivative of Expmap /// Derivative of Expmap
static Matrix6 ExpmapDerivative(const Vector6& xi); static Matrix6 ExpmapDerivative(const Vector6& xi);
/// Derivative of Logmap /// Derivative of Logmap
static Matrix6 LogmapDerivative(const Vector6& xi); static Matrix6 LogmapDerivative(const Pose3& xi);
// Chart at origin, depends on compile-time flag GTSAM_POSE3_EXPMAP
struct ChartAtOrigin {
static Pose3 Retract(const Vector6& v, ChartJacobian H = boost::none);
static Vector6 Local(const Pose3& r, ChartJacobian H = boost::none);
};
using LieGroup<Pose3, 6>::inverse; // version with derivative
/** /**
* wedge for Pose3: * wedge for Pose3:
@ -233,12 +179,9 @@ public:
* v (vx,vy,vz) = 3D velocity * v (vx,vy,vz) = 3D velocity
* @return xihat, 4*4 element of Lie algebra that can be exponentiated * @return xihat, 4*4 element of Lie algebra that can be exponentiated
*/ */
static Matrix wedge(double wx, double wy, double wz, double vx, double vy, double vz) { static Matrix wedge(double wx, double wy, double wz, double vx, double vy,
return (Matrix(4,4) << double vz) {
0.,-wz, wy, vx, return (Matrix(4, 4) << 0., -wz, wy, vx, wz, 0., -wx, vy, -wy, wx, 0., vz, 0., 0., 0., 0.).finished();
wz, 0.,-wx, vy,
-wy, wx, 0., vz,
0., 0., 0., 0.).finished();
} }
/// @} /// @}
@ -252,11 +195,13 @@ public:
* @param Dpoint optional 3*3 Jacobian wrpt point * @param Dpoint optional 3*3 Jacobian wrpt point
* @return point in world coordinates * @return point in world coordinates
*/ */
Point3 transform_from(const Point3& p, Point3 transform_from(const Point3& p, OptionalJacobian<3, 6> Dpose =
OptionalJacobian<3,6> Dpose=boost::none, OptionalJacobian<3,3> Dpoint=boost::none) const; boost::none, OptionalJacobian<3, 3> Dpoint = boost::none) const;
/** syntactic sugar for transform_from */ /** syntactic sugar for transform_from */
inline Point3 operator*(const Point3& p) const { return transform_from(p); } inline Point3 operator*(const Point3& p) const {
return transform_from(p);
}
/** /**
* @brief takes point in world coordinates and transforms it to Pose coordinates * @brief takes point in world coordinates and transforms it to Pose coordinates
@ -265,28 +210,37 @@ public:
* @param Dpoint optional 3*3 Jacobian wrpt point * @param Dpoint optional 3*3 Jacobian wrpt point
* @return point in Pose coordinates * @return point in Pose coordinates
*/ */
Point3 transform_to(const Point3& p, Point3 transform_to(const Point3& p, OptionalJacobian<3, 6> Dpose =
OptionalJacobian<3,6> Dpose = boost::none, boost::none, OptionalJacobian<3, 3> Dpoint = boost::none) const;
OptionalJacobian<3,3> Dpoint = boost::none) const;
/// @} /// @}
/// @name Standard Interface /// @name Standard Interface
/// @{ /// @{
/// get rotation /// get rotation
const Rot3& rotation() const { return R_; } const Rot3& rotation() const {
return R_;
}
/// get translation /// get translation
const Point3& translation() const { return t_; } const Point3& translation() const {
return t_;
}
/// get x /// get x
double x() const { return t_.x(); } double x() const {
return t_.x();
}
/// get y /// get y
double y() const { return t_.y(); } double y() const {
return t_.y();
}
/// get z /// get z
double z() const { return t_.z(); } double z() const {
return t_.z();
}
/** convert to 4*4 matrix */ /** convert to 4*4 matrix */
Matrix4 matrix() const; Matrix4 matrix() const;
@ -299,18 +253,16 @@ public:
* @param point 3D location of landmark * @param point 3D location of landmark
* @return range (double) * @return range (double)
*/ */
double range(const Point3& point, double range(const Point3& point, OptionalJacobian<1, 6> H1 = boost::none,
OptionalJacobian<1,6> H1=boost::none, OptionalJacobian<1, 3> H2 = boost::none) const;
OptionalJacobian<1,3> H2=boost::none) const;
/** /**
* Calculate range to another pose * Calculate range to another pose
* @param pose Other SO(3) pose * @param pose Other SO(3) pose
* @return range (double) * @return range (double)
*/ */
double range(const Pose3& pose, double range(const Pose3& pose, OptionalJacobian<1, 6> H1 = boost::none,
OptionalJacobian<1,6> H1=boost::none, OptionalJacobian<1, 6> H2 = boost::none) const;
OptionalJacobian<1,6> H2=boost::none) const;
/// @} /// @}
/// @name Advanced Interface /// @name Advanced Interface
@ -321,19 +273,24 @@ public:
* exponential map parameterization * exponential map parameterization
* @return a pair of [start, end] indices into the tangent space vector * @return a pair of [start, end] indices into the tangent space vector
*/ */
inline static std::pair<size_t, size_t> translationInterval() { return std::make_pair(3, 5); } inline static std::pair<size_t, size_t> translationInterval() {
return std::make_pair(3, 5);
}
/** /**
* Return the start and end indices (inclusive) of the rotation component of the * Return the start and end indices (inclusive) of the rotation component of the
* exponential map parameterization * exponential map parameterization
* @return a pair of [start, end] indices into the tangent space vector * @return a pair of [start, end] indices into the tangent space vector
*/ */
static std::pair<size_t, size_t> rotationInterval() { return std::make_pair(0, 2); } static std::pair<size_t, size_t> rotationInterval() {
return std::make_pair(0, 2);
}
/// Output stream operator /// Output stream operator
GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, const Pose3& p); GTSAM_EXPORT
friend std::ostream &operator<<(std::ostream &os, const Pose3& p);
private: private:
/** Serialization function */ /** Serialization function */
friend class boost::serialization::access; friend class boost::serialization::access;
template<class Archive> template<class Archive>
@ -343,9 +300,10 @@ public:
} }
/// @} /// @}
};// Pose3 class };
// Pose3 class
/** /**
* wedge for Pose3: * wedge for Pose3:
* @param xi 6-dim twist (omega,v) where * @param xi 6-dim twist (omega,v) where
* omega = 3D angular velocity * omega = 3D angular velocity
@ -365,6 +323,7 @@ typedef std::pair<Point3, Point3> Point3Pair;
GTSAM_EXPORT boost::optional<Pose3> align(const std::vector<Point3Pair>& pairs); GTSAM_EXPORT boost::optional<Pose3> align(const std::vector<Point3Pair>& pairs);
template<> template<>
struct traits_x<Pose3> : public internal::LieGroupTraits<Pose3> {}; struct traits_x<Pose3> : public internal::LieGroupTraits<Pose3> {
};
} // namespace gtsam } // namespace gtsam

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

@ -16,9 +16,8 @@
#include <gtsam/geometry/Pose3.h> #include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/Pose2.h> #include <gtsam/geometry/Pose2.h>
#include <gtsam/base/testLie.h>
#include <gtsam/base/lieProxies.h> #include <gtsam/base/lieProxies.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/numericalDerivative.h>
#include <boost/assign/std/vector.hpp> // for operator += #include <boost/assign/std/vector.hpp> // for operator +=
using namespace boost::assign; using namespace boost::assign;
@ -29,6 +28,8 @@ using namespace boost::assign;
using namespace std; using namespace std;
using namespace gtsam; using namespace gtsam;
#define GTSAM_POSE3_EXPMAP
GTSAM_CONCEPT_TESTABLE_INST(Pose3) GTSAM_CONCEPT_TESTABLE_INST(Pose3)
GTSAM_CONCEPT_LIE_INST(Pose3) GTSAM_CONCEPT_LIE_INST(Pose3)
@ -57,25 +58,24 @@ TEST( Pose3, constructors)
} }
/* ************************************************************************* */ /* ************************************************************************* */
#ifndef GTSAM_POSE3_EXPMAP
TEST( Pose3, retract_first_order) TEST( Pose3, retract_first_order)
{ {
Pose3 id; Pose3 id;
Vector v = zero(6); Vector v = zero(6);
v(0) = 0.3; v(0) = 0.3;
EXPECT(assert_equal(Pose3(R, Point3()), id.retract(v, Pose3::FIRST_ORDER),1e-2)); EXPECT(assert_equal(Pose3(R, Point3()), id.retract(v),1e-2));
v(3)=0.2;v(4)=0.7;v(5)=-2; v(3)=0.2;v(4)=0.7;v(5)=-2;
EXPECT(assert_equal(Pose3(R, P),id.retract(v, Pose3::FIRST_ORDER),1e-2)); EXPECT(assert_equal(Pose3(R, P),id.retract(v),1e-2));
} }
#endif
/* ************************************************************************* */ /* ************************************************************************* */
TEST( Pose3, retract_expmap) TEST( Pose3, retract_expmap)
{ {
Pose3 id; Vector v = zero(6); v(0) = 0.3;
Vector v = zero(6); Pose3 pose = Pose3::Expmap(v);
v(0) = 0.3; EXPECT(assert_equal(Pose3(R, Point3()), pose, 1e-2));
EXPECT(assert_equal(Pose3(R, Point3()), id.retract(v, Pose3::EXPMAP),1e-2)); EXPECT(assert_equal(v,Pose3::Logmap(pose),1e-2));
v(3)=0.2;v(4)=0.394742;v(5)=-2.08998;
EXPECT(assert_equal(Pose3(R, P),id.retract(v, Pose3::EXPMAP),1e-2));
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -463,43 +463,45 @@ TEST( Pose3, transformPose_to)
} }
/* ************************************************************************* */ /* ************************************************************************* */
#ifndef GTSAM_POSE3_EXPMAP
TEST(Pose3, localCoordinates_first_order) TEST(Pose3, localCoordinates_first_order)
{ {
Vector d12 = repeat(6,0.1); Vector d12 = repeat(6,0.1);
Pose3 t1 = T, t2 = t1.retract(d12, Pose3::FIRST_ORDER); Pose3 t1 = T, t2 = t1.retract(d12);
EXPECT(assert_equal(d12, t1.localCoordinates(t2, Pose3::FIRST_ORDER))); EXPECT(assert_equal(d12, t1.localCoordinates(t2)));
} }
#endif
/* ************************************************************************* */ /* ************************************************************************* */
TEST(Pose3, localCoordinates_expmap) TEST(Pose3, localCoordinates_expmap)
{ {
Vector d12 = repeat(6,0.1); Vector d12 = repeat(6,0.1);
Pose3 t1 = T, t2 = t1.retract(d12, Pose3::EXPMAP); Pose3 t1 = T, t2 = t1.expmap(d12);
EXPECT(assert_equal(d12, t1.localCoordinates(t2, Pose3::EXPMAP))); EXPECT(assert_equal(d12, t1.logmap(t2)));
} }
/* ************************************************************************* */ /* ************************************************************************* */
#ifndef GTSAM_POSE3_EXPMAP
TEST(Pose3, manifold_first_order) TEST(Pose3, manifold_first_order)
{ {
Pose3 t1 = T; Pose3 t1 = T;
Pose3 t2 = T3; Pose3 t2 = T3;
Pose3 origin; Pose3 origin;
Vector d12 = t1.localCoordinates(t2, Pose3::FIRST_ORDER); Vector d12 = t1.localCoordinates(t2);
EXPECT(assert_equal(t2, t1.retract(d12, Pose3::FIRST_ORDER))); EXPECT(assert_equal(t2, t1.retract(d12)));
Vector d21 = t2.localCoordinates(t1, Pose3::FIRST_ORDER); Vector d21 = t2.localCoordinates(t1);
EXPECT(assert_equal(t1, t2.retract(d21, Pose3::FIRST_ORDER))); EXPECT(assert_equal(t1, t2.retract(d21)));
} }
#endif
/* ************************************************************************* */ /* ************************************************************************* */
TEST(Pose3, manifold_expmap) TEST(Pose3, manifold_expmap)
{ {
Pose3 t1 = T; Pose3 t1 = T;
Pose3 t2 = T3; Pose3 t2 = T3;
Pose3 origin; Pose3 origin;
Vector d12 = t1.localCoordinates(t2, Pose3::EXPMAP); Vector d12 = t1.logmap(t2);
EXPECT(assert_equal(t2, t1.retract(d12, Pose3::EXPMAP))); EXPECT(assert_equal(t2, t1.expmap(d12)));
Vector d21 = t2.localCoordinates(t1, Pose3::EXPMAP); Vector d21 = t2.logmap(t1);
EXPECT(assert_equal(t1, t2.retract(d21, Pose3::EXPMAP))); EXPECT(assert_equal(t1, t2.expmap(d21)));
// Check that log(t1,t2)=-log(t2,t1) // Check that log(t1,t2)=-log(t2,t1)
EXPECT(assert_equal(d12,-d21)); EXPECT(assert_equal(d12,-d21));
@ -672,22 +674,24 @@ TEST(Pose3, align_2) {
} }
/* ************************************************************************* */ /* ************************************************************************* */
/// exp(xi) exp(y) = exp(xi + dxi) TEST( Pose3, ExpmapDerivative1) {
/// Hence, y = log (exp(-xi)*exp(xi+dxi)) Matrix6 actualH;
Vector6 xi = (Vector(6) << 0.1, 0.2, 0.3, 4.0, 5.0, 6.0).finished(); Vector6 w; w << 0.1, 0.2, 0.3, 4.0, 5.0, 6.0;
Pose3::Expmap(w,actualH);
Vector6 testExpmapDerivative(const Vector6& xi, const Vector6& dxi) { Matrix6 expectedH = numericalDerivative21<Pose3, Vector6,
return Pose3::Logmap(Pose3::Expmap(-xi) * Pose3::Expmap(xi + dxi)); OptionalJacobian<6, 6> >(&Pose3::Expmap, w, boost::none, 1e-2);
EXPECT(assert_equal(expectedH, actualH, 1e-5));
} }
TEST( Pose3, ExpmapDerivative) { /* ************************************************************************* */
Matrix actualDexpL = Pose3::ExpmapDerivative(xi); TEST( Pose3, LogmapDerivative1) {
Matrix expectedDexpL = numericalDerivative11<Vector6, Vector6>( Matrix6 actualH;
boost::bind(testExpmapDerivative, xi, _1), zero(6), 1e-2); Vector6 w; w << 0.1, 0.2, 0.3, 4.0, 5.0, 6.0;
EXPECT(assert_equal(expectedDexpL, actualDexpL, 1e-5)); Pose3 p = Pose3::Expmap(w);
EXPECT(assert_equal(w, Pose3::Logmap(p,actualH), 1e-5));
Matrix actualDexpInvL = Pose3::LogmapDerivative(xi); Matrix6 expectedH = numericalDerivative21<Vector6, Pose3,
EXPECT(assert_equal(expectedDexpL.inverse(), actualDexpInvL, 1e-5)); OptionalJacobian<6, 6> >(&Pose3::Logmap, p, boost::none, 1e-2);
EXPECT(assert_equal(expectedH, actualH, 1e-5));
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -737,6 +741,13 @@ TEST( Pose3, stream)
EXPECT(os.str() == "\n|1, 0, 0|\n|0, 1, 0|\n|0, 0, 1|\n\n[0, 0, 0]';\n"); EXPECT(os.str() == "\n|1, 0, 0|\n|0, 1, 0|\n|0, 0, 1|\n\n[0, 0, 0]';\n");
} }
//******************************************************************************
TEST(Pose3 , Traits) {
check_group_invariants(T2,T3);
check_manifold_invariants(T2,T3);
CHECK_LIE_GROUP_DERIVATIVES(T2,T3);
}
/* ************************************************************************* */ /* ************************************************************************* */
int main(){ TestResult tr; return TestRegistry::runAllTests(tr);} int main(){ TestResult tr; return TestRegistry::runAllTests(tr);}
/* ************************************************************************* */ /* ************************************************************************* */

14
gtsam_unstable/dynamics/SimpleHelicopter.h
View File

@ -7,9 +7,10 @@
#pragma once #pragma once
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/nonlinear/NonlinearFactor.h> #include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/base/numericalDerivative.h>
#include <cmath>
namespace gtsam { namespace gtsam {
@ -29,7 +30,7 @@ class Reconstruction : public NoiseModelFactor3<Pose3, Pose3, Vector6> {
typedef NoiseModelFactor3<Pose3, Pose3, Vector6> Base; typedef NoiseModelFactor3<Pose3, Pose3, Vector6> Base;
public: public:
Reconstruction(Key gKey1, Key gKey, Key xiKey, double h, double mu = 1000.0) : Reconstruction(Key gKey1, Key gKey, Key xiKey, double h, double mu = 1000.0) :
Base(noiseModel::Constrained::All(Pose3::Dim(), fabs(mu)), gKey1, gKey, Base(noiseModel::Constrained::All(6, std::abs(mu)), gKey1, gKey,
xiKey), h_(h) { xiKey), h_(h) {
} }
virtual ~Reconstruction() {} virtual ~Reconstruction() {}
@ -46,7 +47,8 @@ public:
boost::optional<Matrix&> H3 = boost::none) const { boost::optional<Matrix&> H3 = boost::none) const {
Matrix D_gkxi_gk, D_gkxi_exphxi; Matrix D_gkxi_gk, D_gkxi_exphxi;
Pose3 gkxi = gk.compose(Pose3::Expmap(h_*xik), D_gkxi_gk, D_gkxi_exphxi); Pose3 newPose = Pose3::Expmap(h_*xik);
Pose3 gkxi = gk.compose(newPose, D_gkxi_gk, D_gkxi_exphxi);
Matrix D_hx_gk1, D_hx_gkxi; Matrix D_hx_gk1, D_hx_gkxi;
Pose3 hx = gkxi.between(gk1, D_hx_gkxi, D_hx_gk1); Pose3 hx = gkxi.between(gk1, D_hx_gkxi, D_hx_gk1);
@ -60,7 +62,7 @@ public:
} }
if (H3) { if (H3) {
Matrix D_exphxi_xi = Pose3::LogmapDerivative(h_*xik)*h_; Matrix D_exphxi_xi = Pose3::LogmapDerivative(newPose)*h_;
Matrix D_hx_xi = D_hx_gkxi * D_gkxi_exphxi * D_exphxi_xi; Matrix D_hx_xi = D_hx_gkxi * D_gkxi_exphxi * D_exphxi_xi;
*H3 = D_hx_xi; *H3 = D_hx_xi;
} }
@ -92,7 +94,7 @@ public:
DiscreteEulerPoincareHelicopter(Key xiKey1, Key xiKey_1, Key gKey, DiscreteEulerPoincareHelicopter(Key xiKey1, Key xiKey_1, Key gKey,
double h, const Matrix& Inertia, const Vector& Fu, double m, double h, const Matrix& Inertia, const Vector& Fu, double m,
double mu = 1000.0) : double mu = 1000.0) :
Base(noiseModel::Constrained::All(Pose3::Dim(), fabs(mu)), xiKey1, xiKey_1, gKey), Base(noiseModel::Constrained::All(6, std::abs(mu)), xiKey1, xiKey_1, gKey),
h_(h), Inertia_(Inertia), Fu_(Fu), m_(m) { h_(h), Inertia_(Inertia), Fu_(Fu), m_(m) {
} }
virtual ~DiscreteEulerPoincareHelicopter() {} virtual ~DiscreteEulerPoincareHelicopter() {}