35 template <
class Class,
int N>
44 return static_cast<const Class&
>(*this);
56 ChartJacobian H2 = boost::none)
const {
57 if (H1) *H1 = g.inverse().AdjointMap();
63 ChartJacobian H2 = boost::none)
const {
65 if (H1) *H1 = - result.inverse().AdjointMap();
71 if (H) *H = -
derived().AdjointMap();
84 return Class::Logmap(
between(g));
89 ChartJacobian H1, ChartJacobian H2 = boost::none)
const {
93 if (H1) *H1 = g.inverse().AdjointMap();
100 ChartJacobian H1, ChartJacobian H2 = boost::none)
const {
103 TangentVector
v = Class::Logmap(h, (H1 || H2) ? &D_v_h : 0);
104 if (H1) *H1 = - D_v_h * h.inverse().AdjointMap();
111 return Class::ChartAtOrigin::Retract(v);
116 return Class::ChartAtOrigin::Local(g);
121 return Class::ChartAtOrigin::Retract(v,H);
126 return Class::ChartAtOrigin::Local(g,H);
131 return compose(Class::ChartAtOrigin::Retract(v));
136 return Class::ChartAtOrigin::Local(
between(g));
141 ChartJacobian H1, ChartJacobian H2 = boost::none)
const {
143 Class g = Class::ChartAtOrigin::Retract(v, H2 ? &D_g_v : 0);
145 if (H1) *H1 = g.inverse().AdjointMap();
152 ChartJacobian H1, ChartJacobian H2 = boost::none)
const {
155 TangentVector
v = Class::ChartAtOrigin::Local(h, (H1 || H2) ? &D_v_h : 0);
156 if (H1) *H1 = - D_v_h * h.inverse().AdjointMap();
172 template<
class Class>
190 ChartJacobian Horigin = boost::none, ChartJacobian Hother = boost::none) {
191 return origin.localCoordinates(other, Horigin, Hother);
195 ChartJacobian Horigin = boost::none, ChartJacobian Hv = boost::none) {
196 return origin.retract(v, Horigin, Hv);
202 static TangentVector
Logmap(
const Class&
m, ChartJacobian Hm = boost::none) {
203 return Class::Logmap(m, Hm);
206 static Class Expmap(
const TangentVector&
v, ChartJacobian Hv = boost::none) {
211 ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) {
212 return m1.compose(m2, H1, H2);
216 ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) {
217 return m1.between(m2, H1, H2);
221 ChartJacobian
H = boost::none) {
238 template<
class Class>
240 return l1.inverse().compose(l2);
244 template<
class Class>
246 return Class::Logmap(l0.between(lp));
250 template<
class Class>
267 BOOST_STATIC_ASSERT_MSG(
269 "This type's trait does not assert it is a Lie group (or derived)");
285 ChartJacobian Hg,
Hh;
298 static const double _2 = 1. / 2., _12 = 1. / 12., _24 = 1. / 24.;
324 template <
typename T>
326 assert(t >= 0 && t <= 1);
341 typename T::Jacobian
operator()(
const typename T::Jacobian &covariance)
342 {
return adjointMap_ * covariance * adjointMap_.transpose(); }
355 #define GTSAM_CONCEPT_LIE_INST(T) template class gtsam::IsLieGroup<T>; 356 #define GTSAM_CONCEPT_LIE_TYPE(T) typedef gtsam::IsLieGroup<T> _gtsam_IsLieGroup_##T;
tag to assert a type is a manifold
TangentVector localCoordinates(const Class &g, ChartJacobian H1, ChartJacobian H2=boost::none) const
localCoordinates with optional derivatives
OptionalJacobian< dimension, dimension > ChartJacobian
static Class Expmap(const TangentVector &v, ChartJacobian Hv=boost::none)
AngularVelocity bracket(const AngularVelocity &X, const AngularVelocity &Y)
Class between_default(const Class &l1, const Class &l2)
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
Concept check class for variable types with Group properties.
tag to assert a type is a Lie group
static Class Retract(const Class &origin, const TangentVector &v, ChartJacobian Horigin=boost::none, ChartJacobian Hv=boost::none)
static Cal3_S2 K(500, 500, 0.1, 640/2, 480/2)
Pose2_ Expmap(const Vector3_ &xi)
static Class Compose(const Class &m1, const Class &m2, ChartJacobian H1=boost::none, ChartJacobian H2=boost::none)
T BCH(const T &X, const T &Y)
AGC: bracket() only appears in Rot3 tests, should this be used elsewhere?
TangentVector logmap(const Class &g, ChartJacobian H1, ChartJacobian H2=boost::none) const
logmap with optional derivatives
Class retract(const TangentVector &v, ChartJacobian H1, ChartJacobian H2=boost::none) const
retract with optional derivatives
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
Class compose(const Class &g, ChartJacobian H1, ChartJacobian H2=boost::none) const
lie_group_tag structure_category
T expm(const Vector &x, int K=7)
void g(const string &key, int i)
traits< T >::structure_category structure_category_tag
Class between(const Class &g, ChartJacobian H1, ChartJacobian H2=boost::none) const
Class inverse(ChartJacobian H) const
T interpolate(const T &X, const T &Y, double t)
Extra manifold traits for fixed-dimension types.
static Class Retract(const TangentVector &v, ChartJacobian H)
Retract at origin with optional derivative.
Class compose(const Class &g) const
Matrix wedge(const Vector &x)
Eigen::Matrix< double, N, N > Jacobian
OptionalJacobian< N, N > ChartJacobian
Class expmap(const TangentVector &v) const
Base class and basic functions for Manifold types.
Group operator syntax flavors.
Eigen::Triplet< double > T
traits< T >::ManifoldType ManifoldType
BetweenFactor< Rot3 > Between
traits< T >::TangentVector TangentVector
Class expmap_default(const Class &t, const Vector &d)
multiplicative_group_tag group_flavor
TangentVector logmap(const Class &g) const
Class retract(const TangentVector &v) const
retract as required by manifold concept: applies v at *this
BOOST_CONCEPT_USAGE(IsLieGroup)
Eigen::Matrix< double, N, 1 > TangentVector
Both LieGroupTraits and Testable.
static TangentVector LocalCoordinates(const Class &g)
LocalCoordinates at origin: possible in Lie group because it has an identity.
static Class Inverse(const Class &m, ChartJacobian H=boost::none)
Class between(const Class &g) const
static Class Retract(const TangentVector &v)
Retract at origin: possible in Lie group because it has an identity.
static TangentVector Logmap(const Class &m, ChartJacobian Hm=boost::none)
const Class & derived() const
traits< T >::ChartJacobian ChartJacobian
static Class Between(const Class &m1, const Class &m2, ChartJacobian H1=boost::none, ChartJacobian H2=boost::none)
tag to assert a type is a group
static TangentVector LocalCoordinates(const Class &g, ChartJacobian H)
LocalCoordinates at origin with optional derivative.
Class expmap(const TangentVector &v, ChartJacobian H1, ChartJacobian H2=boost::none) const
expmap with optional derivatives
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 x
static TangentVector Local(const Class &origin, const Class &other, ChartJacobian Horigin=boost::none, ChartJacobian Hother=boost::none)
Eigen::Matrix< double, dimension, 1 > TangentVector
Vector logmap_default(const Class &l0, const Class &lp)
TangentVector localCoordinates(const Class &g) const
localCoordinates as required by manifold concept: finds tangent vector between *this and g ...