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
12/**
13 * @file ProductLieGroup.h
14 * @date May, 2015
15 * @author Frank Dellaert
16 * @brief Group product of two Lie Groups
17 */
18
19#pragma once
20
21#include <gtsam/base/Lie.h>
22#include <utility> // pair
23
24namespace gtsam {
25
26/// Template to construct the product Lie group of two other Lie groups
27/// Assumes Lie group structure for G and H
28template<typename G, typename H>
29class ProductLieGroup: public std::pair<G, H> {
30 GTSAM_CONCEPT_ASSERT(IsLieGroup<G>);
31 GTSAM_CONCEPT_ASSERT(IsLieGroup<H>);
32 typedef std::pair<G, H> Base;
33
34protected:
35 constexpr static const size_t dimension1 = traits<G>::dimension;
36 constexpr static const size_t dimension2 = traits<H>::dimension;
37
38public:
39 /// Default constructor yields identity
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
48 /// @name Group
49 /// @{
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 }
66 /// @}
67
68 /// @name Manifold
69 /// @{
70 inline constexpr static size_t dimension = dimension1 + dimension2;
71 inline static size_t Dim() { return dimension; }
72 inline size_t dim() const { return dimension; }
73
74 using TangentVector = Eigen::Matrix<double, static_cast<int>(dimension), 1>;
75 using ChartJacobian = OptionalJacobian<dimension, dimension>;
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 }
93 /// @}
94
95 /// @name Lie Group
96 /// @{
97protected:
98 using Jacobian = Eigen::Matrix<double, static_cast<int>(dimension), static_cast<int>(dimension)>;
99 using Jacobian1 = Eigen::Matrix<double, static_cast<int>(dimension1), static_cast<int>(dimension1)>;
100 using Jacobian2 = Eigen::Matrix<double, static_cast<int>(dimension2), static_cast<int>(dimension2)>;
101
102public:
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(p: between(g));
172 }
173 Jacobian AdjointMap() const {
174 const auto& adjG = traits<G>::AdjointMap(this->first);
175 const auto& adjH = traits<H>::AdjointMap(this->second);
176 size_t d1 = adjG.rows(), d2 = adjH.rows();
177 Matrix adj = Matrix::Zero(rows: d1 + d2, cols: d1 + d2);
178 adj.block(startRow: 0, startCol: 0, blockRows: d1, blockCols: d1) = adjG;
179 adj.block(startRow: d1, startCol: d1, blockRows: d2, blockCols: d2) = adjH;
180 return adj;
181 }
182 /// @}
183
184};
185
186// Define any direct product group to be a model of the multiplicative Group concept
187template<typename G, typename H>
188struct traits<ProductLieGroup<G, H> > : internal::LieGroupTraits<ProductLieGroup<G, H> > {};
189
190} // namespace gtsam
191
192