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 .as_slice()
51 .iter()
52 .map(|value| value * value)
53 .sum::<f32>();
54 assert!(
55 squared_norm.is_finite() && squared_norm > 0.0,
56 "versor quaternion must have a finite nonzero norm"
57 );
58 Self {
59 translation: Var::new(translation, true),
60 rotation: Var::new(rotation, true),
61 center: Var::new(center, false),
62 }
63 }
64
65 #[must_use]
67 pub fn translation(&self) -> Tensor<f32, B> {
68 self.translation.tensor.clone()
69 }
70
71 #[must_use]
73 pub fn rotation(&self) -> Tensor<f32, B> {
74 self.rotation.tensor.clone()
75 }
76
77 #[must_use]
79 pub fn center(&self) -> Tensor<f32, B> {
80 self.center.tensor.clone()
81 }
82
83 #[must_use]
85 pub fn transform_variables(&self, points: &Var<f32, B>) -> Var<f32, B> {
86 let shape = points.tensor.shape();
87 assert!(
88 shape.len() == 2 && shape[1] == 3,
89 "versor transform requires points shaped [N, 3], got {shape:?}"
90 );
91 let target = vec![shape[0], 3];
92 let center = broadcast_to(&reshape(&self.center, [1, 3]), target.clone());
93 let translation = broadcast_to(&reshape(&self.translation, [1, 3]), target);
94 let centered = sub(points, ¢er);
95 let rotated = coeus_autograd::matmul(
96 ¢ered,
97 &coeus_autograd::transpose_2d(&self.rotation_matrix()),
98 );
99 add(&add(&rotated, ¢er), &translation)
100 }
101
102 fn rotation_matrix(&self) -> Var<f32, B> {
103 let norm = sqrt(&sum(&mul(&self.rotation, &self.rotation)));
104 let quaternion = div(&self.rotation, &broadcast_to(&norm, vec![4]));
105 let component = |index| coeus_autograd::slice(&quaternion, &[(index, index + 1)]);
106 let (x, y, z, w) = (component(0), component(1), component(2), component(3));
107 let one = Var::new(Tensor::from_slice_on([1], &[1.0], &B::default()), false);
108 let two = Var::new(Tensor::from_slice_on([1], &[2.0], &B::default()), false);
109 let twice = |value: &Var<f32, B>| mul(&two, value);
110
111 let xx = mul(&x, &x);
112 let yy = mul(&y, &y);
113 let zz = mul(&z, &z);
114 let xy = mul(&x, &y);
115 let xz = mul(&x, &z);
116 let yz = mul(&y, &z);
117 let xw = mul(&x, &w);
118 let yw = mul(&y, &w);
119 let zw = mul(&z, &w);
120
121 let r11 = sub(&one, &twice(&add(&yy, &zz)));
122 let r12 = twice(&sub(&xy, &zw));
123 let r13 = twice(&add(&xz, &yw));
124 let r21 = twice(&add(&xy, &zw));
125 let r22 = sub(&one, &twice(&add(&xx, &zz)));
126 let r23 = twice(&sub(&yz, &xw));
127 let r31 = twice(&sub(&xz, &yw));
128 let r32 = twice(&add(&yz, &xw));
129 let r33 = sub(&one, &twice(&add(&xx, &yy)));
130
131 matrix_from_rows(&[[&r11, &r12, &r13], [&r21, &r22, &r23], [&r31, &r32, &r33]])
132 }
133}
134
135impl<B: Backend + BackendOps<f32> + Default> Transform<B, 3> for VersorRigid3DTransform<B>
136where
137 B::DeviceBuffer<f32>: CpuAddressableStorage<f32> + CpuAddressableStorageMut<f32>,
138{
139 fn transform_points(&self, points: Tensor<f32, B>) -> Tensor<f32, B> {
140 self.transform_variables(&Var::new(points, false)).tensor
141 }
142}
143
144impl<B: Backend + BackendOps<f32> + Default> Module<f32, B> for VersorRigid3DTransform<B>
145where
146 B::DeviceBuffer<f32>: CpuAddressableStorage<f32> + CpuAddressableStorageMut<f32>,
147{
148 fn parameters(&self) -> Vec<Var<f32, B>> {
149 vec![self.translation.clone(), self.rotation.clone()]
150 }
151
152 fn named_parameters(&self) -> Vec<coeus_autograd::Parameter<f32, B>> {
153 vec![
154 coeus_autograd::Parameter::new(self.translation.clone(), "translation"),
155 coeus_autograd::Parameter::new(self.rotation.clone(), "rotation"),
156 ]
157 }
158
159 fn forward(
160 &self,
161 input: &Var<f32, B>,
162 ) -> Result<Var<f32, B>, coeus_nn::ModuleError<<B as coeus_core::ComputeBackend>::Error>> {
163 Ok(self.transform_variables(input))
164 }
165
166 fn load_parameters(&mut self, parameters: &[Var<f32, B>]) {
167 assert_eq!(
168 parameters.len(),
169 2,
170 "invariant: versor transform owns translation and rotation parameters"
171 );
172 self.translation = parameters[0].clone();
173 self.rotation = parameters[1].clone();
174 }
175}
176
177#[cfg(test)]
178mod tests {
179 use super::*;
180 use coeus_core::SequentialBackend;
181
182 #[test]
183 fn quarter_turn_about_x_maps_y_to_z() {
184 let backend = SequentialBackend;
185 let half_angle = std::f32::consts::FRAC_1_SQRT_2;
186 let transform = VersorRigid3DTransform::<SequentialBackend>::new(
187 Tensor::zeros_on([3], &backend),
188 Tensor::from_slice_on([4], &[half_angle, 0.0, 0.0, half_angle], &backend),
189 Tensor::zeros_on([3], &backend),
190 );
191 let point = Tensor::from_slice_on([1, 3], &[0.0, 1.0, 0.0], &backend);
192
193 let transformed = transform.transform_points(point);
194
195 let values = transformed.as_slice();
196 let bound = 8.0 * f32::EPSILON;
197 assert!(values[0].abs() <= bound);
198 assert!(values[1].abs() <= bound);
199 assert!((values[2] - 1.0).abs() <= bound);
200 }
201}