1use nalgebra::{Matrix2, Matrix3, Matrix4, SMatrix, SVector, Vector2, Vector3};
12#[cfg(not(feature = "std"))]
13#[allow(unused_imports)]
14use num_traits::Float;
15
16pub type Vector6 = SVector<f64, 6>;
18pub type Matrix6 = SMatrix<f64, 6, 6>;
20
21const SMALL_ANGLE_SQUARED: f64 = 1.0e-12;
22const NEAR_PI: f64 = 1.0e-6;
23
24pub fn skew(vector: &Vector3<f64>) -> Matrix3<f64> {
26 Matrix3::new(
27 0.0, -vector.z, vector.y, vector.z, 0.0, -vector.x, -vector.y, vector.x, 0.0,
28 )
29}
30
31pub fn unskew(matrix: &Matrix3<f64>) -> Vector3<f64> {
33 Vector3::new(matrix[(2, 1)], matrix[(0, 2)], matrix[(1, 0)])
34}
35
36pub fn so2_exp(angle: f64) -> Matrix2<f64> {
38 let (sin_angle, cos_angle) = angle.sin_cos();
39 Matrix2::new(cos_angle, -sin_angle, sin_angle, cos_angle)
40}
41
42pub fn so2_log(rotation: &Matrix2<f64>) -> f64 {
44 rotation[(1, 0)].atan2(rotation[(0, 0)])
45}
46
47pub fn so3_exp(omega: &Vector3<f64>) -> Matrix3<f64> {
49 let theta_squared = omega.norm_squared();
50 let omega_hat = skew(omega);
51 let omega_hat_squared = omega_hat * omega_hat;
52 let (a, b) = rodrigues_coefficients(theta_squared);
53 Matrix3::identity() + a * omega_hat + b * omega_hat_squared
54}
55
56pub fn so3_log(rotation: &Matrix3<f64>) -> Vector3<f64> {
58 let cos_theta = ((rotation.trace() - 1.0) * 0.5).clamp(-1.0, 1.0);
59 let theta = cos_theta.acos();
60
61 if theta * theta < SMALL_ANGLE_SQUARED {
62 return 0.5 * unskew(&(rotation - rotation.transpose()));
63 }
64
65 if core::f64::consts::PI - theta < NEAR_PI {
66 return theta * axis_near_pi(rotation);
67 }
68
69 let scale = theta / (2.0 * theta.sin());
70 scale * unskew(&(rotation - rotation.transpose()))
71}
72
73pub fn so3_left_jacobian(omega: &Vector3<f64>) -> Matrix3<f64> {
75 let theta_squared = omega.norm_squared();
76 let omega_hat = skew(omega);
77 let omega_hat_squared = omega_hat * omega_hat;
78 let (b, c) = jacobian_coefficients(theta_squared);
79 Matrix3::identity() + b * omega_hat + c * omega_hat_squared
80}
81
82pub fn so3_left_jacobian_inverse(omega: &Vector3<f64>) -> Matrix3<f64> {
84 let theta_squared = omega.norm_squared();
85 let omega_hat = skew(omega);
86 let omega_hat_squared = omega_hat * omega_hat;
87 let coefficient = if theta_squared < SMALL_ANGLE_SQUARED {
88 1.0 / 12.0 + theta_squared / 720.0 + theta_squared * theta_squared / 30_240.0
89 } else {
90 let theta = theta_squared.sqrt();
91 1.0 / theta_squared - (1.0 + theta.cos()) / (2.0 * theta * theta.sin())
92 };
93 Matrix3::identity() - 0.5 * omega_hat + coefficient * omega_hat_squared
94}
95
96pub fn se2_exp(tangent: &Vector3<f64>) -> Matrix3<f64> {
98 let translation_tangent = Vector2::new(tangent.x, tangent.y);
99 let angle = tangent.z;
100 let jacobian = so2_left_jacobian(angle);
101 let translation = jacobian * translation_tangent;
102 let rotation = so2_exp(angle);
103
104 Matrix3::new(
105 rotation[(0, 0)],
106 rotation[(0, 1)],
107 translation.x,
108 rotation[(1, 0)],
109 rotation[(1, 1)],
110 translation.y,
111 0.0,
112 0.0,
113 1.0,
114 )
115}
116
117pub fn se2_log(transform: &Matrix3<f64>) -> Vector3<f64> {
119 let rotation = transform.fixed_view::<2, 2>(0, 0).into_owned();
120 let translation = Vector2::new(transform[(0, 2)], transform[(1, 2)]);
121 let angle = so2_log(&rotation);
122 let translation_tangent = so2_left_jacobian_inverse(angle) * translation;
123 Vector3::new(translation_tangent.x, translation_tangent.y, angle)
124}
125
126pub fn se2_inverse(transform: &Matrix3<f64>) -> Matrix3<f64> {
128 let rotation = transform.fixed_view::<2, 2>(0, 0).into_owned();
129 let translation = Vector2::new(transform[(0, 2)], transform[(1, 2)]);
130 let inverse_rotation = rotation.transpose();
131 let inverse_translation = -(inverse_rotation * translation);
132 Matrix3::new(
133 inverse_rotation[(0, 0)],
134 inverse_rotation[(0, 1)],
135 inverse_translation.x,
136 inverse_rotation[(1, 0)],
137 inverse_rotation[(1, 1)],
138 inverse_translation.y,
139 0.0,
140 0.0,
141 1.0,
142 )
143}
144
145pub fn se2_adjoint(transform: &Matrix3<f64>) -> Matrix3<f64> {
147 let rotation = transform.fixed_view::<2, 2>(0, 0);
148 let tx = transform[(0, 2)];
149 let ty = transform[(1, 2)];
150 Matrix3::new(
151 rotation[(0, 0)],
152 rotation[(0, 1)],
153 ty,
154 rotation[(1, 0)],
155 rotation[(1, 1)],
156 -tx,
157 0.0,
158 0.0,
159 1.0,
160 )
161}
162
163pub fn se3_exp(tangent: &Vector6) -> Matrix4<f64> {
165 let velocity = tangent.fixed_rows::<3>(0).into_owned();
166 let omega = tangent.fixed_rows::<3>(3).into_owned();
167 let rotation = so3_exp(&omega);
168 let translation = so3_left_jacobian(&omega) * velocity;
169
170 Matrix4::new(
171 rotation[(0, 0)],
172 rotation[(0, 1)],
173 rotation[(0, 2)],
174 translation.x,
175 rotation[(1, 0)],
176 rotation[(1, 1)],
177 rotation[(1, 2)],
178 translation.y,
179 rotation[(2, 0)],
180 rotation[(2, 1)],
181 rotation[(2, 2)],
182 translation.z,
183 0.0,
184 0.0,
185 0.0,
186 1.0,
187 )
188}
189
190pub fn se3_log(transform: &Matrix4<f64>) -> Vector6 {
192 let rotation = transform.fixed_view::<3, 3>(0, 0).into_owned();
193 let translation = transform.fixed_view::<3, 1>(0, 3).into_owned();
194 let omega = so3_log(&rotation);
195 let velocity = so3_left_jacobian_inverse(&omega) * translation;
196 Vector6::new(
197 velocity.x, velocity.y, velocity.z, omega.x, omega.y, omega.z,
198 )
199}
200
201pub fn se3_inverse(transform: &Matrix4<f64>) -> Matrix4<f64> {
203 let rotation = transform.fixed_view::<3, 3>(0, 0).into_owned();
204 let translation = transform.fixed_view::<3, 1>(0, 3).into_owned();
205 let inverse_rotation = rotation.transpose();
206 let inverse_translation = -(inverse_rotation * translation);
207 Matrix4::new(
208 inverse_rotation[(0, 0)],
209 inverse_rotation[(0, 1)],
210 inverse_rotation[(0, 2)],
211 inverse_translation.x,
212 inverse_rotation[(1, 0)],
213 inverse_rotation[(1, 1)],
214 inverse_rotation[(1, 2)],
215 inverse_translation.y,
216 inverse_rotation[(2, 0)],
217 inverse_rotation[(2, 1)],
218 inverse_rotation[(2, 2)],
219 inverse_translation.z,
220 0.0,
221 0.0,
222 0.0,
223 1.0,
224 )
225}
226
227pub fn se3_adjoint(transform: &Matrix4<f64>) -> Matrix6 {
229 let rotation = transform.fixed_view::<3, 3>(0, 0).into_owned();
230 let translation = transform.fixed_view::<3, 1>(0, 3).into_owned();
231 let cross_rotation = skew(&translation) * rotation;
232 let mut adjoint = Matrix6::zeros();
233 adjoint.fixed_view_mut::<3, 3>(0, 0).copy_from(&rotation);
234 adjoint
235 .fixed_view_mut::<3, 3>(0, 3)
236 .copy_from(&cross_rotation);
237 adjoint.fixed_view_mut::<3, 3>(3, 3).copy_from(&rotation);
238 adjoint
239}
240
241fn so2_left_jacobian(angle: f64) -> Matrix2<f64> {
242 let angle_squared = angle * angle;
243 let (a, b) = if angle_squared < SMALL_ANGLE_SQUARED {
244 (
245 1.0 - angle_squared / 6.0 + angle_squared * angle_squared / 120.0,
246 angle * (0.5 - angle_squared / 24.0 + angle_squared * angle_squared / 720.0),
247 )
248 } else {
249 (angle.sin() / angle, (1.0 - angle.cos()) / angle)
250 };
251 Matrix2::new(a, -b, b, a)
252}
253
254fn so2_left_jacobian_inverse(angle: f64) -> Matrix2<f64> {
255 let half_angle = 0.5 * angle;
256 let diagonal = if angle * angle < SMALL_ANGLE_SQUARED {
257 1.0 - angle * angle / 12.0 - angle.powi(4) / 720.0
258 } else {
259 half_angle / half_angle.tan()
260 };
261 Matrix2::new(diagonal, half_angle, -half_angle, diagonal)
262}
263
264fn rodrigues_coefficients(theta_squared: f64) -> (f64, f64) {
265 if theta_squared < SMALL_ANGLE_SQUARED {
266 (
267 1.0 - theta_squared / 6.0 + theta_squared * theta_squared / 120.0,
268 0.5 - theta_squared / 24.0 + theta_squared * theta_squared / 720.0,
269 )
270 } else {
271 let theta = theta_squared.sqrt();
272 (theta.sin() / theta, (1.0 - theta.cos()) / theta_squared)
273 }
274}
275
276fn jacobian_coefficients(theta_squared: f64) -> (f64, f64) {
277 if theta_squared < SMALL_ANGLE_SQUARED {
278 (
279 0.5 - theta_squared / 24.0 + theta_squared * theta_squared / 720.0,
280 1.0 / 6.0 - theta_squared / 120.0 + theta_squared * theta_squared / 5_040.0,
281 )
282 } else {
283 let theta = theta_squared.sqrt();
284 (
285 (1.0 - theta.cos()) / theta_squared,
286 (theta - theta.sin()) / (theta_squared * theta),
287 )
288 }
289}
290
291fn axis_near_pi(rotation: &Matrix3<f64>) -> Vector3<f64> {
292 let xx = ((rotation[(0, 0)] + 1.0) * 0.5).max(0.0).sqrt();
293 let yy = ((rotation[(1, 1)] + 1.0) * 0.5).max(0.0).sqrt();
294 let zz = ((rotation[(2, 2)] + 1.0) * 0.5).max(0.0).sqrt();
295
296 let mut axis = if xx >= yy && xx >= zz && xx > f64::EPSILON {
297 Vector3::new(
298 xx,
299 (rotation[(0, 1)] + rotation[(1, 0)]) / (4.0 * xx),
300 (rotation[(0, 2)] + rotation[(2, 0)]) / (4.0 * xx),
301 )
302 } else if yy >= zz && yy > f64::EPSILON {
303 Vector3::new(
304 (rotation[(0, 1)] + rotation[(1, 0)]) / (4.0 * yy),
305 yy,
306 (rotation[(1, 2)] + rotation[(2, 1)]) / (4.0 * yy),
307 )
308 } else if zz > f64::EPSILON {
309 Vector3::new(
310 (rotation[(0, 2)] + rotation[(2, 0)]) / (4.0 * zz),
311 (rotation[(1, 2)] + rotation[(2, 1)]) / (4.0 * zz),
312 zz,
313 )
314 } else {
315 Vector3::x()
316 };
317
318 let antisymmetric = unskew(&(rotation - rotation.transpose()));
319 if axis.dot(&antisymmetric) < 0.0 {
320 axis = -axis;
321 }
322 axis.normalize()
323}
324
325#[cfg(test)]
326mod tests {
327 use super::*;
328
329 const TOLERANCE: f64 = 1.0e-9;
330
331 fn assert_matrix_close<const R: usize, const C: usize>(
332 actual: &SMatrix<f64, R, C>,
333 expected: &SMatrix<f64, R, C>,
334 tolerance: f64,
335 ) {
336 let error = (actual - expected).norm();
337 assert!(
338 error <= tolerance,
339 "matrix error {error} exceeded tolerance {tolerance}\nactual={actual}\nexpected={expected}"
340 );
341 }
342
343 #[test]
344 fn skew_matches_cross_product() {
345 let a = Vector3::new(0.3, -0.7, 1.2);
346 let b = Vector3::new(-0.4, 0.9, 0.2);
347 assert_matrix_close(&(skew(&a) * b), &a.cross(&b), TOLERANCE);
348 }
349
350 #[test]
351 fn so3_round_trips_regular_and_small_rotations() {
352 for omega in [
353 Vector3::new(0.2, -0.3, 0.4),
354 Vector3::new(1.0e-10, -2.0e-10, 3.0e-10),
355 Vector3::zeros(),
356 ] {
357 let recovered = so3_log(&so3_exp(&omega));
358 assert_matrix_close(&recovered, &omega, TOLERANCE);
359 }
360 }
361
362 #[test]
363 fn so3_log_is_stable_near_pi() {
364 let axis = Vector3::new(1.0, -2.0, 0.5).normalize();
365 let omega = axis * (core::f64::consts::PI - 1.0e-8);
366 let recovered_rotation = so3_exp(&so3_log(&so3_exp(&omega)));
367 assert_matrix_close(&recovered_rotation, &so3_exp(&omega), 1.0e-7);
368 }
369
370 #[test]
371 fn so3_jacobian_and_inverse_cancel() {
372 for omega in [
373 Vector3::new(0.4, -0.2, 0.1),
374 Vector3::new(1.0e-10, 2.0e-10, -1.0e-10),
375 ] {
376 let product = so3_left_jacobian(&omega) * so3_left_jacobian_inverse(&omega);
377 assert_matrix_close(&product, &Matrix3::identity(), TOLERANCE);
378 }
379 }
380
381 #[test]
382 fn se2_round_trip_and_inverse() {
383 let tangent = Vector3::new(1.2, -0.7, 0.8);
384 let transform = se2_exp(&tangent);
385 assert_matrix_close(&se2_log(&transform), &tangent, TOLERANCE);
386 assert_matrix_close(
387 &(transform * se2_inverse(&transform)),
388 &Matrix3::identity(),
389 TOLERANCE,
390 );
391 }
392
393 #[test]
394 fn se3_round_trip_and_inverse() {
395 let tangent = Vector6::new(1.2, -0.7, 0.3, 0.2, -0.4, 0.1);
396 let transform = se3_exp(&tangent);
397 assert_matrix_close(&se3_log(&transform), &tangent, TOLERANCE);
398 assert_matrix_close(
399 &(transform * se3_inverse(&transform)),
400 &Matrix4::identity(),
401 TOLERANCE,
402 );
403 }
404
405 #[test]
406 fn adjoints_match_group_conjugation() {
407 let se2_pose = se2_exp(&Vector3::new(0.4, -0.2, 0.7));
408 let se2_delta = Vector3::new(0.2, 0.1, -0.3);
409 assert_matrix_close(
410 &(se2_pose * se2_exp(&se2_delta) * se2_inverse(&se2_pose)),
411 &se2_exp(&(se2_adjoint(&se2_pose) * se2_delta)),
412 TOLERANCE,
413 );
414
415 let se3_pose = se3_exp(&Vector6::new(0.4, -0.2, 0.7, 0.1, 0.3, -0.2));
416 let se3_delta = Vector6::new(0.2, 0.1, -0.3, -0.2, 0.15, 0.1);
417 assert_matrix_close(
418 &(se3_pose * se3_exp(&se3_delta) * se3_inverse(&se3_pose)),
419 &se3_exp(&(se3_adjoint(&se3_pose) * se3_delta)),
420 1.0e-8,
421 );
422 }
423}