Gitiles
Code Review Sign In
realtimeroboticsgroup.org / RealtimeRoboticsGroup / test / 3de38b02fbcca083f00ae6ced0ab9e7716b2eefb / . / include / ceres / internal / euler_angles.h
blob: 38f2702b6b45fcf008b0818197a304e126104024 [file] [log] [blame]
Austin Schuh3de38b02024-06-25 18:25:10 -0700[diff] [blame^]1// Ceres Solver - A fast non-linear least squares minimizer
2// Copyright 2023 Google Inc. All rights reserved.
3// http://ceres-solver.org/
4//
5// Redistribution and use in source and binary forms, with or without
6// modification, are permitted provided that the following conditions are met:
7//
8// * Redistributions of source code must retain the above copyright notice,
9// this list of conditions and the following disclaimer.
10// * Redistributions in binary form must reproduce the above copyright notice,
11// this list of conditions and the following disclaimer in the documentation
12// and/or other materials provided with the distribution.
13// * Neither the name of Google Inc. nor the names of its contributors may be
14// used to endorse or promote products derived from this software without
15// specific prior written permission.
16//
17// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27// POSSIBILITY OF SUCH DAMAGE.
28
29#ifndef CERES_PUBLIC_INTERNAL_EULER_ANGLES_H_
30#define CERES_PUBLIC_INTERNAL_EULER_ANGLES_H_
31
32#include <type_traits>
33
34namespace ceres {
35namespace internal {
36
37// The EulerSystem struct represents an Euler Angle Convention in compile time.
38// It acts like a trait structure and is also used as a tag for dispatching
39// Euler angle conversion function templates
40//
41// Internally, it implements the convention laid out in "Euler angle
42// conversion", Ken Shoemake, Graphics Gems IV, where a choice of axis for the
43// first rotation (out of 3) and 3 binary choices compactly specify all 24
44// rotation conventions
45//
46// - InnerAxis: Axis for the first rotation. This is specified by struct tags
47// axis::X, axis::Y, and axis::Z
48//
49// - Parity: Defines the parity of the axis permutation. The axis sequence has
50// Even parity if the second axis of rotation is 'greater-than' the first axis
51// of rotation according to the order X<Y<Z<X, otherwise it has Odd parity.
52// This is specified by struct tags Even and Odd
53//
54// - AngleConvention: Defines whether Proper Euler Angles (originally defined
55// by Euler, which has the last axis repeated, i.e. ZYZ, ZXZ, etc), or
56// Tait-Bryan Angles (introduced by the nautical and aerospace fields, i.e.
57// using ZYX for roll-pitch-yaw) are used. This is specified by struct Tags
58// ProperEuler and TaitBryan.
59//
60// - FrameConvention: Defines whether the three rotations are be in a global
61// frame of reference (extrinsic) or in a body centred frame of reference
62// (intrinsic). This is specified by struct tags Extrinsic and Intrinsic
63
64namespace axis {
65struct X : std::integral_constant<int, 0> {};
66struct Y : std::integral_constant<int, 1> {};
67struct Z : std::integral_constant<int, 2> {};
68} // namespace axis
69
70struct Even;
71struct Odd;
72
73struct ProperEuler;
74struct TaitBryan;
75
76struct Extrinsic;
77struct Intrinsic;
78
79template <typename InnerAxisType,
80 typename ParityType,
81 typename AngleConventionType,
82 typename FrameConventionType>
83struct EulerSystem {
84 static constexpr bool kIsParityOdd = std::is_same_v<ParityType, Odd>;
85 static constexpr bool kIsProperEuler =
86 std::is_same_v<AngleConventionType, ProperEuler>;
87 static constexpr bool kIsIntrinsic =
88 std::is_same_v<FrameConventionType, Intrinsic>;
89
90 static constexpr int kAxes[3] = {
91 InnerAxisType::value,
92 (InnerAxisType::value + 1 + static_cast<int>(kIsParityOdd)) % 3,
93 (InnerAxisType::value + 2 - static_cast<int>(kIsParityOdd)) % 3};
94};
95
96} // namespace internal
97
98// Define human readable aliases to the type of the tags
99using ExtrinsicXYZ = internal::EulerSystem<internal::axis::X,
100 internal::Even,
101 internal::TaitBryan,
102 internal::Extrinsic>;
103using ExtrinsicXYX = internal::EulerSystem<internal::axis::X,
104 internal::Even,
105 internal::ProperEuler,
106 internal::Extrinsic>;
107using ExtrinsicXZY = internal::EulerSystem<internal::axis::X,
108 internal::Odd,
109 internal::TaitBryan,
110 internal::Extrinsic>;
111using ExtrinsicXZX = internal::EulerSystem<internal::axis::X,
112 internal::Odd,
113 internal::ProperEuler,
114 internal::Extrinsic>;
115using ExtrinsicYZX = internal::EulerSystem<internal::axis::Y,
116 internal::Even,
117 internal::TaitBryan,
118 internal::Extrinsic>;
119using ExtrinsicYZY = internal::EulerSystem<internal::axis::Y,
120 internal::Even,
121 internal::ProperEuler,
122 internal::Extrinsic>;
123using ExtrinsicYXZ = internal::EulerSystem<internal::axis::Y,
124 internal::Odd,
125 internal::TaitBryan,
126 internal::Extrinsic>;
127using ExtrinsicYXY = internal::EulerSystem<internal::axis::Y,
128 internal::Odd,
129 internal::ProperEuler,
130 internal::Extrinsic>;
131using ExtrinsicZXY = internal::EulerSystem<internal::axis::Z,
132 internal::Even,
133 internal::TaitBryan,
134 internal::Extrinsic>;
135using ExtrinsicZXZ = internal::EulerSystem<internal::axis::Z,
136 internal::Even,
137 internal::ProperEuler,
138 internal::Extrinsic>;
139using ExtrinsicZYX = internal::EulerSystem<internal::axis::Z,
140 internal::Odd,
141 internal::TaitBryan,
142 internal::Extrinsic>;
143using ExtrinsicZYZ = internal::EulerSystem<internal::axis::Z,
144 internal::Odd,
145 internal::ProperEuler,
146 internal::Extrinsic>;
147/* Rotating axes */
148using IntrinsicZYX = internal::EulerSystem<internal::axis::X,
149 internal::Even,
150 internal::TaitBryan,
151 internal::Intrinsic>;
152using IntrinsicXYX = internal::EulerSystem<internal::axis::X,
153 internal::Even,
154 internal::ProperEuler,
155 internal::Intrinsic>;
156using IntrinsicYZX = internal::EulerSystem<internal::axis::X,
157 internal::Odd,
158 internal::TaitBryan,
159 internal::Intrinsic>;
160using IntrinsicXZX = internal::EulerSystem<internal::axis::X,
161 internal::Odd,
162 internal::ProperEuler,
163 internal::Intrinsic>;
164using IntrinsicXZY = internal::EulerSystem<internal::axis::Y,
165 internal::Even,
166 internal::TaitBryan,
167 internal::Intrinsic>;
168using IntrinsicYZY = internal::EulerSystem<internal::axis::Y,
169 internal::Even,
170 internal::ProperEuler,
171 internal::Intrinsic>;
172using IntrinsicZXY = internal::EulerSystem<internal::axis::Y,
173 internal::Odd,
174 internal::TaitBryan,
175 internal::Intrinsic>;
176using IntrinsicYXY = internal::EulerSystem<internal::axis::Y,
177 internal::Odd,
178 internal::ProperEuler,
179 internal::Intrinsic>;
180using IntrinsicYXZ = internal::EulerSystem<internal::axis::Z,
181 internal::Even,
182 internal::TaitBryan,
183 internal::Intrinsic>;
184using IntrinsicZXZ = internal::EulerSystem<internal::axis::Z,
185 internal::Even,
186 internal::ProperEuler,
187 internal::Intrinsic>;
188using IntrinsicXYZ = internal::EulerSystem<internal::axis::Z,
189 internal::Odd,
190 internal::TaitBryan,
191 internal::Intrinsic>;
192using IntrinsicZYZ = internal::EulerSystem<internal::axis::Z,
193 internal::Odd,
194 internal::ProperEuler,
195 internal::Intrinsic>;
196
197} // namespace ceres
198
199#endif // CERES_PUBLIC_INTERNAL_EULER_ANGLES_H_