Hat and Vee in geometry
Frank Dellaert
parent
82ec8c03c0
commit
e0c3be9ab7
|
|
@ -203,6 +203,20 @@ Pose2 Pose2::inverse() const {
|
|||
return Pose2(r_.inverse(), r_.unrotate(Point2(-t_.x(), -t_.y())));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Pose2::LieAlgebra Pose2::Hat(const Pose2::TangentVector& xi) {
|
||||
LieAlgebra X;
|
||||
X << 0., -xi.z(), xi.x(),
|
||||
xi.z(), 0., xi.y(),
|
||||
0., 0., 0.;
|
||||
return X;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Pose2::TangentVector Pose2::Vee(const Pose2::LieAlgebra& 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,
|
||||
|
|
|
|||
|
|
@ -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 LieAlgebra Hat(const TangentVector& xi);
|
||||
|
||||
/// Vee maps from Lie algebra to tangent vector
|
||||
static TangentVector Vee(const LieAlgebra& 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 LieAlgebra wedge(double vx, double vy, double w) {
|
||||
return Hat(TangentVector(vx, vy, w));
|
||||
}
|
||||
#endif
|
||||
/// @}
|
||||
|
||||
private:
|
||||
|
||||
|
|
@ -357,12 +361,14 @@ 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>;
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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 LieAlgebra Hat(const TangentVector& xi);
|
||||
|
||||
/// Vee maps from Lie algebra to tangent vector
|
||||
static TangentVector Vee(const LieAlgebra& 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>;
|
||||
|
|
|
|||
|
|
@ -138,6 +138,16 @@ struct traits<QUATERNION_TYPE> {
|
|||
return omega;
|
||||
}
|
||||
|
||||
using LieAlgebra = Matrix3;
|
||||
|
||||
static LieAlgebra Hat(const Vector3& v) {
|
||||
return SO3::Hat(v);
|
||||
}
|
||||
|
||||
static Vector3 Vee(const LieAlgebra& X) {
|
||||
return SO3::Vee(X);
|
||||
}
|
||||
|
||||
/// @}
|
||||
/// @name Manifold traits
|
||||
/// @{
|
||||
|
|
|
|||
|
|
@ -81,6 +81,21 @@ Vector1 Rot2::Logmap(const Rot2& r, OptionalJacobian<1, 1> H) {
|
|||
v << r.theta();
|
||||
return v;
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
Rot2::LieAlgebra Rot2::Hat(const Rot2::TangentVector& xi) {
|
||||
LieAlgebra X;
|
||||
X << 0., -xi.x(),
|
||||
xi.x(), 0.;
|
||||
return X;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Rot2::TangentVector Rot2::Vee(const Rot2::LieAlgebra& X) {
|
||||
TangentVector v;
|
||||
v << X(1, 0);
|
||||
return v;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Matrix2 Rot2::matrix() const {
|
||||
Matrix2 rvalue_;
|
||||
|
|
|
|||
|
|
@ -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 LieAlgebra Hat(const TangentVector& xi);
|
||||
|
||||
/// Vee maps from Lie algebra to tangent vector
|
||||
static TangentVector Vee(const LieAlgebra& 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;
|
||||
|
|
|
|||
|
|
@ -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 LieAlgebra Hat(const TangentVector& xi) { return SO3::Hat(xi); }
|
||||
|
||||
/// Vee maps from Lie algebra to tangent vector
|
||||
static inline TangentVector Vee(const LieAlgebra& X) { return SO3::Vee(X); }
|
||||
|
||||
/// @}
|
||||
/// @name Group Action on Point3
|
||||
/// @{
|
||||
|
|
|
|||
|
|
@ -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:
|
||||
|
|
|
|||
|
|
@ -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() << "]\';";
|
||||
|
|
|
|||
|
|
@ -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 LieAlgebra Hat(const TangentVector& xi);
|
||||
|
||||
/// Vee maps from Lie algebra to tangent vector
|
||||
static TangentVector Vee(const LieAlgebra& X);
|
||||
|
||||
/// @}
|
||||
/// @name Standard interface
|
||||
/// @{
|
||||
|
|
|
|||
|
|
@ -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();
|
||||
|
|
|
|||
|
|
@ -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 LieAlgebra Hat(const TangentVector& xi);
|
||||
|
||||
/// Vee maps from Lie algebra to tangent vector
|
||||
static TangentVector Vee(const LieAlgebra& 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,11 +235,13 @@ 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::LieGroup<Similarity3> {};
|
||||
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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));
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -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(IsLieGroup<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));
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -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);}
|
||||
|
|
|
|||
|
|
@ -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:
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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));
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
|
|
|
|||
Loading…
Reference in New Issue