Go to the documentation of this file.
41 if (
H)
throw std::runtime_error(
"SO<N>::Retract jacobian not implemented.");
44 const auto I = Eigen::MatrixXd::Identity(
n,
n);
52 if (
H)
throw std::runtime_error(
"SO<N>::Local jacobian not implemented.");
53 const size_t n =
R.rows();
54 const auto I = Eigen::MatrixXd::Identity(
n,
n);
55 const Matrix X = (
I -
R.matrix_) * (
I +
R.matrix_).inverse();
61 if (
N==2)
return I_1x1;
62 throw std::runtime_error(
63 "SO<N>::AdjointMap only implemented for SO2, SO3 and SO4.");
68 throw std::runtime_error(
"SO<N>::Expmap only implemented for SO3 and SO4.");
73 throw std::runtime_error(
"SO<N>::ExpmapDerivative only implemented for SO3.");
78 throw std::runtime_error(
"SO<N>::Logmap only implemented for SO3.");
83 throw std::runtime_error(
"O<N>::LogmapDerivative only implemented for SO3.");
99 for (
size_t i = 0;
i <
N;
i++) {
109 std::cout <<
s <<
matrix_ << std::endl;
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
SO inverse() const
inverse of a rotation = transpose
static SO Retract(const TangentVector &xi, ChartJacobian H={})
static MatrixNN Hat(const TangentVector &xi)
typedef and functions to augment Eigen's MatrixXd
static MatrixDD ExpmapDerivative(const TangentVector &omega)
Derivative of Expmap, currently only defined for SO3.
static SO Expmap(const TangentVector &omega, ChartJacobian H={})
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 size_t AmbientDim(size_t d)
SO()
Construct SO<N> identity for N >= 2.
MatrixDD AdjointMap() const
Adjoint map.
A matrix or vector expression mapping an existing array of data.
static TangentVector Logmap(const SO &R, ChartJacobian H={})
constexpr int NSquaredSO(int N)
void print(const std::string &s=std::string()) const
static TangentVector Local(const SO &R, ChartJacobian H={})
EIGEN_DEVICE_FUNC const EIGEN_STRONG_INLINE Scalar * data() const
static TangentVector Vee(const MatrixNN &X)
Inverse of Hat. See note about xi element order in Hat.
VectorN2 vec(OptionalJacobian< internal::NSquaredSO(N), dimension > H={}) const
Rot2 R(Rot2::fromAngle(0.1))
gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:35:33