Update to CoordinateTransformations.kabsch (#50)

`kabsch` has now been implemented in CoordinateTransformations. While
new features are ordinarily not breaking changes, for this package it is
due to name conflict. This PR resolves the name conflict, and bumps
`[compat]`.

Closes #49

Author: timholy

Project.toml

@@ -1,7 +1,7 @@
 name = "GaussianMixtureAlignment"
 uuid = "f2431ed1-b9c2-4fdb-af1b-a74d6c93b3b3"
 authors = ["Tom McGrath <[email protected]> and contributors"]
-version = "0.2.2"
+version = "0.2.3"
 
 [deps]
 Colors = "5ae59095-9a9b-59fe-a467-6f913c188581"
@@ -27,15 +27,15 @@ Makie = "ee78f7c6-11fb-53f2-987a-cfe4a2b5a57a"
 GaussianMixtureAlignmentMakieExt = "Makie"
 
 [compat]
-Colors = "0.12"
-CoordinateTransformations = "0.6"
+Colors = "0.12, 0.13"
+CoordinateTransformations = "0.6.4"
 Distances = "0.10"
 ForwardDiff = "0.10"
 GenericLinearAlgebra = "0.3"
-GeometryBasics = "0.4"
+GeometryBasics = "0.4, 0.5"
 Hungarian = "0.7"
-Makie = "0.21"
-MakieCore = "0.6, 0.7, 0.8"
+Makie = "0.21, 0.22"
+MakieCore = "0.6, 0.7, 0.8, 0.9"
 MutableConvexHulls = "0.2"
 NearestNeighbors = "0.4.1"
 Optim = "1.7"

src/GaussianMixtureAlignment.jl

@@ -36,6 +36,8 @@ using MakieCore
 using GeometryBasics
 using Colors
 
+using CoordinateTransformations: kabsch_centered
+
 export AbstractGaussian, AbstractGMM
 export IsotropicGaussian, IsotropicGMM, IsotropicMultiGMM
 export overlap, force!, gogma_align, rot_gogma_align, trl_gogma_align, tiv_gogma_align

src/goicp/icp.jl

@@ -14,7 +14,7 @@ function iterate_kabsch(P, Q, wp=ones(size(P,2)), wq=ones(size(Q,2)); iterations
         # TO DO: make sure that, for hungarian assignment, the proper weights are selected
 
         prevscore = score
-        tform = kabsch(P, Q, matches, wp, wq)
+        tform = kabsch_matches(P, Q, matches, wp, wq)
         score = squared_deviation(tform(P),Q,matches)
         if prevscore < score
             matches = prevmatches
@@ -38,12 +38,12 @@ function icp(P::AbstractMatrix, Q::AbstractMatrix, wp=ones(size(P,2)), wq=ones(s
 end
 icp(P::AbstractSinglePointSet, Q::AbstractSinglePointSet; kwargs...) = icp(P.coords, Q.coords, P.weights, Q.weights; kwargs...)
 
-iterative_hungarian(args...; kwargs...) = iterate_kabsch(args...; correspondence = hungarian_assignment, kwargs...) 
+iterative_hungarian(args...; kwargs...) = iterate_kabsch(args...; correspondence = hungarian_assignment, kwargs...)
 
 function local_matching_alignment(x::AbstractPointSet, y::AbstractPointSet, block::SearchRegion; matching_fun = iterative_hungarian, kwargs...)
     tformedx = block.R*x + block.T
     matches = matching_fun(tformedx, y; kwargs...)
-    tform = kabsch(x, y, matches)
+    tform = kabsch_matches(x, y, matches)
     score = squared_deviation(tform(x), y, matches)
     R = RotationVec(tform.linear)
     params = (R.sx, R.sy, R.sz, tform.translation...)
@@ -53,7 +53,7 @@ end
 function local_matching_alignment(x::AbstractPointSet, y::AbstractPointSet, block::RotationRegion; matching_fun = iterative_hungarian, kwargs...)
     tformedx = block.R*x + block.T
     matches = matching_fun(tformedx, y; kwargs...)
-    tform = kabsch(x, y, matches)
+    tform = kabsch_matches(x, y, matches)
     score = squared_deviation(tform(x), y, matches)
     R = RotationVec(tform.linear)
     params = (R.sx, R.sy, R.sz)

src/goicp/kabsch.jl

@@ -1,57 +1,33 @@
-# All matrices are DxN, where N is the number of positions and D is the dimensionality
+# The implementation that was originally developed here got upstreamed into CoordinateTransformations:
+#    https://github.com/JuliaGeometry/CoordinateTransformations.jl/pull/97
+# This now extends the upstream implementation.
 
-# Here, P is the probe (to be rotated) and Q is the refereence
-# https://en.wikipedia.org/wiki/Kabsch_algorithm
-# This has been generalized to support weighted points:
-# https://igl.ethz.ch/projects/ARAP/svd_rot.pdf
+CoordinateTransformations.kabsch_centered(P::PointSet, Q::PointSet) = kabsch_centered(P.coords, Q.coords, P.weights .* Q.weights);
+CoordinateTransformations.kabsch(P::PointSet, Q::PointSet) = kabsch(P.coords => Q.coords, P.weights .* Q.weights);
 
-# assuming P and Q are already centered at the origin
-# returns the rotation for alignment
-function kabsch_centered(P,Q,w)
-    @assert size(P) == size(Q)
-    W = diagm(w/sum(w)) # here, the weights are assumed to sum to 1
-    H = P*W*Q'
-    D = Matrix{Float64}(I,size(H,1), size(H,2))
-    # U,Σ,V = svd(H)
-    U,Σ,V = GenericLinearAlgebra.svd(H)
-    D[end] = sign(det(V*U'))
-    return LinearMap(V * D * U')
-end
-
-function kabsch_centered(P,Q,matches::AbstractVector{<:Tuple{Int,Int}},wp=ones(size(P,2)),wq=ones(size(Q,2)))
+function kabsch_centered_matches(P,Q,matches::AbstractVector{<:Tuple{Int,Int}},wp=ones(size(P,2)),wq=ones(size(Q,2)))
     matchedP, matchedQ = matched_points(P,Q,matches)
     w = [wp[i]*wq[j] for (i,j) in matches]
     return kabsch_centered(matchedP, matchedQ, w)
 end
 
-kabsch_centered(P::PointSet, Q::PointSet) = kabsch_centered(P.coords, Q.coords, P.weights .* Q.weights);
-kabsch_centered(P::PointSet, Q::PointSet, matches::AbstractVector{<:Tuple{Int,Int}}) = kabsch_centered(P.coords, Q.coords, matches, P.weights, Q.weights);
+kabsch_centered_matches(P::PointSet, Q::PointSet, matches::AbstractVector{<:Tuple{Int,Int}}) = kabsch_centered_matches(P.coords, Q.coords, matches, P.weights, Q.weights);
 
 # transform DxN matrices
 function (tform::Translation)(A::AbstractMatrix)
     return hcat([tform(A[:,i]) for i=1:size(A,2)]...)
 end
 
-# P and Q are not necessarily centered
-# returns the transformation for alignment
-function kabsch(P, Q, w::AbstractVector=ones(size(P,2)))
-    @assert !any(w.<0) && sum(w)>0
-    wn = w/sum(w) # weights should sum to 1 for computing the centroid
-    centerP, centerQ = center_translation(P,wn), center_translation(Q,wn)
-    R = kabsch_centered(centerP(P), centerQ(Q), wn)
-    return inv(centerQ) ∘ R ∘ centerP
-end
 
-function kabsch(P,Q,matches::AbstractVector{<:Tuple{Int,Int}},wp=ones(size(P,2)),wq=ones(size(Q,2)))
+function kabsch_matches(P,Q,matches::AbstractVector{<:Tuple{Int,Int}},wp=ones(size(P,2)),wq=ones(size(Q,2)))
     matchedP, matchedQ = matched_points(P,Q,matches)
     w = [wp[i]*wq[j] for (i,j) in matches]
-    return kabsch(matchedP, matchedQ, w)
+    return kabsch(matchedP => matchedQ, w)
 end
 
-kabsch(P::PointSet, Q::PointSet) = kabsch(P.coords, Q.coords, P.weights .* Q.weights);
-kabsch(P::PointSet, Q::PointSet, matches::AbstractVector{<:Tuple{Int,Int}}) = kabsch(P.coords, Q.coords, matches, P.weights, Q.weights);
+kabsch_matches(P::PointSet, Q::PointSet, matches::AbstractVector{<:Tuple{Int,Int}}) = kabsch_matches(P.coords, Q.coords, matches, P.weights, Q.weights);
 
-function kabsch(P::AbstractMultiPointSet{N,T,K}, Q::AbstractMultiPointSet{N,T,K}, matchesdict, wp = weights(P), wq = weights(Q)) where {N,T,K}
+function kabsch_matches(P::AbstractMultiPointSet{N,T,K}, Q::AbstractMultiPointSet{N,T,K}, matchesdict, wp = weights(P), wq = weights(Q)) where {N,T,K}
     matchedP, matchedQ = matched_points(P,Q,matchesdict)
     w = Vector{T}()
 
@@ -61,7 +37,7 @@ function kabsch(P::AbstractMultiPointSet{N,T,K}, Q::AbstractMultiPointSet{N,T,K}
         end
     end
 
-    return kabsch(matchedP, matchedQ, w)
+    return kabsch(matchedP => matchedQ, w)
 end
 
 # align via translation only (no rotation)
@@ -72,11 +48,11 @@ function translation_align(P,Q,w)
     return Translation(sum(dists; dims=2))
 end
 
-function translation_align(P,Q,matches::AbstractVector{<:Tuple{Int,Int}},wp=ones(size(P,2)),wq=ones(size(Q,2)))
+function translation_align_matches(P,Q,matches::AbstractVector{<:Tuple{Int,Int}},wp=ones(size(P,2)),wq=ones(size(Q,2)))
     matchedP, matchedQ = matched_points(P,Q,matches)
     w = [wp[i]*wq[j] for (i,j) in matches]
     return translation_align(matchedP, matchedQ, w)
 end
 
 translation_align(P::PointSet, Q::PointSet) = translation_align(P.coords, Q.coords, P.weights .* Q.weights)
-translation_align(P::PointSet, Q::PointSet, matches::AbstractVector{<:Tuple{Int,Int}}) = translation_align(P.coords, Q.coords, matches, P.weights .* Q.weights)
+translation_align_matches(P::PointSet, Q::PointSet, matches::AbstractVector{<:Tuple{Int,Int}}) = translation_align_matches(P.coords, Q.coords, matches, P.weights .* Q.weights)