Go to the documentation of this file.
19 template<
typename T>
struct traits;
24 template<
class Class,
int N>
36 if (H1) *H1 = - Jacobian::Identity();
37 if (H2) *H2 = Jacobian::Identity();
44 if (H1) *H1 = Jacobian::Identity();
45 if (H2) *H2 = Jacobian::Identity();
55 if (Hm) *Hm = Jacobian::Identity();
60 if (Hv) *Hv = Jacobian::Identity();
66 if (H1) *H1 = Jacobian::Identity();
67 if (H2) *H2 = Jacobian::Identity();
73 if (H1) *H1 = - Jacobian::Identity();
74 if (H2) *H2 = Jacobian::Identity();
79 if (
H) *
H = - Jacobian::Identity();
105 return Eigen::MatrixXd::Identity(dim, dim);
110 if (H1) *H1 = - Eye(
origin);
111 if (H2) *H2 = Eye(
other);
118 if (H1) *H1 = Eye(
origin);
119 if (H2) *H2 = Eye(
origin);
129 if (Hm) *Hm = Eye(
m);
142 if (H1) *H1 = Eye(
v1);
143 if (H2) *H2 = Eye(
v2);
149 if (H1) *H1 = - Eye(
v1);
150 if (H2) *H2 = Eye(
v2);
163 template<
class Class>
184 template<
class Class>
206 template<
class Class>
211 template<
typename Scalar>
241 if (H1) (*H1)[0] = -1.0;
242 if (H2) (*H2)[0] = 1.0;
250 if (H1) (*H1)[0] = 1.0;
251 if (H2) (*H2)[0] = 1.0;
259 if (
H) (*H)[0] = 1.0;
264 if (
H) (*H)[0] = 1.0;
282 template<
int M,
int N,
int Options,
int MaxRows,
int MaxCols>
285 Eigen::Matrix<double, M, N, Options, MaxRows, MaxCols>, M * N> {
308 enum { dimension =
M*
N};
316 if (H1) (*H1) = -Jacobian::Identity();
317 if (H2) (*H2) = Jacobian::Identity();
325 if (H1) (*H1) = Jacobian::Identity();
326 if (H2) (*H2) = Jacobian::Identity();
334 if (
H) *
H = Jacobian::Identity();
343 if (
H) *
H = Jacobian::Identity();
353 template<
int M,
int N,
int Options,
int MaxRows,
int MaxCols>
374 throw std::runtime_error(
"Identity not defined for dynamic types");
387 return m.rows() *
m.cols();
397 if (H1) *H1 = -
Eye(
m);
398 if (H2) *H2 =
Eye(
m);
406 if (H1) *H1 =
Eye(
m);
407 if (H2) *H2 =
Eye(
m);
422 static_cast<void>(
H);
423 throw std::runtime_error(
"Expmap not defined for dynamic types");
433 if (H1) *H1 =
Eye(
v1);
434 if (H2) *H2 =
Eye(
v1);
440 if (H1) *H1 = -
Eye(
v1);
441 if (H2) *H2 =
Eye(
v1);
451 template<
int Options,
int MaxRows,
int MaxCols>
457 template<
int Options,
int MaxRows,
int MaxCols>
463 template<
int Options,
int MaxRows,
int MaxCols>
478 "This type's trait does not assert it as a vector space (or derived)");
static Dynamic Retract(const Dynamic &m, const TangentVector &v, ChartJacobian H1={}, ChartJacobian H2={})
traits< T >::structure_category structure_category_tag
static Class Retract(const Class &origin, const TangentVector &v, ChartJacobian H1={}, ChartJacobian H2={})
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
Eigen::Matrix< double, 1, 1 > TangentVector
Eigen::VectorXd TangentVector
static void Print(const Fixed &m, const std::string &str="")
Namespace containing all symbols from the Eigen library.
static TangentVector Logmap(const Class &m, ChartJacobian Hm={})
GTSAM_CONCEPT_ASSERT(HasVectorSpacePrereqs< Class >)
static Scalar Retract(Scalar origin, const TangentVector &v, ChartJacobian H1={}, ChartJacobian H2={})
static Class Inverse(const Class &v, ChartJacobian H={})
Array< double, 1, 3 > e(1./3., 0.5, 2.)
static TangentVector Logmap(const Dynamic &m, ChartJacobian H={})
VectorSpace provides both Testable and VectorSpaceTraits.
static int GetDimension(const Class &m)
Eigen::Matrix< double, dimension, dimension > Jacobian
Eigen::Matrix< double, M, N, Options, MaxRows, MaxCols > Dynamic
bool equal_with_abs_tol(const Eigen::DenseBase< MATRIX > &A, const Eigen::DenseBase< MATRIX > &B, double tol=1e-9)
static bool Equals(Scalar v1, Scalar v2, double tol=1e-8)
OptionalJacobian< Eigen::Dynamic, Eigen::Dynamic > ChartJacobian
OptionalJacobian< N, N > ChartJacobian
static Dynamic Inverse(const Dynamic &m, ChartJacobian H={})
VectorSpaceTraits Implementation for Fixed sizes.
static Scalar Expmap(const TangentVector &v, ChartJacobian H={})
static Dynamic Expmap(const TangentVector &, ChartJacobian H={})
static Class Retract(const Class &origin, const TangentVector &v, ChartJacobian H1={}, ChartJacobian H2={})
static TangentVector Local(const Dynamic &m, const Dynamic &other, ChartJacobian H1={}, ChartJacobian H2={})
OptionalJacobian< dimension, dimension > ChartJacobian
additive_group_tag group_flavor
void print(const Matrix &A, const string &s, ostream &stream)
static Class Between(const Class &v1, const Class &v2)
static TangentVector Logmap(const Fixed &m, ChartJacobian H={})
Eigen::Matrix< double, N, 1 > TangentVector
static Dynamic Compose(const Dynamic &v1, const Dynamic &v2, ChartJacobian H1={}, ChartJacobian H2={})
static Class Between(const Class &v1, const Class &v2, ChartJacobian H1={}, ChartJacobian H2={})
static Class Compose(const Class &v1, const Class &v2)
static Fixed Retract(const Fixed &origin, const TangentVector &v, ChartJacobian H1={}, ChartJacobian H2={})
vector_space_tag structure_category
tag to assert a type is a vector space
static TangentVector Logmap(Scalar m, ChartJacobian H={})
static void Print(Scalar m, const std::string &str="")
static TangentVector Local(const Class &origin, const Class &other, ChartJacobian H1={}, ChartJacobian H2={})
Eigen::Matrix< double, M, N, Options, MaxRows, MaxCols > Fixed
BOOST_CONCEPT_USAGE(HasVectorSpacePrereqs)
BOOST_CONCEPT_USAGE(IsVectorSpace)
Eigen::Matrix< double, N, N > Jacobian
static Class Inverse(const Class &m)
static int GetDimension(const Dynamic &m)
static Class Compose(const Class &v1, const Class &v2, ChartJacobian H1, ChartJacobian H2={})
A matrix or vector expression mapping an existing array of data.
static Class Inverse(const Class &v, ChartJacobian H)
static bool Equals(const Fixed &v1, const Fixed &v2, double tol=1e-8)
vector_space_tag structure_category
static Dynamic Between(const Dynamic &v1, const Dynamic &v2, ChartJacobian H1={}, ChartJacobian H2={})
static Class Between(const Class &v1, const Class &v2, ChartJacobian H1, ChartJacobian H2={})
Eigen::VectorXd TangentVector
Base class and basic functions for Lie types.
Eigen::Matrix< double, dimension, 1 > TangentVector
static void Print(const Dynamic &m, const std::string &str="")
OptionalJacobian< dimension, dimension > ChartJacobian
Requirements on type to pass it to Manifold template below.
static TangentVector Logmap(const Class &m, ChartJacobian Hm={})
additive_group_tag group_flavor
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 origin
static Eigen::MatrixXd Eye(const Class &m)
static Class Compose(const Class &v1, const Class &v2, ChartJacobian H1={}, ChartJacobian H2={})
static TangentVector Local(Scalar origin, Scalar other, ChartJacobian H1={}, ChartJacobian H2={})
tag to assert a type is a Lie group
Array< int, Dynamic, 1 > v
additive_group_tag group_flavor
static Class Expmap(const TangentVector &v, ChartJacobian Hv={})
vector_space_tag structure_category
static TangentVector Local(const Fixed &origin, const Fixed &other, ChartJacobian H1={}, ChartJacobian H2={})
OptionalJacobian< 1, 1 > ChartJacobian
static TangentVector Local(const Class &origin, const Class &other, ChartJacobian H1={}, ChartJacobian H2={})
static int GetDimension(const Class &)
const Vector3 & vector() const
static Fixed Expmap(const TangentVector &v, ChartJacobian H={})
static Jacobian Eye(const Dynamic &m)
static Dynamic Identity()
static bool Equals(const Dynamic &v1, const Dynamic &v2, double tol=1e-8)
vector_space_tag structure_category
static Class Expmap(const TangentVector &v, ChartJacobian Hv={})
Matrix< RealScalar, Dynamic, Dynamic > M
additive_group_tag group_flavor
gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:43:08