gtsam  4.1.0
gtsam
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> {
30  BOOST_CONCEPT_ASSERT((IsLieGroup<G>));
31  BOOST_CONCEPT_ASSERT((IsLieGroup<H>));
32  typedef std::pair<G, H> Base;
33 
34 protected:
35  enum {dimension1 = traits<G>::dimension};
36  enum {dimension2 = traits<H>::dimension};
37 
38 public:
40  ProductLieGroup():Base(traits<G>::Identity(),traits<H>::Identity()) {}
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 
50  typedef multiplicative_group_tag group_flavor;
51  static ProductLieGroup identity() {return ProductLieGroup();}
52 
53  ProductLieGroup operator*(const ProductLieGroup& other) const {
54  return ProductLieGroup(traits<G>::Compose(this->first,other.first),
55  traits<H>::Compose(this->second,other.second));
56  }
57  ProductLieGroup inverse() const {
58  return ProductLieGroup(traits<G>::Inverse(this->first), traits<H>::Inverse(this->second));
59  }
60  ProductLieGroup compose(const ProductLieGroup& g) const {
61  return (*this) * g;
62  }
63  ProductLieGroup between(const ProductLieGroup& g) const {
64  return this->inverse() * g;
65  }
67 
70  enum {dimension = dimension1 + dimension2};
71  inline static size_t Dim() {return dimension;}
72  inline size_t dim() const {return dimension;}
73 
74  typedef Eigen::Matrix<double, dimension, 1> TangentVector;
75  typedef OptionalJacobian<dimension, dimension> ChartJacobian;
76 
77  ProductLieGroup retract(const TangentVector& v, //
78  ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) 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 = boost::none, ChartJacobian H2 = boost::none) 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:
98  typedef Eigen::Matrix<double, dimension, dimension> Jacobian;
99  typedef Eigen::Matrix<double, dimension1, dimension1> Jacobian1;
100  typedef Eigen::Matrix<double, dimension2, dimension2> Jacobian2;
101 
102 public:
103  ProductLieGroup compose(const ProductLieGroup& other, ChartJacobian H1,
104  ChartJacobian H2 = boost::none) 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 = boost::none) 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 = boost::none) {
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 = boost::none) {
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  ProductLieGroup expmap(const TangentVector& v) const {
165  return compose(ProductLieGroup::Expmap(v));
166  }
167  TangentVector logmap(const ProductLieGroup& g) const {
168  return ProductLieGroup::Logmap(between(g));
169  }
171 
172 };
173 
174 // Define any direct product group to be a model of the multiplicative Group concept
175 template<typename G, typename H>
176 struct traits<ProductLieGroup<G, H> > : internal::LieGroupTraits<ProductLieGroup<G, H> > {};
177 
178 } // namespace gtsam
179 
gtsam::IsLieGroup
Lie Group Concept.
Definition: Lie.h:259
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition: concepts.h:30
gtsam::ProductLieGroup::ProductLieGroup
ProductLieGroup()
Default constructor yields identity.
Definition: ProductLieGroup.h:40
gtsam
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
gtsam::ProductLieGroup
Template to construct the product Lie group of two other Lie groups Assumes Lie group structure for G...
Definition: ProductLieGroup.h:29
gtsam::internal::LieGroupTraits
A helper class that implements the traits interface for GTSAM lie groups.
Definition: Lie.h:173
Lie.h
Base class and basic functions for Lie types.