Go to the documentation of this file.
24 #include <gtsam/dllexport.h>
27 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
28 #include <boost/serialization/nvp.hpp>
33 #include <type_traits>
54 class SO :
public LieGroup<SO<N>, internal::DimensionSO(N)> {
80 template <
int N_ =
N, typename =
IsFixed<N_>>
84 template <
int N_ = N,
typename = IsDynamic<N_>>
85 explicit SO(
size_t n = 0) {
88 matrix_ = Eigen::MatrixXd::Identity(
n,
n);
92 template <
typename Derived>
96 template <
typename Derived>
102 template <
typename Derived,
int N_ = N,
typename = IsDynamic<N_>>
105 const int p =
R.rows();
106 assert(
p >= 0 &&
p <=
static_cast<int>(
n) &&
R.cols() ==
p);
107 Q.topLeftCorner(
p,
p) =
R;
112 template <
int M,
int N_ = N,
typename = IsDynamic<N_>>
116 template <
int N_ = N,
typename = IsSO3<N_>>
132 template <
int N_ = N,
typename = IsDynamic<N_>>
134 if (
n == 0)
throw std::runtime_error(
"SO: Dimensionality not known.");
136 static std::uniform_real_distribution<double> randomAngle(-
M_PI,
M_PI);
139 for (
size_t j = 0;
j <
d;
j++) {
146 template <
int N_ = N,
typename = IsFixed<N_>>
166 void print(
const std::string&
s = std::string())
const;
183 template <
int N_ = N,
typename = IsFixed<N_>>
189 template <
int N_ = N,
typename = IsDynamic<N_>>
256 template <
int N_ = N,
typename = IsDynamic<N_>>
259 return MatrixDD::Identity(
d,
d);
301 template <
int N_ = N,
typename = IsFixed<N_>>
303 constexpr
size_t N2 =
static_cast<size_t>(
N *
N);
313 template <
int N_ = N,
typename = IsDynamic<N_>>
317 for (
size_t j = 0;
j <
dim;
j++) {
318 const auto X =
Hat(Vector::Unit(
dim,
j));
328 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
329 template <
class Archive>
330 friend void save(Archive&,
SO&,
const unsigned int);
331 template <
class Archive>
332 friend void load(Archive&,
SO&,
const unsigned int);
333 template <
class Archive>
334 friend void serialize(Archive&,
SO&,
const unsigned int);
335 friend class boost::serialization::access;
382 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
384 template<
class Archive>
387 const unsigned int file_version
390 ar& BOOST_SERIALIZATION_NVP(
M);
Class between(const Class &g) const
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy y set format x g set format y g set format x2 g set format y2 g set format z g set angles radians set nogrid set key title set key left top Right noreverse box linetype linewidth samplen spacing width set nolabel set noarrow set nologscale set logscale x set set pointsize set encoding default set nopolar set noparametric set set set set surface set nocontour set clabel set mapping cartesian set nohidden3d set cntrparam order set cntrparam linear set cntrparam levels auto set cntrparam points set size set set xzeroaxis lt lw set x2zeroaxis lt lw set yzeroaxis lt lw set y2zeroaxis lt lw set tics in set ticslevel set tics set mxtics default set mytics default set mx2tics default set my2tics default set xtics border mirror norotate autofreq set ytics border mirror norotate autofreq set ztics border nomirror norotate autofreq set nox2tics set noy2tics set timestamp bottom norotate set rrange[*:*] noreverse nowriteback set trange[*:*] noreverse nowriteback set urange[*:*] noreverse nowriteback set vrange[*:*] noreverse nowriteback set xlabel matrix size set x2label set timefmt d m y n H
static SO AxisAngle(const Vector3 &axis, double theta)
Constructor from axis and angle. Only defined for SO3.
typename std::enable_if< N_==Eigen::Dynamic, void >::type IsDynamic
SO inverse() const
inverse of a rotation = transpose
static SO Retract(const TangentVector &xi, ChartJacobian H={})
Namespace containing all symbols from the Eigen library.
bool equals(const SO &other, double tol) const
static SO ChordalMean(const std::vector< SO > &rotations)
static const double d[K][N]
static MatrixNN Hat(const TangentVector &xi)
Eigen::Matrix< double, dimension, dimension > MatrixDD
static size_t Dimension(size_t n)
Template implementations for SO(n)
static Matrix VectorizedGenerators(size_t n=0)
Calculate n^2 x dim matrix of vectorized Lie algebra generators for SO(n)
Class compose(const Class &g) const
Represents a 3D rotation as a rotation angle around an arbitrary 3D axis.
OptionalJacobian< dimension, dimension > ChartJacobian
static MatrixDD ExpmapDerivative(const TangentVector &omega)
Derivative of Expmap, currently only defined for SO3.
bool equal_with_abs_tol(const Eigen::DenseBase< MATRIX > &A, const Eigen::DenseBase< MATRIX > &B, double tol=1e-9)
SO(const SO< M > &R)
Construct dynamic SO(n) from Fixed SO<M>
static SO Identity()
SO<N> identity for N >= 2.
static SO Expmap(const TangentVector &omega, ChartJacobian H={})
typename std::enable_if< N_==3, void >::type IsSO3
static MatrixDD LogmapDerivative(const TangentVector &omega)
Derivative of Logmap, currently only defined for SO3.
MatrixNN matrix_
Rotation matrix.
static Matrix VectorizedGenerators()
Calculate N^2 x dim matrix of vectorized Lie algebra generators for SO(N)
static SO< N > Retract(const TangentVector &v)
Retract at origin: possible in Lie group because it has an identity.
constexpr int DimensionSO(int N)
Calculate dimensionality of SO<N> manifold, or return Dynamic if so.
static SO Identity(size_t n=0)
SO<N> identity for N == Eigen::Dynamic.
const MatrixNN & matrix() const
Return matrix.
Both LieGroupTraits and Testable.
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Eigen::Matrix< double, N, N > MatrixNN
EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR Index rows() const EIGEN_NOEXCEPT
SO(const Eigen::AngleAxisd &angleAxis)
Constructor from AngleAxisd.
Eigen::Matrix< double, dimension, 1 > TangentVector
static size_t AmbientDim(size_t d)
static SO Lift(size_t n, const Eigen::MatrixBase< Derived > &R)
Named constructor from lower dimensional matrix.
Base class and basic functions for Manifold types.
make_shared trampoline function to ensure proper alignment
SO()
Construct SO<N> identity for N >= 2.
MatrixDD AdjointMap() const
Adjoint map.
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....
SO operator*(const SO &other) const
Multiplication.
A matrix or vector expression mapping an existing array of data.
static TangentVector Logmap(const SO &R, ChartJacobian H={})
void g(const string &key, int i)
constexpr int NSquaredSO(int N)
typename std::enable_if< N_ >=2, void >::type IsFixed
void save(const Matrix &A, const string &s, const string &filename)
The quaternion class used to represent 3D orientations and rotations.
void print(const std::string &s=std::string()) const
Base class and basic functions for Lie types.
A matrix or vector expression mapping an existing expression.
EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR Index cols() const EIGEN_NOEXCEPT
SO(const Eigen::MatrixBase< Derived > &R)
Constructor from Eigen Matrix, dynamic version.
JacobiRotation< float > G
SO(size_t n=0)
Construct SO<N> identity for N == Eigen::Dynamic.
static SO FromMatrix(const Eigen::MatrixBase< Derived > &R)
Named constructor from Eigen Matrix.
static TangentVector Local(const SO &R, ChartJacobian H={})
static TangentVector Vee(const MatrixNN &X)
Inverse of Hat. See note about xi element order in Hat.
static SO ClosestTo(const MatrixNN &M)
#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign)
Eigen::Matrix< double, internal::NSquaredSO(N), 1 > VectorN2
Base class for all dense matrices, vectors, and expressions.
static SO Random(std::mt19937 &rng)
Random SO(N) element (no big claims about uniformity)
static int Dim()
Return compile-time dimensionality: fixed size N or Eigen::Dynamic.
VectorN2 vec(OptionalJacobian< internal::NSquaredSO(N), dimension > H={}) const
static MatrixDD IdentityJacobian(size_t n)
internal::nested_eval< T, 1 >::type eval(const T &xpr)
Jet< T, N > sqrt(const Jet< T, N > &f)
Rot2 R(Rot2::fromAngle(0.1))
Matrix< RealScalar, Dynamic, Dynamic > M
gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:35:33