Pose3 lost its mojo! But there are errors in Expmap/Logmap derivatives around identity...
dellaert
parent
2624e9876d
commit
47ff09f6c8
|
|
@ -80,6 +80,22 @@ struct LieGroup {
|
|||
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 {
|
||||
return compose(Class::ChartAtOrigin::Retract(v));
|
||||
}
|
||||
|
|
|
|||
|
|
@ -23,6 +23,8 @@
|
|||
#include <iostream>
|
||||
#include <cmath>
|
||||
|
||||
#define GTSAM_POSE3_EXPMAP
|
||||
|
||||
using namespace std;
|
||||
|
||||
namespace gtsam {
|
||||
|
|
@ -36,6 +38,12 @@ Pose3::Pose3(const Pose2& pose2) :
|
|||
Point3(pose2.x(), pose2.y(), 0)) {
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Pose3 Pose3::inverse() const {
|
||||
Rot3 Rt = R_.inverse();
|
||||
return Pose3(Rt, Rt * (-t_));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// Calculate Adjoint map
|
||||
// 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) {
|
||||
if (H) CONCEPT_NOT_IMPLEMENTED;
|
||||
|
||||
if (H) *H = LogmapDerivative(p);
|
||||
Vector3 w = Rot3::Logmap(p.rotation()), T = p.translation().vector();
|
||||
double t = w.norm();
|
||||
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 {
|
||||
Vector3 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);
|
||||
Pose3 Pose3::ChartAtOrigin::Retract(const Vector6& xi, ChartJacobian H) {
|
||||
#ifdef GTSAM_POSE3_EXPMAP
|
||||
return Expmap(xi, H);
|
||||
#else
|
||||
if (H) CONCEPT_NOT_IMPLEMENTED;
|
||||
return Pose3(Rot3::Retract(xi.head<3>()), Point3(xi.tail<3>()));
|
||||
#endif
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// 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
|
||||
return compose(Expmap(xi));
|
||||
} else if (mode == Pose3::FIRST_ORDER) {
|
||||
// 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);
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// 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);
|
||||
}
|
||||
Vector6 Pose3::ChartAtOrigin::Local(const Pose3& T, ChartJacobian H) {
|
||||
#ifdef GTSAM_POSE3_EXPMAP
|
||||
return Logmap(T, H);
|
||||
#else
|
||||
if (H) CONCEPT_NOT_IMPLEMENTED;
|
||||
Vector6 xi;
|
||||
xi << Rot3::Logmap(T.rotation()), T.translation().vector();
|
||||
return xi;
|
||||
#endif
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -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));
|
||||
Matrix3 Jw = Rot3::LogmapDerivative(w);
|
||||
Matrix3 Q = computeQforExpmapDerivative(xi);
|
||||
|
|
@ -262,20 +238,6 @@ Matrix6 Pose3::LogmapDerivative(const Vector6& xi) {
|
|||
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 {
|
||||
const Matrix3 R = R_.matrix();
|
||||
|
|
@ -326,31 +288,6 @@ Point3 Pose3::transform_to(const Point3& p, OptionalJacobian<3,6> Dpose,
|
|||
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,
|
||||
OptionalJacobian<1, 3> H2) const {
|
||||
|
|
|
|||
|
|
@ -19,12 +19,6 @@
|
|||
|
||||
#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/Rot3.h>
|
||||
#include <gtsam/base/Lie.h>
|
||||
|
|
@ -39,7 +33,7 @@ class Pose2;
|
|||
* @addtogroup geometry
|
||||
* \nosubgrouping
|
||||
*/
|
||||
class GTSAM_EXPORT Pose3{
|
||||
class GTSAM_EXPORT Pose3: public LieGroup<Pose3, 6> {
|
||||
public:
|
||||
|
||||
/** Pose Concept requirements */
|
||||
|
|
@ -48,13 +42,11 @@ public:
|
|||
|
||||
private:
|
||||
|
||||
Rot3 R_; ///< Rotation gRp, between global and pose frame
|
||||
Rot3 R_; ///< Rotation gRp, between global and pose frame
|
||||
Point3 t_; ///< Translation gTp, from global origin to pose frame origin
|
||||
|
||||
public:
|
||||
|
||||
enum { dimension = 6 };
|
||||
|
||||
/// @name Standard Constructors
|
||||
/// @{
|
||||
|
||||
|
|
@ -77,7 +69,6 @@ public:
|
|||
R_(R), t_(t) {
|
||||
}
|
||||
|
||||
|
||||
/** Construct from Pose2 */
|
||||
explicit Pose3(const Pose2& pose2);
|
||||
|
||||
|
|
@ -107,251 +98,218 @@ public:
|
|||
}
|
||||
|
||||
/// inverse transformation with derivatives
|
||||
Pose3 inverse(OptionalJacobian<6, 6> H1 = boost::none) 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;
|
||||
Pose3 inverse() const;
|
||||
|
||||
/// compose syntactic sugar
|
||||
Pose3 operator*(const Pose3& T) const {
|
||||
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
|
||||
/// @name Lie Group
|
||||
/// @{
|
||||
|
||||
/** Enum to indicate which method should be used in Pose3::retract() and
|
||||
* Pose3::localCoordinates()
|
||||
/// Exponential map at identity - create a rotation from canonical coordinates \f$ [R_x,R_y,R_z,T_x,T_y,T_z] \f$
|
||||
static Pose3 Expmap(const Vector& xi, OptionalJacobian<6, 6> H = boost::none);
|
||||
|
||||
/// 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);
|
||||
|
||||
/**
|
||||
* 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)
|
||||
*/
|
||||
enum CoordinatesMode {
|
||||
EXPMAP, ///< The correct exponential map, computationally expensive.
|
||||
FIRST_ORDER ///< A fast first-order approximation to the exponential map.
|
||||
Matrix6 AdjointMap() const; /// FIXME Not tested - marked as incorrect
|
||||
|
||||
/**
|
||||
* 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$
|
||||
*/
|
||||
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
|
||||
* [ad(w,v)] = [w^, zero3; v^, w^]
|
||||
* Note that this is the matrix representation of the adjoint operator for se3 Lie algebra,
|
||||
* aka the Lie bracket, and also the derivative of Adjoint map for the Lie group SE3.
|
||||
*
|
||||
* Let \f$ \hat{\xi}_i \f$ be the se3 Lie algebra, and \f$ \hat{\xi}_i^\vee = \xi_i = [\omega_i,v_i] \in \mathbb{R}^6\f$ be its
|
||||
* vector representation.
|
||||
* We have the following relationship:
|
||||
* \f$ [\hat{\xi}_1,\hat{\xi}_2]^\vee = ad_{\xi_1}(\xi_2) = [ad_{(\omega_1,v_1)}]*\xi_2 \f$
|
||||
*
|
||||
* We use this to compute the discrete version of the inverse right-trivialized tangent map,
|
||||
* and its inverse transpose in the discrete Euler Poincare' (DEP) operator.
|
||||
*
|
||||
*/
|
||||
static Matrix6 adjointMap(const Vector6 &xi);
|
||||
|
||||
/**
|
||||
* 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);
|
||||
|
||||
/**
|
||||
* 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);
|
||||
|
||||
/// Derivative of Expmap
|
||||
static Matrix6 ExpmapDerivative(const Vector6& xi);
|
||||
|
||||
/// Derivative of Logmap
|
||||
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);
|
||||
};
|
||||
|
||||
/// 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
|
||||
/// @{
|
||||
|
||||
/// Exponential map at identity - create a rotation from canonical coordinates \f$ [R_x,R_y,R_z,T_x,T_y,T_z] \f$
|
||||
static Pose3 Expmap(const Vector& xi, OptionalJacobian<6, 6> H = boost::none);
|
||||
|
||||
/// 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);
|
||||
|
||||
/// 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
|
||||
* 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)
|
||||
*/
|
||||
Matrix6 AdjointMap() const; /// FIXME Not tested - marked as incorrect
|
||||
|
||||
/**
|
||||
* 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$
|
||||
*/
|
||||
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
|
||||
* [ad(w,v)] = [w^, zero3; v^, w^]
|
||||
* Note that this is the matrix representation of the adjoint operator for se3 Lie algebra,
|
||||
* aka the Lie bracket, and also the derivative of Adjoint map for the Lie group SE3.
|
||||
*
|
||||
* Let \f$ \hat{\xi}_i \f$ be the se3 Lie algebra, and \f$ \hat{\xi}_i^\vee = \xi_i = [\omega_i,v_i] \in \mathbb{R}^6\f$ be its
|
||||
* vector representation.
|
||||
* We have the following relationship:
|
||||
* \f$ [\hat{\xi}_1,\hat{\xi}_2]^\vee = ad_{\xi_1}(\xi_2) = [ad_{(\omega_1,v_1)}]*\xi_2 \f$
|
||||
*
|
||||
* We use this to compute the discrete version of the inverse right-trivialized tangent map,
|
||||
* and its inverse transpose in the discrete Euler Poincare' (DEP) operator.
|
||||
*
|
||||
*/
|
||||
static Matrix6 adjointMap(const Vector6 &xi);
|
||||
|
||||
/**
|
||||
* 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);
|
||||
|
||||
/**
|
||||
* 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);
|
||||
|
||||
public:
|
||||
|
||||
/// Derivative of Expmap
|
||||
static Matrix6 ExpmapDerivative(const Vector6& xi);
|
||||
|
||||
/// Derivative of Logmap
|
||||
static Matrix6 LogmapDerivative(const Vector6& xi);
|
||||
|
||||
/**
|
||||
* wedge for Pose3:
|
||||
* @param xi 6-dim twist (omega,v) where
|
||||
* omega = (wx,wy,wz) 3D angular velocity
|
||||
* v (vx,vy,vz) = 3D velocity
|
||||
* @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) {
|
||||
return (Matrix(4,4) <<
|
||||
0.,-wz, wy, vx,
|
||||
wz, 0.,-wx, vy,
|
||||
-wy, wx, 0., vz,
|
||||
0., 0., 0., 0.).finished();
|
||||
}
|
||||
|
||||
/// @}
|
||||
/// @name Group Action on Point3
|
||||
/// @{
|
||||
|
||||
/**
|
||||
* @brief takes point in Pose coordinates and transforms it to world coordinates
|
||||
* @param p point in Pose coordinates
|
||||
* @param Dpose optional 3*6 Jacobian wrpt this pose
|
||||
* @param Dpoint optional 3*3 Jacobian wrpt point
|
||||
* @return point in world coordinates
|
||||
*/
|
||||
Point3 transform_from(const Point3& p,
|
||||
OptionalJacobian<3,6> Dpose=boost::none, OptionalJacobian<3,3> Dpoint=boost::none) const;
|
||||
|
||||
/** syntactic sugar for transform_from */
|
||||
inline Point3 operator*(const Point3& p) const { return transform_from(p); }
|
||||
|
||||
/**
|
||||
* @brief takes point in world coordinates and transforms it to Pose coordinates
|
||||
* @param p point in world coordinates
|
||||
* @param Dpose optional 3*6 Jacobian wrpt this pose
|
||||
* @param Dpoint optional 3*3 Jacobian wrpt point
|
||||
* @return point in Pose coordinates
|
||||
*/
|
||||
Point3 transform_to(const Point3& p,
|
||||
OptionalJacobian<3,6> Dpose = boost::none,
|
||||
OptionalJacobian<3,3> Dpoint = boost::none) const;
|
||||
|
||||
/// @}
|
||||
/// @name Standard Interface
|
||||
/// @{
|
||||
|
||||
/// get rotation
|
||||
const Rot3& rotation() const { return R_; }
|
||||
|
||||
/// get translation
|
||||
const Point3& translation() const { return t_; }
|
||||
|
||||
/// get x
|
||||
double x() const { return t_.x(); }
|
||||
|
||||
/// get y
|
||||
double y() const { return t_.y(); }
|
||||
|
||||
/// get z
|
||||
double z() const { return t_.z(); }
|
||||
|
||||
/** convert to 4*4 matrix */
|
||||
Matrix4 matrix() const;
|
||||
|
||||
/** receives a pose in world coordinates and transforms it to local coordinates */
|
||||
Pose3 transform_to(const Pose3& pose) const;
|
||||
|
||||
/**
|
||||
* Calculate range to a landmark
|
||||
* @param point 3D location of landmark
|
||||
* @return range (double)
|
||||
*/
|
||||
double range(const Point3& point,
|
||||
OptionalJacobian<1,6> H1=boost::none,
|
||||
OptionalJacobian<1,3> H2=boost::none) const;
|
||||
|
||||
/**
|
||||
* Calculate range to another pose
|
||||
* @param pose Other SO(3) pose
|
||||
* @return range (double)
|
||||
*/
|
||||
double range(const Pose3& pose,
|
||||
OptionalJacobian<1,6> H1=boost::none,
|
||||
OptionalJacobian<1,6> H2=boost::none) const;
|
||||
|
||||
/// @}
|
||||
/// @name Advanced Interface
|
||||
/// @{
|
||||
|
||||
/**
|
||||
* Return the start and end indices (inclusive) of the translation component of the
|
||||
* exponential map parameterization
|
||||
* @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); }
|
||||
|
||||
/**
|
||||
* Return the start and end indices (inclusive) of the rotation component of the
|
||||
* exponential map parameterization
|
||||
* @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); }
|
||||
|
||||
/// Output stream operator
|
||||
GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, const Pose3& p);
|
||||
|
||||
private:
|
||||
/** Serialization function */
|
||||
friend class boost::serialization::access;
|
||||
template<class Archive>
|
||||
void serialize(Archive & ar, const unsigned int version) {
|
||||
ar & BOOST_SERIALIZATION_NVP(R_);
|
||||
ar & BOOST_SERIALIZATION_NVP(t_);
|
||||
}
|
||||
/// @}
|
||||
|
||||
};// Pose3 class
|
||||
using LieGroup<Pose3, 6>::inverse; // version with derivative
|
||||
|
||||
/**
|
||||
* wedge for Pose3:
|
||||
* @param xi 6-dim twist (omega,v) where
|
||||
* omega = 3D angular velocity
|
||||
* v = 3D velocity
|
||||
* omega = (wx,wy,wz) 3D angular velocity
|
||||
* v (vx,vy,vz) = 3D velocity
|
||||
* @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) {
|
||||
return (Matrix(4, 4) << 0., -wz, wy, vx, wz, 0., -wx, vy, -wy, wx, 0., vz, 0., 0., 0., 0.).finished();
|
||||
}
|
||||
|
||||
/// @}
|
||||
/// @name Group Action on Point3
|
||||
/// @{
|
||||
|
||||
/**
|
||||
* @brief takes point in Pose coordinates and transforms it to world coordinates
|
||||
* @param p point in Pose coordinates
|
||||
* @param Dpose optional 3*6 Jacobian wrpt this pose
|
||||
* @param Dpoint optional 3*3 Jacobian wrpt point
|
||||
* @return point in world coordinates
|
||||
*/
|
||||
Point3 transform_from(const Point3& p, OptionalJacobian<3, 6> Dpose =
|
||||
boost::none, OptionalJacobian<3, 3> Dpoint = boost::none) const;
|
||||
|
||||
/** syntactic sugar for transform_from */
|
||||
inline Point3 operator*(const Point3& p) const {
|
||||
return transform_from(p);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief takes point in world coordinates and transforms it to Pose coordinates
|
||||
* @param p point in world coordinates
|
||||
* @param Dpose optional 3*6 Jacobian wrpt this pose
|
||||
* @param Dpoint optional 3*3 Jacobian wrpt point
|
||||
* @return point in Pose coordinates
|
||||
*/
|
||||
Point3 transform_to(const Point3& p, OptionalJacobian<3, 6> Dpose =
|
||||
boost::none, OptionalJacobian<3, 3> Dpoint = boost::none) const;
|
||||
|
||||
/// @}
|
||||
/// @name Standard Interface
|
||||
/// @{
|
||||
|
||||
/// get rotation
|
||||
const Rot3& rotation() const {
|
||||
return R_;
|
||||
}
|
||||
|
||||
/// get translation
|
||||
const Point3& translation() const {
|
||||
return t_;
|
||||
}
|
||||
|
||||
/// get x
|
||||
double x() const {
|
||||
return t_.x();
|
||||
}
|
||||
|
||||
/// get y
|
||||
double y() const {
|
||||
return t_.y();
|
||||
}
|
||||
|
||||
/// get z
|
||||
double z() const {
|
||||
return t_.z();
|
||||
}
|
||||
|
||||
/** convert to 4*4 matrix */
|
||||
Matrix4 matrix() const;
|
||||
|
||||
/** receives a pose in world coordinates and transforms it to local coordinates */
|
||||
Pose3 transform_to(const Pose3& pose) const;
|
||||
|
||||
/**
|
||||
* Calculate range to a landmark
|
||||
* @param point 3D location of landmark
|
||||
* @return range (double)
|
||||
*/
|
||||
double range(const Point3& point, OptionalJacobian<1, 6> H1 = boost::none,
|
||||
OptionalJacobian<1, 3> H2 = boost::none) const;
|
||||
|
||||
/**
|
||||
* Calculate range to another pose
|
||||
* @param pose Other SO(3) pose
|
||||
* @return range (double)
|
||||
*/
|
||||
double range(const Pose3& pose, OptionalJacobian<1, 6> H1 = boost::none,
|
||||
OptionalJacobian<1, 6> H2 = boost::none) const;
|
||||
|
||||
/// @}
|
||||
/// @name Advanced Interface
|
||||
/// @{
|
||||
|
||||
/**
|
||||
* Return the start and end indices (inclusive) of the translation component of the
|
||||
* exponential map parameterization
|
||||
* @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);
|
||||
}
|
||||
|
||||
/**
|
||||
* Return the start and end indices (inclusive) of the rotation component of the
|
||||
* exponential map parameterization
|
||||
* @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);
|
||||
}
|
||||
|
||||
/// Output stream operator
|
||||
GTSAM_EXPORT
|
||||
friend std::ostream &operator<<(std::ostream &os, const Pose3& p);
|
||||
|
||||
private:
|
||||
/** Serialization function */
|
||||
friend class boost::serialization::access;
|
||||
template<class Archive>
|
||||
void serialize(Archive & ar, const unsigned int version) {
|
||||
ar & BOOST_SERIALIZATION_NVP(R_);
|
||||
ar & BOOST_SERIALIZATION_NVP(t_);
|
||||
}
|
||||
/// @}
|
||||
|
||||
};
|
||||
// Pose3 class
|
||||
|
||||
/**
|
||||
* wedge for Pose3:
|
||||
* @param xi 6-dim twist (omega,v) where
|
||||
* omega = 3D angular velocity
|
||||
* v = 3D velocity
|
||||
* @return xihat, 4*4 element of Lie algebra that can be exponentiated
|
||||
*/
|
||||
template<>
|
||||
inline Matrix wedge<Pose3>(const Vector& xi) {
|
||||
return Pose3::wedge(xi(0), xi(1), xi(2), xi(3), xi(4), xi(5));
|
||||
|
|
@ -365,6 +323,7 @@ typedef std::pair<Point3, Point3> Point3Pair;
|
|||
GTSAM_EXPORT boost::optional<Pose3> align(const std::vector<Point3Pair>& pairs);
|
||||
|
||||
template<>
|
||||
struct traits_x<Pose3> : public internal::LieGroupTraits<Pose3> {};
|
||||
struct traits_x<Pose3> : public internal::LieGroupTraits<Pose3> {
|
||||
};
|
||||
|
||||
} // namespace gtsam
|
||||
|
|
|
|||
|
|
@ -16,9 +16,8 @@
|
|||
|
||||
#include <gtsam/geometry/Pose3.h>
|
||||
#include <gtsam/geometry/Pose2.h>
|
||||
#include <gtsam/base/testLie.h>
|
||||
#include <gtsam/base/lieProxies.h>
|
||||
#include <gtsam/base/Testable.h>
|
||||
#include <gtsam/base/numericalDerivative.h>
|
||||
|
||||
#include <boost/assign/std/vector.hpp> // for operator +=
|
||||
using namespace boost::assign;
|
||||
|
|
@ -29,6 +28,8 @@ using namespace boost::assign;
|
|||
using namespace std;
|
||||
using namespace gtsam;
|
||||
|
||||
#define GTSAM_POSE3_EXPMAP
|
||||
|
||||
GTSAM_CONCEPT_TESTABLE_INST(Pose3)
|
||||
GTSAM_CONCEPT_LIE_INST(Pose3)
|
||||
|
||||
|
|
@ -57,25 +58,24 @@ TEST( Pose3, constructors)
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
#ifndef GTSAM_POSE3_EXPMAP
|
||||
TEST( Pose3, retract_first_order)
|
||||
{
|
||||
Pose3 id;
|
||||
Vector v = zero(6);
|
||||
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;
|
||||
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)
|
||||
{
|
||||
Pose3 id;
|
||||
Vector v = zero(6);
|
||||
v(0) = 0.3;
|
||||
EXPECT(assert_equal(Pose3(R, Point3()), id.retract(v, Pose3::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::EXPMAP),1e-2));
|
||||
Vector v = zero(6); v(0) = 0.3;
|
||||
Pose3 pose = Pose3::Expmap(v);
|
||||
EXPECT(assert_equal(Pose3(R, Point3()), pose, 1e-2));
|
||||
EXPECT(assert_equal(v,Pose3::Logmap(pose),1e-2));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -463,43 +463,45 @@ TEST( Pose3, transformPose_to)
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
#ifndef GTSAM_POSE3_EXPMAP
|
||||
TEST(Pose3, localCoordinates_first_order)
|
||||
{
|
||||
Vector d12 = repeat(6,0.1);
|
||||
Pose3 t1 = T, t2 = t1.retract(d12, Pose3::FIRST_ORDER);
|
||||
EXPECT(assert_equal(d12, t1.localCoordinates(t2, Pose3::FIRST_ORDER)));
|
||||
Pose3 t1 = T, t2 = t1.retract(d12);
|
||||
EXPECT(assert_equal(d12, t1.localCoordinates(t2)));
|
||||
}
|
||||
|
||||
#endif
|
||||
/* ************************************************************************* */
|
||||
TEST(Pose3, localCoordinates_expmap)
|
||||
{
|
||||
Vector d12 = repeat(6,0.1);
|
||||
Pose3 t1 = T, t2 = t1.retract(d12, Pose3::EXPMAP);
|
||||
EXPECT(assert_equal(d12, t1.localCoordinates(t2, Pose3::EXPMAP)));
|
||||
Pose3 t1 = T, t2 = t1.expmap(d12);
|
||||
EXPECT(assert_equal(d12, t1.logmap(t2)));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
#ifndef GTSAM_POSE3_EXPMAP
|
||||
TEST(Pose3, manifold_first_order)
|
||||
{
|
||||
Pose3 t1 = T;
|
||||
Pose3 t2 = T3;
|
||||
Pose3 origin;
|
||||
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)));
|
||||
Vector d12 = t1.localCoordinates(t2);
|
||||
EXPECT(assert_equal(t2, t1.retract(d12)));
|
||||
Vector d21 = t2.localCoordinates(t1);
|
||||
EXPECT(assert_equal(t1, t2.retract(d21)));
|
||||
}
|
||||
|
||||
#endif
|
||||
/* ************************************************************************* */
|
||||
TEST(Pose3, manifold_expmap)
|
||||
{
|
||||
Pose3 t1 = T;
|
||||
Pose3 t2 = T3;
|
||||
Pose3 origin;
|
||||
Vector d12 = t1.localCoordinates(t2, Pose3::EXPMAP);
|
||||
EXPECT(assert_equal(t2, t1.retract(d12, Pose3::EXPMAP)));
|
||||
Vector d21 = t2.localCoordinates(t1, Pose3::EXPMAP);
|
||||
EXPECT(assert_equal(t1, t2.retract(d21, Pose3::EXPMAP)));
|
||||
Vector d12 = t1.logmap(t2);
|
||||
EXPECT(assert_equal(t2, t1.expmap(d12)));
|
||||
Vector d21 = t2.logmap(t1);
|
||||
EXPECT(assert_equal(t1, t2.expmap(d21)));
|
||||
|
||||
// Check that log(t1,t2)=-log(t2,t1)
|
||||
EXPECT(assert_equal(d12,-d21));
|
||||
|
|
@ -672,22 +674,24 @@ TEST(Pose3, align_2) {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
/// exp(xi) exp(y) = exp(xi + dxi)
|
||||
/// Hence, y = log (exp(-xi)*exp(xi+dxi))
|
||||
Vector6 xi = (Vector(6) << 0.1, 0.2, 0.3, 4.0, 5.0, 6.0).finished();
|
||||
|
||||
Vector6 testExpmapDerivative(const Vector6& xi, const Vector6& dxi) {
|
||||
return Pose3::Logmap(Pose3::Expmap(-xi) * Pose3::Expmap(xi + dxi));
|
||||
TEST( Pose3, ExpmapDerivative1) {
|
||||
Matrix6 actualH;
|
||||
Vector6 w; w << 0.1, 0.2, 0.3, 4.0, 5.0, 6.0;
|
||||
Pose3::Expmap(w,actualH);
|
||||
Matrix6 expectedH = numericalDerivative21<Pose3, Vector6,
|
||||
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);
|
||||
Matrix expectedDexpL = numericalDerivative11<Vector6, Vector6>(
|
||||
boost::bind(testExpmapDerivative, xi, _1), zero(6), 1e-2);
|
||||
EXPECT(assert_equal(expectedDexpL, actualDexpL, 1e-5));
|
||||
|
||||
Matrix actualDexpInvL = Pose3::LogmapDerivative(xi);
|
||||
EXPECT(assert_equal(expectedDexpL.inverse(), actualDexpInvL, 1e-5));
|
||||
/* ************************************************************************* */
|
||||
TEST( Pose3, LogmapDerivative1) {
|
||||
Matrix6 actualH;
|
||||
Vector6 w; w << 0.1, 0.2, 0.3, 4.0, 5.0, 6.0;
|
||||
Pose3 p = Pose3::Expmap(w);
|
||||
EXPECT(assert_equal(w, Pose3::Logmap(p,actualH), 1e-5));
|
||||
Matrix6 expectedH = numericalDerivative21<Vector6, Pose3,
|
||||
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");
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
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);}
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
|
|
@ -7,9 +7,10 @@
|
|||
|
||||
#pragma once
|
||||
|
||||
#include <gtsam/base/numericalDerivative.h>
|
||||
#include <gtsam/geometry/Pose3.h>
|
||||
#include <gtsam/nonlinear/NonlinearFactor.h>
|
||||
#include <gtsam/geometry/Pose3.h>
|
||||
#include <gtsam/base/numericalDerivative.h>
|
||||
#include <cmath>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
|
|
@ -29,7 +30,7 @@ class Reconstruction : public NoiseModelFactor3<Pose3, Pose3, Vector6> {
|
|||
typedef NoiseModelFactor3<Pose3, Pose3, Vector6> Base;
|
||||
public:
|
||||
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) {
|
||||
}
|
||||
virtual ~Reconstruction() {}
|
||||
|
|
@ -46,7 +47,8 @@ public:
|
|||
boost::optional<Matrix&> H3 = boost::none) const {
|
||||
|
||||
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;
|
||||
Pose3 hx = gkxi.between(gk1, D_hx_gkxi, D_hx_gk1);
|
||||
|
|
@ -60,7 +62,7 @@ public:
|
|||
}
|
||||
|
||||
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;
|
||||
*H3 = D_hx_xi;
|
||||
}
|
||||
|
|
@ -92,7 +94,7 @@ public:
|
|||
DiscreteEulerPoincareHelicopter(Key xiKey1, Key xiKey_1, Key gKey,
|
||||
double h, const Matrix& Inertia, const Vector& Fu, double m,
|
||||
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) {
|
||||
}
|
||||
virtual ~DiscreteEulerPoincareHelicopter() {}
|
||||
|
|
|
|||
Loading…
Reference in New Issue