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

Merge pull request #2045 from borglab/fix/HatAndVee 

Hat and Vee
release/4.3a0
Frank Dellaert 2025-03-09 12:15:27 -04:00 committed by GitHub
parent 51d7483da6 f302f40f03
commit 6a765f2452
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
33 changed files with 651 additions and 130 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

68
gtsam/base/Lie.h
View File

@ -171,21 +171,21 @@ namespace internal {
/// Assumes existence of: identity, dimension, localCoordinates, retract,
/// and additionally Logmap, Expmap, compose, between, and inverse
template<class Class>
struct LieGroupTraits: GetDimensionImpl<Class, Class::dimension> {
typedef lie_group_tag structure_category;
struct LieGroupTraits: public GetDimensionImpl<Class, Class::dimension> {
using structure_category = lie_group_tag;
/// @name Group
/// @{
typedef multiplicative_group_tag group_flavor;
using group_flavor = multiplicative_group_tag;
static Class Identity() { return Class::Identity();}
/// @}
/// @name Manifold
/// @{
typedef Class ManifoldType;
using ManifoldType = Class;
inline constexpr static auto dimension = Class::dimension;
typedef Eigen::Matrix<double, dimension, 1> TangentVector;
typedef OptionalJacobian<dimension, dimension> ChartJacobian;
using TangentVector = Eigen::Matrix<double, dimension, 1>;
using ChartJacobian = OptionalJacobian<dimension, dimension>;
static TangentVector Local(const Class& origin, const Class& other,
ChartJacobian Horigin = {}, ChartJacobian Hother = {}) {
@ -225,10 +225,28 @@ struct LieGroupTraits: GetDimensionImpl<Class, Class::dimension> {
/// @}
};
/// Both LieGroupTraits and Testable
template<class Class> struct LieGroup: LieGroupTraits<Class>, Testable<Class> {};
} // \ namepsace internal
/// Adds LieAlgebra, Hat, and Vie to LieGroupTraits
template<class Class> struct MatrixLieGroupTraits: LieGroupTraits<Class> {
using LieAlgebra = typename Class::LieAlgebra;
using TangentVector = typename LieGroupTraits<Class>::TangentVector;
static LieAlgebra Hat(const TangentVector& v) {
return Class::Hat(v);
}
static TangentVector Vee(const LieAlgebra& X) {
return Class::Vee(X);
}
};
/// Both LieGroupTraits and Testable
template<class Class> struct MatrixLieGroup: MatrixLieGroupTraits<Class>, Testable<Class> {};
} // \ namespace internal
/**
* These core global functions can be specialized by new Lie types
@ -269,7 +287,7 @@ public:
(std::is_base_of<lie_group_tag, structure_category_tag>::value),
"This type's trait does not assert it is a Lie group (or derived)");
// group opertations with Jacobians
// group operations with Jacobians
g = traits<T>::Compose(g, h, Hg, Hh);
g = traits<T>::Between(g, h, Hg, Hh);
g = traits<T>::Inverse(g, Hg);
@ -286,6 +304,25 @@ private:
ChartJacobian Hg, Hh;
};
/**
* Matrix Lie Group Concept
*/
template<typename T>
class IsMatrixLieGroup: public IsLieGroup<T> {
public:
typedef typename traits<T>::LieAlgebra LieAlgebra;
typedef typename traits<T>::TangentVector TangentVector;
BOOST_CONCEPT_USAGE(IsMatrixLieGroup) {
// hat and vee
X = traits<T>::Hat(xi);
xi = traits<T>::Vee(X);
}
private:
LieAlgebra X;
TangentVector xi;
};
/**
* Three term approximation of the Baker-Campbell-Hausdorff formula
* In non-commutative Lie groups, when composing exp(Z) = exp(X)exp(Y)
@ -301,22 +338,21 @@ T BCH(const T& X, const T& Y) {
return T(X + Y + _2 * X_Y + _12 * bracket(X - Y, X_Y) - _24 * bracket(Y, bracket(X, X_Y)));
}
/**
* Declaration of wedge (see Murray94book) used to convert
* from n exponential coordinates to n*n element of the Lie algebra
*/
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V43
/// @deprecated: use T::Hat
template <class T> Matrix wedge(const Vector& x);
#endif
/**
* Exponential map given exponential coordinates
* class T needs a wedge<> function and a constructor from Matrix
* class T needs a constructor from Matrix.
* @param x exponential coordinates, vector of size n
* @ return a T
*/
template <class T>
T expm(const Vector& x, int K=7) {
Matrix xhat = wedge<T>(x);
return T(expm(xhat,K));
T expm(const Vector& x, int K = 7) {
auto xhat = T::Hat(x);
return T(expm(xhat, K));
}
/**

20
gtsam/base/VectorSpace.h
View File

@ -51,6 +51,8 @@ struct VectorSpaceImpl {
/// @name Lie Group
/// @{
typedef Eigen::Matrix<double, N, 1> LieAlgebra;
static TangentVector Logmap(const Class& m, ChartJacobian Hm = {}) {
if (Hm) *Hm = Jacobian::Identity();
return m.vector();
@ -80,6 +82,13 @@ struct VectorSpaceImpl {
return -v;
}
static LieAlgebra Hat(const TangentVector& v) {
return v;
}
static TangentVector Vee(const LieAlgebra& X) {
return X;
}
/// @}
};
@ -411,6 +420,8 @@ struct DynamicTraits {
/// @name Lie Group
/// @{
using LieAlgebra = Dynamic;
static TangentVector Logmap(const Dynamic& m, ChartJacobian H = {}) {
if (H) *H = Eye(m);
TangentVector result(GetDimension(m));
@ -441,6 +452,15 @@ struct DynamicTraits {
if (H2) *H2 = Eye(v1);
return v2 - v1;
}
static LieAlgebra Hat(const TangentVector& v) {
return v;
}
static TangentVector Vee(const LieAlgebra& X) {
return X;
}
/// @}
};

14
gtsam/geometry/Pose2.cpp
View File

@ -203,6 +203,20 @@ Pose2 Pose2::inverse() const {
return Pose2(r_.inverse(), r_.unrotate(Point2(-t_.x(), -t_.y())));
}
/* ************************************************************************* */
Matrix3 Pose2::Hat(const Pose2::TangentVector& xi) {
Matrix3 X;
X << 0., -xi.z(), xi.x(),
xi.z(), 0., xi.y(),
0., 0., 0.;
return X;
}
/* ************************************************************************* */
Pose2::TangentVector Pose2::Vee(const Matrix3& X) {
return TangentVector(X(0, 2), X(1, 2), X(1,0));
}
/* ************************************************************************* */
// see doc/math.lyx, SE(2) section
Point2 Pose2::transformTo(const Point2& point,

50
gtsam/geometry/Pose2.h
View File

@ -40,9 +40,12 @@ class GTSAM_EXPORT Pose2: public LieGroup<Pose2, 3> {
public:
/** Pose Concept requirements */
typedef Rot2 Rotation;
typedef Point2 Translation;
/// Pose Concept requirements
using Rotation = Rot2;
using Translation = Point2;
/// LieGroup Concept requirements
using LieAlgebra = Matrix3;
private:
@ -183,21 +186,6 @@ public:
static Matrix3 adjointMap_(const Vector3 &xi) { return adjointMap(xi);}
static Vector3 adjoint_(const Vector3 &xi, const Vector3 &y) { return adjoint(xi, y);}
/**
* wedge for SE(2):
* @param xi 3-dim twist (v,omega) where
* omega is angular velocity
* v (vx,vy) = 2D velocity
* @return xihat, 3*3 element of Lie algebra that can be exponentiated
*/
static inline Matrix3 wedge(double vx, double vy, double w) {
Matrix3 m;
m << 0.,-w, vx,
w, 0., vy,
0., 0., 0.;
return m;
}
/// Derivative of Expmap
static Matrix3 ExpmapDerivative(const Vector3& v);
@ -212,6 +200,12 @@ public:
using LieGroup<Pose2, 3>::inverse; // version with derivative
/// Hat maps from tangent vector to Lie algebra
static Matrix3 Hat(const Vector3& xi);
/// Vee maps from Lie algebra to tangent vector
static Vector3 Vee(const Matrix3& X);
/// @}
/// @name Group Action on Point2
/// @{
@ -339,6 +333,16 @@ public:
friend std::ostream &operator<<(std::ostream &os, const Pose2& p);
/// @}
/// @name deprecated
/// @{
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V43
/// @deprecated: use Hat
static inline Matrix3 wedge(double vx, double vy, double w) {
return Hat(TangentVector(vx, vy, w));
}
#endif
/// @}
private:
@ -357,22 +361,24 @@ public:
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
}; // Pose2
/** specialization for pose2 wedge function (generic template in Lie.h) */
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V43
/// @deprecated: use T::Hat
template <>
inline Matrix wedge<Pose2>(const Vector& xi) {
// NOTE(chris): Need eval() as workaround for Apple clang + avx2.
return Matrix(Pose2::wedge(xi(0),xi(1),xi(2))).eval();
return Matrix(Pose2::Hat(xi)).eval();
}
#endif
// Convenience typedef
using Pose2Pair = std::pair<Pose2, Pose2>;
using Pose2Pairs = std::vector<Pose2Pair>;
template <>
struct traits<Pose2> : public internal::LieGroup<Pose2> {};
struct traits<Pose2> : public internal::MatrixLieGroup<Pose2> {};
template <>
struct traits<const Pose2> : public internal::LieGroup<Pose2> {};
struct traits<const Pose2> : public internal::MatrixLieGroup<Pose2> {};
// bearing and range traits, used in RangeFactor
template <typename T>

15
gtsam/geometry/Pose3.cpp
View File

@ -160,6 +160,21 @@ Vector6 Pose3::adjointTranspose(const Vector6& xi, const Vector6& y,
return adT_xi * y;
}
/* ************************************************************************* */
Matrix4 Pose3::Hat(const Vector6& xi) {
Matrix4 X;
const double wx = xi(0), wy = xi(1), wz = xi(2), vx = xi(3), vy = xi(4), vz = xi(5);
X << 0., -wz, wy, vx, wz, 0., -wx, vy, -wy, wx, 0., vz, 0., 0., 0., 0.;
return X;
}
/* ************************************************************************* */
Vector6 Pose3::Vee(const Matrix4& Xi) {
Vector6 xi;
xi << Xi(2, 1), Xi(0, 2), Xi(1, 0), Xi(0, 3), Xi(1, 3), Xi(2, 3);
return xi;
}
/* ************************************************************************* */
void Pose3::print(const std::string& s) const {
std::cout << (s.empty() ? s : s + " ") << *this << std::endl;

42
gtsam/geometry/Pose3.h
View File

@ -141,6 +141,8 @@ public:
/// @name Lie Group
/// @{
using LieAlgebra = Matrix4;
/// 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 Vector6& xi, OptionalJacobian<6, 6> Hxi = {});
@ -230,16 +232,16 @@ public:
using LieGroup<Pose3, 6>::inverse; // version with derivative
/**
* wedge for Pose3:
* Hat 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();
}
static Matrix4 Hat(const Vector6& xi);
/// Vee maps from Lie algebra to tangent vector
static Vector6 Vee(const Matrix4& X);
/// @}
/// @name Group Action on Point3
@ -394,6 +396,19 @@ public:
GTSAM_EXPORT
friend std::ostream &operator<<(std::ostream &os, const Pose3& p);
/// @}
/// @name deprecated
/// @{
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V43
/// @deprecated: use Hat
static inline LieAlgebra wedge(double wx, double wy, double wz, double vx, double vy,
double vz) {
return Hat(TangentVector(wx, wy, wz, vx, vy, vz));
}
#endif
/// @}
private:
#if GTSAM_ENABLE_BOOST_SERIALIZATION
/** Serialization function */
@ -414,17 +429,14 @@ public:
};
// 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
*/
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V43
/// @deprecated: use T::Hat
template<>
inline Matrix wedge<Pose3>(const Vector& xi) {
return Pose3::wedge(xi(0), xi(1), xi(2), xi(3), xi(4), xi(5));
// NOTE(chris): Need eval() as workaround for Apple clang + avx2.
return Matrix(Pose3::Hat(xi)).eval();
}
#endif
// Convenience typedef
using Pose3Pair = std::pair<Pose3, Pose3>;
@ -434,10 +446,10 @@ using Pose3Pairs = std::vector<std::pair<Pose3, Pose3> >;
typedef std::vector<Pose3> Pose3Vector;
template <>
struct traits<Pose3> : public internal::LieGroup<Pose3> {};
struct traits<Pose3> : public internal::MatrixLieGroup<Pose3> {};
template <>
struct traits<const Pose3> : public internal::LieGroup<Pose3> {};
struct traits<const Pose3> : public internal::MatrixLieGroup<Pose3> {};
// bearing and range traits, used in RangeFactor
template <>

10
gtsam/geometry/Quaternion.h
View File

@ -138,6 +138,16 @@ struct traits<QUATERNION_TYPE> {
return omega;
}
using LieAlgebra = Matrix3;
static Matrix3 Hat(const Vector3& v) {
return SO3::Hat(v);
}
static Vector3 Vee(const Matrix3& X) {
return SO3::Vee(X);
}
/// @}
/// @name Manifold traits
/// @{

15
gtsam/geometry/Rot2.cpp
View File

@ -81,6 +81,21 @@ Vector1 Rot2::Logmap(const Rot2& r, OptionalJacobian<1, 1> H) {
v << r.theta();
return v;
}
/* ************************************************************************* */
Matrix2 Rot2::Hat(const Vector1& xi) {
Matrix2 X;
X << 0., -xi.x(),
xi.x(), 0.;
return X;
}
/* ************************************************************************* */
Vector1 Rot2::Vee(const Matrix2& X) {
TangentVector v;
v << X(1, 0);
return v;
}
/* ************************************************************************* */
Matrix2 Rot2::matrix() const {
Matrix2 rvalue_;

17
gtsam/geometry/Rot2.h
View File

@ -33,7 +33,6 @@ namespace gtsam {
* \nosubgrouping
*/
class GTSAM_EXPORT Rot2 : public LieGroup<Rot2, 1> {
/** we store cos(theta) and sin(theta) */
double c_, s_;
@ -125,6 +124,8 @@ namespace gtsam {
/// @name Lie Group
/// @{
using LieAlgebra = Matrix2;
/// Exponential map at identity - create a rotation from canonical coordinates
static Rot2 Expmap(const Vector1& v, ChartJacobian H = {});
@ -156,6 +157,12 @@ namespace gtsam {
using LieGroup<Rot2, 1>::inverse; // version with derivative
/// Hat maps from tangent vector to Lie algebra
static Matrix2 Hat(const Vector1& xi);
/// Vee maps from Lie algebra to tangent vector
static Vector1 Vee(const Matrix2& X);
/// @}
/// @name Group Action on Point2
/// @{
@ -216,7 +223,9 @@ namespace gtsam {
/** Find closest valid rotation matrix, given a 2x2 matrix */
static Rot2 ClosestTo(const Matrix2& M);
private:
/// @}
private:
#if GTSAM_ENABLE_BOOST_SERIALIZATION
/** Serialization function */
friend class boost::serialization::access;
@ -230,9 +239,9 @@ namespace gtsam {
};
template<>
struct traits<Rot2> : public internal::LieGroup<Rot2> {};
struct traits<Rot2> : public internal::MatrixLieGroup<Rot2> {};
template<>
struct traits<const Rot2> : public internal::LieGroup<Rot2> {};
struct traits<const Rot2> : public internal::MatrixLieGroup<Rot2> {};
} // gtsam

12
gtsam/geometry/Rot3.h
View File

@ -371,6 +371,8 @@ class GTSAM_EXPORT Rot3 : public LieGroup<Rot3, 3> {
/// @name Lie Group
/// @{
using LieAlgebra = Matrix3;
/**
* Exponential map at identity - create a rotation from canonical coordinates
* \f$ [R_x,R_y,R_z] \f$ using Rodrigues' formula
@ -407,6 +409,12 @@ class GTSAM_EXPORT Rot3 : public LieGroup<Rot3, 3> {
using LieGroup<Rot3, 3>::inverse; // version with derivative
/// Hat maps from tangent vector to Lie algebra
static inline Matrix3 Hat(const Vector3& xi) { return SO3::Hat(xi); }
/// Vee maps from Lie algebra to tangent vector
static inline Vector3 Vee(const Matrix3& X) { return SO3::Vee(X); }
/// @}
/// @name Group Action on Point3
/// @{
@ -581,10 +589,10 @@ class GTSAM_EXPORT Rot3 : public LieGroup<Rot3, 3> {
const Matrix3& A, OptionalJacobian<3, 9> H = {});
template<>
struct traits<Rot3> : public internal::LieGroup<Rot3> {};
struct traits<Rot3> : public internal::MatrixLieGroup<Rot3> {};
template<>
struct traits<const Rot3> : public internal::LieGroup<Rot3> {};
struct traits<const Rot3> : public internal::MatrixLieGroup<Rot3> {};
} // namespace gtsam

6
gtsam/geometry/SO3.h
View File

@ -224,13 +224,13 @@ struct GTSAM_EXPORT DexpFunctor : public ExpmapFunctor {
} // namespace so3
/*
* Define the traits. internal::LieGroup provides both Lie group and Testable
* Define the traits. internal::MatrixLieGroup provides both Lie group and Testable
*/
template <>
struct traits<SO3> : public internal::LieGroup<SO3> {};
struct traits<SO3> : public internal::MatrixLieGroup<SO3> {};
template <>
struct traits<const SO3> : public internal::LieGroup<SO3> {};
struct traits<const SO3> : public internal::MatrixLieGroup<SO3> {};
} // end namespace gtsam

6
gtsam/geometry/SO4.h
View File

@ -106,13 +106,13 @@ void serialize(Archive &ar, SO4 &Q, const unsigned int /*version*/) {
#endif
/*
* Define the traits. internal::LieGroup provides both Lie group and Testable
* Define the traits. internal::MatrixLieGroup provides both Lie group and Testable
*/
template <>
struct traits<SO4> : public internal::LieGroup<SO4> {};
struct traits<SO4> : public internal::MatrixLieGroup<SO4> {};
template <>
struct traits<const SO4> : public internal::LieGroup<SO4> {};
struct traits<const SO4> : public internal::MatrixLieGroup<SO4> {};
} // end namespace gtsam

9
gtsam/geometry/SOn.h
View File

@ -59,6 +59,9 @@ class SO : public LieGroup<SO<N>, internal::DimensionSO(N)> {
using VectorN2 = Eigen::Matrix<double, internal::NSquaredSO(N), 1>;
using MatrixDD = Eigen::Matrix<double, dimension, dimension>;
/// LieGroup Concept requirements
using LieAlgebra = MatrixNN;
GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(true)
protected:
@ -393,14 +396,14 @@ void serialize(
#endif
/*
* Define the traits. internal::LieGroup provides both Lie group and Testable
* Define the traits. internal::MatrixLieGroup provides both Lie group and Testable
*/
template <int N>
struct traits<SO<N>> : public internal::LieGroup<SO<N>> {};
struct traits<SO<N>> : public internal::MatrixLieGroup<SO<N>> {};
template <int N>
struct traits<const SO<N>> : public internal::LieGroup<SO<N>> {};
struct traits<const SO<N>> : public internal::MatrixLieGroup<SO<N>> {};
} // namespace gtsam

18
gtsam/geometry/Similarity2.cpp
View File

@ -222,6 +222,24 @@ Matrix4 Similarity2::AdjointMap() const {
throw std::runtime_error("Similarity2::AdjointMap not implemented");
}
Matrix3 Similarity2::Hat(const Vector4 &xi) {
const auto w = xi[2];
const auto u = xi.head<2>();
const double lambda = xi[3];
Matrix3 W;
W << 0, -w, u[0],
w, 0, u[1],
0, 0, -lambda;
return W;
}
Vector4 Similarity2::Vee(const Matrix3 &Xi) {
Vector4 xi;
xi[2] = Xi(1, 0);
xi.head<2>() = Xi.topRightCorner<2, 1>();
xi[3] = -Xi(2, 2);
return xi;
}
std::ostream& operator<<(std::ostream& os, const Similarity2& p) {
os << "[" << p.rotation().theta() << " " << p.translation().transpose() << " "
<< p.scale() << "]\';";

12
gtsam/geometry/Similarity2.h
View File

@ -139,6 +139,8 @@ class GTSAM_EXPORT Similarity2 : public LieGroup<Similarity2, 4> {
/// @name Lie Group
/// @{
using LieAlgebra = Matrix3;
/**
* Log map at the identity
* \f$ [t_x, t_y, \delta, \lambda] \f$
@ -167,6 +169,12 @@ class GTSAM_EXPORT Similarity2 : public LieGroup<Similarity2, 4> {
using LieGroup<Similarity2, 4>::inverse;
/// Hat maps from tangent vector to Lie algebra
static Matrix3 Hat(const Vector4& xi);
/// Vee maps from Lie algebra to tangent vector
static Vector4 Vee(const Matrix3& X);
/// @}
/// @name Standard interface
/// @{
@ -193,9 +201,9 @@ class GTSAM_EXPORT Similarity2 : public LieGroup<Similarity2, 4> {
};
template <>
struct traits<Similarity2> : public internal::LieGroup<Similarity2> {};
struct traits<Similarity2> : public internal::MatrixLieGroup<Similarity2> {};
template <>
struct traits<const Similarity2> : public internal::LieGroup<Similarity2> {};
struct traits<const Similarity2> : public internal::MatrixLieGroup<Similarity2> {};
} // namespace gtsam

10
gtsam/geometry/Similarity3.cpp
View File

@ -192,7 +192,7 @@ Similarity3 Similarity3::Align(const Pose3Pairs &abPosePairs) {
return internal::alignGivenR(abPointPairs, aRb_estimate);
}
Matrix4 Similarity3::wedge(const Vector7 &xi) {
Matrix4 Similarity3::Hat(const Vector7 &xi) {
// http://www.ethaneade.org/latex2html/lie/node29.html
const auto w = xi.head<3>();
const auto u = xi.segment<3>(3);
@ -202,6 +202,14 @@ Matrix4 Similarity3::wedge(const Vector7 &xi) {
return W;
}
Vector7 Similarity3::Vee(const Matrix4 &Xi) {
Vector7 xi;
xi.head<3>() = Rot3::Vee(Xi.topLeftCorner<3, 3>());
xi.segment<3>(3) = Xi.topRightCorner<3, 1>();
xi[6] = -Xi(3, 3);
return xi;
}
Matrix7 Similarity3::AdjointMap() const {
// http://www.ethaneade.org/latex2html/lie/node30.html
const Matrix3 R = R_.matrix();

43
gtsam/geometry/Similarity3.h
View File

@ -139,6 +139,8 @@ class GTSAM_EXPORT Similarity3 : public LieGroup<Similarity3, 7> {
/// @name Lie Group
/// @{
using LieAlgebra = Matrix4;
/** Log map at the identity
* \f$ [R_x,R_y,R_z, t_x, t_y, t_z, \lambda] \f$
*/
@ -164,17 +166,19 @@ class GTSAM_EXPORT Similarity3 : public LieGroup<Similarity3, 7> {
using LieGroup<Similarity3, 7>::inverse;
/**
* wedge for Similarity3:
* @param xi 7-dim twist (w,u,lambda) where
* @return 4*4 element of Lie algebra that can be exponentiated
* TODO(frank): rename to Hat, make part of traits
*/
static Matrix4 wedge(const Vector7& xi);
/// Project from one tangent space to another
Matrix7 AdjointMap() const;
/**
* Hat for Similarity3:
* @param xi 7-dim twist (w,u,lambda) where
* @return 4*4 element of Lie algebra that can be exponentiated
*/
static Matrix4 Hat(const Vector7& xi);
/// Vee maps from Lie algebra to tangent vector
static Vector7 Vee(const Matrix4& X);
/// @}
/// @name Standard interface
/// @{
@ -197,6 +201,17 @@ class GTSAM_EXPORT Similarity3 : public LieGroup<Similarity3, 7> {
/// Dimensionality of tangent space = 7 DOF
inline size_t dim() const { return 7; }
/// @}
/// @name Deprecated
/// @{
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V43
/// @deprecated: use Similarity3::Hat
static Matrix4 wedge(const Vector7& xi) {
return Similarity3::Hat(xi);
}
#endif
/// @}
/// @name Helper functions
/// @{
@ -220,15 +235,17 @@ class GTSAM_EXPORT Similarity3 : public LieGroup<Similarity3, 7> {
/// @}
};
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V43
/// @deprecated: use Similarity3::Hat
template <>
inline Matrix wedge<Similarity3>(const Vector& xi) {
return Similarity3::wedge(xi);
return Similarity3::Hat(xi);
}
#endif
template <>
struct traits<Similarity3> : public internal::MatrixLieGroup<Similarity3> {};
template <>
struct traits<Similarity3> : public internal::LieGroup<Similarity3> {};
template <>
struct traits<const Similarity3> : public internal::LieGroup<Similarity3> {};
struct traits<const Similarity3> : public internal::MatrixLieGroup<Similarity3> {};
} // namespace gtsam

78
gtsam/geometry/geometry.i
View File

@ -174,7 +174,10 @@ class Rot2 {
// Lie Group
static gtsam::Rot2 Expmap(gtsam::Vector v);
static gtsam::Vector Logmap(const gtsam::Rot2& p);
gtsam::Rot2 expmap(gtsam::Vector v);
gtsam::Vector logmap(const gtsam::Rot2& p);
static gtsam::Matrix Hat(const gtsam::Vector& xi);
static gtsam::Vector Vee(const gtsam::Matrix& xi);
// Group Action on Point2
gtsam::Point2 rotate(const gtsam::Point2& point) const;
@ -216,6 +219,14 @@ class SO3 {
// Operator Overloads
gtsam::SO3 operator*(const gtsam::SO3& R) const;
// Lie Group
static gtsam::SO3 Expmap(gtsam::Vector v);
static gtsam::Vector Logmap(const gtsam::SO3& p);
gtsam::SO3 expmap(gtsam::Vector v);
gtsam::Vector logmap(const gtsam::SO3& p);
static gtsam::Matrix Hat(const gtsam::Vector& xi);
static gtsam::Vector Vee(const gtsam::Matrix& xi);
// Manifold
gtsam::SO3 retract(gtsam::Vector v) const;
gtsam::Vector localCoordinates(const gtsam::SO3& R) const;
@ -246,6 +257,14 @@ class SO4 {
// Operator Overloads
gtsam::SO4 operator*(const gtsam::SO4& Q) const;
// Lie Group
static gtsam::SO4 Expmap(gtsam::Vector v);
static gtsam::Vector Logmap(const gtsam::SO4& p);
gtsam::SO4 expmap(gtsam::Vector v);
gtsam::Vector logmap(const gtsam::SO4& p);
static gtsam::Matrix Hat(const gtsam::Vector& xi);
static gtsam::Vector Vee(const gtsam::Matrix& xi);
// Manifold
gtsam::SO4 retract(gtsam::Vector v) const;
gtsam::Vector localCoordinates(const gtsam::SO4& Q) const;
@ -276,6 +295,14 @@ class SOn {
// Operator Overloads
gtsam::SOn operator*(const gtsam::SOn& Q) const;
// Lie Group
static gtsam::SOn Expmap(gtsam::Vector v);
static gtsam::Vector Logmap(const gtsam::SOn& p);
gtsam::SOn expmap(gtsam::Vector v);
gtsam::Vector logmap(const gtsam::SOn& p);
static gtsam::Matrix Hat(const gtsam::Vector& xi);
static gtsam::Vector Vee(const gtsam::Matrix& xi);
// Manifold
gtsam::SOn retract(gtsam::Vector v) const;
gtsam::Vector localCoordinates(const gtsam::SOn& Q) const;
@ -346,6 +373,14 @@ class Rot3 {
gtsam::Rot3 retract(gtsam::Vector v) const;
gtsam::Vector localCoordinates(const gtsam::Rot3& p) const;
// Lie group
static gtsam::Rot3 Expmap(gtsam::Vector v);
static gtsam::Vector Logmap(const gtsam::Rot3& p);
gtsam::Rot3 expmap(const gtsam::Vector& v);
gtsam::Vector logmap(const gtsam::Rot3& p);
static gtsam::Matrix Hat(const gtsam::Vector& xi);
static gtsam::Vector Vee(const gtsam::Matrix& xi);
// Group Action on Point3
gtsam::Point3 rotate(const gtsam::Point3& p) const;
gtsam::Point3 unrotate(const gtsam::Point3& p) const;
@ -358,10 +393,6 @@ class Rot3 {
gtsam::Unit3 unrotate(const gtsam::Unit3& p) const;
// Standard Interface
static gtsam::Rot3 Expmap(gtsam::Vector v);
static gtsam::Vector Logmap(const gtsam::Rot3& p);
gtsam::Rot3 expmap(const gtsam::Vector& v);
gtsam::Vector logmap(const gtsam::Rot3& p);
gtsam::Matrix matrix() const;
gtsam::Matrix transpose() const;
gtsam::Point3 column(size_t index) const;
@ -417,8 +448,12 @@ class Pose2 {
// Lie Group
static gtsam::Pose2 Expmap(gtsam::Vector v);
static gtsam::Vector Logmap(const gtsam::Pose2& p);
static gtsam::Pose2 Expmap(gtsam::Vector v, Eigen::Ref<Eigen::MatrixXd> H);
static gtsam::Vector Logmap(const gtsam::Pose2& p, Eigen::Ref<Eigen::MatrixXd> H);
gtsam::Pose2 expmap(gtsam::Vector v);
gtsam::Pose2 expmap(gtsam::Vector v, Eigen::Ref<Eigen::MatrixXd> H1, Eigen::Ref<Eigen::MatrixXd> H2);
gtsam::Vector logmap(const gtsam::Pose2& p);
gtsam::Vector logmap(const gtsam::Pose2& p, Eigen::Ref<Eigen::MatrixXd> H);
gtsam::Vector logmap(const gtsam::Pose2& p, Eigen::Ref<Eigen::MatrixXd> H1, Eigen::Ref<Eigen::MatrixXd> H2);
static gtsam::Matrix ExpmapDerivative(gtsam::Vector v);
static gtsam::Matrix LogmapDerivative(const gtsam::Pose2& v);
gtsam::Matrix AdjointMap() const;
@ -426,7 +461,8 @@ class Pose2 {
static gtsam::Matrix adjointMap_(gtsam::Vector v);
static gtsam::Vector adjoint_(gtsam::Vector xi, gtsam::Vector y);
static gtsam::Vector adjointTranspose(gtsam::Vector xi, gtsam::Vector y);
static gtsam::Matrix wedge(double vx, double vy, double w);
static gtsam::Matrix Hat(const gtsam::Vector& xi);
static gtsam::Vector Vee(const gtsam::Matrix& xi);
// Group Actions on Point2
gtsam::Point2 transformFrom(const gtsam::Point2& p) const;
@ -496,11 +532,13 @@ class Pose3 {
// Lie Group
static gtsam::Pose3 Expmap(gtsam::Vector v);
static gtsam::Pose3 Expmap(gtsam::Vector v, Eigen::Ref<Eigen::MatrixXd> Hxi);
static gtsam::Vector Logmap(const gtsam::Pose3& pose);
static gtsam::Vector Logmap(const gtsam::Pose3& pose, Eigen::Ref<Eigen::MatrixXd> Hpose);
static gtsam::Vector Logmap(const gtsam::Pose3& p);
static gtsam::Pose3 Expmap(gtsam::Vector v, Eigen::Ref<Eigen::MatrixXd> H);
static gtsam::Vector Logmap(const gtsam::Pose3& p, Eigen::Ref<Eigen::MatrixXd> H);
gtsam::Pose3 expmap(gtsam::Vector v);
gtsam::Vector logmap(const gtsam::Pose3& pose);
gtsam::Pose3 expmap(gtsam::Vector v, Eigen::Ref<Eigen::MatrixXd> H1, Eigen::Ref<Eigen::MatrixXd> H2);
gtsam::Vector logmap(const gtsam::Pose3& p);
gtsam::Vector logmap(const gtsam::Pose3& p, Eigen::Ref<Eigen::MatrixXd> H1, Eigen::Ref<Eigen::MatrixXd> H2);
gtsam::Matrix AdjointMap() const;
gtsam::Vector Adjoint(gtsam::Vector xi) const;
gtsam::Vector Adjoint(gtsam::Vector xi, Eigen::Ref<Eigen::MatrixXd> H_this,
@ -515,8 +553,8 @@ class Pose3 {
static gtsam::Vector adjointTranspose(gtsam::Vector xi, gtsam::Vector y);
static gtsam::Matrix ExpmapDerivative(gtsam::Vector xi);
static gtsam::Matrix LogmapDerivative(const gtsam::Pose3& xi);
static gtsam::Matrix wedge(double wx, double wy, double wz, double vx, double vy,
double vz);
static gtsam::Matrix Hat(const gtsam::Vector& xi);
static gtsam::Vector Vee(const gtsam::Matrix& xi);
// Group Action on Point3
gtsam::Point3 transformFrom(const gtsam::Point3& point) const;
@ -1147,6 +1185,14 @@ class Similarity2 {
static gtsam::Similarity2 Align(const gtsam::Point2Pairs& abPointPairs);
static gtsam::Similarity2 Align(const gtsam::Pose2Pairs& abPosePairs);
// Lie group
static gtsam::Similarity2 Expmap(gtsam::Vector v);
static gtsam::Vector Logmap(const gtsam::Similarity2& p);
gtsam::Similarity2 expmap(const gtsam::Vector& v);
gtsam::Vector logmap(const gtsam::Similarity2& p);
static gtsam::Matrix Hat(const gtsam::Vector& xi);
static gtsam::Vector Vee(const gtsam::Matrix& xi);
// Standard Interface
bool equals(const gtsam::Similarity2& sim, double tol) const;
void print(const std::string& s = "") const;
@ -1171,6 +1217,14 @@ class Similarity3 {
static gtsam::Similarity3 Align(const gtsam::Point3Pairs& abPointPairs);
static gtsam::Similarity3 Align(const gtsam::Pose3Pairs& abPosePairs);
// Lie group
static gtsam::Similarity3 Expmap(gtsam::Vector v);
static gtsam::Vector Logmap(const gtsam::Similarity3& p);
gtsam::Similarity3 expmap(const gtsam::Vector& v);
gtsam::Vector logmap(const gtsam::Similarity3& p);
static gtsam::Matrix Hat(const gtsam::Vector& xi);
static gtsam::Vector Vee(const gtsam::Matrix& xi);
// Standard Interface
bool equals(const gtsam::Similarity3& sim, double tol) const;
void print(const std::string& s = "") const;

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

@ -37,7 +37,7 @@ GTSAM_CONCEPT_LIE_INST(Pose2)
TEST(Pose2 , Concept) {
GTSAM_CONCEPT_ASSERT(IsGroup<Pose2 >);
GTSAM_CONCEPT_ASSERT(IsManifold<Pose2 >);
GTSAM_CONCEPT_ASSERT(IsLieGroup<Pose2 >);
GTSAM_CONCEPT_ASSERT(IsMatrixLieGroup<Pose2 >);
}
/* ************************************************************************* */
@ -141,6 +141,27 @@ TEST(Pose2, expmap0d) {
EXPECT(assert_equal(expected, actual, 1e-5));
}
/* ************************************************************************* */
TEST(Pose2, HatAndVee) {
// Create a few test vectors
Vector3 v1(1, 2, 3);
Vector3 v2(0.1, -0.5, 1.0);
Vector3 v3(0.0, 0.0, 0.0);
// Test that Vee(Hat(v)) == v for various inputs
EXPECT(assert_equal(v1, Pose2::Vee(Pose2::Hat(v1))));
EXPECT(assert_equal(v2, Pose2::Vee(Pose2::Hat(v2))));
EXPECT(assert_equal(v3, Pose2::Vee(Pose2::Hat(v3))));
// Check the structure of the Lie Algebra element
Matrix3 expected;
expected << 0, -3, 1,
3, 0, 2,
0, 0, 0;
EXPECT(assert_equal(expected, Pose2::Hat(v1)));
}
/* ************************************************************************* */
// test case for screw motion in the plane
namespace screwPose2 {
@ -186,17 +207,16 @@ TEST(Pose2, Adjoint_full) {
}
/* ************************************************************************* */
// assert that T*wedge(xi)*T^-1 is equal to wedge(Ad_T(xi))
// assert that T*Hat(xi)*T^-1 is equal to Hat(Ad_T(xi))
TEST(Pose2, Adjoint_hat) {
Pose2 T(1, 2, 3);
auto hat = [](const Vector& xi) { return ::wedge<Pose2>(xi); };
Matrix3 expected = T.matrix() * hat(screwPose2::xi) * T.matrix().inverse();
Matrix3 xiprime = hat(T.Adjoint(screwPose2::xi));
Matrix3 expected = T.matrix() * Pose2::Hat(screwPose2::xi) * T.matrix().inverse();
Matrix3 xiprime = Pose2::Hat(T.Adjoint(screwPose2::xi));
EXPECT(assert_equal(expected, xiprime, 1e-6));
Vector3 xi2(4, 5, 6);
Matrix3 expected2 = T.matrix() * hat(xi2) * T.matrix().inverse();
Matrix3 xiprime2 = hat(T.Adjoint(xi2));
Matrix3 expected2 = T.matrix() * Pose2::Hat(xi2) * T.matrix().inverse();
Matrix3 xiprime2 = Pose2::Hat(T.Adjoint(xi2));
EXPECT(assert_equal(expected2, xiprime2, 1e-6));
}

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

@ -41,6 +41,13 @@ static const Pose3 T2(Rot3::Rodrigues(0.3,0.2,0.1),P2);
static const Pose3 T3(Rot3::Rodrigues(-90, 0, 0), Point3(1, 2, 3));
static const double tol=1e-5;
//******************************************************************************
TEST(Pose3 , Concept) {
GTSAM_CONCEPT_ASSERT(IsGroup<Pose3 >);
GTSAM_CONCEPT_ASSERT(IsManifold<Pose3 >);
GTSAM_CONCEPT_ASSERT(IsMatrixLieGroup<Pose3 >);
}
/* ************************************************************************* */
TEST( Pose3, equals)
{
@ -221,20 +228,41 @@ TEST(Pose3, AdjointTranspose)
}
/* ************************************************************************* */
// assert that T*wedge(xi)*T^-1 is equal to wedge(Ad_T(xi))
TEST(Pose3, HatAndVee) {
// Create a few test vectors
Vector6 v1(1, 2, 3, 4, 5, 6);
Vector6 v2(0.1, -0.5, 1.0, -1.0, 0.5, 2.0);
Vector6 v3(0.0, 0.0, 0.0, 0.0, 0.0, 0.0);
// Test that Vee(Hat(v)) == v for various inputs
EXPECT(assert_equal(v1, Pose3::Vee(Pose3::Hat(v1))));
EXPECT(assert_equal(v2, Pose3::Vee(Pose3::Hat(v2))));
EXPECT(assert_equal(v3, Pose3::Vee(Pose3::Hat(v3))));
// Check the structure of the Lie Algebra element
Matrix4 expected;
expected << 0, -3, 2, 4,
3, 0, -1, 5,
-2, 1, 0, 6,
0, 0, 0, 0;
EXPECT(assert_equal(expected, Pose3::Hat(v1)));
}
/* ************************************************************************* */
// assert that T*Hat(xi)*T^-1 is equal to Hat(Ad_T(xi))
TEST(Pose3, Adjoint_hat)
{
auto hat = [](const Vector& xi) { return ::wedge<Pose3>(xi); };
Matrix4 expected = T.matrix() * hat(screwPose3::xi) * T.matrix().inverse();
Matrix4 xiprime = hat(T.Adjoint(screwPose3::xi));
Matrix4 expected = T.matrix() * Pose3::Hat(screwPose3::xi) * T.matrix().inverse();
Matrix4 xiprime = Pose3::Hat(T.Adjoint(screwPose3::xi));
EXPECT(assert_equal(expected, xiprime, 1e-6));
Matrix4 expected2 = T2.matrix() * hat(screwPose3::xi) * T2.matrix().inverse();
Matrix4 xiprime2 = hat(T2.Adjoint(screwPose3::xi));
Matrix4 expected2 = T2.matrix() * Pose3::Hat(screwPose3::xi) * T2.matrix().inverse();
Matrix4 xiprime2 = Pose3::Hat(T2.Adjoint(screwPose3::xi));
EXPECT(assert_equal(expected2, xiprime2, 1e-6));
Matrix4 expected3 = T3.matrix() * hat(screwPose3::xi) * T3.matrix().inverse();
Matrix4 xiprime3 = hat(T3.Adjoint(screwPose3::xi));
Matrix4 expected3 = T3.matrix() * Pose3::Hat(screwPose3::xi) * T3.matrix().inverse();
Matrix4 xiprime3 = Pose3::Hat(T3.Adjoint(screwPose3::xi));
EXPECT(assert_equal(expected3, xiprime3, 1e-6));
}

2
gtsam/geometry/tests/testQuaternion.cpp
View File

@ -31,7 +31,7 @@ typedef traits<Q>::ChartJacobian QuaternionJacobian;
TEST(Quaternion , Concept) {
GTSAM_CONCEPT_ASSERT(IsGroup<Quaternion >);
GTSAM_CONCEPT_ASSERT(IsManifold<Quaternion >);
GTSAM_CONCEPT_ASSERT(IsLieGroup<Quaternion >);
GTSAM_CONCEPT_ASSERT(IsMatrixLieGroup<Quaternion >);
}
//******************************************************************************

19
gtsam/geometry/tests/testRot2.cpp
View File

@ -103,6 +103,25 @@ TEST(Rot2, logmap)
CHECK(assert_equal(expected, actual));
}
/* ************************************************************************* */
TEST(Rot2, HatAndVee) {
// Create a few test vectors
Vector1 v1 = (Vector1() << 1).finished();
Vector1 v2 = (Vector1() << 0.1).finished();
Vector1 v3 = (Vector1() << 0.0).finished();
// Test that Vee(Hat(v)) == v for various inputs
EXPECT(assert_equal(v1, Rot2::Vee(Rot2::Hat(v1))));
EXPECT(assert_equal(v2, Rot2::Vee(Rot2::Hat(v2))));
EXPECT(assert_equal(v3, Rot2::Vee(Rot2::Hat(v3))));
// Check the structure of the Lie Algebra element
Matrix2 expected;
expected << 0., -1., 1., 0.;
EXPECT(assert_equal(expected, Rot2::Hat(v1)));
}
/* ************************************************************************* */
// rotate and derivatives
inline Point2 rotate_(const Rot2 & R, const Point2& p) {return R.rotate(p);}

30
gtsam/geometry/tests/testRot3.cpp
View File

@ -40,7 +40,7 @@ static double error = 1e-9, epsilon = 0.001;
TEST(Rot3 , Concept) {
GTSAM_CONCEPT_ASSERT(IsGroup<Rot3 >);
GTSAM_CONCEPT_ASSERT(IsManifold<Rot3 >);
GTSAM_CONCEPT_ASSERT(IsLieGroup<Rot3 >);
GTSAM_CONCEPT_ASSERT(IsMatrixLieGroup<Rot3 >);
}
/* ************************************************************************* */
@ -324,6 +324,34 @@ TEST(Rot3, manifold_expmap)
CHECK(assert_equal(R5,R3*R2));
}
/* ************************************************************************* */
TEST(Rot3, HatAndVee) {
// Create a few test vectors
Vector3 v1(1, 2, 3);
Vector3 v2(0.1, -0.5, 1.0);
Vector3 v3(0.0, 0.0, 0.0);
// Test that Vee(Hat(v)) == v for various inputs
EXPECT(assert_equal(v1, Rot3::Vee(Rot3::Hat(v1))));
EXPECT(assert_equal(v2, Rot3::Vee(Rot3::Hat(v2))));
EXPECT(assert_equal(v3, Rot3::Vee(Rot3::Hat(v3))));
// Check the structure of the Lie Algebra element
Matrix3 expected;
expected << 0, -3, 2,
3, 0, -1,
-2, 1, 0;
EXPECT(assert_equal(expected, Rot3::Hat(v1)));
}
/* ************************************************************************* */
// Checks correct exponential map (Expmap) with brute force matrix exponential
TEST(Rot3, BruteForceExpmap) {
const Vector3 xi(0.1, 0.2, 0.3);
EXPECT(assert_equal(Rot3::Expmap(xi), expm<Rot3>(xi), 1e-6));
}
/* ************************************************************************* */
class AngularVelocity : public Vector3 {
public:

2
gtsam/geometry/tests/testSO3.cpp
View File

@ -38,7 +38,7 @@ TEST(SO3, Identity) {
TEST(SO3, Concept) {
GTSAM_CONCEPT_ASSERT(IsGroup<SO3>);
GTSAM_CONCEPT_ASSERT(IsManifold<SO3>);
GTSAM_CONCEPT_ASSERT(IsLieGroup<SO3>);
GTSAM_CONCEPT_ASSERT(IsMatrixLieGroup<SO3>);
}
//******************************************************************************

2
gtsam/geometry/tests/testSO4.cpp
View File

@ -44,7 +44,7 @@ TEST(SO4, Identity) {
TEST(SO4, Concept) {
GTSAM_CONCEPT_ASSERT(IsGroup<SO4>);
GTSAM_CONCEPT_ASSERT(IsManifold<SO4>);
GTSAM_CONCEPT_ASSERT(IsLieGroup<SO4>);
GTSAM_CONCEPT_ASSERT(IsMatrixLieGroup<SO4>);
}
//******************************************************************************

2
gtsam/geometry/tests/testSOn.cpp
View File

@ -86,7 +86,7 @@ TEST(SOn, SO5) {
TEST(SOn, Concept) {
GTSAM_CONCEPT_ASSERT(IsGroup<SOn>);
GTSAM_CONCEPT_ASSERT(IsManifold<SOn>);
GTSAM_CONCEPT_ASSERT(IsLieGroup<SOn>);
GTSAM_CONCEPT_ASSERT(IsMatrixLieGroup<SOn>);
}
//******************************************************************************

30
gtsam/geometry/tests/testSimilarity2.cpp
View File

@ -37,7 +37,7 @@ static const double s = 4;
TEST(Similarity2, Concepts) {
GTSAM_CONCEPT_ASSERT(IsGroup<Similarity2>);
GTSAM_CONCEPT_ASSERT(IsManifold<Similarity2>);
GTSAM_CONCEPT_ASSERT(IsLieGroup<Similarity2>);
GTSAM_CONCEPT_ASSERT(IsMatrixLieGroup<Similarity2>);
}
//******************************************************************************
@ -56,6 +56,34 @@ TEST(Similarity2, Getters) {
EXPECT_DOUBLES_EQUAL(1.0, sim2_default.scale(), 1e-9);
}
/* ************************************************************************* */
TEST(Similarity2, HatAndVee) {
// Create a few test vectors
Vector4 v1(1, 2, 3, 4);
Vector4 v2(0.1, -0.5, 1.0, -1.0);
Vector4 v3(0.0, 0.0, 0.0, 0.0);
// Test that Vee(Hat(v)) == v for various inputs
EXPECT(assert_equal(v1, Similarity2::Vee(Similarity2::Hat(v1))));
EXPECT(assert_equal(v2, Similarity2::Vee(Similarity2::Hat(v2))));
EXPECT(assert_equal(v3, Similarity2::Vee(Similarity2::Hat(v3))));
// Check the structure of the Lie Algebra element
Matrix3 expected;
expected << 0, -3, 1,
3, 0, 2,
0, 0, -4;
EXPECT(assert_equal(expected, Similarity2::Hat(v1)));
}
/* ************************************************************************* */
// Checks correct exponential map (Expmap) with brute force matrix exponential
TEST_DISABLED(Similarity2, BruteForceExpmap) {
const Vector4 xi(0.1, 0.2, 0.3, 0.4);
EXPECT(assert_equal(Similarity2::Expmap(xi), expm<Similarity2>(xi), 1e-4));
}
//******************************************************************************
int main() {
TestResult tr;

39
gtsam/geometry/tests/testSimilarity3.cpp
View File

@ -56,7 +56,7 @@ const double degree = M_PI / 180;
TEST(Similarity3, Concepts) {
GTSAM_CONCEPT_ASSERT(IsGroup<Similarity3 >);
GTSAM_CONCEPT_ASSERT(IsManifold<Similarity3 >);
GTSAM_CONCEPT_ASSERT(IsLieGroup<Similarity3 >);
GTSAM_CONCEPT_ASSERT(IsMatrixLieGroup<Similarity3 >);
}
//******************************************************************************
@ -80,14 +80,43 @@ TEST(Similarity3, Getters) {
EXPECT_DOUBLES_EQUAL(7.0, sim3.scale(), 1e-9);
}
/* ************************************************************************* */
TEST(Similarity3, HatAndVee) {
// Create a few test vectors
Vector7 v1(1, 2, 3, 4, 5, 6, 7);
Vector7 v2(0.1, -0.5, 1.0, -1.0, 0.5, 2.0, -0.3);
Vector7 v3(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0);
// Test that Vee(Hat(v)) == v for various inputs
EXPECT(assert_equal(v1, Similarity3::Vee(Similarity3::Hat(v1))));
EXPECT(assert_equal(v2, Similarity3::Vee(Similarity3::Hat(v2))));
EXPECT(assert_equal(v3, Similarity3::Vee(Similarity3::Hat(v3))));
// Check the structure of the Lie Algebra element
Matrix4 expected;
expected << 0, -3, 2, 4,
3, 0, -1, 5,
-2, 1, 0, 6,
0, 0, 0, -7;
EXPECT(assert_equal(expected, Similarity3::Hat(v1)));
}
/* ************************************************************************* */
// Checks correct exponential map (Expmap) with brute force matrix exponential
TEST(Similarity3, BruteForceExpmap) {
const Vector7 xi(0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7);
EXPECT(assert_equal(Similarity3::Expmap(xi), expm<Similarity3>(xi), 1e-4));
}
//******************************************************************************
TEST(Similarity3, AdjointMap) {
const Matrix4 T = T2.matrix();
// Check Ad with actual definition
Vector7 delta;
delta << 0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7;
Matrix4 W = Similarity3::wedge(delta);
Matrix4 TW = Similarity3::wedge(T2.AdjointMap() * delta);
Matrix4 W = Similarity3::Hat(delta);
Matrix4 TW = Similarity3::Hat(T2.AdjointMap() * delta);
EXPECT(assert_equal(TW, Matrix4(T * W * T.inverse()), 1e-9));
}
@ -209,10 +238,6 @@ TEST(Similarity3, ExpLogMap) {
Similarity3 expZero = Similarity3::Expmap(zeros);
Similarity3 ident = Similarity3::Identity();
EXPECT(assert_equal(expZero, ident));
// Compare to matrix exponential, using expm in Lie.h
EXPECT(
assert_equal(expm<Similarity3>(delta), Similarity3::Expmap(delta), 1e-3));
}
//******************************************************************************

23
gtsam/navigation/NavState.cpp
View File

@ -270,6 +270,29 @@ Matrix9 NavState::LogmapDerivative(const NavState& state) {
return J;
}
//------------------------------------------------------------------------------
Matrix5 NavState::Hat(const Vector9& xi) {
Matrix5 X;
const double wx = xi(0), wy = xi(1), wz = xi(2);
const double px = xi(3), py = xi(4), pz = xi(5);
const double vx = xi(6), vy = xi(7), vz = xi(8);
X << 0., -wz, wy, px, vx,
wz, 0., -wx, py, vy,
-wy, wx, 0., pz, vz,
0., 0., 0., 0., 0.,
0., 0., 0., 0., 0.;
return X;
}
//------------------------------------------------------------------------------
Vector9 NavState::Vee(const Matrix5& Xi) {
Vector9 xi;
xi << Xi(2, 1), Xi(0, 2), Xi(1, 0),
Xi(0, 3), Xi(1, 3), Xi(2, 3),
Xi(0, 4), Xi(1, 4), Xi(2, 4);
return xi;
}
//------------------------------------------------------------------------------
NavState NavState::ChartAtOrigin::Retract(const Vector9& xi,
ChartJacobian Hxi) {

29
gtsam/navigation/NavState.h
View File

@ -56,18 +56,27 @@ public:
NavState() :
t_(0, 0, 0), v_(Vector3::Zero()) {
}
/// Construct from attitude, position, velocity
NavState(const Rot3& R, const Point3& t, const Velocity3& v) :
R_(R), t_(t), v_(v) {
}
/// Construct from pose and velocity
NavState(const Pose3& pose, const Velocity3& v) :
R_(pose.rotation()), t_(pose.translation()), v_(v) {
}
/// Construct from SO(3) and R^6
NavState(const Matrix3& R, const Vector6& tv) :
R_(R), t_(tv.head<3>()), v_(tv.tail<3>()) {
}
/// Construct from Matrix5
NavState(const Matrix5& T) :
R_(T.block<3, 3>(0, 0)), t_(T.block<3, 1>(0, 3)), v_(T.block<3, 1>(0, 4)) {
}
/// Named constructor with derivatives
static NavState Create(const Rot3& R, const Point3& t, const Velocity3& v,
OptionalJacobian<9, 3> H1 = {},
@ -154,10 +163,6 @@ public:
/// Syntactic sugar
const Rot3& rotation() const { return attitude(); };
/// @}
/// @name Lie Group
/// @{
// Tangent space sugar.
// TODO(frank): move to private navstate namespace in cpp
static Eigen::Block<Vector9, 3, 1> dR(Vector9& v) {
@ -189,6 +194,12 @@ public:
OptionalJacobian<9, 9> H1 = {}, OptionalJacobian<9, 9> H2 =
{}) const;
/// @}
/// @name Lie Group
/// @{
using LieAlgebra = Matrix5;
/**
* Exponential map at identity - create a NavState from canonical coordinates
* \f$ [R_x,R_y,R_z,T_x,T_y,T_z,V_x,V_y,V_z] \f$
@ -242,6 +253,12 @@ public:
static Vector9 Local(const NavState& state, ChartJacobian Hstate = {});
};
/// Hat maps from tangent vector to Lie algebra
static Matrix5 Hat(const Vector9& xi);
/// Vee maps from Lie algebra to tangent vector
static Vector9 Vee(const Matrix5& X);
/// @}
/// @name Dynamics
/// @{
@ -283,9 +300,9 @@ private:
// Specialize NavState traits to use a Retract/Local that agrees with IMUFactors
template <>
struct traits<NavState> : public internal::LieGroup<NavState> {};
struct traits<NavState> : public internal::MatrixLieGroup<NavState> {};
template <>
struct traits<const NavState> : public internal::LieGroup<NavState> {};
struct traits<const NavState> : public internal::MatrixLieGroup<NavState> {};
} // namespace gtsam

15
gtsam/navigation/navigation.i
View File

@ -54,9 +54,24 @@ class NavState {
gtsam::Vector velocity() const;
gtsam::Pose3 pose() const;
// Manifold
gtsam::NavState retract(const gtsam::Vector& x) const;
gtsam::Vector localCoordinates(const gtsam::NavState& g) const;
// Lie Group
static gtsam::NavState Expmap(gtsam::Vector v);
static gtsam::Vector Logmap(const gtsam::NavState& p);
static gtsam::NavState Expmap(gtsam::Vector v, Eigen::Ref<Eigen::MatrixXd> H);
static gtsam::Vector Logmap(const gtsam::NavState& p, Eigen::Ref<Eigen::MatrixXd> H);
gtsam::NavState expmap(gtsam::Vector v);
gtsam::NavState expmap(gtsam::Vector v, Eigen::Ref<Eigen::MatrixXd> H1, Eigen::Ref<Eigen::MatrixXd> H2);
gtsam::Vector logmap(const gtsam::NavState& p);
gtsam::Vector logmap(const gtsam::NavState& p, Eigen::Ref<Eigen::MatrixXd> H1, Eigen::Ref<Eigen::MatrixXd> H2);
gtsam::Matrix AdjointMap() const;
gtsam::Vector Adjoint(gtsam::Vector xi) const;
static gtsam::Matrix Hat(const gtsam::Vector& xi);
static gtsam::Vector Vee(const gtsam::Matrix& xi);
// enabling serialization functionality
void serialize() const;
};

64
gtsam/navigation/tests/testNavState.cpp
View File

@ -52,6 +52,13 @@ static const NavState T2(Rot3::Rodrigues(0.3, 0.2, 0.1), P2, V2);
static const NavState T3(Rot3::Rodrigues(-90, 0, 0), Point3(5, 6, 7),
Point3(1, 2, 3));
//******************************************************************************
TEST(NavState, Concept) {
GTSAM_CONCEPT_ASSERT(IsGroup<NavState >);
GTSAM_CONCEPT_ASSERT(IsManifold<NavState >);
GTSAM_CONCEPT_ASSERT(IsMatrixLieGroup<NavState >);
}
/* ************************************************************************* */
TEST(NavState, Constructor) {
std::function<NavState(const Rot3&, const Point3&, const Vector3&)> create =
@ -533,6 +540,63 @@ TEST(NavState, Adjoint_compose_full) {
EXPECT(assert_equal(expected, actual, 1e-6));
}
/* ************************************************************************* */
TEST(NavState, HatAndVee) {
// Create a few test vectors
Vector9 v1(1, 2, 3, 4, 5, 6, 7, 8, 9);
Vector9 v2(0.1, -0.5, 1.0, -1.0, 0.5, 2.0, 0.3, -0.2, 0.8);
Vector9 v3 = Vector9::Zero();
// Test that Vee(Hat(v)) == v for various inputs
EXPECT(assert_equal(v1, NavState::Vee(NavState::Hat(v1))));
EXPECT(assert_equal(v2, NavState::Vee(NavState::Hat(v2))));
EXPECT(assert_equal(v3, NavState::Vee(NavState::Hat(v3))));
// Check the structure of the Lie Algebra element
Matrix5 expected;
expected << 0, -3, 2, 4, 7,
3, 0, -1, 5, 8,
-2, 1, 0, 6, 9,
0, 0, 0, 0, 0,
0, 0, 0, 0, 0;
EXPECT(assert_equal(expected, NavState::Hat(v1)));
}
/* ************************************************************************* */
// Checks correct exponential map (Expmap) with brute force matrix exponential
TEST(NavState, BruteForceExpmap1) {
const Vector9 xi(0, 0, 0, 14, 24, 34, 15, 25, 35);
EXPECT(assert_equal(NavState::Expmap(xi), expm<NavState>(xi), 1e-6));
}
TEST(NavState, BruteForceExpmap2) {
const Vector9 xi(0.1, 0.2, 0.3, 0, 0, 0, 0, 0, 0);
EXPECT(assert_equal(NavState::Expmap(xi), expm<NavState>(xi), 1e-6));
}
TEST(NavState, BruteForceExpmap3) {
const Vector9 xi(0.1, 0.2, 0.3, 4, 5, 6, 7, 8, 9);
EXPECT(assert_equal(NavState::Expmap(xi), expm<NavState>(xi), 1e-6));
}
/* ************************************************************************* */
// assert that T*Hat(xi)*T^-1 is equal to Hat(Ad_T(xi))
TEST(NavState, Adjoint_hat)
{
Matrix5 expected = T.matrix() * NavState::Hat(screwNavState::xi) * T.matrix().inverse();
Matrix5 xiprime = NavState::Hat(T.Adjoint(screwNavState::xi));
EXPECT(assert_equal(expected, xiprime, 1e-6));
Matrix5 expected2 = T2.matrix() * NavState::Hat(screwNavState::xi) * T2.matrix().inverse();
Matrix5 xiprime2 = NavState::Hat(T2.Adjoint(screwNavState::xi));
EXPECT(assert_equal(expected2, xiprime2, 1e-6));
Matrix5 expected3 = T3.matrix() * NavState::Hat(screwNavState::xi) * T3.matrix().inverse();
Matrix5 xiprime3 = NavState::Hat(T3.Adjoint(screwNavState::xi));
EXPECT(assert_equal(expected3, xiprime3, 1e-6));
}
/* ************************************************************************* */
TEST(NavState, Retract_LocalCoordinates) {
Vector9 d;

1
python/gtsam/tests/test_DiscreteBayesTree.py
View File

@ -98,6 +98,7 @@ class TestDiscreteBayesNet(GtsamTestCase):
self.assertFalse(bayesTree.empty())
self.assertEqual(12, bayesTree.size())
@unittest.skip("Too Slow")
def test_discrete_bayes_tree_lookup(self):
"""Check that we can have a multi-frontal lookup table."""
# Make a small planning-like graph: 3 states, 2 actions