Skip to content

Commit 0df550b

Browse files
LdDlLdDl
authored
Predict pos (#3)
* test new predict fn * write test for simple/kalman blob for predictions * kalman test
1 parent 270e701 commit 0df550b

File tree

4 files changed

+182
-16
lines changed

4 files changed

+182
-16
lines changed

‎v2/blob/kalman_blob.go‎

Lines changed: 10 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -89,21 +89,23 @@ func NewKalmanBlobie(rect image.Rectangle, options *BlobOptions) Blobie {
8989

9090
// PredictNextPosition - Predict next N coordinates
9191
func (b *KalmanBlobie) PredictNextPosition(n int) {
92-
account := min(n, len((*b).Track))
93-
prev := len((*b).Track) - 1
94-
current := prev - 1
95-
var deltaX, deltaY, sum int = 0, 0, 0
92+
account := min(n, len(b.Track))
93+
current := len(b.Track) - 1
94+
prev := current - 1
95+
deltaX, deltaY, sum := 0, 0, 0
9696
for i := 1; i < int(account); i++ {
97-
deltaX += (((*b).Track)[current].X - ((*b).Track)[prev].X) * i
98-
deltaY += (((*b).Track)[current].Y - ((*b).Track)[prev].Y) * i
97+
deltaX += (b.Track[current].X - b.Track[prev].X) * (account - i)
98+
deltaY += (b.Track[current].Y - b.Track[prev].Y) * (account - i)
9999
sum += i
100+
current = prev
101+
prev = current - 1
100102
}
101103
if sum > 0 {
102104
deltaX /= sum
103105
deltaY /= sum
104106
}
105-
(*b).PredictedNextPosition.X = (*b).Track[len((*b).Track)-1].X + deltaX
106-
(*b).PredictedNextPosition.Y = (*b).Track[len((*b).Track)-1].Y + deltaY
107+
b.PredictedNextPosition.X = b.Track[len(b.Track)-1].X + deltaX
108+
b.PredictedNextPosition.Y = b.Track[len(b.Track)-1].Y + deltaY
107109
}
108110

109111
// Update - Update info about blob

‎v2/blob/kalman_predict_pos_test.go‎

Lines changed: 87 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,87 @@
1+
package blob
2+
3+
import (
4+
"image"
5+
"testing"
6+
)
7+
8+
func TestKalmanPredictPos(t *testing.T) {
9+
var (
10+
testPoints = [][]int{
11+
[]int{0, 0},
12+
[]int{1, 1},
13+
[]int{2, 2},
14+
[]int{4, 4},
15+
[]int{6, 6},
16+
[]int{9, 9},
17+
[]int{11, 11},
18+
[]int{16, 16},
19+
[]int{20, 20},
20+
}
21+
kalmanFilteredPoints = [][]int{
22+
[]int{0, 0},
23+
[]int{0, 0},
24+
[]int{1, 1},
25+
[]int{3, 3},
26+
[]int{5, 5},
27+
[]int{7, 7},
28+
[]int{10, 10},
29+
[]int{13, 13},
30+
[]int{17, 17},
31+
}
32+
correctPredictions = [][]int{
33+
[]int{0, 0},
34+
[]int{0, 0},
35+
[]int{0, 0},
36+
[]int{1, 1},
37+
[]int{4, 4},
38+
[]int{6, 6},
39+
[]int{8, 8},
40+
[]int{12, 12},
41+
[]int{15, 15},
42+
}
43+
)
44+
45+
maxPointsInTrack := 150
46+
classID := 1
47+
className := "just_an_object"
48+
maxNoMatch := 5
49+
50+
rectHalfHeight := 30
51+
rectHalfWidth := 75
52+
53+
commonOptions := BlobOptions{
54+
ClassID: classID,
55+
ClassName: className,
56+
MaxPointsInTrack: maxPointsInTrack,
57+
TimeDeltaSeconds: 1.0,
58+
}
59+
60+
var b Blobie
61+
62+
for i := range testPoints {
63+
centerOne := testPoints[i]
64+
rectOne := image.Rect(centerOne[0]-rectHalfWidth, centerOne[1]-rectHalfHeight, centerOne[0]+rectHalfWidth, centerOne[1]+rectHalfHeight)
65+
blobOne := NewKalmanBlobie(rectOne, &commonOptions)
66+
if b == nil {
67+
// Fill data on first iteration
68+
b = blobOne
69+
}
70+
b.PredictNextPosition(maxNoMatch)
71+
b.Update(blobOne)
72+
forCheck := b.(*KalmanBlobie)
73+
smoothedCenter := kalmanFilteredPoints[i]
74+
if forCheck.Center.X != smoothedCenter[0] {
75+
t.Errorf("Center.X on %d-th iteration should be %d, but got %d", i, smoothedCenter[0], forCheck.Center.X)
76+
}
77+
if forCheck.Center.Y != smoothedCenter[1] {
78+
t.Errorf("Center.Y on %d-th iteration should be %d, but got %d", i, smoothedCenter[1], forCheck.Center.Y)
79+
}
80+
if forCheck.PredictedNextPosition.X != correctPredictions[i][0] {
81+
t.Errorf("PredictedNextPosition.X on %d-th iteration should be %d, but got %d", i, correctPredictions[i][0], forCheck.PredictedNextPosition.X)
82+
}
83+
if forCheck.PredictedNextPosition.Y != correctPredictions[i][1] {
84+
t.Errorf("PredictedNextPosition.Y on %d-th iteration should be %d, but got %d", i, correctPredictions[i][1], forCheck.PredictedNextPosition.Y)
85+
}
86+
}
87+
}

‎v2/blob/simple_blob.go‎

Lines changed: 10 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -97,21 +97,23 @@ func NewBlobieDefaults(rect image.Rectangle) *SimpleBlobie {
9797

9898
// PredictNextPosition - Predict next N coordinates
9999
func (b *SimpleBlobie) PredictNextPosition(n int) {
100-
account := min(n, len((*b).Track))
101-
prev := len((*b).Track) - 1
102-
current := prev - 1
103-
var deltaX, deltaY, sum int = 0, 0, 0
100+
account := min(n, len(b.Track))
101+
current := len(b.Track) - 1
102+
prev := current - 1
103+
deltaX, deltaY, sum := 0, 0, 0
104104
for i := 1; i < int(account); i++ {
105-
deltaX += (((*b).Track)[current].X - ((*b).Track)[prev].X) * i
106-
deltaY += (((*b).Track)[current].Y - ((*b).Track)[prev].Y) * i
105+
deltaX += (b.Track[current].X - b.Track[prev].X) * (account - i)
106+
deltaY += (b.Track[current].Y - b.Track[prev].Y) * (account - i)
107107
sum += i
108+
current = prev
109+
prev = current - 1
108110
}
109111
if sum > 0 {
110112
deltaX /= sum
111113
deltaY /= sum
112114
}
113-
(*b).PredictedNextPosition.X = (*b).Track[len((*b).Track)-1].X + deltaX
114-
(*b).PredictedNextPosition.Y = (*b).Track[len((*b).Track)-1].Y + deltaY
115+
b.PredictedNextPosition.X = b.Track[len(b.Track)-1].X + deltaX
116+
b.PredictedNextPosition.Y = b.Track[len(b.Track)-1].Y + deltaY
115117
}
116118

117119
// Update - Update info about blob

‎v2/blob/simple_predict_pos_test.go‎

Lines changed: 75 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,75 @@
1+
package blob
2+
3+
import (
4+
"image"
5+
"testing"
6+
)
7+
8+
func TestSimplePredictPos(t *testing.T) {
9+
var (
10+
testPoints = [][]int{
11+
[]int{0, 0},
12+
[]int{1, 1},
13+
[]int{2, 2},
14+
[]int{4, 4},
15+
[]int{6, 6},
16+
[]int{9, 9},
17+
[]int{11, 11},
18+
[]int{16, 16},
19+
[]int{20, 20},
20+
}
21+
correctPredictions = [][]int{
22+
[]int{0, 0},
23+
[]int{0, 0},
24+
[]int{1, 1},
25+
[]int{2, 2},
26+
[]int{5, 5},
27+
[]int{7, 7},
28+
[]int{11, 11},
29+
[]int{13, 13},
30+
[]int{19, 19},
31+
}
32+
)
33+
34+
maxPointsInTrack := 150
35+
classID := 1
36+
className := "just_an_object"
37+
maxNoMatch := 5
38+
39+
rectHalfHeight := 30
40+
rectHalfWidth := 75
41+
42+
commonOptions := BlobOptions{
43+
ClassID: classID,
44+
ClassName: className,
45+
MaxPointsInTrack: maxPointsInTrack,
46+
TimeDeltaSeconds: 1.0,
47+
}
48+
49+
var b Blobie
50+
51+
for i := range testPoints {
52+
centerOne := testPoints[i]
53+
rectOne := image.Rect(centerOne[0]-rectHalfWidth, centerOne[1]-rectHalfHeight, centerOne[0]+rectHalfWidth, centerOne[1]+rectHalfHeight)
54+
blobOne := NewSimpleBlobie(rectOne, &commonOptions)
55+
if b == nil {
56+
// Fill data on first iteration
57+
b = blobOne
58+
}
59+
b.PredictNextPosition(maxNoMatch)
60+
b.Update(blobOne)
61+
forCheck := b.(*SimpleBlobie)
62+
if forCheck.Center.X != centerOne[0] {
63+
t.Errorf("Center.X on %d-th iteration should be %d, but got %d", i, centerOne[0], forCheck.Center.X)
64+
}
65+
if forCheck.Center.Y != centerOne[1] {
66+
t.Errorf("Center.Y on %d-th iteration should be %d, but got %d", i, centerOne[1], forCheck.Center.Y)
67+
}
68+
if forCheck.PredictedNextPosition.X != correctPredictions[i][0] {
69+
t.Errorf("PredictedNextPosition.X on %d-th iteration should be %d, but got %d", i, correctPredictions[i][0], forCheck.PredictedNextPosition.X)
70+
}
71+
if forCheck.PredictedNextPosition.Y != correctPredictions[i][1] {
72+
t.Errorf("PredictedNextPosition.Y on %d-th iteration should be %d, but got %d", i, correctPredictions[i][1], forCheck.PredictedNextPosition.Y)
73+
}
74+
}
75+
}

0 commit comments

Comments
 (0)