engeom 0.4.2

Library for working with geometry with a focus on engineering metrology applications.
Documentation
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
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
//! Utilities for building Jacobian rows for 3d geometric optimization problems.
//!
//! A **Jacobian** is a matrix of partial derivatives. In this module, each row describes how a
//! distance-like residual changes when the underlying transform parameters are perturbed. These
//! derivatives are especially useful in iterative solvers such as least-squares optimization,
//! where the Jacobian helps the solver decide how to update position and orientation parameters.
//!
//! The functionality in this module focuses on common geometric distance models used throughout the
//! crate:
//!
//! - Point-to-plane approximations, where a point is compared against a nearby surface point and
//!   normal
//! - A reverse point-to-plane form for cases where the reference entity is transformed instead of
//!   the test point, this is used for multi-mesh alignments
//! - Point-to-point distance derivatives
//! - Helpers for inserting a single Jacobian row into a larger matrix
//!
//! These helpers are intended to make it easy to assemble Jacobians for alignment, registration,
//! and fitting routines in a consistent 6-parameter rigid-body setting.

use super::*;
use crate::geom3::align3::params::AlignValues3;
use crate::geom3::{Point3, SurfacePoint3};
use parry2d_f64::na::Dim;
use parry3d_f64::na::{Matrix, RawStorageMut, Storage, U6};

/// This is a helper function for computing the partial derivatives of the parameters (a single
/// row of the Jacobian matrix) for a distance function approximated by a point and its closest
/// point on a plane.  This is a reasonable approximation for distances measured between points and
/// the surface of a mesh, or points and a point/normal cloud where the points act locally like
/// planes.
///
/// This approximation assumes that the vector from the point to the closest point on the plane is
/// very close to (if not exactly) the normal of the plane.
///
/// # Arguments
///
/// * `p`: the test point (a sample point in the data being optimized)
/// * `c`: the reference surface point (a point and normal in the model) closest to `p`
/// * `rc`: a point which is the center of rotation for the test points
///
/// returns: Matrix<f64, Const<6>, Const<1>, ArrayStorage<f64, 6, 1>>
pub fn point_plane_jacobian(p: &Point3, c: &SurfacePoint3, params: &RcParams3) -> AlignStorage3 {
    let s = c.scalar_projection(p).signum();

    // The point with relation to the current center of rotation
    let from_rc = Point3::from(p - params.current_rc());

    point_plane_core(s, c, from_rc, params)
}

/// This is a helper function for computing the partial derivatives of the parameters for a
/// distance function approximated by a point and its closest point on a plane (represented as
/// a `SurfacePoint3`), but where the parameters being evaluated are the transform of the reference
/// plane, not the test point.
///
/// This is very similar to `point_plane_jacobian`, but is used in multi-entity simultaneous
/// alignments where not only are the test points being transformed, but the reference points are
/// potentially being transformed by a transform on that entity.
///
/// # Arguments
///
/// * `p`: the test point (a sample point in the data being optimized)
/// * `c`: the reference surface point (a point and normal in the model) closest to `p`
/// * `rc`: a point which is the center of rotation **for the reference points**
///
/// returns: Matrix<f64, Const<6>, Const<1>, ArrayStorage<f64, 6, 1>>
pub fn point_plane_jacobian_rev(
    p: &Point3,
    c: &SurfacePoint3,
    params: &RcParams3,
) -> AlignStorage3 {
    let s = c.scalar_projection(p).signum();

    // The point with relation to the current center of rotation
    let from_rc = Point3::from(c.point - params.current_rc());

    point_plane_core(-s, c, from_rc, params)
}

/// A core function for computing the partial derivatives of the parameters for a distance function
/// approximated by a point and its closest point on a plane (represented as a `SurfacePoint3`).
/// The internal functionality is common between `point_plane_jacobian` and
/// `point_plane_jacobian_rev`, the two callers, which differ only in the sign of the scalar
/// projection of the test point onto the plane.
///
/// # Arguments
///
/// * `s`: the sign of the scalar projection of the test point onto the plane
/// * `c`: the reference surface point (a point and normal in the model) closest to `p`
/// * `from_rc`: the point with relation to the current center of rotation
/// * `params`: the parameters of the current alignment
///
/// returns: Matrix<f64, Const<6>, Const<1>, ArrayStorage<f64, 6, 1>>
fn point_plane_core(
    s: f64,
    c: &SurfacePoint3,
    from_rc: Point3,
    params: &RcParams3,
) -> AlignStorage3 {
    let mut result = AlignStorage3::zeros();
    let n = c.normal.into_inner() * s;

    result[0] = n.x;
    result[1] = n.y;
    result[2] = n.z;

    result[3] = n.dot(&(params.rotations().rd.x * from_rc).coords);
    result[4] = n.dot(&(params.rotations().rd.y * from_rc).coords);
    result[5] = n.dot(&(params.rotations().rd.z * from_rc).coords);

    result
}

/// This is a helper function to calculate the partial derivatives of the parameters for a residual
/// distance between a test point and a surface point on a target entity.
///
/// # Arguments
///
/// * `p`: the test point, transformed into the target entity's coordinate system
/// * `c`: the closest point on the target surface
/// * `align`: the current alignment parameters, allowing for fast access of the direction vectors
///   associated with the different partial differentials.
///
/// returns: Matrix<f64, Const<6>, Const<1>, ArrayStorage<f64, 6, 1>>
pub fn point_surf_jacobian(
    p: &impl PCoords<3>,
    c: &impl SPCoords<3>,
    align: &AlignValues3,
) -> AlignStorage3 {
    let mut result = AlignStorage3::zeros();

    // We'll grab the sign of the scalar projection, allowing us to know if we're outside or inside
    // the target surface.
    let sign = c.scalar_projection(p).signum();

    // First, we want to calculate the deviation direction. If the magnitude of the deviation is
    // close to zero, we'll use the surface normal instead, as the two will converge as the point
    // approaches the surface.
    let dev = p.coords() - c.coords();
    let dir = if dev.norm_squared() < 1e-16 {
        c.normal().into_inner()
    } else {
        dev.normalize() * sign
    };

    // The transformations will be the dot product of the deviation direction and the partial
    // differential translation directions. For instance, if the translation is going across the
    // surface, there's no real penalty because we're staying equidistant.
    result[0] = val_or_zero(align.dtx.dot(&dir), align.dof.tx);
    result[1] = val_or_zero(align.dty.dot(&dir), align.dof.ty);
    result[2] = val_or_zero(align.dtz.dot(&dir), align.dof.tz);

    // The rotations will be the dot product of the deviation direction and the partial differential
    // rotation directions.
    //
    // These are evaluated at the closest point `c` rather than the test point `p`, even though
    // the residual's derivative nominally calls for `p` (it is `p` that the parameters move, `c`
    // being a fixed position on the stationary target). The two are exactly equal, so the choice
    // is immaterial:
    //
    // `pre_rot`'s rotation part is the inverse of `offset`'s rotation and each `m_dr*` is
    // `offset`'s rotation times an Euler partial, so the translation cancels in the difference
    // and `dr(p) - dr(c) = post_rot * E * post_rot^-1 * (p - c)`. `euler_partials` builds every
    // `E` as `Q * SK * Q'` for some rotation `Q`, which is skew-symmetric, and conjugating by
    // `post_rot` keeps it so. Since `v' * S * v == 0` for any skew-symmetric `S`, and `dir` is
    // parallel to `p - c`, the difference contributes exactly zero.
    //
    // The 2D counterpart in `geom2::align2::jacobian` uses `p` instead, purely because it reads
    // more directly as the derivative; its `stress_against_numeric` test confirms both forms
    // agree against a finite-difference estimate far from convergence.
    result[3] = val_or_zero(align.drx(c).dot(&dir), align.dof.rx);
    result[4] = val_or_zero(align.dry(c).dot(&dir), align.dof.ry);
    result[5] = val_or_zero(align.drz(c).dot(&dir), align.dof.rz);

    result
}

fn val_or_zero(value: f64, condition: bool) -> f64 {
    if condition { value } else { 0.0 }
}

/// Generic helper to copy the contents of a single row into a larger jacobian matrix of either
/// fixed or dynamic row count
///
/// # Arguments
///
/// * `j`: The source Jacobian row to copy
/// * `matrix`: The destination matrix to copy into
/// * `row`: The row index in the destination matrix to copy into
///
/// returns: ()
pub fn copy_jacobian<R, S>(j: &AlignStorage3, matrix: &mut Matrix<f64, R, U6, S>, row: usize)
where
    R: Dim,
    S: RawStorageMut<f64, R, U6> + Storage<f64, R, U6>,
{
    matrix.row_mut(row).copy_from_slice(j.as_slice());
}

#[cfg(test)]
mod tests {
    //! For lack of a more clever way of testing the jacobians, I've written a bunch of numerical
    //! versions of the jacobian calculations. The speed of the LM solver in the real implementation
    //! depends pretty heavily on how the real jacobian calculations work from making the assumption
    //! that an infinitesimally small motion of a test point against the closest surface of a
    //! reference won't change the location of the closest point, allowing the partial differentials
    //! to be approximated just from the deviation and surface directions.
    //!
    //! Since I had to figure these out on my own, I wanted to check them against something that
    //! I had relatively high confidence in, and the numerical method seemed like the most
    //! straightforward, obviously correct option.  I use the numerical alternative to check against
    //! a few different categories of cases.
    use super::*;
    use crate::geom3::Point3;
    use approx::assert_relative_eq;
    use parry3d_f64::na::{Dyn, Owned};
    use std::f64::consts::PI;

    const NUMERIC_EPSILON: f64 = 1e-8;

    fn point_plane_numeric(
        params: &RcParams3,
        p: &Point3,
        closest: &SurfacePoint3,
        index: usize,
    ) -> f64 {
        let mut params = params.clone();
        let t_i = params.transform().inverse();
        let mut x = *params.x();
        x[index] += NUMERIC_EPSILON;
        params.set(&x);
        let t = params.transform() * t_i;

        let moved = t * *p;
        let d0 = closest.scalar_projection(p).abs();
        let d1 = closest.scalar_projection(&moved).abs();
        (d1 - d0) / NUMERIC_EPSILON
    }

    fn point_plane_numeric_rev(
        params: &RcParams3,
        p: &Point3,
        closest: &SurfacePoint3,
        index: usize,
    ) -> f64 {
        let mut params = params.clone();
        let t_i = params.transform().inverse();
        let mut x = *params.x();
        x[index] += NUMERIC_EPSILON;
        params.set(&x);
        let t = params.transform() * t_i;

        // Get the original value
        let d0 = closest.scalar_projection(p);

        // Move the reference point
        let moved = closest.transformed_by(&t);
        let d1 = moved.scalar_projection(p);

        (d1 - d0) / NUMERIC_EPSILON
    }

    /// Numerical approximation of the derivative of the Euclidean distance |p - c| with respect
    /// to parameter `index`, using a forward finite difference.  The point `p` is moved by the
    /// incremental transform produced by perturbing the parameter, and the distance to the fixed
    /// reference point `c` is measured before and after.
    #[test]
    fn test_point_plane_translation() {
        let p = Point3::new(1.0, 2.0, 3.0);
        let c = Point3::new(0.0, 0.0, 0.0);
        let sp = SurfacePoint3::new_normalize(c, p - c);

        let initial = Iso3::from_parts(
            Translation3::new(8.0, -5.0, -6.0),
            UnitQuaternion::from_euler_angles(-0.2, 0.3, 0.5),
        );

        let rc = Point3::new(-1.0, -2.0, 3.0);
        let params = RcParams3::from_initial(&initial, &(initial.inverse() * rc));

        let test = point_plane_jacobian(&p, &sp, &params);

        assert_relative_eq!(
            point_plane_numeric(&params, &p, &sp, 0),
            test.x,
            epsilon = 1e-6
        );
        assert_relative_eq!(
            point_plane_numeric(&params, &p, &sp, 1),
            test.y,
            epsilon = 1e-6
        );
        assert_relative_eq!(
            point_plane_numeric(&params, &p, &sp, 2),
            test.z,
            epsilon = 1e-6
        );
    }

    /// This is the simplest possible test of the jacobians for the rotations, and involves a
    /// parameter set that is currently an identity transform and a center of rotation which is
    /// currently at the origin.
    #[test]
    fn test_point_plane_simple_rotations() {
        let p = Point3::new(1.0, 2.0, 3.0);
        let c = Point3::new(1.0, 3.0, 4.0);
        let sp = SurfacePoint3::new_normalize(c, p - c);
        let rc = Point3::origin();

        let params = RcParams3::from_initial(&Iso3::identity(), &rc);
        let test = point_plane_jacobian(&p, &sp, &params);

        let expected_w = point_plane_numeric(&params, &p, &sp, 3);
        let expected_a = point_plane_numeric(&params, &p, &sp, 4);
        let expected_b = point_plane_numeric(&params, &p, &sp, 5);
        assert_relative_eq!(expected_w, test.w, epsilon = 1e-6);
        assert_relative_eq!(expected_a, test.a, epsilon = 1e-6);
        assert_relative_eq!(expected_b, test.b, epsilon = 1e-6);
    }

    /// This test is a little more complicated, and involves a reference center of rotation which
    /// is not at the origin. However the parameter set is still an identity transform.
    #[test]
    fn test_point_plane_centered_rotations() {
        let p = Point3::new(1.0, 2.0, 3.0);
        let c = Point3::new(1.0, 3.0, 4.0);
        let sp = SurfacePoint3::new_normalize(c, p - c);
        let rc = Point3::new(-1.2, -3.5, -0.75);

        let params = RcParams3::from_initial(&Iso3::identity(), &rc);
        let test = point_plane_jacobian(&p, &sp, &params);

        let expected_w = point_plane_numeric(&params, &p, &sp, 3);
        let expected_a = point_plane_numeric(&params, &p, &sp, 4);
        let expected_b = point_plane_numeric(&params, &p, &sp, 5);
        assert_relative_eq!(expected_w, test.w, epsilon = 1e-6);
        assert_relative_eq!(expected_a, test.a, epsilon = 1e-6);
        assert_relative_eq!(expected_b, test.b, epsilon = 1e-6);
    }

    #[test]
    fn test_point_plane_initial_rz_rotation() {
        let p1 = Point3::new(1.0, 0.0, 0.0);
        let c = Point3::new(1.0, 0.0, 0.0);
        let sp = SurfacePoint3::new_normalize(c, Vector3::new(0.0, 1.0, 0.0));
        let rc1 = Point3::origin();

        let initial = Iso3::from_parts(
            Translation3::identity(),
            UnitQuaternion::from_euler_angles(0.0, 0.0, -PI / 2.0),
        );

        let rc0 = initial.inverse() * rc1;
        let p0 = initial.inverse() * p1;

        let params = RcParams3::from_initial(&initial, &rc0);
        assert_relative_eq!(*params.current_rc(), rc1, epsilon = 1e-6);
        assert_relative_eq!(params.transform() * p0, p1, epsilon = 1e-6);

        let test = point_plane_jacobian(&p1, &sp, &params);
        //
        let expected_w = point_plane_numeric(&params, &p1, &sp, 3);
        let expected_a = point_plane_numeric(&params, &p1, &sp, 4);
        let expected_b = point_plane_numeric(&params, &p1, &sp, 5);
        assert_relative_eq!(expected_w, test.w, epsilon = 1e-6);
        assert_relative_eq!(expected_a, test.a, epsilon = 1e-6);
        assert_relative_eq!(expected_b, test.b, epsilon = 1e-6);
    }

    #[test]
    fn test_point_plane_rotation() {
        // These are the constructs after being transformed by the initial transform
        let p1 = Point3::new(10.0, 20.0, 30.0);
        let r1 = Point3::new(5.0, 8.0, 18.0);
        let c = Point3::new(12.0, 18.0, 28.0);
        let sp = SurfacePoint3::new_normalize(c, p1 - c);

        let initial = Iso3::from_parts(
            Translation3::new(8.0, -5.0, -6.0),
            UnitQuaternion::from_euler_angles(-0.2, 0.3, 0.5),
        );

        let p0 = initial.inverse() * p1;
        let r0 = initial.inverse() * r1;

        let params = RcParams3::from_initial(&initial, &r0);
        assert_relative_eq!(p1, params.transform() * p0, epsilon = 1e-8);
        assert_relative_eq!(r1, params.transform() * r0, epsilon = 1e-8);

        let test = point_plane_jacobian(&p1, &sp, &params);
        let expected_w = point_plane_numeric(&params, &p1, &sp, 3);
        let expected_a = point_plane_numeric(&params, &p1, &sp, 4);
        let expected_b = point_plane_numeric(&params, &p1, &sp, 5);
        assert_relative_eq!(expected_w, test.w, epsilon = 1e-6);
        assert_relative_eq!(expected_a, test.a, epsilon = 1e-6);
        assert_relative_eq!(expected_b, test.b, epsilon = 1e-6);
    }

    fn rev_test() -> (RcParams3, Point3, SurfacePoint3, AlignStorage3) {
        let p = Point3::new(2.0, 3.0, 4.0);
        let cp = Point3::new(1.0, 2.0, 3.0);
        let cn = Vector3::new(1.0, 1.0, 1.0);
        let c = SurfacePoint3::new_normalize(cp, cn);
        let rc = Point3::new(-0.5, -0.75, 3.25);

        let initial = Iso3::from_parts(
            Translation3::new(4.0, 3.0, 2.0),
            UnitQuaternion::from_euler_angles(0.2, -0.3, 0.5),
        );

        let rc0 = initial.inverse() * rc;
        let params = RcParams3::from_initial(&initial, &rc0);

        let test = point_plane_jacobian_rev(&p, &c, &params);
        (params, p, c, test)
    }

    #[test]
    fn test_point_plane_rev_translation() {
        let (params, p, c, test) = rev_test();
        assert_relative_eq!(
            point_plane_numeric_rev(&params, &p, &c, 0),
            test.x,
            epsilon = 1e-6
        );
        assert_relative_eq!(
            point_plane_numeric_rev(&params, &p, &c, 1),
            test.y,
            epsilon = 1e-6
        );
        assert_relative_eq!(
            point_plane_numeric_rev(&params, &p, &c, 2),
            test.z,
            epsilon = 1e-6
        );
    }

    #[test]
    fn test_point_plane_rev_rotation() {
        let (params, p, c, test) = rev_test();
        assert_relative_eq!(
            point_plane_numeric_rev(&params, &p, &c, 3),
            test.w,
            epsilon = 1e-6
        );
        assert_relative_eq!(
            point_plane_numeric_rev(&params, &p, &c, 4),
            test.a,
            epsilon = 1e-6
        );
        assert_relative_eq!(
            point_plane_numeric_rev(&params, &p, &c, 5),
            test.b,
            epsilon = 1e-6
        );
    }

    #[test]
    fn test_jacobian_copy() {
        let x = AlignStorage3::new(1.0, 2.0, 3.0, 4.0, 5.0, 6.0);
        let mut target = Matrix::<f64, Dyn, U6, Owned<f64, Dyn, U6>>::zeros(10);
        // Copy the jacobian into the target matrix
        copy_jacobian(&x, &mut target, 4);

        assert_eq!(target[(4, 0)], 1.0);
        assert_eq!(target[(4, 1)], 2.0);
        assert_eq!(target[(4, 2)], 3.0);
        assert_eq!(target[(4, 3)], 4.0);
        assert_eq!(target[(4, 4)], 5.0);
        assert_eq!(target[(4, 5)], 6.0);
    }
}