ProductLieGroup.h
Go to the documentation of this file.
1 /* ----------------------------------------------------------------------------
2 
3  * GTSAM Copyright 2010, Georgia Tech Research Corporation,
4  * Atlanta, Georgia 30332-0415
5  * All Rights Reserved
6  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
7 
8  * See LICENSE for the license information
9 
10  * -------------------------------1------------------------------------------- */
11 
19 #pragma once
20 
21 #include <gtsam/base/Lie.h>
22 #include <utility> // pair
23 
24 namespace gtsam {
25 
28 template<typename G, typename H>
29 class ProductLieGroup: public std::pair<G, H> {
32  typedef std::pair<G, H> Base;
33 
34 protected:
37 
38 public:
41 
42  // Construct from two subgroup elements
43  ProductLieGroup(const G& g, const H& h):Base(g,h) {}
44 
45  // Construct from base
46  ProductLieGroup(const Base& base):Base(base) {}
47 
52 
54  return ProductLieGroup(traits<G>::Compose(this->first,other.first),
55  traits<H>::Compose(this->second,other.second));
56  }
58  return ProductLieGroup(traits<G>::Inverse(this->first), traits<H>::Inverse(this->second));
59  }
61  return (*this) * g;
62  }
64  return this->inverse() * g;
65  }
67 
71  inline static size_t Dim() {return dimension;}
72  inline size_t dim() const {return dimension;}
73 
76 
77  ProductLieGroup retract(const TangentVector& v, //
78  ChartJacobian H1 = {}, ChartJacobian H2 = {}) const {
79  if (H1||H2) throw std::runtime_error("ProductLieGroup::retract derivatives not implemented yet");
80  G g = traits<G>::Retract(this->first, v.template head<dimension1>());
81  H h = traits<H>::Retract(this->second, v.template tail<dimension2>());
82  return ProductLieGroup(g,h);
83  }
84  TangentVector localCoordinates(const ProductLieGroup& g, //
85  ChartJacobian H1 = {}, ChartJacobian H2 = {}) const {
86  if (H1||H2) throw std::runtime_error("ProductLieGroup::localCoordinates derivatives not implemented yet");
87  typename traits<G>::TangentVector v1 = traits<G>::Local(this->first, g.first);
88  typename traits<H>::TangentVector v2 = traits<H>::Local(this->second, g.second);
89  TangentVector v;
90  v << v1, v2;
91  return v;
92  }
94 
97 protected:
101 
102 public:
103  ProductLieGroup compose(const ProductLieGroup& other, ChartJacobian H1,
104  ChartJacobian H2 = {}) const {
105  Jacobian1 D_g_first; Jacobian2 D_h_second;
106  G g = traits<G>::Compose(this->first,other.first, H1 ? &D_g_first : 0);
107  H h = traits<H>::Compose(this->second,other.second, H1 ? &D_h_second : 0);
108  if (H1) {
109  H1->setZero();
110  H1->template topLeftCorner<dimension1,dimension1>() = D_g_first;
111  H1->template bottomRightCorner<dimension2,dimension2>() = D_h_second;
112  }
113  if (H2) *H2 = Jacobian::Identity();
114  return ProductLieGroup(g,h);
115  }
116  ProductLieGroup between(const ProductLieGroup& other, ChartJacobian H1,
117  ChartJacobian H2 = {}) const {
118  Jacobian1 D_g_first; Jacobian2 D_h_second;
119  G g = traits<G>::Between(this->first,other.first, H1 ? &D_g_first : 0);
120  H h = traits<H>::Between(this->second,other.second, H1 ? &D_h_second : 0);
121  if (H1) {
122  H1->setZero();
123  H1->template topLeftCorner<dimension1,dimension1>() = D_g_first;
124  H1->template bottomRightCorner<dimension2,dimension2>() = D_h_second;
125  }
126  if (H2) *H2 = Jacobian::Identity();
127  return ProductLieGroup(g,h);
128  }
129  ProductLieGroup inverse(ChartJacobian D) const {
130  Jacobian1 D_g_first; Jacobian2 D_h_second;
131  G g = traits<G>::Inverse(this->first, D ? &D_g_first : 0);
132  H h = traits<H>::Inverse(this->second, D ? &D_h_second : 0);
133  if (D) {
134  D->setZero();
135  D->template topLeftCorner<dimension1,dimension1>() = D_g_first;
136  D->template bottomRightCorner<dimension2,dimension2>() = D_h_second;
137  }
138  return ProductLieGroup(g,h);
139  }
140  static ProductLieGroup Expmap(const TangentVector& v, ChartJacobian Hv = {}) {
141  Jacobian1 D_g_first; Jacobian2 D_h_second;
142  G g = traits<G>::Expmap(v.template head<dimension1>(), Hv ? &D_g_first : 0);
143  H h = traits<H>::Expmap(v.template tail<dimension2>(), Hv ? &D_h_second : 0);
144  if (Hv) {
145  Hv->setZero();
146  Hv->template topLeftCorner<dimension1,dimension1>() = D_g_first;
147  Hv->template bottomRightCorner<dimension2,dimension2>() = D_h_second;
148  }
149  return ProductLieGroup(g,h);
150  }
151  static TangentVector Logmap(const ProductLieGroup& p, ChartJacobian Hp = {}) {
152  Jacobian1 D_g_first; Jacobian2 D_h_second;
153  typename traits<G>::TangentVector v1 = traits<G>::Logmap(p.first, Hp ? &D_g_first : 0);
154  typename traits<H>::TangentVector v2 = traits<H>::Logmap(p.second, Hp ? &D_h_second : 0);
155  TangentVector v;
156  v << v1, v2;
157  if (Hp) {
158  Hp->setZero();
159  Hp->template topLeftCorner<dimension1,dimension1>() = D_g_first;
160  Hp->template bottomRightCorner<dimension2,dimension2>() = D_h_second;
161  }
162  return v;
163  }
164  static TangentVector LocalCoordinates(const ProductLieGroup& p, ChartJacobian Hp = {}) {
165  return Logmap(p, Hp);
166  }
167  ProductLieGroup expmap(const TangentVector& v) const {
168  return compose(ProductLieGroup::Expmap(v));
169  }
170  TangentVector logmap(const ProductLieGroup& g) const {
171  return ProductLieGroup::Logmap(between(g));
172  }
174 
175 };
176 
177 // Define any direct product group to be a model of the multiplicative Group concept
178 template<typename G, typename H>
179 struct traits<ProductLieGroup<G, H> > : internal::LieGroupTraits<ProductLieGroup<G, H> > {};
180 
181 } // namespace gtsam
182 
Eigen::Matrix< double, dimension1, dimension1 > Jacobian1
ProductLieGroup compose(const ProductLieGroup &g) const
ProductLieGroup operator*(const ProductLieGroup &other) const
Vector v2
Vector v1
static TangentVector Logmap(const ProductLieGroup &p, ChartJacobian Hp={})
JacobiRotation< float > G
multiplicative_group_tag group_flavor
ProductLieGroup(const G &g, const H &h)
Eigen::Matrix< double, dimension, dimension > Jacobian
TangentVector logmap(const ProductLieGroup &g) const
ProductLieGroup inverse(ChartJacobian D) const
Pose2_ Expmap(const Vector3_ &xi)
ProductLieGroup()
Default constructor yields identity.
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 ProductLieGroup Expmap(const TangentVector &v, ChartJacobian Hv={})
void g(const string &key, int i)
Definition: testBTree.cpp:41
ProductLieGroup between(const ProductLieGroup &g) const
ProductLieGroup expmap(const TangentVector &v) const
static TangentVector LocalCoordinates(const ProductLieGroup &p, ChartJacobian Hp={})
ProductLieGroup compose(const ProductLieGroup &other, ChartJacobian H1, ChartJacobian H2={}) const
ProductLieGroup(const Base &base)
TangentVector localCoordinates(const ProductLieGroup &g, ChartJacobian H1={}, ChartJacobian H2={}) const
OptionalJacobian< dimension, dimension > ChartJacobian
Group operator syntax flavors.
Definition: Group.h:40
Array< int, Dynamic, 1 > v
static ProductLieGroup Identity()
BetweenFactor< Rot3 > Between
Base class and basic functions for Lie types.
traits
Definition: chartTesting.h:28
const double h
ProductLieGroup inverse() const
GTSAM_CONCEPT_ASSERT(IsLieGroup< G >)
ProductLieGroup between(const ProductLieGroup &other, ChartJacobian H1, ChartJacobian H2={}) const
ProductLieGroup retract(const TangentVector &v, ChartJacobian H1={}, ChartJacobian H2={}) const
float * p
std::pair< G, H > Base
Eigen::Matrix< double, dimension2, dimension2 > Jacobian2
Eigen::Matrix< double, dimension, 1 > TangentVector
Annotation indicating that a class derives from another given type.
Definition: attr.h:61
The matrix class, also used for vectors and row-vectors.


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:35:26