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);
Base class and basic functions for Lie types.
#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.
Template implementations for SO(n)
constexpr int DimensionSO(int N)
Calculate dimensionality of SO<N> manifold, or return Dynamic if so.
Definition SOn.h:39
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