diff --git a/spatial/transform/doc.go b/spatial/transform/doc.go new file mode 100644 index 00000000..e63d846a --- /dev/null +++ b/spatial/transform/doc.go @@ -0,0 +1,6 @@ +// Copyright ©2025 The Gonum Authors. All rights reserved. +// Use of this source code is governed by a BSD-style +// license that can be found in the LICENSE file. + +// Package transform provides functions for spatial transformations. +package transform // import "gonum.org/v1/gonum/spatial/transform" diff --git a/spatial/transform/errors.go b/spatial/transform/errors.go new file mode 100644 index 00000000..73b7598f --- /dev/null +++ b/spatial/transform/errors.go @@ -0,0 +1,20 @@ +// Copyright ©2025 The Gonum Authors. All rights reserved. +// Use of this source code is governed by a BSD-style +// license that can be found in the LICENSE file. + +package transform + +import ( + "errors" + "fmt" +) + +// DegenerateInputError represents an error due to input data with variance +// below a threshold, which would cause numerical instability. +type DegenerateInputError float64 + +func (e DegenerateInputError) Error() string { + return fmt.Sprintf("variance too low: %v", float64(e)) +} + +var ErrFactorizationFailed = errors.New("transform: factorization failed") diff --git a/spatial/transform/umeyama.go b/spatial/transform/umeyama.go new file mode 100644 index 00000000..d48f8286 --- /dev/null +++ b/spatial/transform/umeyama.go @@ -0,0 +1,237 @@ +// Copyright ©2025 The Gonum Authors. All rights reserved. +// Use of this source code is governed by a BSD-style +// license that can be found in the LICENSE file. + +package transform + +import ( + "math" + + "gonum.org/v1/gonum/mat" + "gonum.org/v1/gonum/stat" +) + +// Umeyama finds the similarity transformation between two sets of points +// that minimizes the mean squared error between them. +// +// The transformation relates two sets of n corresponding points {x_i} +// and {y_i} as: +// +// y_i ≈ c * R * x_i + t, i=0,...,n-1 +// +// where c is the scale factor, R is the rotation matrix and t is +// the translation vector. +// +// The point sets are represented as two n×m matrices X and Y, where +// m is the number of dimensions and x_i and y_i are stored in the i-th +// row of X and Y, respectively. Typically, m is equal to 2 or 3. +// +// The Umeyama type allows inspecting the variance of the input data +// before computing the transformation, providing flexibility in handling +// degenerate cases. It is recommended to always perform a check before +// computation to prevent numerical instability and/or division +// by zero unless there is a specific reason not to do so. +// +// Example usage: +// +// u := transform.NewUmeyama(x, y) +// if u.Var() < myThreshold { +// // Handle degenerate case +// } +// c, r, t, ok := u.Transform() +// if !ok { +// // Handle failure +// } +// +// For a simple one-call interface with automatic variance checking, use +// the UmeyamaTransform function instead. +// +// Reference: +// "Least-Squares Estimation of Transformation Parameters Between Two Point Patterns" +// by Shinji Umeyama, IEEE Transactions on Pattern Analysis and Machine Intelligence, +// Vol. 13, No. 4, April 1991, [doi:10.1109/34.88573]. +// [doi:10.1109/34.88573]: https://doi.org/10.1109/34.88573 +type Umeyama struct { + x, y *mat.Dense + n, m int + muX, muY *mat.VecDense + varX float64 +} + +// NewUmeyama creates a new Umeyama similarity transformation calculator +// for the given point sets x and y. +// +// The point sets are represented as two n×m matrices X and Y, where +// m is the number of dimensions and each row represents a point. +// If the dimensions of X and Y are not equal, NewUmeyama will panic. +// +// NewUmeyama computes the means and variance of the input points but does +// not perform the full transformation calculation. Use the Var method to +// inspect the variance, and the Transform method to compute the transformation +// parameters. +func NewUmeyama(x, y *mat.Dense) *Umeyama { + n, m := x.Dims() + rowsY, colsY := y.Dims() + + // Check dimensions. + if n != rowsY || m != colsY { + panic("transform: dimensions of x and y do not match") + } + + // Calculate means and variance of x. + muX := mat.NewVecDense(m, nil) + muY := mat.NewVecDense(m, nil) + + colX := make([]float64, n) + colY := make([]float64, n) + + var varX float64 + + for j := 0; j < m; j++ { + mat.Col(colX, j, x) + mat.Col(colY, j, y) + + meanX, varXj := stat.PopMeanVariance(colX, nil) + + muY.SetVec(j, stat.Mean(colY, nil)) + muX.SetVec(j, meanX) + + varX += varXj + } + + return &Umeyama{ + x: x, + y: y, + n: n, + m: m, + muX: muX, + muY: muY, + varX: varX, + } +} + +// Var returns the variance of point set x. +// +// This can be used to detect degenerate input cases where the variance +// is too low, which may cause numerical instability and/or division by zero +// in the transformation calculation. +func (u *Umeyama) Var() float64 { + return u.varX +} + +// Transform computes and returns the similarity transformation parameters. +// +// Transform returns the scale factor c, the rotation matrix R, the translation +// vector t, and a boolean ok indicating success. The transformation parameters +// best align the point sets according to Umeyama's algorithm. +// +// If the required SVD fails, Transform will return ok as false. +func (u *Umeyama) Transform() (c float64, r *mat.Dense, t *mat.VecDense, ok bool) { + // Center the matrices. + xc := mat.NewDense(u.n, u.m, nil) + yc := mat.NewDense(u.n, u.m, nil) + + for i := 0; i < u.n; i++ { + for j := 0; j < u.m; j++ { + xc.Set(i, j, u.x.At(i, j)-u.muX.AtVec(j)) + yc.Set(i, j, u.y.At(i, j)-u.muY.AtVec(j)) + } + } + + // Calculate covariance matrix. + covXY := mat.NewDense(u.m, u.m, nil) + covXY.Mul(yc.T(), xc) + covXY.Scale(1/float64(u.n), covXY) + + // Singular Value Decomposition + var svd mat.SVD + if !svd.Factorize(covXY, mat.SVDFull) { + return 0, nil, nil, false + } + + // Get U and V. + var uu, v mat.Dense + svd.UTo(&uu) + svd.VTo(&v) + + // Create identity matrix. + s := mat.NewDiagDense(u.m, nil) + for i := 0; i < u.m; i++ { + s.SetDiag(i, 1) + } + + // Check determinants to ensure proper rotation matrix (not reflection). + if mat.Det(&uu)*mat.Det(&v) < 0 { + s.SetDiag(u.m-1, -1) + } + + // Calculate scale factor c. + singularValues := svd.Values(nil) + for i := 0; i < u.m; i++ { + c += singularValues[i] * s.At(i, i) + } + c /= u.varX + + // Calculate rotation matrix R. + r = mat.NewDense(u.m, u.m, nil) + r.Product(&uu, s, v.T()) + + // Calculate translation vector t. + t = mat.NewVecDense(u.m, nil) + rMuX := mat.NewVecDense(u.m, nil) + rMuX.MulVec(r, u.muX) + + t.CopyVec(u.muY) + t.AddScaledVec(t, -c, rMuX) + + return c, r, t, true +} + +// UmeyamaTransform finds the similarity transformation between two sets of points +// that minimizes the mean squared error between them. +// +// The transformation relates two sets of n corresponding points {x_i} +// and {y_i} as: +// +// y_i ≈ c * R * x_i + t, i=0,...,n-1 +// +// where c is the scale factor, R is the rotation matrix and t is +// the translation vector. +// +// The point sets are represented as two n×m matrices X and Y, where +// m is the number of dimensions and x_i and y_i are stored in the i-th +// row of X and Y, respectively. Typically, m is equal to 2 or 3. +// If the dimensions of X and Y are not equal, UmeyamaTransform will panic. +// +// UmeyamaTransform returns the scale factor c, the rotation matrix R and the translation +// vector t. +// +// If the required SVD fails, UmeyamaTransform will return a mat.ErrFailedSVD. +// +// UmeyamaTransform automatically checks for degenerate input by comparing the variance +// of x with machine epsilon. This is necessary because a variance equal or close +// to zero may cause numerical instability and/or division by zero. +// In case of variance ≤ machine epsilon, UmeyamaTransform will return a DegenerateInputError. +// +// For more control over variance checking, use NewUmeyama to create an Umeyama +// instance, inspect its variance with the Var method, and call Transform if appropriate. +// +// Reference: +// "Least-Squares Estimation of Transformation Parameters Between Two Point Patterns" +// by Shinji Umeyama, IEEE Transactions on Pattern Analysis and Machine Intelligence, +// Vol. 13, No. 4, April 1991, [doi:10.1109/34.88573]. +// [doi:10.1109/34.88573]: https://doi.org/10.1109/34.88573 +func UmeyamaTransform(x, y *mat.Dense) (c float64, r *mat.Dense, t *mat.VecDense, err error) { + u := NewUmeyama(x, y) + + if u.varX <= math.Nextafter(1.0, 2.0)-1.0 { + return 0, nil, nil, DegenerateInputError(u.varX) + } + + var ok bool + c, r, t, ok = u.Transform() + if !ok { + return 0, nil, nil, ErrFactorizationFailed + } + return c, r, t, nil +} diff --git a/spatial/transform/umeyama_test.go b/spatial/transform/umeyama_test.go new file mode 100644 index 00000000..f45147b5 --- /dev/null +++ b/spatial/transform/umeyama_test.go @@ -0,0 +1,213 @@ +// Copyright ©2025 The Gonum Authors. All rights reserved. +// Use of this source code is governed by a BSD-style +// license that can be found in the LICENSE file. + +package transform + +import ( + "errors" + "testing" + + "gonum.org/v1/gonum/floats/scalar" + "gonum.org/v1/gonum/mat" +) + +var umeyamaTests = []struct { + name string + from *mat.Dense + to *mat.Dense + wantScale float64 + wantRot *mat.Dense + wantTrans *mat.VecDense +}{ + { + name: "2D_case_from_paper", + from: mat.NewDense(3, 2, []float64{ + 0, 0, + 1, 0, + 0, 2, + }), + to: mat.NewDense(3, 2, []float64{ + 0, 0, + -1, 0, + 0, 2, + }), + wantScale: 0.7211102550927978, + wantRot: mat.NewDense(2, 2, []float64{ + 0.8320502943378437, 0.554700196225229, + -0.554700196225229, 0.8320502943378436, + }), + wantTrans: mat.NewVecDense(2, []float64{ + -0.8, + 0.4, + }), + }, + { + name: "2D_identity", + from: mat.NewDense(3, 2, []float64{ + 0, 0, + 1, 1, + 2, 2, + }), + to: mat.NewDense(3, 2, []float64{ + 0, 0, + 1, 1, + 2, 2, + }), + wantScale: 1, + wantRot: mat.NewDense(2, 2, []float64{ + 1, 0, + 0, 1, + }), + wantTrans: mat.NewVecDense(2, []float64{ + 0, + 0, + }), + }, + { + name: "2D_rotation_90deg", + from: mat.NewDense(3, 2, []float64{ + 0, 0, + 1, 0, + 1, 1, + }), + to: mat.NewDense(3, 2, []float64{ + 0, 0, + 0, 1, + -1, 1, + }), + wantScale: 1, + wantRot: mat.NewDense(2, 2, []float64{ + 0, -1, + 1, -0, + }), + wantTrans: mat.NewVecDense(2, []float64{ + 0, + 0, + }), + }, + { + name: "2D_scale_2x", + from: mat.NewDense(3, 2, []float64{ + 0, 0, + 1, 1, + 2, 2, + }), + to: mat.NewDense(3, 2, []float64{ + 0, 0, + 2, 2, + 4, 4, + }), + wantScale: 2, + wantRot: mat.NewDense(2, 2, []float64{ + 1, 0, + 0, 1, + }), + wantTrans: mat.NewVecDense(2, []float64{ + 0, + 0, + }), + }, + { + name: "2D_translation", + from: mat.NewDense(3, 2, []float64{ + 0, 0, + 1, 1, + 2, 2, + }), + to: mat.NewDense(3, 2, []float64{ + 3, 3, + 4, 4, + 5, 5, + }), + wantScale: 1, + wantRot: mat.NewDense(2, 2, []float64{ + 1, 0, + 0, 1, + }), + wantTrans: mat.NewVecDense(2, []float64{ + 3, + 3, + }), + }, + { + name: "3D_case", + from: mat.NewDense(3, 3, []float64{ + 0, 0, 1, + 1, 0, 3, + 2, 5, 8, + }), + to: mat.NewDense(3, 3, []float64{ + 1, 2, 4, + 0, 1, 6, + 1, 7, 11, + }), + wantScale: 1.0205423989219404, + wantRot: mat.NewDense(3, 3, []float64{ + 0.5699453289954445, 0.5900767342443888, -0.5718144538744644, + -0.5030534073108366, 0.8008235178014148, 0.324990711758234, + 0.6496919203355019, 0.10242627123762431, 0.7532657350571071, + }), + wantTrans: mat.NewVecDense(3, []float64{ + 1.4155929948174535, + 1.1579295387121973, + 3.0877861136679647, + }), + }, +} + +func TestUmeyamaTransform(t *testing.T) { + tol := 1e-14 + + for _, test := range umeyamaTests { + t.Run(test.name, func(t *testing.T) { + scale, rotation, translation, err := UmeyamaTransform(test.from, test.to) + if err != nil { + t.Fatalf("UmeyamaTransform returned error: %v", err) + } + + // Check scale + if !scalar.EqualWithinAbs(scale, test.wantScale, tol) { + t.Errorf("Scale = %v, want %v", scale, test.wantScale) + } + + // Check rotation + var rDiff mat.Dense + rDiff.Sub(rotation, test.wantRot) + diff := rDiff.Norm(1) + + if diff > tol { + t.Errorf("unexpected rotation matrix, |R_got-R_want| = %v", diff) + } + + // Check translation + var tDiff mat.VecDense + tDiff.SubVec(translation, test.wantTrans) + diff = tDiff.Norm(1) + + if diff > tol { + t.Errorf("unexpected translation vector, |t_got-t_want| = %v", diff) + } + }) + } +} + +func TestUmeyamaDegenerateInput(t *testing.T) { + from := mat.NewDense(3, 2, []float64{ + 1, 1, + 1, 1, + 1, 1, + }) + to := mat.NewDense(3, 2, []float64{ + 2, 2, + 2, 2, + 2, 2, + }) + + _, _, _, err := UmeyamaTransform(from, to) + + var degenerateInputError DegenerateInputError + if !errors.As(err, °enerateInputError) { + t.Errorf("Expected DegenerateInputError, got %v", err) + } +}