1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
// #[cfg(test)]
// mod tests {
// use crate::lie::base::{LieAlgebraSE3, LieGroupSE3, LieVectorSE3};
// use crate::linalg::{Const, OMatrix, OVector, U2, U3};
// /// import Re-exports of hifitime (for time) and nalgebra (for matrix)
// use crate::{
// linalg,
// time::{Duration, Epoch, Unit},
// };
// #[test]
// fn test_vec6_group() {
// let w = OVector::<f64, U3>::new(1.2, 2.5, 3.7);
// let w = w / w.norm();
// let vec6 = LieVectorSE3 {
// w: w,
// v: OVector::<f64, U3>::new(5.6, 6.7, 7.8),
// };
// let group = LieGroupSE3::from(&vec6);
// let vec6_back = group.to_vec6();
// assert!((&vec6_back.w - &vec6.w).norm() < 1e-10);
// assert!((&vec6_back.v - &vec6.v).norm() < 1e-10);
// }
// #[test]
// fn test_vec6_algebra_group() {
// let ws = [
// OVector::<f64, U3>::new(1.2, 2.5, 3.7),
// OVector::<f64, U3>::new(-1.2, -2.5, 3.7),
// OVector::<f64, U3>::new(1e-8, 1e-8, 1e-8),
// ];
// for w in ws.into_iter() {
// let w = OVector::<f64, U3>::new(1.2, 2.5, 3.7);
// let w = w / w.norm();
// let vec6 = LieVectorSE3 {
// w: w,
// v: OVector::<f64, U3>::new(5.6, 6.7, 7.8),
// };
// let hat4 = vec6.to_algebra();
// let group = LieGroupSE3::from_algebra(&hat4);
// let hat4_back = group.to_algebra();
// let vec6_back = LieVectorSE3::from_algebra(&hat4_back);
// assert!((&vec6_back.w - &vec6.w).norm() < 1e-10);
// assert!((&vec6_back.v - &vec6.v).norm() < 1e-10);
// assert!((&hat4_back - hat4).norm() < 1e-10);
// }
// }
// #[test]
// fn test_adjoint_matrix() {
// let w = OVector::<f64, U3>::new(1.2, 2.5, 3.7);
// let w = w / w.norm();
// let vec6 = LieVectorSE3 {
// w: w,
// v: OVector::<f64, U3>::new(5.6, 6.7, 7.8),
// };
// let group = LieGroupSE3::from(&vec6);
// let tau = OVector::<f64, U3>::new(5.2, 5.5, 2.7);
// let tau = w / w.norm();
// let tau_vec = LieVectorSE3 {
// w: w,
// v: OVector::<f64, U3>::new(1.6, 2.7, 3.8),
// };
// let tau_hat = tau_vec.to_algebra();
// // 1. use adjoint_action
// let tau_hat_i = group.adjoint_action(&tau_hat);
// let tau_vec_i = LieVectorSE3::from_algebra(&tau_hat_i);
// // 2. use adjoint matrix
// let adj = group.adjoint_matrix();
// let tau_column = tau_vec.to_vec6();
// let tau_column_j = adj * tau_column;
// let tau_vec_j = LieVectorSE3::from_vec6(&tau_column_j);
// let tau_hat_j = tau_vec_j.to_algebra();
// assert!((&tau_hat_i - &tau_hat_j).norm() < 1e-10);
// assert!((&tau_vec_i.w - &tau_vec_j.w).norm() < 1e-10);
// assert!((&tau_vec_i.v - &tau_vec_j.v).norm() < 1e-10);
// }
// #[test]
// fn test_action_point() {
// let w = OVector::<f64, U3>::new(0.0, 0.0, 0.0);
// let w = if w.norm() != 0.0 { w / w.norm() } else { w };
// let vec6 = LieVectorSE3 {
// w: w,
// v: OVector::<f64, U3>::new(1.0, 2.0, 3.0),
// };
// let group = LieGroupSE3::from(&vec6);
// let p = OVector::<f64, U3>::new(3.0, 4.0, 5.0);
// let p2 = group.action_on_point(&p);
// let p2_expect = OVector::<f64, U3>::new(4.0, 6.0, 8.0);
// assert!((&p2 - p2_expect).norm() < 1e-10);
// }
// #[test]
// fn test_action_around_circle() {
// let radius = 100.0;
// let half_pi = core::f64::consts::PI / 2.0;
// let mut pose_group = LieGroupSE3::from_r_t(
// OMatrix::<f64, U3, U3>::identity(),
// OVector::<f64, U3>::new(radius, 0.0, 0.0),
// );
// let expected_head_global = [
// OVector::<f64, U3>::new(100.0, 1.0, 0.0),
// OVector::<f64, U3>::new(-1.0, 100.0, 0.0),
// OVector::<f64, U3>::new(-100.0, -1.0, 0.0),
// OVector::<f64, U3>::new(1.0, -100.0, 0.0),
// ];
// let expected_head_vec_global = [
// OVector::<f64, U3>::new(0.0, 1.0, 0.0),
// OVector::<f64, U3>::new(-1.0, 0.0, 0.0),
// OVector::<f64, U3>::new(0.0, -1.0, 0.0),
// OVector::<f64, U3>::new(1.0, 0.0, 0.0),
// ];
// for i in 0..4 {
// let angle = half_pi * i as f64;
// let x = radius * angle.cos();
// let y = radius * angle.sin();
// let z = 0.0;
// let t = OVector::<f64, U3>::new(x, y, z);
// let ebi = OVector::<f64, U3>::new(angle.cos(), angle.sin(), 0.0);
// let ebj = OVector::<f64, U3>::new(-angle.sin(), angle.cos(), 0.0);
// let ebk = OVector::<f64, U3>::new(0.0, 0.0, 1.0);
// let r = OMatrix::<f64, U3, U3>::from_columns(&[ebi, ebj, ebk]);
// // the pose obtained based on geometry
// let geometry_pose_group = LieGroupSE3::from_r_t(r, t);
// let head_local = OVector::<f64, U3>::new(0.0, 1.0, 0.0);
// let head_global = geometry_pose_group.action_on_point(&head_local);
// let head_vec_local = OVector::<f64, U3>::new(0.0, 1.0, 0.0);
// let head_alg_local = LieVectorSE3 {
// w: OVector::<f64, U3>::zeros(),
// v: head_vec_local,
// }
// .to_algebra();
// let head_alg_global = geometry_pose_group.adjoint_action(&head_alg_local);
// let head_vec_global = LieVectorSE3::from_algebra(&head_alg_global).v;
// let head_global_error = head_global - expected_head_global[i];
// let head_vec_global_error = head_vec_global - expected_head_vec_global[i];
// assert!(head_global_error.norm() < 1e-10);
// assert!(head_vec_global_error.norm() < 1e-10);
// }
// }
// }