/
align.go
96 lines (81 loc) · 1.91 KB
/
align.go
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
package scan
/*
Find transform
[a b c [sx [dx
d e f sy = dy
g h i] 1] 1]
Matrix A, column X, column B
Ax=b
A is data
[sx1 sy1 1 0 0 0 0 0 0
0 0 0 sx1 sy1 1 0 0 0
0 0 0 0 0 0 sx1 sy1 1]
... for other source points
x is the column of unknown transform factors we solve for
[a b c d e f g h i]
b is the column
[dx1 dy1 1 ...]
biga = A.T() . A
atb = A.T() . b
diagonalize [biga | atb]
*/
// go get -u -t gonum.org/v1/gonum/...
import (
"gonum.org/v1/gonum/mat"
)
type FPoint struct {
X float64
Y float64
}
func FPointFromInt(x, y int) FPoint {
return FPoint{X: float64(x), Y: float64(y)}
}
func (fp *FPoint) SetInt(x, y int) {
fp.X = float64(x)
fp.Y = float64(y)
}
// FindTransform solves for best-fit affine transform which maps source positions to dest positions.
func FindTransform(sources, dests []FPoint) []float64 {
if len(sources) != len(dests) {
return nil
}
data := make([]float64, len(sources)*9*3)
dest := make([]float64, len(sources)*3)
for i, sp := range sources {
dp := dests[i]
rp := i * 3 * 9
data[rp+0] = sp.X
data[rp+1] = sp.Y
data[rp+2] = 1.0
dest[i*3] = dp.X
rp = ((i * 3) + 1) * 9
data[rp+3] = sp.X
data[rp+4] = sp.Y
data[rp+5] = 1.0
dest[(i*3)+1] = dp.Y
rp = ((i * 3) + 2) * 9
data[rp+6] = sp.X
data[rp+7] = sp.Y
data[rp+8] = 1.0
dest[(i*3)+2] = 1.0
}
A := mat.NewDense(len(sources)*3, 9, data)
b := mat.NewDense(len(sources)*3, 1, dest)
var x mat.Dense
x.Solve(A, b)
// fmt.Printf("solution ?\nx = %v\n", mat.Formatted(&x))
return mat.Col(nil, 0, &x)
}
// TransformError returns the sum of squared differences of dests vs sources transformed to dests
func TransformError(sources, dests []FPoint, t AffineTransform) float64 {
// TODO: also return drop-an-outlier version?
ssd := float64(0)
for i, sp := range sources {
dp := dests[i]
ox, oy := t.Transform(sp.X, sp.Y)
dx := ox - dp.X
dy := oy - dp.Y
ssd += (dx * dx) + (dy * dy)
}
return ssd
}