-
Notifications
You must be signed in to change notification settings - Fork 110
/
homographies.go
126 lines (109 loc) · 3.87 KB
/
homographies.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
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
package transform
import (
"math"
"github.com/golang/geo/r2"
"github.com/golang/geo/r3"
"github.com/pkg/errors"
"gonum.org/v1/gonum/floats"
"gonum.org/v1/gonum/mat"
"gonum.org/v1/gonum/stat"
"go.viam.com/rdk/rimage"
"go.viam.com/rdk/utils"
)
// ComputeNormalizationMatFromSliceVecs computes the normalization matrix from a slice of vectors
// from Multiple View Geometry. Richard Hartley and Andrew Zisserman. Alg 4.2 p109.
func ComputeNormalizationMatFromSliceVecs(pts []r2.Point) *mat.Dense {
out := mat.NewDense(3, 3, nil)
xs, ys := rimage.SliceVecsToXsYs(pts)
avgX := stat.Mean(xs, nil)
avgY := stat.Mean(ys, nil)
diffX := make([]float64, len(xs))
copy(diffX, xs)
floats.AddConst(avgX, diffX)
diffY := make([]float64, len(ys))
copy(diffY, ys)
floats.AddConst(avgY, diffY)
norms := make([]float64, len(xs))
for i := 0; i < len(pts); i++ {
norms[i] = math.Sqrt(diffX[i]*diffX[i] + diffY[i]*diffY[i])
}
scaleFactor := math.Sqrt(2) / stat.Mean(norms, nil)
out.Set(0, 0, scaleFactor)
out.Set(0, 2, -scaleFactor*avgX)
out.Set(1, 1, scaleFactor)
out.Set(1, 2, -scaleFactor*avgY)
out.Set(2, 2, 1)
return out
}
// MeanVarianceCol is a helper function to compute the normalization matrix for least squares homography estimation
// It estimates the mean and variance of a column in a *mat.Dense.
func MeanVarianceCol(m mat.Matrix, j int) (float64, float64) {
r, _ := m.Dims()
col := make([]float64, r)
mat.Col(col, j, m)
mean := stat.Mean(col, nil)
std := stat.StdDev(col, nil)
return mean, std
}
// getNormalizationMatrix gets the matrix to center the points on (0,0).
func getNormalizationMatrix(pts *mat.Dense) *mat.Dense {
avgX, stdX := MeanVarianceCol(pts, 0)
avgY, stdY := MeanVarianceCol(pts, 1)
sX := math.Sqrt(2. / stdX)
sY := math.Sqrt(2. / stdY)
data := []float64{sX, 0., -sX * avgX, 0., sY, -sY * avgY, 0, 0, 1}
outMat := mat.NewDense(3, 3, data)
// [
// [s_x, 0, -s_x * avg_x],
// [0, s_y, -s_y * avg_y],
// [0, 0, 1]
//]
return outMat
}
// SelectFourPointPairs randomly selects 4 pairs of points in two point slices of the same length.
func SelectFourPointPairs(p1, p2 []r2.Point) ([]r2.Point, []r2.Point, error) {
if len(p1) != len(p2) {
err := errors.New("p1 and p2 should have the same length")
return nil, nil, err
}
indices, err := utils.SelectNIndicesWithoutReplacement(4, len(p1))
if err != nil {
return nil, nil, err
}
// create 2 4-point-slices of selected indices
s1 := make([]r2.Point, 0, 4)
s2 := make([]r2.Point, 0, 4)
for _, idx := range indices {
s1 = append(s1, p1[idx])
s2 = append(s2, p2[idx])
}
return s1, s2, nil
}
// geometricDistance computes the distance of point1 transformed by the homography h 1->2 to point 2.
func geometricDistance(p1, p2 r2.Point, h mat.Matrix) float64 {
pt1 := mat.NewDense(3, 1, []float64{p1.X, p1.Y, 1.0})
pt2 := mat.NewDense(3, 1, []float64{p2.X, p2.Y, 1.0})
pt2Tilde := mat.NewDense(3, 1, nil)
pt2Tilde.Mul(h, pt1)
pt2Tilde.Scale(1./pt2Tilde.At(2, 0), pt2Tilde)
p := r3.Vector{X: pt2.At(0, 0), Y: pt2.At(1, 0), Z: pt2.At(2, 0)}
q := r3.Vector{X: pt2Tilde.At(0, 0), Y: pt2Tilde.At(1, 0), Z: pt2Tilde.At(2, 0)}
errVec := p.Sub(q)
return errVec.Norm()
}
// are4PointsNonCollinear returns true if 4 points are not collinear 3 by 3.
func are4PointsNonCollinear(p1, p2, p3, p4 r2.Point) bool {
return !rimage.AreCollinear(p1, p2, p3, 0.01) && !rimage.AreCollinear(p2, p3, p4, 0.01) &&
!rimage.AreCollinear(p1, p3, p4, 0.01) && !rimage.AreCollinear(p1, p2, p4, 0.01)
}
// ApplyNormalizationMat applies a normalization matrix to a slice of r2.Point.
func ApplyNormalizationMat(h *mat.Dense, pts []r2.Point) []r2.Point {
outPoints := make([]r2.Point, len(pts))
for i, pt := range pts {
x := h.At(0, 0)*pt.X + h.At(0, 1)*pt.Y + h.At(0, 2)
y := h.At(1, 0)*pt.X + h.At(1, 1)*pt.Y + h.At(1, 2)
// z := h.At(2, 0)*pt.X + h.At(2, 1)*pt.Y + h.At(2, 2)
outPoints[i] = r2.Point{X: x, Y: y}
}
return outPoints
}