farm-ng-core
lie_group.h
Go to the documentation of this file.
1 // Copyright (c) 2011, Hauke Strasdat
2 // Copyright (c) 2012, Steven Lovegrove
3 // Copyright (c) 2021, farm-ng, inc.
4 //
5 // Use of this source code is governed by an MIT-style
6 // license that can be found in the LICENSE file or at
7 // https://opensource.org/licenses/MIT.
8 
9 #pragma once
10 #include "sophus/concepts/params.h"
11 
12 namespace sophus {
13 template <class TScalar, int kN>
14 class UnitVector;
15 
16 namespace concepts {
17 
18 template <class TT>
19 concept LieGroupImpl =
20  ParamsImpl<TT> && Tangent<TT> && std::is_same_v<
21  typename TT::Point,
22  Eigen::Vector<typename TT::Scalar, TT::kPointDim>> &&
23  (TT::kPointDim == TT::kAmbientDim // inhomogeneous point representation
24  || TT::kPointDim + 1 ==
25  TT::kAmbientDim) // or homogeneous point representation
26  && requires(
27  typename TT::Tangent tangent,
28  typename TT::Point point,
29  Eigen::Vector<CompatScalarEx<typename TT::Scalar>, TT::kPointDim>
30  compatible_point,
31  UnitVector<typename TT::Scalar, TT::kPointDim> direction,
32  UnitVector<CompatScalarEx<typename TT::Scalar>, TT::kPointDim>
33  compatible_direction,
34  typename TT::Params params,
35  Eigen::Vector<CompatScalarEx<typename TT::Scalar>, TT::kNumParams>
36  compatible_params,
37  Eigen::Matrix<typename TT::Scalar, TT::kAmbientDim, TT::kAmbientDim>
38  matrix,
39  Eigen::Matrix<typename TT::Scalar, TT::kDof, TT::kDof> adjoint) {
40  // constructors and factories
41  { TT::identityParams() } -> ConvertibleTo<typename TT::Params>;
42 
43  // Manifold / Lie Group concepts
44 
45  { TT::exp(tangent) } -> ConvertibleTo<typename TT::Params>;
46 
47  { TT::log(params) } -> ConvertibleTo<typename TT::Tangent>;
48 
49  {
50  TT::hat(tangent)
51  } -> ConvertibleTo<
52  Eigen::Matrix<typename TT::Scalar, TT::kAmbientDim, TT::kAmbientDim>>;
53 
54  { TT::vee(matrix) } -> ConvertibleTo<typename TT::Tangent>;
55 
56  // group operations
57  { TT::multiplication(params, params) } -> ConvertibleTo<typename TT::Params>;
58 
59 #if __cplusplus >= 202002L
60  {
61  TT::multiplication(params, compatible_params)
62  } -> ConvertibleTo<typename TT::template ParamsReturn<
63  CompatScalarEx<typename TT::Scalar>>>;
64 #endif
65 
66  { TT::inverse(params) } -> ConvertibleTo<typename TT::Params>;
67 
68  // Group actions
69  { TT::action(params, point) } -> ConvertibleTo<typename TT::Point>;
70 
71 #if __cplusplus >= 202002L
72  {
73  TT::action(params, compatible_point)
74  } -> ConvertibleTo<
75  typename TT::template PointReturn<CompatScalarEx<typename TT::Scalar>>>;
76 #endif
77 
78  {
79  TT::action(params, direction)
80  } -> ConvertibleTo<UnitVector<typename TT::Scalar, TT::kPointDim>>;
81 
82 #if __cplusplus >= 202002L
83  {
84  TT::action(params, compatible_direction)
85  } -> ConvertibleTo<typename TT::template UnitVectorReturn<
86  CompatScalarEx<typename TT::Scalar>>>;
87 #endif
88 
89  {
90  TT::toAmbient(point)
91  } -> ConvertibleTo<Eigen::Vector<typename TT::Scalar, TT::kAmbientDim>>;
92 
93  {
94  TT::adj(params)
95  } -> ConvertibleTo<Eigen::Matrix<typename TT::Scalar, TT::kDof, TT::kDof>>;
96 
97  // Matrices
98 
99  {
100  TT::compactMatrix(params)
101  } -> ConvertibleTo<
102  Eigen::Matrix<typename TT::Scalar, TT::kPointDim, TT::kAmbientDim>>;
103 
104  {
105  TT::matrix(params)
106  } -> ConvertibleTo<
107  Eigen::Matrix<typename TT::Scalar, TT::kAmbientDim, TT::kAmbientDim>>;
108 
109  // Derivatives
110 
111  {
112  TT::ad(tangent)
113  } -> ConvertibleTo<Eigen::Matrix<typename TT::Scalar, TT::kDof, TT::kDof>>;
114 
115  // {
116  // TT::dxExpX(tangent)
117  // } -> ConvertibleTo<
118  // Eigen::Matrix<typename TT::Scalar, TT::kNumParams, TT::kDof>>;
119 
120  {
121  TT::dxExpXAt0()
122  } -> ConvertibleTo<
123  Eigen::Matrix<typename TT::Scalar, TT::kNumParams, TT::kDof>>;
124 
125  {
126  TT::dxExpXTimesPointAt0(point)
127  } -> ConvertibleTo<
128  Eigen::Matrix<typename TT::Scalar, TT::kPointDim, TT::kDof>>;
129 
130  {
131  TT::dxThisMulExpXAt0(params)
132  } -> ConvertibleTo<
133  Eigen::Matrix<typename TT::Scalar, TT::kNumParams, TT::kDof>>;
134 
135  {
136  TT::dxLogThisInvTimesXAtThis(params)
137  } -> ConvertibleTo<
138  Eigen::Matrix<typename TT::Scalar, TT::kDof, TT::kNumParams>>;
139 };
140 
141 // Ideally, the LieSubgroupFunctions is not necessary and all these
142 // properties can be deduced.
143 template <class TT>
144 concept LieFactorGroupImpl = LieGroupImpl<TT> && requires(
145  typename TT::Tangent tangent,
146  typename TT::Params params,
147  typename TT::Point point) {
148  {
149  TT::matV(params, tangent)
150  } -> ConvertibleTo<
151  Eigen::Matrix<typename TT::Scalar, TT::kPointDim, TT::kPointDim>>;
152 
153  {
154  TT::matVInverse(params, tangent)
155  } -> ConvertibleTo<
156  Eigen::Matrix<typename TT::Scalar, TT::kPointDim, TT::kPointDim>>;
157 
158  {
159  TT::adjOfTranslation(params, point)
160  } -> ConvertibleTo<
161  Eigen::Matrix<typename TT::Scalar, TT::kPointDim, TT::kDof>>;
162 
163  {
164  TT::adOfTranslation(point)
165  } -> ConvertibleTo<
166  Eigen::Matrix<typename TT::Scalar, TT::kPointDim, TT::kPointDim>>;
167 };
168 
169 template <class TT>
170 concept LieGroup = LieGroupImpl<typename TT::Impl> && Params<TT> &&
171  std::is_same_v<
172  typename TT::Point,
173  Eigen::Vector<typename TT::Scalar, TT::kPointDim>> &&
174  requires(
175  TT g,
176  typename TT::Tangent tangent,
177  typename TT::Point point,
178  Eigen::Vector<CompatScalarEx<typename TT::Scalar>, TT::kPointDim>
179  compatible_point,
180  UnitVector<typename TT::Scalar, TT::kPointDim> direction,
181  UnitVector<CompatScalarEx<typename TT::Scalar>, TT::kPointDim>
182  compatible_direction,
183  typename TT::Params params,
184  Eigen::Matrix<typename TT::Scalar, TT::kAmbientDim, TT::kAmbientDim>
185  matrix,
186  Eigen::Matrix<typename TT::Scalar, TT::kDof, TT::kDof> adjoint) {
187  // Manifold / Lie Group concepts
188 
189  { TT::exp(tangent) } -> ConvertibleTo<TT>;
190 
191  { g.log() } -> ConvertibleTo<typename TT::Tangent>;
192 
193  // group operations
194  { g.operator*(g) } -> ConvertibleTo<TT>;
195 
196  { g.inverse() } -> ConvertibleTo<TT>;
197 
198  // Group actions
199  { g.operator*(point) } -> ConvertibleTo<typename TT::Point>;
200 
201 #if __cplusplus >= 202002L
202  {
203  g.operator*(compatible_point)
204  } -> ConvertibleTo<
205  typename TT::template PointReturn<CompatScalarEx<typename TT::Scalar>>>;
206 #endif
207 
208  {
209  g.operator*(direction)
210  } -> ConvertibleTo<UnitVector<typename TT::Scalar, TT::kPointDim>>;
211 
212 #if __cplusplus >= 202002L
213  {
214  g.operator*(compatible_direction)
215  } -> ConvertibleTo<typename TT::template UnitVectorReturn<
216  CompatScalarEx<typename TT::Scalar>>>;
217 #endif
218 
219  {
220  g.adj()
221  } -> ConvertibleTo<Eigen::Matrix<typename TT::Scalar, TT::kDof, TT::kDof>>;
222 
223  { g.leftPlus(tangent) } -> ConvertibleTo<TT>;
224 
225  { g.rightPlus(tangent) } -> ConvertibleTo<TT>;
226 
227  { g.leftMinus(g) } -> ConvertibleTo<typename TT::Tangent>;
228 
229  { g.rightMinus(g) } -> ConvertibleTo<typename TT::Tangent>;
230 
231  // Matrices
232 
233  {
234  g.compactMatrix()
235  } -> ConvertibleTo<
236  Eigen::Matrix<typename TT::Scalar, TT::kPointDim, TT::kAmbientDim>>;
237 
238  {
239  g.matrix()
240  } -> ConvertibleTo<
241  Eigen::Matrix<typename TT::Scalar, TT::kAmbientDim, TT::kAmbientDim>>;
242 
243  // Derivatives
244  {
245  TT::ad(tangent)
246  } -> ConvertibleTo<Eigen::Matrix<typename TT::Scalar, TT::kDof, TT::kDof>>;
247 
248  // {
249  // T::dxExpX(tangent)
250  // }
251  // -> ConvertibleTo<Eigen::Matrix<typename T::Scalar, T::kNumParams,
252  // T::kDof>>;
253 
254  {
255  TT::dxExpXAt0()
256  } -> ConvertibleTo<
257  Eigen::Matrix<typename TT::Scalar, TT::kNumParams, TT::kDof>>;
258 
259  {
260  TT::dxExpXTimesPointAt0(point)
261  } -> ConvertibleTo<
262  Eigen::Matrix<typename TT::Scalar, TT::kPointDim, TT::kDof>>;
263 
264  {
265  g.dxThisMulExpXAt0()
266  } -> ConvertibleTo<
267  Eigen::Matrix<typename TT::Scalar, TT::kNumParams, TT::kDof>>;
268 
269  {
270  g.dxLogThisInvTimesXAtThis()
271  } -> ConvertibleTo<
272  Eigen::Matrix<typename TT::Scalar, TT::kDof, TT::kNumParams>>;
273 
274  { TT::elementExamples() } -> ConvertibleTo<std::vector<TT>>;
275 };
276 
277 } // namespace concepts
278 } // namespace sophus
sophus::concepts::UnitVector
Definition: division_ring.h:17
sophus::concepts::LieGroupImpl
concept LieGroupImpl
Definition: lie_group.h:19
sophus::UnitVector
Definition: lie_group.h:14
sophus
Image MutImage, owning images types.
Definition: num_diff.h:20
sophus::concepts::Tangent
concept Tangent
Definition: params.h:34
sophus::concepts::LieGroup
concept LieGroup
Definition: lie_group.h:170
params.h
sophus::concepts::LieFactorGroupImpl
concept LieFactorGroupImpl
Definition: lie_group.h:144
sophus::concepts::CompatScalarEx
Definition: params.h:63
sophus::concepts::ConvertibleTo
concept ConvertibleTo
Definition: utils.h:33
sophus::concepts::Params
concept Params
Definition: params.h:41