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.
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