ritk_transform/transform/affine/
versor.rs1use super::rigid::matrix_from_rows;
4use coeus_autograd::{add, broadcast_to, div, mul, reshape, sqrt, sub, sum, Var};
5use coeus_core::{Backend, CpuAddressableStorage, CpuAddressableStorageMut};
6use coeus_nn::Module;
7use coeus_ops::BackendOps;
8use ritk_core::transform::Transform;
9use ritk_image::tensor::Tensor;
10
11#[derive(Clone)]
15pub struct VersorRigid3DTransform<B: Backend + BackendOps<f32>> {
16 translation: Var<f32, B>,
17 rotation: Var<f32, B>,
18 center: Var<f32, B>,
19}
20
21impl<B: Backend + BackendOps<f32> + Default> VersorRigid3DTransform<B>
22where
23 B::DeviceBuffer<f32>: CpuAddressableStorage<f32> + CpuAddressableStorageMut<f32>,
24{
25 #[must_use]
32 pub fn new(
33 translation: Tensor<f32, B>,
34 rotation: Tensor<f32, B>,
35 center: Tensor<f32, B>,
36 ) -> Self {
37 assert_eq!(
38 translation.shape(),
39 [3],
40 "versor translation must have shape [3]"
41 );
42 assert_eq!(
43 rotation.shape(),
44 [4],
45 "versor quaternion must have shape [4]"
46 );
47 assert_eq!(center.shape(), [3], "versor center must have shape [3]");
48 let squared_norm = rotation
49 .to_contiguous()
50 .expect("versor transform requires backend evaluation to succeed")
51 .as_slice()
52 .iter()
53 .map(|value| value * value)
54 .sum::<f32>();
55 assert!(
56 squared_norm.is_finite() && squared_norm > 0.0,
57 "versor quaternion must have a finite nonzero norm"
58 );
59 Self {
60 translation: Var::new(translation, true)
61 .expect("versor transform requires backend evaluation to succeed"),
62 rotation: Var::new(rotation, true)
63 .expect("versor transform requires backend evaluation to succeed"),
64 center: Var::new(center, false)
65 .expect("versor transform requires backend evaluation to succeed"),
66 }
67 }
68
69 #[must_use]
71 pub fn translation(&self) -> Tensor<f32, B> {
72 self.translation.tensor.clone()
73 }
74
75 #[must_use]
77 pub fn rotation(&self) -> Tensor<f32, B> {
78 self.rotation.tensor.clone()
79 }
80
81 #[must_use]
83 pub fn center(&self) -> Tensor<f32, B> {
84 self.center.tensor.clone()
85 }
86
87 #[must_use]
89 pub fn transform_variables(&self, points: &Var<f32, B>) -> Var<f32, B> {
90 let shape = points.tensor.shape();
91 assert!(
92 shape.len() == 2 && shape[1] == 3,
93 "versor transform requires points shaped [N, 3], got {shape:?}"
94 );
95 let target = vec![shape[0], 3];
96 let center_row = reshape(&self.center, [1, 3])
97 .expect("versor transform requires backend evaluation to succeed");
98 let translation_row = reshape(&self.translation, [1, 3])
99 .expect("versor transform requires backend evaluation to succeed");
100 let center = broadcast_to(¢er_row, target.clone())
101 .expect("versor transform requires backend evaluation to succeed");
102 let translation = broadcast_to(&translation_row, target)
103 .expect("versor transform requires backend evaluation to succeed");
104 let centered =
105 sub(points, ¢er).expect("versor transform requires backend evaluation to succeed");
106 let rotation_vars = self.rotation_matrix();
107 let rotation_t = coeus_autograd::transpose_2d(&rotation_vars)
108 .expect("versor transform requires backend evaluation to succeed");
109 let rotated = coeus_autograd::matmul(¢ered, &rotation_t)
110 .expect("versor transform requires backend evaluation to succeed");
111 let centered_output = add(&rotated, ¢er)
112 .expect("versor transform requires backend evaluation to succeed");
113 add(¢ered_output, &translation)
114 .expect("versor transform requires backend evaluation to succeed")
115 }
116
117 fn rotation_matrix(&self) -> Var<f32, B> {
118 let rotation_sq = mul(&self.rotation, &self.rotation)
119 .expect("versor transform requires backend evaluation to succeed");
120 let rotation_sum =
121 sum(&rotation_sq).expect("versor transform requires backend evaluation to succeed");
122 let norm =
123 sqrt(&rotation_sum).expect("versor transform requires backend evaluation to succeed");
124 let norm_wide = broadcast_to(&norm, vec![4])
125 .expect("versor transform requires backend evaluation to succeed");
126 let quaternion = div(&self.rotation, &norm_wide)
127 .expect("versor transform requires backend evaluation to succeed");
128 let component = |index| {
129 coeus_autograd::slice(&quaternion, &[(index, index + 1)])
130 .expect("versor transform requires backend evaluation to succeed")
131 };
132 let (x, y, z, w) = (component(0), component(1), component(2), component(3));
133 let one = Var::new(
134 Tensor::from_slice_on([1], &[1.0], &B::default())
135 .expect("versor transform requires backend evaluation to succeed"),
136 false,
137 )
138 .expect("versor transform requires backend evaluation to succeed");
139 let two = Var::new(
140 Tensor::from_slice_on([1], &[2.0], &B::default())
141 .expect("versor transform requires backend evaluation to succeed"),
142 false,
143 )
144 .expect("versor transform requires backend evaluation to succeed");
145 let twice = |value: &Var<f32, B>| {
146 mul(&two, value).expect("versor transform requires backend evaluation to succeed")
147 };
148
149 let xx = mul(&x, &x).expect("versor transform requires backend evaluation to succeed");
150 let yy = mul(&y, &y).expect("versor transform requires backend evaluation to succeed");
151 let zz = mul(&z, &z).expect("versor transform requires backend evaluation to succeed");
152 let xy = mul(&x, &y).expect("versor transform requires backend evaluation to succeed");
153 let xz = mul(&x, &z).expect("versor transform requires backend evaluation to succeed");
154 let yz = mul(&y, &z).expect("versor transform requires backend evaluation to succeed");
155 let xw = mul(&x, &w).expect("versor transform requires backend evaluation to succeed");
156 let yw = mul(&y, &w).expect("versor transform requires backend evaluation to succeed");
157 let zw = mul(&z, &w).expect("versor transform requires backend evaluation to succeed");
158
159 let r11_inner =
160 add(&yy, &zz).expect("versor transform requires backend evaluation to succeed");
161 let r11 = sub(&one, &twice(&r11_inner))
162 .expect("versor transform requires backend evaluation to succeed");
163 let r12_inner =
164 sub(&xy, &zw).expect("versor transform requires backend evaluation to succeed");
165 let r12 = twice(&r12_inner);
166 let r13_inner =
167 add(&xz, &yw).expect("versor transform requires backend evaluation to succeed");
168 let r13 = twice(&r13_inner);
169 let r21_inner =
170 add(&xy, &zw).expect("versor transform requires backend evaluation to succeed");
171 let r21 = twice(&r21_inner);
172 let r22_inner =
173 add(&xx, &zz).expect("versor transform requires backend evaluation to succeed");
174 let r22 = sub(&one, &twice(&r22_inner))
175 .expect("versor transform requires backend evaluation to succeed");
176 let r23_inner =
177 sub(&yz, &xw).expect("versor transform requires backend evaluation to succeed");
178 let r23 = twice(&r23_inner);
179 let r31_inner =
180 sub(&xz, &yw).expect("versor transform requires backend evaluation to succeed");
181 let r31 = twice(&r31_inner);
182 let r32_inner =
183 add(&yz, &xw).expect("versor transform requires backend evaluation to succeed");
184 let r32 = twice(&r32_inner);
185 let r33_inner =
186 add(&xx, &yy).expect("versor transform requires backend evaluation to succeed");
187 let r33 = sub(&one, &twice(&r33_inner))
188 .expect("versor transform requires backend evaluation to succeed");
189
190 matrix_from_rows(&[[&r11, &r12, &r13], [&r21, &r22, &r23], [&r31, &r32, &r33]])
191 }
192}
193
194impl<B: Backend + BackendOps<f32> + Default> Transform<B, 3> for VersorRigid3DTransform<B>
195where
196 B::DeviceBuffer<f32>: CpuAddressableStorage<f32> + CpuAddressableStorageMut<f32>,
197{
198 fn transform_points(&self, points: Tensor<f32, B>) -> Tensor<f32, B> {
199 self.transform_variables(
200 &Var::new(points, false)
201 .expect("versor transform requires backend evaluation to succeed"),
202 )
203 .tensor
204 }
205}
206
207impl<B: Backend + BackendOps<f32> + Default> Module<f32, B> for VersorRigid3DTransform<B>
208where
209 B::DeviceBuffer<f32>: CpuAddressableStorage<f32> + CpuAddressableStorageMut<f32>,
210{
211 fn parameters(&self) -> Vec<Var<f32, B>> {
212 vec![self.translation.clone(), self.rotation.clone()]
213 }
214
215 fn named_parameters(&self) -> Vec<coeus_autograd::Parameter<f32, B>> {
216 vec![
217 coeus_autograd::Parameter::new(self.translation.clone(), "translation"),
218 coeus_autograd::Parameter::new(self.rotation.clone(), "rotation"),
219 ]
220 }
221
222 fn forward(
223 &self,
224 input: &Var<f32, B>,
225 ) -> Result<Var<f32, B>, coeus_nn::ModuleError<<B as coeus_core::ComputeBackend>::Error>> {
226 Ok(self.transform_variables(input))
227 }
228
229 fn load_parameters(&mut self, parameters: &[Var<f32, B>]) {
230 assert_eq!(
231 parameters.len(),
232 2,
233 "invariant: versor transform owns translation and rotation parameters"
234 );
235 self.translation = parameters[0].clone();
236 self.rotation = parameters[1].clone();
237 }
238}
239
240#[cfg(test)]
241mod tests {
242 use super::*;
243 use coeus_core::SequentialBackend;
244
245 #[test]
246 fn quarter_turn_about_x_maps_y_to_z() {
247 let backend = SequentialBackend;
248 let half_angle = std::f32::consts::FRAC_1_SQRT_2;
249 let transform = VersorRigid3DTransform::<SequentialBackend>::new(
250 Tensor::zeros_on([3], &backend).expect("test tensor is valid"),
251 Tensor::from_slice_on([4], &[half_angle, 0.0, 0.0, half_angle], &backend)
252 .expect("test tensor is valid"),
253 Tensor::zeros_on([3], &backend).expect("test tensor is valid"),
254 );
255 let point = Tensor::from_slice_on([1, 3], &[0.0, 1.0, 0.0], &backend)
256 .expect("test tensor is valid");
257
258 let transformed = transform.transform_points(point);
259
260 let values = transformed.as_slice();
261 let bound = 8.0 * f32::EPSILON;
262 assert!(values[0].abs() <= bound);
263 assert!(values[1].abs() <= bound);
264 assert!((values[2] - 1.0).abs() <= bound);
265 }
266}