24#include <gtsam/dllexport.h>
27#include <boost/serialization/nvp.hpp>
40 return (N < 0) ? Eigen::Dynamic : N * (N - 1) / 2;
44constexpr int NSquaredSO(
int N) {
return (N < 0) ? Eigen::Dynamic : N * N; }
52class SO :
public LieGroup<SO<N>, internal::DimensionSO(N)> {
54 enum { dimension = internal::DimensionSO(N) };
55 using MatrixNN = Eigen::Matrix<double, N, N>;
56 using VectorN2 = Eigen::Matrix<double, internal::NSquaredSO(N), 1>;
57 using MatrixDD = Eigen::Matrix<double, dimension, dimension>;
67 using IsDynamic =
typename std::enable_if<N_ == Eigen::Dynamic, void>::type;
69 using IsFixed =
typename std::enable_if<N_ >= 2,
void>::type;
71 using IsSO3 =
typename std::enable_if<N_ == 3, void>::type;
78 template <
int N_ = N,
typename = IsFixed<N_>>
82 template <
int N_ = N,
typename = IsDynamic<N_>>
83 explicit SO(
size_t n = 0) {
86 matrix_ = Eigen::MatrixXd::Identity(n, n);
90 template <
typename Derived>
91 explicit SO(
const Eigen::MatrixBase<Derived>& R) :
matrix_(R.eval()) {}
94 template <
typename Derived>
100 template <
typename Derived,
int N_ = N,
typename = IsDynamic<N_>>
101 static SO Lift(
size_t n,
const Eigen::MatrixBase<Derived> &R) {
102 Matrix Q = Matrix::Identity(n, n);
103 const int p = R.rows();
104 assert(p >= 0 && p <=
static_cast<int>(n) && R.cols() == p);
105 Q.topLeftCorner(p, p) = R;
110 template <
int M,
int N_ = N,
typename = IsDynamic<N_>>
114 template <
int N_ = N,
typename = IsSO3<N_>>
115 explicit SO(
const Eigen::AngleAxisd& angleAxis) :
matrix_(angleAxis) {}
130 template <
int N_ = N,
typename = IsDynamic<N_>>
131 static SO Random(std::mt19937& rng,
size_t n = 0) {
132 if (n == 0)
throw std::runtime_error(
"SO: Dimensionality not known.");
134 static std::uniform_real_distribution<double> randomAngle(-M_PI, M_PI);
135 const size_t d = SO::Dimension(n);
137 for (
size_t j = 0; j < d; j++) {
138 xi(j) = randomAngle(rng);
144 template <
int N_ = N,
typename = IsFixed<N_>>
157 size_t rows()
const {
return matrix_.rows(); }
158 size_t cols()
const {
return matrix_.cols(); }
164 void print(
const std::string& s = std::string())
const;
166 bool equals(
const SO& other,
double tol)
const {
176 assert(dim() == other.dim());
181 template <
int N_ = N,
typename = IsFixed<N_>>
187 template <
int N_ = N,
typename = IsDynamic<N_>>
199 using TangentVector = Eigen::Matrix<double, dimension, 1>;
203 static int Dim() {
return dimension; }
207 static size_t Dimension(
size_t n) {
return n * (n - 1) / 2; }
210 static size_t AmbientDim(
size_t d) {
return (1 + std::sqrt(1 + 8 * d)) / 2; }
214 size_t dim()
const {
return Dimension(
static_cast<size_t>(
matrix_.rows())); }
231 static MatrixNN
Hat(
const TangentVector& xi);
234 static void Hat(
const Vector &xi, Eigen::Ref<MatrixNN> X);
237 static TangentVector
Vee(
const MatrixNN& X);
254 template <
int N_ = N,
typename = IsDynamic<N_>>
255 static MatrixDD IdentityJacobian(
size_t n) {
256 const size_t d = Dimension(n);
257 return MatrixDD::Identity(d, d);
270 static SO Expmap(
const TangentVector& omega, ChartJacobian H = boost::none);
278 static TangentVector
Logmap(
const SO& R, ChartJacobian H = boost::none);
299 template <
int N_ = N,
typename = IsFixed<N_>>
301 constexpr size_t N2 =
static_cast<size_t>(N * N);
302 Eigen::Matrix<double, N2, dimension> G;
303 for (
size_t j = 0; j < dimension; j++) {
304 const auto X =
Hat(Vector::Unit(dimension, j));
305 G.col(j) = Eigen::Map<const VectorN2>(X.data());
311 template <
int N_ = N,
typename = IsDynamic<N_>>
313 const size_t n2 = n * n, dim = Dimension(n);
315 for (
size_t j = 0; j < dim; j++) {
316 const auto X =
Hat(Vector::Unit(dim, j));
317 G.col(j) = Eigen::Map<const Matrix>(X.data(), n2, 1);
326 template <
class Archive>
327 friend void save(Archive&,
SO&,
const unsigned int);
328 template <
class Archive>
329 friend void load(Archive&,
SO&,
const unsigned int);
330 template <
class Archive>
331 friend void serialize(Archive&,
SO&,
const unsigned int);
332 friend class boost::serialization::access;
338using SOn = SO<Eigen::Dynamic>;
359using DynamicJacobian = OptionalJacobian<Eigen::Dynamic, Eigen::Dynamic>;
363SOn LieGroup<SOn, Eigen::Dynamic>::compose(
const SOn& g, DynamicJacobian H1,
364 DynamicJacobian H2)
const;
368SOn LieGroup<SOn, Eigen::Dynamic>::between(
const SOn& g, DynamicJacobian H1,
369 DynamicJacobian H2)
const;
376typename SOn::VectorN2
SOn::vec(DynamicJacobian H)
const;
379template<
class Archive>
382 const unsigned int file_version
384 Matrix& M = Q.matrix_;
385 ar& BOOST_SERIALIZATION_NVP(M);
#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign)
This marks a GTSAM object to require alignment.
Definition types.h:317
make_shared trampoline function to ensure proper alignment
Base class and basic functions for Manifold types.
Base class and basic functions for Lie types.
constexpr int DimensionSO(int N)
Calculate dimensionality of SO<N> manifold, or return Dynamic if so.
Definition SOn.h:39
Template implementations for SO(n)
Global functions in a separate testing namespace.
Definition chartTesting.h:28
std::string serialize(const T &input)
serializes to a string
Definition serialization.h:113
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
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
A CRTP helper class that implements Lie group methods Prerequisites: methods operator*,...
Definition Lie.h:37
static SO< N > Retract(const TangentVector &v)
Retract at origin: possible in Lie group because it has an identity.
Definition Lie.h:111
Both LieGroupTraits and Testable.
Definition Lie.h:229
OptionalJacobian is an Eigen::Ref like class that can take be constructed using either a fixed size o...
Definition OptionalJacobian.h:41
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Definition Rot3.h:58
Manifold of special orthogonal rotation matrices SO<N>.
Definition SOn.h:52
static SO FromMatrix(const Eigen::MatrixBase< Derived > &R)
Named constructor from Eigen Matrix.
Definition SOn.h:95
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
static Matrix VectorizedGenerators()
Calculate N^2 x dim matrix of vectorized Lie algebra generators for SO(N)
Definition SOn.h:300
SO inverse() const
inverse of a rotation = transpose
Definition SOn.h:193
VectorN2 vec(OptionalJacobian< internal::NSquaredSO(N), dimension > H=boost::none) const
Return vectorized rotation matrix in column order.
Definition SOn-inl.h:88
static SO ChordalMean(const std::vector< SO > &rotations)
Named constructor that finds chordal mean , currently only defined for SO3.
SO operator*(const SO &other) const
Multiplication.
Definition SOn.h:175
static TangentVector Vee(const MatrixNN &X)
Inverse of Hat. See note about xi element order in Hat.
Definition SOn-inl.h:35
MatrixNN matrix_
Rotation matrix.
Definition SOn.h:62
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
static SO AxisAngle(const Vector3 &axis, double theta)
Constructor from axis and angle. Only defined for SO3.
MatrixDD AdjointMap() const
Adjoint map.
Definition SO4.cpp:159
static void Hat(const Vector &xi, Eigen::Ref< MatrixNN > X)
In-place version of Hat (see details there), implements recursion.
static SO Identity()
SO<N> identity for N >= 2.
Definition SOn.h:182
static int Dim()
Return compile-time dimensionality: fixed size N or Eigen::Dynamic.
Definition SOn.h:203
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
static MatrixDD ExpmapDerivative(const TangentVector &omega)
Derivative of Expmap, currently only defined for SO3.
Definition SOn-inl.h:72
static MatrixDD LogmapDerivative(const TangentVector &omega)
Derivative of Logmap, currently only defined for SO3.
Definition SOn-inl.h:82
SO(const SO< M > &R)
Construct dynamic SO(n) from Fixed SO<M>
Definition SOn.h:111
SO(size_t n=0)
Construct SO<N> identity for N == Eigen::Dynamic.
Definition SOn.h:83
SO()
Construct SO<N> identity for N >= 2.
Definition SOn.h:79
static SO Lift(size_t n, const Eigen::MatrixBase< Derived > &R)
Named constructor from lower dimensional matrix.
Definition SOn.h:101
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
static SO Identity(size_t n=0)
SO<N> identity for N == Eigen::Dynamic.
Definition SOn.h:188
const MatrixNN & matrix() const
Return matrix.
Definition SOn.h:155
static SO Random(std::mt19937 &rng)
Random SO(N) element (no big claims about uniformity)
Definition SOn.h:145
static SO ClosestTo(const MatrixNN &M)
Named constructor that finds SO(n) matrix closest to M in Frobenius norm, currently only defined for ...
SO(const Eigen::AngleAxisd &angleAxis)
Constructor from AngleAxisd.
Definition SOn.h:115
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
SO(const Eigen::MatrixBase< Derived > &R)
Constructor from Eigen Matrix, dynamic version.
Definition SOn.h:91
static TangentVector Local(const SO &R, ChartJacobian H=boost::none)
Inverse of Retract.
Definition SOn-inl.h:50
static SO Retract(const TangentVector &xi, ChartJacobian H=boost::none)
Retract uses Cayley map.
Definition SOn-inl.h:40