Loading...
Searching...
No Matches
SOn.h
Go to the documentation of this file.
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
#pragma once
#include <gtsam/base/Lie.h>
#include <gtsam/base/Manifold.h>
#include <gtsam/base/make_shared.h>
#include <gtsam/dllexport.h>
#include <Eigen/Core>
#include <boost/serialization/nvp.hpp>
#include <iostream> // TODO(frank): how to avoid?
#include <string>
#include <type_traits>
#include <vector>
#include <random>
namespace gtsam {
namespace internal {
constexpr int DimensionSO(int N) {
return (N < 0) ? Eigen::Dynamic : N * (N - 1) / 2;
}
// Calculate N^2 at compile time, or return Dynamic if so
constexpr int NSquaredSO(int N) { return (N < 0) ? Eigen::Dynamic : N * N; }
} // namespace internal
template <int N>
class SO : public LieGroup<SO<N>, internal::DimensionSO(N)> {
public:
enum { dimension = internal::DimensionSO(N) };
using MatrixNN = Eigen::Matrix<double, N, N>;
using VectorN2 = Eigen::Matrix<double, internal::NSquaredSO(N), 1>;
using MatrixDD = Eigen::Matrix<double, dimension, dimension>;
GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(true)
protected:
MatrixNN matrix_;
// enable_if_t aliases, used to specialize constructors/methods, see
// https://www.fluentcpp.com/2018/05/18/make-sfinae-pretty-2-hidden-beauty-sfinae/
template <int N_>
using IsDynamic = typename std::enable_if<N_ == Eigen::Dynamic, void>::type;
template <int N_>
using IsFixed = typename std::enable_if<N_ >= 2, void>::type;
template <int N_>
using IsSO3 = typename std::enable_if<N_ == 3, void>::type;
public:
template <int N_ = N, typename = IsFixed<N_>>
SO() : matrix_(MatrixNN::Identity()) {}
template <int N_ = N, typename = IsDynamic<N_>>
explicit SO(size_t n = 0) {
// We allow for n=0 as the default constructor, needed for serialization,
// wrappers etc.
matrix_ = Eigen::MatrixXd::Identity(n, n);
}
template <typename Derived>
explicit SO(const Eigen::MatrixBase<Derived>& R) : matrix_(R.eval()) {}
template <typename Derived>
static SO FromMatrix(const Eigen::MatrixBase<Derived>& R) {
return SO(R);
}
template <typename Derived, int N_ = N, typename = IsDynamic<N_>>
static SO Lift(size_t n, const Eigen::MatrixBase<Derived> &R) {
Matrix Q = Matrix::Identity(n, n);
const int p = R.rows();
assert(p >= 0 && p <= static_cast<int>(n) && R.cols() == p);
Q.topLeftCorner(p, p) = R;
return SO(Q);
}
template <int M, int N_ = N, typename = IsDynamic<N_>>
explicit SO(const SO<M>& R) : matrix_(R.matrix()) {}
template <int N_ = N, typename = IsSO3<N_>>
explicit SO(const Eigen::AngleAxisd& angleAxis) : matrix_(angleAxis) {}
static SO AxisAngle(const Vector3& axis, double theta);
static SO ClosestTo(const MatrixNN& M);
static SO ChordalMean(const std::vector<SO>& rotations);
template <int N_ = N, typename = IsDynamic<N_>>
static SO Random(std::mt19937& rng, size_t n = 0) {
if (n == 0) throw std::runtime_error("SO: Dimensionality not known.");
// TODO(frank): this might need to be re-thought
static std::uniform_real_distribution<double> randomAngle(-M_PI, M_PI);
const size_t d = SO::Dimension(n);
Vector xi(d);
for (size_t j = 0; j < d; j++) {
xi(j) = randomAngle(rng);
}
return SO::Retract(xi);
}
template <int N_ = N, typename = IsFixed<N_>>
static SO Random(std::mt19937& rng) {
// By default, use dynamic implementation above. Specialized for SO(3).
return SO(SO<Eigen::Dynamic>::Random(rng, N).matrix());
}
const MatrixNN& matrix() const { return matrix_; }
size_t rows() const { return matrix_.rows(); }
size_t cols() const { return matrix_.cols(); }
void print(const std::string& s = std::string()) const;
bool equals(const SO& other, double tol) const {
return equal_with_abs_tol(matrix_, other.matrix_, tol);
}
SO operator*(const SO& other) const {
assert(dim() == other.dim());
return SO(matrix_ * other.matrix_);
}
template <int N_ = N, typename = IsFixed<N_>>
static SO Identity() {
return SO();
}
template <int N_ = N, typename = IsDynamic<N_>>
static SO Identity(size_t n = 0) {
return SO(n);
}
SO inverse() const { return SO(matrix_.transpose()); }
using TangentVector = Eigen::Matrix<double, dimension, 1>;
using ChartJacobian = OptionalJacobian<dimension, dimension>;
static int Dim() { return dimension; }
// Calculate manifold dimensionality for SO(n).
// Available as dimension or Dim() for fixed N.
static size_t Dimension(size_t n) { return n * (n - 1) / 2; }
// Calculate ambient dimension n from manifold dimensionality d.
static size_t AmbientDim(size_t d) { return (1 + std::sqrt(1 + 8 * d)) / 2; }
// Calculate run-time dimensionality of manifold.
// Available as dimension or Dim() for fixed N.
size_t dim() const { return Dimension(static_cast<size_t>(matrix_.rows())); }
static MatrixNN Hat(const TangentVector& xi);
static void Hat(const Vector &xi, Eigen::Ref<MatrixNN> X);
static TangentVector Vee(const MatrixNN& X);
// Chart at origin
struct ChartAtOrigin {
static SO Retract(const TangentVector& xi, ChartJacobian H = boost::none);
static TangentVector Local(const SO& R, ChartJacobian H = boost::none);
};
// Return dynamic identity DxD Jacobian for given SO(n)
template <int N_ = N, typename = IsDynamic<N_>>
static MatrixDD IdentityJacobian(size_t n) {
const size_t d = Dimension(n);
return MatrixDD::Identity(d, d);
}
MatrixDD AdjointMap() const;
static SO Expmap(const TangentVector& omega, ChartJacobian H = boost::none);
static MatrixDD ExpmapDerivative(const TangentVector& omega);
static TangentVector Logmap(const SO& R, ChartJacobian H = boost::none);
static MatrixDD LogmapDerivative(const TangentVector& omega);
// inverse with optional derivative
using LieGroup<SO<N>, internal::DimensionSO(N)>::inverse;
VectorN2 vec(OptionalJacobian<internal::NSquaredSO(N), dimension> H =
boost::none) const;
template <int N_ = N, typename = IsFixed<N_>>
static Matrix VectorizedGenerators() {
constexpr size_t N2 = static_cast<size_t>(N * N);
Eigen::Matrix<double, N2, dimension> G;
for (size_t j = 0; j < dimension; j++) {
const auto X = Hat(Vector::Unit(dimension, j));
G.col(j) = Eigen::Map<const VectorN2>(X.data());
}
return G;
}
template <int N_ = N, typename = IsDynamic<N_>>
static Matrix VectorizedGenerators(size_t n = 0) {
const size_t n2 = n * n, dim = Dimension(n);
Matrix G(n2, dim);
for (size_t j = 0; j < dim; j++) {
const auto X = Hat(Vector::Unit(dim, j));
G.col(j) = Eigen::Map<const Matrix>(X.data(), n2, 1);
}
return G;
}
template <class Archive>
friend void save(Archive&, SO&, const unsigned int);
template <class Archive>
friend void load(Archive&, SO&, const unsigned int);
template <class Archive>
friend void serialize(Archive&, SO&, const unsigned int);
friend class boost::serialization::access;
friend class Rot3; // for serialize
};
using SOn = SO<Eigen::Dynamic>;
/*
* Specialize dynamic Hat and Vee, because recursion depends on dynamic nature.
* The definition is in SOn.cpp. Fixed-size SO3 and SO4 have their own version,
* and implementation for other fixed N is in SOn-inl.h.
*/
template <>
GTSAM_EXPORT
Matrix SOn::Hat(const Vector& xi);
template <>
GTSAM_EXPORT
Vector SOn::Vee(const Matrix& X);
/*
* Specialize dynamic compose and between, because the derivative is unknowable
* by the LieGroup implementations, who return a fixed-size matrix for H2.
*/
using DynamicJacobian = OptionalJacobian<Eigen::Dynamic, Eigen::Dynamic>;
template <>
GTSAM_EXPORT
SOn LieGroup<SOn, Eigen::Dynamic>::compose(const SOn& g, DynamicJacobian H1,
DynamicJacobian H2) const;
template <>
GTSAM_EXPORT
SOn LieGroup<SOn, Eigen::Dynamic>::between(const SOn& g, DynamicJacobian H1,
DynamicJacobian H2) const;
/*
* Specialize dynamic vec.
*/
template <>
GTSAM_EXPORT
typename SOn::VectorN2 SOn::vec(DynamicJacobian H) const;
template<class Archive>
void serialize(
Archive& ar, SOn& Q,
const unsigned int file_version
) {
Matrix& M = Q.matrix_;
ar& BOOST_SERIALIZATION_NVP(M);
}
/*
* Define the traits. internal::LieGroup provides both Lie group and Testable
*/
template <int N>
struct traits<SO<N>> : public internal::LieGroup<SO<N>> {};
template <int N>
struct traits<const SO<N>> : public internal::LieGroup<SO<N>> {};
} // namespace gtsam
#include "SOn-inl.h"
make_shared.h
make_shared trampoline function to ensure proper alignment
GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF
#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign)
This marks a GTSAM object to require alignment.
Definition types.h:317
Manifold.h
Base class and basic functions for Manifold types.
Lie.h
Base class and basic functions for Lie types.
gtsam::internal::DimensionSO
constexpr int DimensionSO(int N)
Calculate dimensionality of SO<N> manifold, or return Dynamic if so.
Definition SOn.h:39
SOn-inl.h
Template implementations for SO(n).
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
gtsam::serialize
std::string serialize(const T &input)
serializes to a string
Definition serialization.h:113
gtsam::save
void save(const Matrix &A, const string &s, const string &filename)
save a matrix to file, which can be loaded by matlab
Definition Matrix.cpp:167
gtsam::equal_with_abs_tol
bool equal_with_abs_tol(const Eigen::DenseBase< MATRIX > &A, const Eigen::DenseBase< MATRIX > &B, double tol=1e-9)
equals with a tolerance
Definition Matrix.h:81
gtsam::TangentVector
gtsam::LieGroup
A CRTP helper class that implements Lie group methods Prerequisites: methods operator*,...
Definition Lie.h:37
gtsam::LieGroup< SO< N >, internal::DimensionSO(N)>::Retract
static SO< N > Retract(const TangentVector &v)
Definition Lie.h:111
gtsam::internal::LieGroup
Both LieGroupTraits and Testable.
Definition Lie.h:229
gtsam::OptionalJacobian
OptionalJacobian is an Eigen::Ref like class that can take be constructed using either a fixed size o...
Definition OptionalJacobian.h:41
gtsam::Rot3
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Definition Rot3.h:58
gtsam::SO
Manifold of special orthogonal rotation matrices SO<N>.
Definition SOn.h:52
gtsam::SO::FromMatrix
static SO FromMatrix(const Eigen::MatrixBase< Derived > &R)
Named constructor from Eigen Matrix.
Definition SOn.h:95
gtsam::SO::Expmap
static SO Expmap(const TangentVector &omega, ChartJacobian H=boost::none)
Exponential map at identity - create a rotation from canonical coordinates.
Definition SOn-inl.h:67
gtsam::SO::VectorizedGenerators
static Matrix VectorizedGenerators()
Calculate N^2 x dim matrix of vectorized Lie algebra generators for SO(N).
Definition SOn.h:300
gtsam::SO::inverse
SO inverse() const
inverse of a rotation = transpose
Definition SOn.h:193
gtsam::SO::vec
VectorN2 vec(OptionalJacobian< internal::NSquaredSO(N), dimension > H=boost::none) const
Return vectorized rotation matrix in column order.
Definition SOn-inl.h:88
gtsam::SO::ChordalMean
static SO ChordalMean(const std::vector< SO > &rotations)
Named constructor that finds chordal mean , currently only defined for SO3.
gtsam::SO::operator*
SO operator*(const SO &other) const
Multiplication.
Definition SOn.h:175
gtsam::SO::Vee
static TangentVector Vee(const MatrixNN &X)
Inverse of Hat. See note about xi element order in Hat.
Definition SOn-inl.h:35
gtsam::SO< 3 >::matrix_
MatrixNN matrix_
Definition SOn.h:62
gtsam::SO::Logmap
static TangentVector Logmap(const SO &R, ChartJacobian H=boost::none)
Log map at identity - returns the canonical coordinates of this rotation.
Definition SOn-inl.h:77
gtsam::SO::AxisAngle
static SO AxisAngle(const Vector3 &axis, double theta)
Constructor from axis and angle. Only defined for SO3.
gtsam::SO::AdjointMap
MatrixDD AdjointMap() const
Adjoint map.
Definition SOn-inl.h:60
gtsam::SO::Hat
static void Hat(const Vector &xi, Eigen::Ref< MatrixNN > X)
In-place version of Hat (see details there), implements recursion.
gtsam::SO< 3 >::Identity
static SO Identity()
Definition SOn.h:182
gtsam::SO::Dim
static int Dim()
Return compile-time dimensionality: fixed size N or Eigen::Dynamic.
Definition SOn.h:203
gtsam::SO::Hat
static MatrixNN Hat(const TangentVector &xi)
Hat operator creates Lie algebra element corresponding to d-vector, where d is the dimensionality of ...
Definition SOn-inl.h:29
gtsam::SO::ExpmapDerivative
static MatrixDD ExpmapDerivative(const TangentVector &omega)
Derivative of Expmap, currently only defined for SO3.
Definition SOn-inl.h:72
gtsam::SO::LogmapDerivative
static MatrixDD LogmapDerivative(const TangentVector &omega)
Derivative of Logmap, currently only defined for SO3.
Definition SOn-inl.h:82
gtsam::SO::SO
SO(const SO< M > &R)
Construct dynamic SO(n) from Fixed SO<M>.
Definition SOn.h:111
gtsam::SO::SO
SO(size_t n=0)
Construct SO<N> identity for N == Eigen::Dynamic.
Definition SOn.h:83
gtsam::SO::SO
SO()
Construct SO<N> identity for N >= 2.
Definition SOn.h:79
gtsam::SO::Lift
static SO Lift(size_t n, const Eigen::MatrixBase< Derived > &R)
Named constructor from lower dimensional matrix.
Definition SOn.h:101
gtsam::SO::Random
static SO Random(std::mt19937 &rng, size_t n=0)
Random SO(n) element (no big claims about uniformity). SO(3) is specialized in SO3....
Definition SOn.h:131
gtsam::SO::Identity
static SO Identity(size_t n=0)
SO<N> identity for N == Eigen::Dynamic.
Definition SOn.h:188
gtsam::SO< 3 >::matrix
const MatrixNN & matrix() const
Definition SOn.h:155
gtsam::SO::Random
static SO Random(std::mt19937 &rng)
Random SO(N) element (no big claims about uniformity).
Definition SOn.h:145
gtsam::SO::ClosestTo
static SO ClosestTo(const MatrixNN &M)
Named constructor that finds SO(n) matrix closest to M in Frobenius norm, currently only defined for ...
gtsam::SO::SO
SO(const Eigen::AngleAxisd &angleAxis)
Constructor from AngleAxisd.
Definition SOn.h:115
gtsam::SO::VectorizedGenerators
static Matrix VectorizedGenerators(size_t n=0)
Calculate n^2 x dim matrix of vectorized Lie algebra generators for SO(n).
Definition SOn.h:312
gtsam::SO::SO
SO(const Eigen::MatrixBase< Derived > &R)
Constructor from Eigen Matrix, dynamic version.
Definition SOn.h:91
gtsam::SO::ChartAtOrigin
Definition SOn.h:240
gtsam::SO::ChartAtOrigin::Local
static TangentVector Local(const SO &R, ChartJacobian H=boost::none)
Inverse of Retract.
Definition SOn-inl.h:50
gtsam::SO::ChartAtOrigin::Retract
static SO Retract(const TangentVector &xi, ChartJacobian H=boost::none)
Retract uses Cayley map.
Definition SOn-inl.h:40