Construct similarity transformations from point-pairs (#97)

Given two sets of points, this constructs rigid (rotation
+ translation) and similarity (rotation, translation, and scaling)
transformations that align the first set to the second in the
least-squares sense. Similar to #87, but with constraints on the nature
of the transformation.

Co-authored by: Tom McGrath <72618709+tmcgrath325@users.noreply.github.com>

Author: timholy

README.md

@@ -178,11 +178,19 @@ julia> from_points = [[0, 0], [1, 0], [0, 1]];
 julia> to_points   = [[1, 1], [3, 1], [1.5, 3]];
 
 julia> AffineMap(from_points => to_points)
-AffineMap([1.9999999999999996 0.4999999999999999; -5.551115123125783e-16 2.0], [0.9999999999999999, 1.0000000000000002])
+AffineMap([2.0 0.5; 0.0 2.0], [1.0, 1.0])
 ```
 
 The points can be supplied as a collection of vectors or as a matrix with points as columns.
 
+If you want to restrict the transformation to be rigid (rotation + translation)
+or similar (rotation, translation, and scaling), use `kabsch` instead:
+
+```julia
+julia> rigid = kabsch(from_points => to_points)
+AffineMap([0.9912279006826346 0.132163720091018; -0.1321637200910178 0.9912279006826348], [1.4588694597421157, 1.380311939802794])
+```
+
 #### Perspective transformations
 
 The perspective transformation maps real-space coordinates to those on a virtual

docs/src/api.md

@@ -22,6 +22,7 @@ AffineMap(::Transformation, ::Any)
 AffineMap(::Pair)
 LinearMap
 Translation
+kabsch
 ```
 
 ## 2D Coordinates

docs/src/index.md

@@ -179,7 +179,7 @@ and a translation, e.g. `Translation(v) ∘ LinearMap(v)` (or any combination of
 
 `AffineMap`s can be constructed to fit point pairs `from_points => to_points`:
 
-```jldoctest; filter=[r"(2\.0|1\.9999\d+)" => "2.0", r"(0\.5|0\.49999\d+)" => "0.5", r"(0\.0|[ -]\d\.\d+e-\d\d)" => "0.0", r"(1\.0(?!0)|1\.0000\d+|0\.9999\d+)" => "1.0"]
+```jldoctest lsq; filter=[r"(2\.0|1\.9999\d+)" => "2.0", r"(0\.5|0\.49999\d+)" => "0.5", r"(0\.0|[ -]\d\.\d+e-\d\d)" => "0.0", r"(1\.0(?!0)|1\.0000\d+|0\.9999\d+)" => "1.0"]
 julia> from_points = [[0, 0], [1, 0], [0, 1]];
 
 julia> to_points   = [[1, 1], [3, 1], [1.5, 3]];
@@ -190,6 +190,15 @@ AffineMap([2.0 0.5; 0.0 2.0], [1.0, 1.0])
 
 (You may get slightly different numerical values due to roundoff errors.) The points can be supplied as a collection of vectors or as a matrix with points as columns.
 
+If you want to restrict the transformation to be rigid (rotation + translation)
+or similar (rotation, translation, and scaling), use `kabsch` instead:
+
+```jldoctest lsq; filter=[r"0\.9912\d+" => "0.9912", r"0\.1321\d+" => "0.1321", r"1\.4588\d+" => "1.4588", r"1\.3803\d+" => "1.3803"]
+julia> rigid = kabsch(from_points => to_points)
+AffineMap([0.9912279006826346 0.132163720091018; -0.1321637200910178 0.9912279006826348], [1.4588694597421157, 1.380311939802794])
+```
+
+
 ### Perspective transformations
 
 The perspective transformation maps real-space coordinates to those on a virtual

src/CoordinateTransformations.jl

@@ -21,10 +21,12 @@ export SphericalFromCartesian, CartesianFromSpherical,
 export AbstractAffineMap
 export AffineMap, LinearMap, Translation
 export PerspectiveMap, cameramap
+export kabsch
 
 include("core.jl")
 include("coordinatesystems.jl")
 include("affine.jl")
 include("perspective.jl")
+include("kabsch.jl")
 
 end # module

src/kabsch.jl

@@ -0,0 +1,58 @@
+# Compute rigid and similarity transformations between point sets
+
+# For rigid transformations, we use:
+# Kabsch, Wolfgang. "A discussion of the solution for the best rotation to
+# relate two sets of vectors." Acta Crystallographica Section A: Crystal
+# Physics, Diffraction, Theoretical and General Crystallography 34.5 (1978):
+# 827-828.
+# This has been generalized to support weighted points:
+# https://igl.ethz.ch/projects/ARAP/svd_rot.pdf
+# We add the component for similarity transformations from:
+# Umeyama, Shinji. "Least-squares estimation of transformation parameters
+# between two point patterns." IEEE Transactions on Pattern Analysis & Machine
+# Intelligence 13.04 (1991): 376-380.
+
+# See also
+# https://en.wikipedia.org/wiki/Kabsch_algorithm
+
+
+# All matrices are DxN, where N is the number of positions and D is the dimensionality
+
+# Here, P is the probe (to be rotated) and Q is the refereence
+
+# `kabsch_centered` assumes P and Q are already centered at the origin
+# returns the rotation (optionally with scaling) for alignment
+function kabsch_centered(P, Q, w; scale::Bool=false, svd::F = LinearAlgebra.svd) where F
+    @assert size(P) == size(Q)
+    W = Diagonal(w/sum(w))
+    H = P*W*Q'
+    U,Σ,V = svd(H)
+    Ddiag = ones(eltype(H), size(H,1))
+    Ddiag[end] = sign(det(V*U'))
+    c = scale ? sum(Σ .* Ddiag) / sum(P .* (P*W)) : 1
+    return LinearMap(V * Diagonal(c * Ddiag) * U')
+end
+
+"""
+    kabsch(from_points => to_points, w=ones(npoints); scale::Bool=false, svd=LinearAlgebra.svd) → trans
+
+Compute the rigid transformation (or similarity transformation, if `scale=true`)
+that aligns `from_points` to `to_points` in a least-squares sense.
+
+Optionally specify the non-negative weights `w` for each point. The default value of the weight
+is 1 for each point.
+
+For
+differentiability, use `svd = GenericLinearAlgebra.svd` or other differentiable
+singular value decomposition.
+"""
+function kabsch(pr::Pair{<:AbstractMatrix, <:AbstractMatrix}, w::AbstractVector=ones(size(pr.first,2)); scale::Bool=false, kwargs...)
+    P, Q = pr
+    any(<(0), w) && throw(ArgumentError("weights must be non-negative"))
+    all(iszero, w) && throw(ArgumentError("weights must not all be zero"))
+    wn = w/sum(w)
+    centerP, centerQ = P*wn, Q*wn
+    R = kabsch_centered(P .- centerP, Q .- centerQ, w; scale, kwargs...)
+    return inv(Translation(-centerQ)) ∘ R ∘ Translation(-centerP)
+end
+kabsch((from_points, to_points)::Pair, args...; kwargs...) = kabsch(column_matrix(from_points) => column_matrix(to_points), args...; kwargs...)

test/affine.jl

@@ -144,5 +144,46 @@ end
         to_points = map(A, from_points)
         A2 = AffineMap(from_points => to_points)
         @test A2 ≈ A
+
+        ## Rigid transformations
+        θ = π / 7
+        R = [cos(θ) -sin(θ); sin(θ) cos(θ)]
+        v = [0.87, 0.15]
+        A = AffineMap(R, v)
+        from_points = [[0.0, 0.0], [1.0, 0.0], [0.0, 1.0], [1.0, 1.0]]
+        to_points = map(A, from_points)
+        A2 = @inferred(kabsch(from_points => to_points))
+        @test A2 ≈ A
+        # with weights
+        A2 = kabsch(from_points => to_points, [0.2, 0.7, 0.9, 0.3])
+        @test A2 ≈ A
+        A2 = kabsch(reduce(hcat, from_points) => reduce(hcat, to_points))
+        @test A2 ≈ A
+        from_points = ([0.0, 0.0], [1.0, 0.0], [0.0, 1.0], [1.0, 1.0])
+        to_points = map(A, from_points)
+        A2 = kabsch(from_points => to_points)
+        @test A2 ≈ A
+        # with user-specified SVD
+        A2 = @inferred(kabsch(from_points => to_points; svd=LinearAlgebra.svd))
+        @test A2 ≈ A
+        # when a rigid transformation is not possible
+        A2 = kabsch(from_points => 1.1 .* from_points)
+        @test A2.linear' * A2.linear ≈ I
+
+        @test_throws "weights must be non-negative" kabsch(from_points => to_points, [0.2, -0.7, 0.9, 0.3])
+        @test_throws "weights must not all be zero" kabsch(from_points => to_points, [0.0, 0.0, 0.0, 0.0])
+
+        # Similarity transformations
+        θ = π / 7
+        R = [cos(θ) -sin(θ); sin(θ) cos(θ)]
+        v = [0.87, 0.15]
+        c = 1.15
+        A = AffineMap(c * R, v)
+        from_points = [[0.0, 0.0], [1.0, 0.0], [0.0, 1.0], [1.0, 1.0]]
+        to_points = map(A, from_points)
+        A2 = @inferred(kabsch(from_points => to_points; scale=true))
+        @test A2 ≈ A
+        A2 = @inferred(kabsch(from_points => to_points, [0.2, 0.7, 0.9, 0.3]; scale=true))
+        @test A2 ≈ A
     end
 end