sophus_lie/groups/
rotation3.rs

1use core::{
2    borrow::Borrow,
3    f64,
4    marker::PhantomData,
5};
6
7use log::warn;
8use sophus_autodiff::{
9    linalg::{
10        EPS_F64,
11        MatF64,
12        VecF64,
13    },
14    manifold::IsTangent,
15    params::{
16        HasParams,
17        IsParamsImpl,
18    },
19};
20
21use crate::{
22    EmptySliceError,
23    HasAverage,
24    HasDisambiguate,
25    IsLieGroupImpl,
26    IsRealLieFactorGroupImpl,
27    IsRealLieGroupImpl,
28    lie_group::{
29        LieGroup,
30        average::{
31            IterativeAverageError,
32            iterative_average,
33        },
34    },
35    prelude::*,
36    quaternion::QuaternionImpl,
37};
38extern crate alloc;
39
40/// 3d rotations - element of the Special Orthogonal group SO(3)
41///
42/// ## Generic parameters
43///
44///  * BATCH
45///     - batch dimension. If S is f64 or [sophus_autodiff::dual::DualScalar] then BATCH=1.
46///  * DM, DN
47///     - DM x DN is the static shape of the Jacobian to be computed if S == DualScalar<DM,DN>. If S
48///       == f64, then DM==0, DN==0.
49///
50/// ## Overview
51///
52/// * **Tangent space**: DoF - **(ω₀, ω₁. ω₂)**, `ω` the 3-d **angular** rate
53/// * **Internal parameters**:  4 – **[RE(q), IM(q)₀, IM(q)₁, IM(q)₂]**, unit quaternion `q` (|q| =
54///   1).
55/// * **Action space:** 3 (SO(3) acts on 3-d points)
56/// * **Matrix size:** 3 (represented as 3 × 3 matrices)
57///
58/// ### Group structure
59///
60/// The group of 3d rotations is represented by unit quaternions. The corresponding *matrix
61/// representation* is
62/// ```ascii
63/// R   =  (r² - vᵀv)I + 2vᵀv + 2r[v]ₓ
64///
65///
66///        / 1 - 2(y·y + z·z)     2(x·y - r·z)       2(x·z r·y)     \
67///        |                                                        |
68///     =  |   2(x·y + r·z)     1 - 2(x·x + z·z)     2(y·z - r·x)   |
69///        |                                                        |
70///        \   2(x·z - r·y)       2(y·z + r·x)     1 - 2(x·x + y·y) /
71/// ```
72/// with ``q = [r, v] = [r, (x, y, z)].`` ``R`` is a
73/// 3x3 rotation matrix with ``R·Rᵀ = I`` and ``det(R) = 1``.
74///
75///
76/// The *group operation* is quaternion multiplication:
77/// ```ascii
78/// qₗ ⊗ qᵣ =  rₗ · vᵣ  +  qᵣ · vₗ  +  vₗ × qᵣ
79/// ```
80/// where ``qₗ = [rₗ, vₗ]`` and ``qᵣ = [rᵣ, vᵣ]`` are the left and right quaternion operands.
81///
82/// In rotation matrix form, the group operation is simply matrix multiplication:
83///
84/// ```ascii
85/// Rₗ ⊗ Rᵣ =  Rₗ·Rᵣ
86/// ```
87///
88/// The inverse of a rotation is given by the quaternion conjugate:
89/// ```ascii
90/// q⁻¹ =  RE(q) - IM(q)
91/// ```
92/// In matrix form, the inverse of a rotation is given by the transpose:
93/// ```ascii
94/// R⁻¹ = Rᵀ
95/// ```
96///
97/// ### Lie group properties
98///
99/// The tangent space of the 3d rotation group is the space of rotational velocities. Alternatively,
100/// it can be understood in terms of axis-angle representation. Given a rotation around an axis
101/// ``v`` with angle ``θ``, the corresponding tangent vector is given by ``v · θ``.
102///
103/// Tangent vectors are mapped to the Lie algebra matrix representation via the *hat* operator:
104///
105/// ```ascii
106///        /                 \
107///        |  0    -ω₂    ω₁ |
108///        |                 |
109/// ω^  =  |  ω₂    0    -ω₀ |
110///        |                 |
111///        | -ω₁    ω₀    0  |
112///        \                 /
113/// ```
114///
115/// For the group of 3d rotations, also known as the Special Orthogonal group SO(3), the
116/// hat-operator is the skew-symmetric matrix operator: ``ω^ = [ω]ₓ``, which is closely related
117/// to the cross product: ``[ω]ₓ · y = x × y``.
118///
119/// The *exponential map*: ``exp: ℝ³ -> SO(3)`` from rotational velocities to 3d rotations given by
120/// the Rodrigues' rotation:
121///
122/// ```ascii
123/// exp(x) = cos(θ) · I + sin(θ) · [ω]ₓ + (1 - cos(θ)) · [ω]ₓ²
124/// ```
125/// The exponential map is surjective, meaning that every rotation can be represented by a
126/// tangent vector. Its inverse, the *logarithm map*: ``log: SO(3) -> ℝ³`` is restricted
127/// such that the rotation angle is in the range ``[-π, π]``.
128///
129/// Like most Lie groups, 3d rotations do not commute, i.e., in general ``R · Q · R⁻¹ ≠ Q``. Hence,
130/// it is of interest to as what ``R · Q · R⁻¹`` is equal to instead. This is called the *group
131/// adjoint*:
132/// ```ascii
133/// Adjᵪ: ℝ³ -> ℝ³, Adjᵪ(ω) = X ⊗ hat(ω) ⊗ X⁻¹.
134/// ```
135/// The adjoint is a linear map, hence there exists a matrix representation ``M`` such that
136/// ``Adjᵪ(ω) = M · ω``. For the group of 3d rotations, the adjoint of group element ``R`` is given
137/// by the rotation matrix itself:
138/// ```ascii
139/// Adj(ω) = R · ω
140/// ```
141///
142/// If one takes the derivative of the adjoint map, then
143/// one gets the *adjoint representation* of the: ``∂t Adjᵪ(ω) = UV - VU``.
144/// ```ascii
145/// adᵩ(ω) = (φ^·ω^ - ω^·φ^)ᵛ
146/// ```
147/// The adjoint representation is a linear map, hence there exists a matrix representation
148/// ``A`` such that ``adᵩ(ω) = A · ω``. For 3d rotations, the adjoint representation is
149/// given by the skew-symmetric matrix operator:
150/// ```ascii
151/// ad(ωᵣ)  =  [ωₗ]ₓ · ωᵣ  =  ωₗ × ωᵣ
152/// ```
153pub type Rotation3<S, const BATCH: usize, const DM: usize, const DN: usize> =
154    LieGroup<S, 3, 4, 3, 3, BATCH, DM, DN, Rotation3Impl<S, BATCH, DM, DN>>;
155
156/// 3d rotation with f64 scalar type a - element of the Special Orthogonal group SO(3)
157///
158/// See [Rotation3] for details.
159pub type Rotation3F64 = Rotation3<f64, 1, 0, 0>;
160
161/// 3d rotation implementation.
162///
163/// See [Rotation3] for details.
164#[derive(Debug, Copy, Clone, Default)]
165pub struct Rotation3Impl<
166    S: IsScalar<BATCH, DM, DN>,
167    const BATCH: usize,
168    const DM: usize,
169    const DN: usize,
170> {
171    phantom: PhantomData<S>,
172}
173
174impl<S: IsScalar<BATCH, DM, DN>, const BATCH: usize, const DM: usize, const DN: usize>
175    HasDisambiguate<S, 4, BATCH, DM, DN> for Rotation3Impl<S, BATCH, DM, DN>
176{
177    fn disambiguate(params: S::Vector<4>) -> S::Vector<4> {
178        // make sure real component is always positive
179        let is_positive = S::from_f64(0.0).less_equal(&params.elem(0));
180
181        params.select(&is_positive, -params)
182    }
183}
184
185impl<S: IsScalar<BATCH, DM, DN>, const BATCH: usize, const DM: usize, const DN: usize>
186    IsParamsImpl<S, 4, BATCH, DM, DN> for Rotation3Impl<S, BATCH, DM, DN>
187{
188    fn params_examples() -> alloc::vec::Vec<S::Vector<4>> {
189        const NEAR_PI: f64 = f64::consts::PI - 1e-6;
190        const NEAR_MINUS_PI: f64 = f64::consts::PI - 1e-6;
191
192        alloc::vec![
193            *Rotation3::<S, BATCH, DM, DN>::exp(S::Vector::<3>::from_f64_array([0.0, 0.0, 0.0]))
194                .params(),
195            *Rotation3::<S, BATCH, DM, DN>::exp(S::Vector::<3>::from_f64_array([0.1, 0.5, -0.1]))
196                .params(),
197            *Rotation3::<S, BATCH, DM, DN>::exp(S::Vector::<3>::from_f64_array([0.1, 2.0, -0.1]))
198                .params(),
199            *Rotation3::<S, BATCH, DM, DN>::exp(S::Vector::<3>::from_f64_array([0.0, 0.2, 1.0]))
200                .params(),
201            *Rotation3::<S, BATCH, DM, DN>::exp(S::Vector::<3>::from_f64_array([-0.2, 0.0, 0.8]))
202                .params(),
203            // Test cases around +π and -π
204            *Rotation3::<S, BATCH, DM, DN>::exp(S::Vector::<3>::from_f64_array([
205                NEAR_PI, 0.0, 0.0
206            ]))
207            .params(), // +π rotation about the x-axis
208            *Rotation3::<S, BATCH, DM, DN>::exp(S::Vector::<3>::from_f64_array([
209                NEAR_MINUS_PI,
210                0.0,
211                0.0
212            ]))
213            .params(), // -π rotation about the x-axis
214            *Rotation3::<S, BATCH, DM, DN>::exp(S::Vector::<3>::from_f64_array([
215                0.0, NEAR_PI, 0.0
216            ]))
217            .params(), // +π rotation about the y-axis
218            *Rotation3::<S, BATCH, DM, DN>::exp(S::Vector::<3>::from_f64_array([
219                0.0,
220                NEAR_MINUS_PI,
221                0.0
222            ]))
223            .params(), // -π rotation about the y-axis
224            *Rotation3::<S, BATCH, DM, DN>::exp(S::Vector::<3>::from_f64_array([
225                0.0, 0.0, NEAR_PI
226            ]))
227            .params(), // +π rotation about the z-axis
228            *Rotation3::<S, BATCH, DM, DN>::exp(S::Vector::<3>::from_f64_array([
229                0.0,
230                0.0,
231                NEAR_MINUS_PI
232            ]))
233            .params(), // -π rotation about the z-axis
234            // Close to +π and -π, but not exactly, to test boundary behavior
235            *Rotation3::<S, BATCH, DM, DN>::exp(S::Vector::<3>::from_f64_array([3.0, 0.0, 0.0]))
236                .params(),
237            *Rotation3::<S, BATCH, DM, DN>::exp(S::Vector::<3>::from_f64_array([-3.0, 0.0, 0.0]))
238                .params(),
239            *Rotation3::<S, BATCH, DM, DN>::exp(S::Vector::<3>::from_f64_array([0.0, 3.0, 0.0]))
240                .params(),
241            *Rotation3::<S, BATCH, DM, DN>::exp(S::Vector::<3>::from_f64_array([0.0, -3.0, 0.0]))
242                .params(),
243            *Rotation3::<S, BATCH, DM, DN>::exp(S::Vector::<3>::from_f64_array([0.0, 0.0, 3.0]))
244                .params(),
245            *Rotation3::<S, BATCH, DM, DN>::exp(S::Vector::<3>::from_f64_array([0.0, 0.0, -3.0]))
246                .params(),
247            // Halfway to π rotations, to cover intermediate rotations
248            *Rotation3::<S, BATCH, DM, DN>::exp(S::Vector::<3>::from_f64_array([
249                f64::consts::FRAC_PI_2,
250                0.0,
251                0.0,
252            ]))
253            .params(), // +π/2 about x-axis
254            *Rotation3::<S, BATCH, DM, DN>::exp(S::Vector::<3>::from_f64_array([
255                0.0,
256                f64::consts::FRAC_PI_2,
257                0.0,
258            ]))
259            .params(), // +π/2 about y-axis
260            *Rotation3::<S, BATCH, DM, DN>::exp(S::Vector::<3>::from_f64_array([
261                0.0,
262                0.0,
263                f64::consts::FRAC_PI_2,
264            ]))
265            .params(), // +π/2 about z-axis
266            *Rotation3::<S, BATCH, DM, DN>::exp(S::Vector::<3>::from_f64_array([
267                -f64::consts::FRAC_PI_2,
268                0.0,
269                0.0,
270            ]))
271            .params(), // -π/2 about x-axis
272            *Rotation3::<S, BATCH, DM, DN>::exp(S::Vector::<3>::from_f64_array([
273                0.0,
274                -f64::consts::FRAC_PI_2,
275                0.0,
276            ]))
277            .params(), // -π/2 about y-axis
278            *Rotation3::<S, BATCH, DM, DN>::exp(S::Vector::<3>::from_f64_array([
279                0.0,
280                0.0,
281                -f64::consts::FRAC_PI_2,
282            ]))
283            .params(), // -π/2 about z-axis
284            // Complex combination rotations around the boundary
285            *Rotation3::<S, BATCH, DM, DN>::exp(S::Vector::<3>::from_f64_array([
286                NEAR_PI / 2.0,
287                NEAR_PI / 2.0,
288                0.0,
289            ]))
290            .params(),
291            *Rotation3::<S, BATCH, DM, DN>::exp(S::Vector::<3>::from_f64_array([
292                NEAR_MINUS_PI / 2.0,
293                0.0,
294                NEAR_PI / 2.0,
295            ]))
296            .params(),
297            *Rotation3::<S, BATCH, DM, DN>::exp(S::Vector::<3>::from_f64_array([
298                0.0,
299                NEAR_PI / 2.0,
300                NEAR_PI / 2.0,
301            ]))
302            .params(),
303        ]
304    }
305
306    fn invalid_params_examples() -> alloc::vec::Vec<S::Vector<4>> {
307        alloc::vec![
308            S::Vector::<4>::from_f64_array([0.0, 0.0, 0.0, 0.0]),
309            S::Vector::<4>::from_f64_array([0.5, 0.5, 0.5, 0.0]),
310            S::Vector::<4>::from_f64_array([0.5, -0.5, 0.5, 1.0]),
311        ]
312    }
313
314    fn are_params_valid(params: S::Vector<4>) -> S::Mask {
315        let norm = params.borrow().norm();
316        (norm - S::from_f64(1.0))
317            .abs()
318            .less_equal(&S::from_f64(EPS_F64))
319    }
320}
321
322impl<S: IsScalar<BATCH, DM, DN>, const BATCH: usize, const DM: usize, const DN: usize>
323    IsTangent<S, 3, BATCH, DM, DN> for Rotation3Impl<S, BATCH, DM, DN>
324{
325    fn tangent_examples() -> alloc::vec::Vec<S::Vector<3>> {
326        alloc::vec![
327            S::Vector::<3>::from_f64_array([0.0, 0.0, 0.0]),
328            S::Vector::<3>::from_f64_array([0.0001, 0.0, 0.0]),
329            S::Vector::<3>::from_f64_array([1.0, 0.0, 0.0]),
330            S::Vector::<3>::from_f64_array([0.0, 1.0, 0.0]),
331            S::Vector::<3>::from_f64_array([0.0, 0.0, 1.0]),
332            S::Vector::<3>::from_f64_array([0.5, 0.5, 0.1]),
333            S::Vector::<3>::from_f64_array([-0.1, -0.5, -0.5]),
334            S::Vector::<3>::from_f64_array([2.5, 0.5, 0.1]),
335            S::Vector::<3>::from_f64_array([0.5, 2.5, 0.1]),
336            S::Vector::<3>::from_f64_array([0.5, 0.1, -2.5]),
337        ]
338    }
339}
340
341impl<S: IsScalar<BATCH, DM, DN>, const BATCH: usize, const DM: usize, const DN: usize>
342    IsLieGroupImpl<S, 3, 4, 3, 3, BATCH, DM, DN> for Rotation3Impl<S, BATCH, DM, DN>
343{
344    const IS_ORIGIN_PRESERVING: bool = true;
345    const IS_AXIS_DIRECTION_PRESERVING: bool = false;
346    const IS_DIRECTION_VECTOR_PRESERVING: bool = false;
347    const IS_SHAPE_PRESERVING: bool = true;
348    const IS_DISTANCE_PRESERVING: bool = true;
349    const IS_PARALLEL_LINE_PRESERVING: bool = true;
350
351    fn identity_params() -> S::Vector<4> {
352        QuaternionImpl::<S, BATCH, DM, DN>::one()
353    }
354
355    fn adj(params: &S::Vector<4>) -> S::Matrix<3, 3> {
356        Self::matrix(params)
357    }
358
359    fn ad(omega: S::Vector<3>) -> S::Matrix<3, 3> {
360        Self::hat(omega)
361    }
362
363    fn exp(omega: S::Vector<3>) -> S::Vector<4> {
364        const EPS: f64 = EPS_F64;
365        let theta_sq = omega.squared_norm();
366
367        let theta_po4 = theta_sq * theta_sq;
368        let theta = theta_sq.sqrt();
369        let half_theta: S = S::from_f64(0.5) * theta;
370
371        let near_zero = theta_sq.less_equal(&S::from_f64(EPS * EPS));
372
373        let imag_factor = (S::from_f64(0.5) - S::from_f64(1.0 / 48.0) * theta_sq
374            + S::from_f64(1.0 / 3840.0) * theta_po4)
375            .select(&near_zero, half_theta.sin() / theta);
376
377        let real_factor = (S::from_f64(1.0) - S::from_f64(1.0 / 8.0) * theta_sq
378            + S::from_f64(1.0 / 384.0) * theta_po4)
379            .select(&near_zero, half_theta.cos());
380
381        S::Vector::<4>::from_array([
382            real_factor,
383            imag_factor * omega.elem(0),
384            imag_factor * omega.elem(1),
385            imag_factor * omega.elem(2),
386        ])
387    }
388
389    fn log(params: &S::Vector<4>) -> S::Vector<3> {
390        const EPS: f64 = EPS_F64;
391        let ivec: S::Vector<3> = params.get_fixed_subvec::<3>(1);
392
393        let squared_n = ivec.squared_norm();
394        let w = params.elem(0);
395
396        let near_zero = squared_n.less_equal(&S::from_f64(EPS * EPS));
397
398        let w_sq = w * w;
399        let t0 = S::from_f64(2.0) / w - S::from_f64(2.0 / 3.0) * squared_n / (w_sq * w);
400
401        let n = squared_n.sqrt();
402
403        let sign = S::from_f64(-1.0).select(&w.less_equal(&S::from_f64(0.0)), S::from_f64(1.0));
404        let atan_nbyw = sign * n.atan2(sign * w);
405
406        let t = S::from_f64(2.0) * atan_nbyw / n;
407
408        let two_atan_nbyd_by_n = t0.select(&near_zero, t);
409
410        ivec.scaled(two_atan_nbyd_by_n)
411    }
412
413    fn hat(omega: S::Vector<3>) -> S::Matrix<3, 3> {
414        let o0 = omega.elem(0);
415        let o1 = omega.elem(1);
416        let o2 = omega.elem(2);
417
418        S::Matrix::from_array2([
419            [S::zero(), -o2, o1],
420            [o2, S::zero(), -o0],
421            [-o1, o0, S::zero()],
422        ])
423    }
424
425    fn vee(omega_hat: S::Matrix<3, 3>) -> S::Vector<3> {
426        S::Vector::<3>::from_array([
427            omega_hat.elem([2, 1]),
428            omega_hat.elem([0, 2]),
429            omega_hat.elem([1, 0]),
430        ])
431    }
432
433    fn inverse(params: &S::Vector<4>) -> S::Vector<4> {
434        QuaternionImpl::<S, BATCH, DM, DN>::conjugate(params)
435    }
436
437    fn transform(params: &S::Vector<4>, point: S::Vector<3>) -> S::Vector<3> {
438        Self::matrix(params) * point
439    }
440
441    fn to_ambient(point: S::Vector<3>) -> S::Vector<3> {
442        point
443    }
444
445    fn compact(params: &S::Vector<4>) -> S::Matrix<3, 3> {
446        Self::matrix(params)
447    }
448
449    fn matrix(params: &S::Vector<4>) -> S::Matrix<3, 3> {
450        let r = params.elem(0);
451        let x = params.elem(1);
452        let y = params.elem(2);
453        let z = params.elem(3);
454        let one = S::from_f64(1.0);
455        let two = S::from_f64(2.0);
456
457        let two_x = two * x;
458        let two_y = two * y;
459        let two_z = two * z;
460        let two_r = two * r;
461
462        let two_xx = two_x * x;
463        let two_yy = two_y * y;
464        let two_zz = two_z * z;
465        let two_xy = two_x * y;
466        let two_xz = two_x * z;
467        let two_yz = two_y * z;
468        let two_rx = two_r * x;
469        let two_ry = two_r * y;
470        let two_rz = two_r * z;
471
472        S::Matrix::from_array2([
473            [
474                one - (two_yy + two_zz),
475                (two_xy - two_rz),
476                (two_xz + two_ry),
477            ],
478            [
479                (two_xy + two_rz),
480                one - (two_xx + two_zz),
481                (two_yz - two_rx),
482            ],
483            [
484                (two_xz - two_ry),
485                (two_yz + two_rx),
486                one - (two_xx + two_yy),
487            ],
488        ])
489    }
490
491    type GenG<S2: IsScalar<BATCH, DM, DN>> = Rotation3Impl<S2, BATCH, DM, DN>;
492    type RealG = Rotation3Impl<S::RealScalar, BATCH, 0, 0>;
493    type DualG<const M: usize, const N: usize> = Rotation3Impl<S::DualScalar<M, N>, BATCH, M, N>;
494
495    fn group_mul(lhs_params: &S::Vector<4>, rhs_params: S::Vector<4>) -> S::Vector<4> {
496        QuaternionImpl::<S, BATCH, DM, DN>::mult(lhs_params, rhs_params)
497    }
498}
499
500impl<S: IsRealScalar<BATCH>, const BATCH: usize> IsRealLieGroupImpl<S, 3, 4, 3, 3, BATCH>
501    for Rotation3Impl<S, BATCH, 0, 0>
502{
503    fn dx_exp_x_at_0() -> S::Matrix<4, 3> {
504        S::Matrix::from_f64_array2([
505            [0.0, 0.0, 0.0],
506            [0.5, 0.0, 0.0],
507            [0.0, 0.5, 0.0],
508            [0.0, 0.0, 0.5],
509        ])
510    }
511
512    fn da_a_mul_b(a: S::Vector<4>, b: S::Vector<4>) -> S::Matrix<4, 4> {
513        let lhs_re = a.elem(0);
514        let rhs_re = b.elem(0);
515
516        let lhs_ivec = a.get_fixed_subvec::<3>(1);
517        let rhs_ivec = b.get_fixed_subvec::<3>(1);
518
519        let re = lhs_re * rhs_re - lhs_ivec.dot(rhs_ivec);
520
521        let is_positive = S::from_f64(0.0).less_equal(&re);
522
523        let b_real = b.elem(0);
524        let b_imag0 = b.elem(1);
525        let b_imag1 = b.elem(2);
526        let b_imag2 = b.elem(3);
527
528        S::Matrix::<4, 4>::from_array2([
529            [b_real, -b_imag0, -b_imag1, -b_imag2],
530            [b_imag0, b_real, b_imag2, -b_imag1],
531            [b_imag1, -b_imag2, b_real, b_imag0],
532            [b_imag2, b_imag1, -b_imag0, b_real],
533        ])
534        .select(
535            &is_positive,
536            S::Matrix::<4, 4>::from_array2([
537                [-b_real, b_imag0, b_imag1, b_imag2],
538                [-b_imag0, -b_real, -b_imag2, b_imag1],
539                [-b_imag1, b_imag2, -b_real, -b_imag0],
540                [-b_imag2, -b_imag1, b_imag0, -b_real],
541            ]),
542        )
543    }
544
545    fn db_a_mul_b(a: S::Vector<4>, b: S::Vector<4>) -> S::Matrix<4, 4> {
546        let lhs_re = a.elem(0);
547        let rhs_re = b.elem(0);
548        let lhs_ivec = a.get_fixed_subvec::<3>(1);
549        let rhs_ivec = b.get_fixed_subvec::<3>(1);
550        let re = lhs_re * rhs_re - lhs_ivec.dot(rhs_ivec);
551        let is_positive = S::from_f64(0.0).less_equal(&re);
552
553        let a_real = a.elem(0);
554        let a_imag0 = a.elem(1);
555        let a_imag1 = a.elem(2);
556        let a_imag2 = a.elem(3);
557
558        S::Matrix::<4, 4>::from_array2([
559            [a_real, -a_imag0, -a_imag1, -a_imag2],
560            [a_imag0, a_real, -a_imag2, a_imag1],
561            [a_imag1, a_imag2, a_real, -a_imag0],
562            [a_imag2, -a_imag1, a_imag0, a_real],
563        ])
564        .select(
565            &is_positive,
566            S::Matrix::<4, 4>::from_array2([
567                [-a_real, a_imag0, a_imag1, a_imag2],
568                [-a_imag0, -a_real, a_imag2, -a_imag1],
569                [-a_imag1, -a_imag2, -a_real, a_imag0],
570                [-a_imag2, a_imag1, -a_imag0, -a_real],
571            ]),
572        )
573    }
574
575    fn dx_exp_x_times_point_at_0(point: S::Vector<3>) -> S::Matrix<3, 3> {
576        Self::hat(-point)
577    }
578
579    fn dx_exp(omega: S::Vector<3>) -> S::Matrix<4, 3> {
580        let theta_sq = omega.squared_norm();
581
582        let near_zero = theta_sq.less_equal(&S::from_f64(EPS_F64));
583
584        let dx0 = Self::dx_exp_x_at_0();
585
586        let omega_0 = omega.elem(0);
587        let omega_1 = omega.elem(1);
588        let omega_2 = omega.elem(2);
589        let theta = theta_sq.sqrt();
590        let a = (S::from_f64(0.5) * theta).sin() / theta;
591        let b = (S::from_f64(0.5) * theta).cos() / (theta_sq)
592            - S::from_f64(2.0) * (S::from_f64(0.5) * theta).sin() / (theta_sq * theta);
593
594        let dx = S::Matrix::from_array2([
595            [-omega_0 * a, -omega_1 * a, -omega_2 * a],
596            [
597                omega_0 * omega_0 * b + S::from_f64(2.0) * a,
598                omega_0 * omega_1 * b,
599                omega_0 * omega_2 * b,
600            ],
601            [
602                omega_0 * omega_1 * b,
603                omega_1 * omega_1 * b + S::from_f64(2.0) * a,
604                omega_1 * omega_2 * b,
605            ],
606            [
607                omega_0 * omega_2 * b,
608                omega_1 * omega_2 * b,
609                omega_2 * omega_2 * b + S::from_f64(2.0) * a,
610            ],
611        ])
612        .scaled(S::from_f64(0.5));
613        dx0.select(&near_zero, dx)
614    }
615
616    fn has_shortest_path_ambiguity(params: &S::Vector<4>) -> S::Mask {
617        let theta = Self::log(params).norm();
618        (theta - S::from_f64(core::f64::consts::PI))
619            .abs()
620            .less_equal(&S::from_f64(EPS_F64.sqrt()))
621    }
622
623    fn left_jacobian(omega: S::Vector<3>) -> S::Matrix<3, 3> {
624        let theta_sq = omega.squared_norm();
625        let near_zero = theta_sq.less_equal(&S::from_f64(EPS_F64));
626        let id = S::Matrix::<3, 3>::identity();
627        let omega_hat = Self::hat(omega);
628
629        let jl0 = id - omega_hat.scaled(S::from_f64(0.5));
630
631        let theta = theta_sq.sqrt();
632        let a = (S::ones() - theta.cos()) / theta_sq;
633        let b = (theta - theta.sin()) / (theta_sq * theta);
634        let jl = id + omega_hat.scaled(a) + omega_hat.mat_mul(omega_hat).scaled(b);
635
636        jl0.select(&near_zero, jl)
637    }
638
639    /// Inverse left Jacobian  Jₗ⁻¹(ω).
640    fn inv_left_jacobian(omega: S::Vector<3>) -> S::Matrix<3, 3> {
641        let theta_sq = omega.squared_norm();
642        let near_zero = theta_sq.less_equal(&S::from_f64(EPS_F64));
643        let id = S::Matrix::<3, 3>::identity();
644        let omega_hat = Self::hat(omega);
645
646        let jli0 = id + omega_hat.scaled(S::from_f64(0.5));
647
648        let theta = theta_sq.sqrt();
649        let sin_theta = theta.sin();
650        let cos_theta = theta.cos();
651        let c = (S::ones() / theta_sq)
652            - (S::from_f64(1.0) + cos_theta) / (S::from_f64(2.0) * theta * sin_theta);
653        let jli = id - omega_hat.scaled(S::from_f64(0.5)) + omega_hat.mat_mul(omega_hat).scaled(c);
654
655        jli0.select(&near_zero, jli)
656    }
657
658    fn dparams_matrix(params: &<S>::Vector<4>, col_idx: usize) -> <S>::Matrix<3, 4> {
659        let re = params.elem(0);
660        let i = params.elem(1);
661        let j = params.elem(2);
662        let k = params.elem(3);
663
664        //  helper lambda:
665        let scaled = |val: S, factor: f64| -> S { val * S::from_f64(factor) };
666
667        match col_idx {
668            // --------------------------------------------------
669            // col_x
670            //
671            // partial wrt re => (0,       2k,    -2j)
672            // partial wrt i  => (0,       2j,     2k)
673            // partial wrt j  => (-4j,     2i,     -2re)
674            // partial wrt k  => (-4k,     2re,    2i)
675            0 => S::Matrix::from_array2([
676                [S::zero(), S::zero(), scaled(j, -4.0), scaled(k, -4.0)],
677                [
678                    scaled(k, 2.0),
679                    scaled(j, 2.0),
680                    scaled(i, 2.0),
681                    scaled(re, 2.0),
682                ],
683                [
684                    scaled(j, -2.0),
685                    scaled(k, 2.0),
686                    scaled(re, -2.0),
687                    scaled(i, 2.0),
688                ],
689            ]),
690
691            // --------------------------------------------------
692            // col_y
693            //
694            // partial wrt re => (-2k,        0,       2i)
695            // partial wrt i  => (2j,        -4i,      2re)
696            // partial wrt j  => (2i,         0,       2k)
697            // partial wrt k  => (-2re,      -4k,      2j)
698            1 => S::Matrix::from_array2([
699                [
700                    scaled(k, -2.0),  // row0, partial wrt re
701                    scaled(j, 2.0),   // row0, partial wrt i
702                    scaled(i, 2.0),   // row0, partial wrt j
703                    scaled(re, -2.0), // row0, partial wrt k
704                ],
705                [
706                    S::zero(),       // row1, partial wrt re
707                    scaled(i, -4.0), // row1, partial wrt i
708                    S::zero(),       // row1, partial wrt j
709                    scaled(k, -4.0), // row1, partial wrt k
710                ],
711                [
712                    scaled(i, 2.0),  // row2, partial wrt re
713                    scaled(re, 2.0), // row2, partial wrt i
714                    scaled(k, 2.0),  // row2, partial wrt j
715                    scaled(j, 2.0),  // row2, partial wrt k
716                ],
717            ]),
718
719            // --------------------------------------------------
720            // col_z
721            //
722            // partial wrt re => ( 2j,      -2i,      0 )
723            // partial wrt i  => ( 2k,      -2re,   -4i )
724            // partial wrt j  => ( 2re,      2k,    -4j )
725            // partial wrt k  => ( 2i,       2j,     0 )
726            2 => S::Matrix::from_array2([
727                [
728                    scaled(j, 2.0),  // row0 wrt re
729                    scaled(k, 2.0),  // row0 wrt i
730                    scaled(re, 2.0), // row0 wrt j
731                    scaled(i, 2.0),  // row0 wrt k
732                ],
733                [
734                    scaled(i, -2.0),  // row1 wrt re
735                    scaled(re, -2.0), // row1 wrt i
736                    scaled(k, 2.0),   // row1 wrt j
737                    scaled(j, 2.0),   // row1 wrt k
738                ],
739                [
740                    S::zero(),       // row2 wrt re
741                    scaled(i, -4.0), // row2 wrt i
742                    scaled(j, -4.0), // row2 wrt j
743                    S::zero(),       // row2 wrt k
744                ],
745            ]),
746
747            _ => panic!("Invalid column index: {}", col_idx),
748        }
749    }
750}
751
752impl<S: IsScalar<BATCH, DM, DN>, const BATCH: usize, const DM: usize, const DN: usize>
753    crate::IsLieFactorGroupImpl<S, 3, 4, 3, BATCH, DM, DN> for Rotation3Impl<S, BATCH, DM, DN>
754{
755    type GenFactorG<S2: IsScalar<BATCH, M, N>, const M: usize, const N: usize> =
756        Rotation3Impl<S2, BATCH, M, N>;
757    type RealFactorG = Rotation3Impl<S::RealScalar, BATCH, 0, 0>;
758    type DualFactorG<const M: usize, const N: usize> =
759        Rotation3Impl<S::DualScalar<M, N>, BATCH, M, N>;
760
761    fn mat_v(omega: S::Vector<3>) -> S::Matrix<3, 3> {
762        let theta_sq = omega.squared_norm();
763        let mat_omega: S::Matrix<3, 3> = Rotation3Impl::<S, BATCH, DM, DN>::hat(omega);
764        let mat_omega_sq = mat_omega.mat_mul(mat_omega);
765
766        let near_zero = theta_sq.less_equal(&S::from_f64(EPS_F64));
767
768        let mat_v0 = S::Matrix::<3, 3>::identity() + mat_omega.scaled(S::from_f64(0.5));
769
770        let theta = theta_sq.sqrt();
771        let mat_v = S::Matrix::<3, 3>::identity()
772            + mat_omega.scaled((S::from_f64(1.0) - theta.cos()) / theta_sq)
773            + mat_omega_sq.scaled((theta - theta.sin()) / (theta_sq * theta));
774
775        mat_v0.select(&near_zero, mat_v)
776    }
777
778    fn mat_v_inverse(omega: S::Vector<3>) -> S::Matrix<3, 3> {
779        let theta_sq = omega.dot(omega);
780        let mat_omega: S::Matrix<3, 3> = Rotation3Impl::<S, BATCH, DM, DN>::hat(omega);
781        let mat_omega_sq = mat_omega.mat_mul(mat_omega);
782
783        let near_zero = theta_sq.less_equal(&S::from_f64(EPS_F64));
784
785        let mat_v_inv0 = S::Matrix::<3, 3>::identity() - mat_omega.scaled(S::from_f64(0.5))
786            + mat_omega_sq.scaled(S::from_f64(1. / 12.));
787
788        let theta = theta_sq.sqrt();
789        let half_theta = S::from_f64(0.5) * theta;
790
791        let mat_v_inv = S::Matrix::<3, 3>::identity() - mat_omega.scaled(S::from_f64(0.5))
792            + mat_omega_sq.scaled(
793                (S::from_f64(1.0)
794                    - (S::from_f64(0.5) * theta * half_theta.cos()) / half_theta.sin())
795                    / (theta * theta),
796            );
797
798        mat_v_inv0.select(&near_zero, mat_v_inv)
799    }
800
801    fn adj_of_translation(params: &S::Vector<4>, point: S::Vector<3>) -> S::Matrix<3, 3> {
802        Rotation3Impl::<S, BATCH, DM, DN>::hat(point)
803            .mat_mul(Rotation3Impl::<S, BATCH, DM, DN>::matrix(params))
804    }
805
806    fn ad_of_translation(point: S::Vector<3>) -> S::Matrix<3, 3> {
807        Rotation3Impl::<S, BATCH, DM, DN>::hat(point)
808    }
809}
810
811impl<S: IsRealScalar<BATCH>, const BATCH: usize> IsRealLieFactorGroupImpl<S, 3, 4, 3, BATCH>
812    for Rotation3Impl<S, BATCH, 0, 0>
813{
814    fn dx_mat_v(omega: S::Vector<3>) -> [S::Matrix<3, 3>; 3] {
815        let theta_sq = omega.squared_norm();
816        let theta_p4 = theta_sq * theta_sq;
817        let dt_mat_omega_pos_idx = [[2, 1], [0, 2], [1, 0]];
818        let dt_mat_omega_neg_idx = [[1, 2], [2, 0], [0, 1]];
819
820        let near_zero = theta_sq.less_equal(&S::from_f64(EPS_F64));
821
822        let mat_omega: S::Matrix<3, 3> = Rotation3Impl::<S, BATCH, 0, 0>::hat(omega);
823        let mat_omega_sq = mat_omega.mat_mul(mat_omega);
824
825        let omega_x = omega.elem(0);
826        let omega_y = omega.elem(1);
827        let omega_z = omega.elem(2);
828
829        let theta = theta_sq.sqrt();
830        let domega_theta =
831            S::Vector::from_array([omega_x / theta, omega_y / theta, omega_z / theta]);
832
833        let a = (S::ones() - theta.cos()) / theta_sq;
834        let dt_a = (S::from_f64(-2.0) + S::from_f64(2.0) * theta.cos() + theta * theta.sin())
835            / (theta * theta_sq);
836
837        let b = (theta - theta.sin()) / (theta_sq * theta);
838        let dt_b = -(S::from_f64(2.0) * theta + theta * theta.cos()
839            - S::from_f64(3.0) * theta.sin())
840            / (theta_p4);
841
842        let dt_mat_omega_sq = [
843            S::Matrix::from_array2([
844                [S::zeros(), omega_y, omega_z],
845                [omega_y, S::from_f64(-2.0) * omega_x, S::zeros()],
846                [omega_z, S::zeros(), S::from_f64(-2.0) * omega_x],
847            ]),
848            S::Matrix::from_array2([
849                [S::from_f64(-2.0) * omega_y, omega_x, S::zeros()],
850                [omega_x, S::zeros(), omega_z],
851                [S::zeros(), omega_z, S::from_f64(-2.0) * omega_y],
852            ]),
853            S::Matrix::from_array2([
854                [S::from_f64(-2.0) * omega_z, S::zeros(), omega_x],
855                [S::zeros(), S::from_f64(-2.0) * omega_z, omega_y],
856                [omega_x, omega_y, S::zeros()],
857            ]),
858        ];
859
860        let a = S::from_f64(0.5).select(&near_zero, a);
861
862        let set = |i| {
863            let tmp0 = mat_omega.scaled(dt_a * domega_theta.elem(i));
864            let tmp1 = dt_mat_omega_sq[i].scaled(b);
865            let tmp2 = mat_omega_sq.scaled(dt_b * domega_theta.elem(i));
866
867            let mut l_i: S::Matrix<3, 3> =
868                S::Matrix::zeros().select(&near_zero, tmp0 + tmp1 + tmp2);
869            let pos_idx = dt_mat_omega_pos_idx[i];
870            *l_i.elem_mut(pos_idx) = a + l_i.elem(pos_idx);
871
872            let neg_idx = dt_mat_omega_neg_idx[i];
873            *l_i.elem_mut(neg_idx) = -a + l_i.elem(neg_idx);
874            l_i
875        };
876
877        let l: [S::Matrix<3, 3>; 3] = [set(0), set(1), set(2)];
878
879        l
880    }
881
882    fn dparams_matrix_times_point(params: &S::Vector<4>, point: S::Vector<3>) -> S::Matrix<3, 4> {
883        let r = params.elem(0);
884        let ivec0 = params.elem(1);
885        let ivec1 = params.elem(2);
886        let ivec2 = params.elem(3);
887
888        let p0 = point.elem(0);
889        let p1 = point.elem(1);
890        let p2 = point.elem(2);
891
892        S::Matrix::from_array2([
893            [
894                S::from_f64(2.0) * ivec1 * p2 - S::from_f64(2.0) * ivec2 * p1,
895                S::from_f64(2.0) * ivec1 * p1 + S::from_f64(2.0) * ivec2 * p2,
896                S::from_f64(2.0) * r * p2 + S::from_f64(2.0) * ivec0 * p1
897                    - S::from_f64(4.0) * ivec1 * p0,
898                S::from_f64(-2.0) * r * p1 + S::from_f64(2.0) * ivec0 * p2
899                    - S::from_f64(4.0) * ivec2 * p0,
900            ],
901            [
902                S::from_f64(-2.0) * ivec0 * p2 + S::from_f64(2.0) * ivec2 * p0,
903                S::from_f64(-2.0) * r * p2 - S::from_f64(4.0) * ivec0 * p1
904                    + S::from_f64(2.0) * ivec1 * p0,
905                S::from_f64(2.0) * ivec0 * p0 + S::from_f64(2.0) * ivec2 * p2,
906                S::from_f64(2.0) * r * p0 + S::from_f64(2.0) * ivec1 * p2
907                    - S::from_f64(4.0) * ivec2 * p1,
908            ],
909            [
910                S::from_f64(2.0) * ivec0 * p1 - S::from_f64(2.0) * ivec1 * p0,
911                S::from_f64(2.0) * r * p1 - S::from_f64(4.0) * ivec0 * p2
912                    + S::from_f64(2.0) * ivec2 * p0,
913                S::from_f64(-2.0) * r * p0 - S::from_f64(4.0) * ivec1 * p2
914                    + S::from_f64(2.0) * ivec2 * p1,
915                S::from_f64(2.0) * ivec0 * p0 + S::from_f64(2.0) * ivec1 * p1,
916            ],
917        ])
918    }
919
920    fn dx_mat_v_inverse(omega: S::Vector<3>) -> [S::Matrix<3, 3>; 3] {
921        let theta_sq = omega.squared_norm();
922        let theta = theta_sq.sqrt();
923        let half_theta = S::from_f64(0.5) * theta;
924        let mat_omega: S::Matrix<3, 3> = Rotation3Impl::<S, BATCH, 0, 0>::hat(omega);
925        let mat_omega_sq = mat_omega.mat_mul(mat_omega);
926
927        let dt_mat_omega_pos_idx = [[2, 1], [0, 2], [1, 0]];
928        let dt_mat_omega_neg_idx = [[1, 2], [2, 0], [0, 1]];
929
930        let omega_x = omega.elem(0);
931        let omega_y = omega.elem(1);
932        let omega_z = omega.elem(2);
933
934        let near_zero = theta_sq.less_equal(&S::from_f64(EPS_F64));
935
936        let domega_theta =
937            S::Vector::from_array([omega_x / theta, omega_y / theta, omega_z / theta]);
938
939        let c = (S::from_f64(1.0)
940            - (S::from_f64(0.5) * theta * half_theta.cos()) / (half_theta.sin()))
941            / theta_sq;
942
943        let dt_c = (S::from_f64(-2.0)
944            + (S::from_f64(0.25) * theta_sq) / (half_theta.sin() * half_theta.sin())
945            + (half_theta * half_theta.cos()) / half_theta.sin())
946            / (theta * theta_sq);
947
948        let dt_mat_omega_sq: &[S::Matrix<3, 3>; 3] = &[
949            S::Matrix::from_array2([
950                [S::from_f64(0.0), omega_y, omega_z],
951                [omega_y, S::from_f64(-2.0) * omega_x, S::from_f64(0.0)],
952                [omega_z, S::from_f64(0.0), S::from_f64(-2.0) * omega_x],
953            ]),
954            S::Matrix::from_array2([
955                [S::from_f64(-2.0) * omega_y, omega_x, S::from_f64(0.0)],
956                [omega_x, S::from_f64(0.0), omega_z],
957                [S::from_f64(0.0), omega_z, S::from_f64(-2.0) * omega_y],
958            ]),
959            S::Matrix::from_array2([
960                [S::from_f64(-2.0) * omega_z, S::from_f64(0.0), omega_x],
961                [S::from_f64(0.0), S::from_f64(-2.0) * omega_z, omega_y],
962                [omega_x, omega_y, S::from_f64(0.0)],
963            ]),
964        ];
965
966        let set = |i| -> S::Matrix<3, 3> {
967            let dt_mat_omega_sq_i: &S::Matrix<3, 3> = &dt_mat_omega_sq[i];
968            let mut l_i: S::Matrix<3, 3> = S::Matrix::zeros().select(
969                &near_zero,
970                dt_mat_omega_sq_i.scaled(c) + mat_omega_sq.scaled(domega_theta.elem(i) * dt_c),
971            );
972
973            let pos_idx = dt_mat_omega_pos_idx[i];
974            *l_i.elem_mut(pos_idx) = S::from_f64(-0.5) + l_i.elem(pos_idx);
975
976            let neg_idx = dt_mat_omega_neg_idx[i];
977            *l_i.elem_mut(neg_idx) = S::from_f64(0.5) + l_i.elem(neg_idx);
978            l_i
979        };
980
981        let l: [S::Matrix<3, 3>; 3] = [set(0), set(1), set(2)];
982
983        l
984    }
985
986    fn left_jacobian_of_translation(omega: S::Vector<3>, upsilon: S::Vector<3>) -> S::Matrix<3, 3> {
987        let theta_sq = omega.squared_norm();
988        let near_zero = theta_sq.less_equal(&S::from_f64(EPS_F64));
989        let upsilon_hat: S::Matrix<3, 3> = Rotation3::<S, BATCH, 0, 0>::hat(upsilon);
990        let omega_hat: S::Matrix<3, 3> = Rotation3::<S, BATCH, 0, 0>::hat(omega);
991
992        // 2-nd-order BCH series:
993        let q0 = upsilon_hat.scaled(S::from_f64(0.5))
994            + omega_hat
995                .mat_mul(upsilon_hat)
996                .scaled(S::from_f64(1.0 / 6.0));
997
998        // Exact closed form, see Barfoot (Eq. 7.86)
999        // http://asrl.utias.utoronto.ca/~tdb/bib/barfoot_ser17.pdf
1000        let theta = theta_sq.sqrt();
1001        let (sin_t, cos_t) = (theta.sin(), theta.cos());
1002        let a = S::from_f64(0.5);
1003        let b = (theta - sin_t) / (theta * theta * theta);
1004        let c = (theta * theta + S::from_f64(2.0) * cos_t - S::from_f64(2.0))
1005            / (S::from_f64(2.0) * (theta * theta * theta * theta));
1006        let d = (S::from_f64(2.0) * theta - S::from_f64(3.0) * sin_t + theta * cos_t)
1007            / (S::from_f64(2.0) * (theta * theta * theta * theta * theta));
1008
1009        let q = upsilon_hat.scaled(a)
1010            + (omega_hat.mat_mul(upsilon_hat)
1011                + upsilon_hat.mat_mul(omega_hat)
1012                + omega_hat.mat_mul(upsilon_hat).mat_mul(omega_hat))
1013            .scaled(b)
1014            + (omega_hat.mat_mul(omega_hat).mat_mul(upsilon_hat)
1015                + upsilon_hat.mat_mul(omega_hat).mat_mul(omega_hat)
1016                - omega_hat
1017                    .mat_mul(upsilon_hat)
1018                    .mat_mul(omega_hat)
1019                    .scaled(S::from_f64(3.0)))
1020            .scaled(c)
1021            + (omega_hat
1022                .mat_mul(upsilon_hat)
1023                .mat_mul(omega_hat)
1024                .mat_mul(omega_hat)
1025                + omega_hat
1026                    .mat_mul(omega_hat)
1027                    .mat_mul(upsilon_hat)
1028                    .mat_mul(omega_hat))
1029            .scaled(d);
1030
1031        q0.select(&near_zero, q)
1032    }
1033}
1034
1035impl<S: IsScalar<BATCH, DM, DN>, const BATCH: usize, const DM: usize, const DN: usize>
1036    Rotation3<S, BATCH, DM, DN>
1037{
1038    /// Rotation around the x-axis.
1039    pub fn rot_x<U>(theta: U) -> Self
1040    where
1041        U: Borrow<S>,
1042    {
1043        let theta: &S = theta.borrow();
1044        Rotation3::exp(S::Vector::<3>::from_array([*theta, S::zero(), S::zero()]))
1045    }
1046
1047    /// Rotation around the y-axis.
1048    pub fn rot_y<U>(theta: U) -> Self
1049    where
1050        U: Borrow<S>,
1051    {
1052        let theta: &S = theta.borrow();
1053        Rotation3::exp(S::Vector::<3>::from_array([S::zero(), *theta, S::zero()]))
1054    }
1055
1056    /// Rotation around the z-axis.
1057    pub fn rot_z<U>(theta: U) -> Self
1058    where
1059        U: Borrow<S>,
1060    {
1061        let theta: &S = theta.borrow();
1062        Rotation3::exp(S::Vector::<3>::from_array([S::zero(), S::zero(), *theta]))
1063    }
1064}
1065
1066impl<S: IsSingleScalar<DM, DN> + PartialOrd, const DM: usize, const DN: usize>
1067    Rotation3<S, 1, DM, DN>
1068{
1069    /// Is orthogonal with determinant 1.
1070    pub fn is_orthogonal_with_positive_det(mat: &MatF64<3, 3>, thr: f64) -> bool {
1071        // We expect: R * R^T = I   and   det(R) > 0
1072        //
1073        // (If R is orthogonal, then det(R) = +/-1.)
1074        let max_abs: f64 = ((mat * mat.transpose()) - MatF64::identity()).abs().max();
1075        max_abs < thr && mat.determinant() > 0.0
1076    }
1077
1078    /// From a 3x3 rotation matrix. The matrix must be a valid rotation matrix,
1079    /// i.e., it must be orthogonal with determinant 1, otherwise None is returned.
1080    pub fn try_from_mat<M>(mat_r: M) -> Option<Rotation3<S, 1, DM, DN>>
1081    where
1082        M: Borrow<S::SingleMatrix<3, 3>>,
1083    {
1084        let mat_r = mat_r.borrow();
1085        if !Self::is_orthogonal_with_positive_det(&mat_r.single_real_matrix(), EPS_F64) {
1086            return None;
1087        }
1088        // Quaternions, Ken Shoemake
1089        // https://campar.in.tum.de/twiki/pub/Chair/DwarfTutorial/quatut.pdf
1090        //
1091
1092        //     | 1 - 2*y^2 - 2*z^2  2*x*y - 2*z*w      2*x*z + 2*y*w |
1093        // R = | 2*x*y + 2*z*w      1 - 2*x^2 - 2*z^2  2*y*z - 2*x*w |
1094        //     | 2*x*z - 2*y*w      2*y*z + 2*x*w      1 - 2*x^2 - 2*y^2 |
1095
1096        let trace = mat_r.elem([0, 0]) + mat_r.elem([1, 1]) + mat_r.elem([2, 2]);
1097
1098        let q_params = if trace > S::from_f64(0.0) {
1099            // Calculate w first:
1100            //
1101            //   tr = trace(R)
1102            //      = 3 - 4*x^2 - 4*y^2 - 4*z^2
1103            //      = 3 - 4*(w^2 - w^2 + x^2 + y^2 + z^2)
1104            //      = 3 - 4*(1 - w^2)            [ since w^2 + x^2 + y^2 + z^2 = 1]
1105            //      = 1 + 2*w^2
1106            //
1107            //   w = sqrt(tr + 1) / 2
1108            //
1109            // (Note: We are in the tr>0 case, "tr + 1" is always positive,
1110            //        hence we can safely take the square root, and |w| > 0.5.)
1111            let sqrt_trace_plus_one = (S::from_f64(1.0) + trace).sqrt();
1112            let w = S::from_f64(0.5) * sqrt_trace_plus_one;
1113
1114            // Now x:
1115            //
1116            //    R12 = 2*y*z - 2*x*w
1117            //    R21 = 2*y*z + 2*x*w
1118            //
1119            //    "2*y*z" = R12 + 2*x*w
1120            //    "2*y*z" = R21 - 2*x*w
1121            //
1122            //  Hence,
1123            //
1124            //    R12 + 2*x*w = R21 - 2*x*w
1125            //          4*x*w = R21 - R12
1126            //              x = (R21 - R12) / 4*w
1127            //
1128            // Similarly, for y and z.
1129
1130            // f := 1 / (4*w) = 0.5 / sqrt(tr + 1)
1131            //
1132            // (Note: Since |w| > 0.5, the division is never close to zero.)
1133            let factor = S::from_f64(0.5) / sqrt_trace_plus_one;
1134
1135            S::Vector::<4>::from_array([
1136                w,
1137                factor * (mat_r.elem([2, 1]) - mat_r.elem([1, 2])),
1138                factor * (mat_r.elem([0, 2]) - mat_r.elem([2, 0])),
1139                factor * (mat_r.elem([1, 0]) - mat_r.elem([0, 1])),
1140            ])
1141        } else {
1142            // Let us assume that R00 is the largest diagonal entry.
1143            // If not, we will change the order of the indices accordingly.
1144            let mut i = 0;
1145            if mat_r.elem([1, 1]) > mat_r.elem([0, 0]) {
1146                i = 1;
1147            }
1148            if mat_r.elem([2, 2]) > mat_r.elem([i, i]) {
1149                i = 2;
1150            }
1151            let j = (i + 1) % 3;
1152            let k = (j + 1) % 3;
1153
1154            // Now we calculate q.x as follows (for i = 0, j = 1, k = 2):
1155            //
1156            // R00 - R11 - R22
1157            //  = 1 - 2*y^2 - 2*z^2 - 1 + 2*x^2 + 2*z^2 - 1 + 2*x^2 + 2*y^2
1158            //  = 4*x^2 - 1
1159            //
1160            // <=>
1161            //
1162            // x = sqrt((R00 - R11 - R22 + 1) / 4)
1163            //   = sqrt(R00 - R11 - R22 + 1) / 2
1164            //
1165            // (Note: Since the trace is negative, and R00 is the largest diagonal entry,
1166            //        R00 - R11 - R22 + 1 is always positive.)
1167            let sqrt = (mat_r.elem([i, i]) - mat_r.elem([j, j]) - mat_r.elem([k, k])
1168                + S::from_f64(1.0))
1169            .sqrt();
1170            let mut q = S::Vector::<4>::zeros();
1171            *q.elem_mut(i + 1) = S::from_f64(0.5) * sqrt;
1172
1173            // For w:
1174            //
1175            // R21 - R12 = 2*y*z + 2*x*w - 2*y*z + 2*x*w
1176            //           = 4*x*w
1177            //
1178            // <=>
1179            //
1180            // w = (R21 - R12) / 4*x
1181            //   = (R21 - R12) / (2*sqrt(R00 - R11 - R22 + 1))
1182
1183            let one_over_two_s = S::from_f64(0.5) / sqrt;
1184            *q.elem_mut(0) = (mat_r.elem([k, j]) - mat_r.elem([j, k])) * one_over_two_s;
1185
1186            // For y:
1187            //
1188            // R01 + R10 = 2*x*y + 2*z*w + 2*x*y - 2*z*w
1189            //           = 4*x*y
1190            //
1191            // <=>
1192            //
1193            // y = (R01 + R10) / 4*x
1194            //   = (R01 + R10) / (2*sqrt(R00 - R11 - R22 + 1))
1195            *q.elem_mut(j + 1) = (mat_r.elem([j, i]) + mat_r.elem([i, j])) * one_over_two_s;
1196
1197            // For z ...
1198            *q.elem_mut(k + 1) = (mat_r.elem([k, i]) + mat_r.elem([i, k])) * one_over_two_s;
1199            q
1200        };
1201
1202        Some(Rotation3::from_params(q_params))
1203    }
1204}
1205
1206impl From<nalgebra::UnitQuaternion<f64>> for Rotation3F64 {
1207    fn from(q: nalgebra::UnitQuaternion<f64>) -> Self {
1208        Self::from_params(VecF64::from_array([
1209            q.scalar(),
1210            q.imag().x,
1211            q.imag().y,
1212            q.imag().z,
1213        ]))
1214    }
1215}
1216
1217impl From<Rotation3F64> for nalgebra::UnitQuaternion<f64> {
1218    fn from(rotation: Rotation3F64) -> Self {
1219        let params = rotation.params();
1220        nalgebra::Unit::new_normalize(nalgebra::Quaternion::new(
1221            params[0], params[1], params[2], params[3],
1222        ))
1223    }
1224}
1225
1226#[test]
1227fn rotation3_prop_tests() {
1228    #[cfg(feature = "simd")]
1229    use sophus_autodiff::dual::DualBatchScalar;
1230    use sophus_autodiff::dual::DualScalar;
1231    #[cfg(feature = "simd")]
1232    use sophus_autodiff::linalg::BatchScalarF64;
1233
1234    use crate::lie_group::{
1235        factor_lie_group::RealFactorLieGroupTest,
1236        real_lie_group::RealLieGroupTest,
1237    };
1238
1239    Rotation3F64::test_suite();
1240    #[cfg(feature = "simd")]
1241    Rotation3::<BatchScalarF64<8>, 8, 0, 0>::test_suite();
1242    Rotation3::<DualScalar<3, 1>, 1, 3, 1>::test_suite();
1243    #[cfg(feature = "simd")]
1244    Rotation3::<DualBatchScalar<8, 1, 1>, 8, 1, 1>::test_suite();
1245
1246    Rotation3F64::run_real_tests();
1247    #[cfg(feature = "simd")]
1248    Rotation3::<BatchScalarF64<8>, 8, 0, 0>::run_real_tests();
1249
1250    Rotation3F64::run_real_factor_tests();
1251    #[cfg(feature = "simd")]
1252    Rotation3::<BatchScalarF64<8>, 8, 0, 0>::run_real_factor_tests();
1253}
1254
1255impl<S: IsSingleScalar<DM, DN> + PartialOrd, const DM: usize, const DN: usize>
1256    HasAverage<S, 3, 4, 3, 3, DM, DN> for Rotation3<S, 1, DM, DN>
1257{
1258    fn average(
1259        parent_from_body_transforms: &[Rotation3<S, 1, DM, DN>],
1260    ) -> Result<Self, EmptySliceError> {
1261        // todo: Implement close form solution.
1262
1263        match iterative_average(parent_from_body_transforms, 50) {
1264            Ok(parent_from_body_average) => Ok(parent_from_body_average),
1265            Err(err) => match err {
1266                IterativeAverageError::EmptySlice => Err(EmptySliceError),
1267                IterativeAverageError::NotConverged {
1268                    max_iteration_count,
1269                    parent_from_body_estimate,
1270                } => {
1271                    warn!(
1272                        "iterative_average did not converge (iters={max_iteration_count}), returning best guess."
1273                    );
1274                    Ok(parent_from_body_estimate)
1275                }
1276            },
1277        }
1278    }
1279}
1280
1281#[test]
1282fn from_matrix_test() {
1283    use approx::assert_relative_eq;
1284    use log::info;
1285
1286    for q in Rotation3F64::element_examples() {
1287        let mat: MatF64<3, 3> = q.matrix();
1288
1289        info!("mat = {mat:?}");
1290        let q2: Rotation3F64 = Rotation3::try_from_mat(mat).unwrap();
1291        let mat2 = q2.matrix();
1292
1293        info!("mat2 = {mat2:?}");
1294        assert_relative_eq!(mat, mat2, epsilon = EPS_F64);
1295    }
1296
1297    // Iterate over all tangent too, just to get more examples.
1298    for t in Rotation3F64::tangent_examples() {
1299        let mat: MatF64<3, 3> = Rotation3F64::exp(t).matrix();
1300        info!("mat = {mat:?}");
1301        let t2: Rotation3F64 = Rotation3::try_from_mat(mat).unwrap();
1302        let mat2 = t2.matrix();
1303        info!("mat2 = {mat2:?}");
1304        assert_relative_eq!(mat, mat2, epsilon = EPS_F64);
1305    }
1306}
1307
1308#[test]
1309fn test_nalgebra_interop() {
1310    use approx::assert_relative_eq;
1311
1312    let rotation = Rotation3F64::rot_x(0.5);
1313    let na_rotation: nalgebra::UnitQuaternion<f64> = rotation.into();
1314
1315    assert_relative_eq!(rotation.params()[0], na_rotation.scalar(), epsilon = 1e-10);
1316    assert_relative_eq!(
1317        rotation.params()[1],
1318        na_rotation.vector().x,
1319        epsilon = 1e-10
1320    );
1321    assert_relative_eq!(
1322        rotation.params()[2],
1323        na_rotation.vector().y,
1324        epsilon = 1e-10
1325    );
1326    assert_relative_eq!(
1327        rotation.params()[3],
1328        na_rotation.vector().z,
1329        epsilon = 1e-10
1330    );
1331
1332    let roundtrip_rotation = Rotation3F64::from(na_rotation);
1333    assert_relative_eq!(
1334        rotation.params(),
1335        roundtrip_rotation.params(),
1336        epsilon = 1e-10
1337    );
1338}
sophus_lie/groups/rotation3.rs

sophus_lie/groups/
rotation3.rs
