Skip to main content

ritk_transform/transform/affine/
versor.rs

1//! Trainable three-dimensional quaternion rigid transform.
2
3use 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/// Quaternion rotation and translation about a fixed center.
12///
13/// The quaternion component order is `(x, y, z, w)`.
14#[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    /// Construct a trainable versor transform.
26    ///
27    /// # Panics
28    ///
29    /// Panics unless translation and center are `[3]`, rotation is `[4]`, and
30    /// the quaternion has a finite nonzero norm.
31    #[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    /// Clone the current translation.
70    #[must_use]
71    pub fn translation(&self) -> Tensor<f32, B> {
72        self.translation.tensor.clone()
73    }
74
75    /// Clone the current quaternion.
76    #[must_use]
77    pub fn rotation(&self) -> Tensor<f32, B> {
78        self.rotation.tensor.clone()
79    }
80
81    /// Clone the fixed center.
82    #[must_use]
83    pub fn center(&self) -> Tensor<f32, B> {
84        self.center.tensor.clone()
85    }
86
87    /// Apply the transform while retaining the Coeus autograd graph.
88    #[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(&center_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, &center).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(&centered, &rotation_t)
110            .expect("versor transform requires backend evaluation to succeed");
111        let centered_output = add(&rotated, &center)
112            .expect("versor transform requires backend evaluation to succeed");
113        add(&centered_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}