Repository navigation
Expand file tree
/
Copy pathBlobTrack.cpp
More file actions
147 lines (116 loc) · 6.57 KB
/
Copy pathBlobTrack.cpp
File metadata and controls
147 lines (116 loc) · 6.57 KB
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
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
#include "BlobTrack.h"
#include <vector>
BlobTrack::BlobTrack()
{
}
BlobTrack::~BlobTrack()
{
}
void BlobTrack::predictNextPosition(target_Blob &existingBlob) {
int numPositions = existingBlob.centerPositions.size();
if (numPositions == 1) {
existingBlob.predictedNextPosition.x = existingBlob.centerPositions.back().x;
existingBlob.predictedNextPosition.y = existingBlob.centerPositions.back().y;
}
else if (numPositions == 2) {
int deltaX = existingBlob.centerPositions[1].x - existingBlob.centerPositions[0].x;
int deltaY = existingBlob.centerPositions[1].y - existingBlob.centerPositions[0].y;
existingBlob.predictedNextPosition.x = existingBlob.centerPositions.back().x + deltaX;
existingBlob.predictedNextPosition.y = existingBlob.centerPositions.back().y + deltaY;
}
else if (numPositions == 3) {
int sumOfXChanges = ((existingBlob.centerPositions[2].x - existingBlob.centerPositions[1].x) * 2) +
((existingBlob.centerPositions[1].x - existingBlob.centerPositions[0].x) * 1);
int deltaX = (int)std::round((float)sumOfXChanges / 3.0);
int sumOfYChanges = ((existingBlob.centerPositions[2].y - existingBlob.centerPositions[1].y) * 2) +
((existingBlob.centerPositions[1].y - existingBlob.centerPositions[0].y) * 1);
int deltaY = (int)std::round((float)sumOfYChanges / 3.0);
existingBlob.predictedNextPosition.x = existingBlob.centerPositions.back().x + deltaX;
existingBlob.predictedNextPosition.y = existingBlob.centerPositions.back().y + deltaY;
}
else if (numPositions == 4) {
int sumOfXChanges = ((existingBlob.centerPositions[3].x - existingBlob.centerPositions[2].x) * 3) +
((existingBlob.centerPositions[2].x - existingBlob.centerPositions[1].x) * 2) +
((existingBlob.centerPositions[1].x - existingBlob.centerPositions[0].x) * 1);
int deltaX = (int)std::round((float)sumOfXChanges / 6.0);
int sumOfYChanges = ((existingBlob.centerPositions[3].y - existingBlob.centerPositions[2].y) * 3) +
((existingBlob.centerPositions[2].y - existingBlob.centerPositions[1].y) * 2) +
((existingBlob.centerPositions[1].y - existingBlob.centerPositions[0].y) * 1);
int deltaY = (int)std::round((float)sumOfYChanges / 6.0);
existingBlob.predictedNextPosition.x = existingBlob.centerPositions.back().x + deltaX;
existingBlob.predictedNextPosition.y = existingBlob.centerPositions.back().y + deltaY;
}
else if (numPositions >= 5) {
int sumOfXChanges = ((existingBlob.centerPositions[numPositions - 1].x - existingBlob.centerPositions[numPositions - 2].x) * 4) +
((existingBlob.centerPositions[numPositions - 2].x - existingBlob.centerPositions[numPositions - 3].x) * 3) +
((existingBlob.centerPositions[numPositions - 3].x - existingBlob.centerPositions[numPositions - 4].x) * 2) +
((existingBlob.centerPositions[numPositions - 4].x - existingBlob.centerPositions[numPositions - 5].x) * 1);
int deltaX = (int)std::round((float)sumOfXChanges / 10.0);
int sumOfYChanges = ((existingBlob.centerPositions[numPositions - 1].y - existingBlob.centerPositions[numPositions - 2].y) * 4) +
((existingBlob.centerPositions[numPositions - 2].y - existingBlob.centerPositions[numPositions - 3].y) * 3) +
((existingBlob.centerPositions[numPositions - 3].y - existingBlob.centerPositions[numPositions - 4].y) * 2) +
((existingBlob.centerPositions[numPositions - 4].y - existingBlob.centerPositions[numPositions - 5].y) * 1);
int deltaY = (int)std::round((float)sumOfYChanges / 10.0);
existingBlob.predictedNextPosition.x = existingBlob.centerPositions.back().x + deltaX;
existingBlob.predictedNextPosition.y = existingBlob.centerPositions.back().y + deltaY;
}
else {
// should never get here!
}
}
void BlobTrack::matchCurrentFrameBlobsToExistingBlobs(std::vector<target_Blob> &existingBlobs, std::vector<target_Blob> ¤tFrameBlobs)
{
for (auto &existingBlob : existingBlobs)
{
existingBlob.blnCurrentMatchFoundOrNewBlob = false;
predictNextPosition(existingBlob);
}
for(auto ¤tFrameBlob : currentFrameBlobs){
int intIndexOfLeastDistance = 0;
double dblLeastDistance = 1000000.0;
for (unsigned int i = 0; i < existingBlobs.size(); i++){
if (existingBlobs[i].blnStillBeingTracked == true){
//根据中心点之间的距离，去匹配已经存在的目标.
double dblDistance = distanceBetweenPoints(currentFrameBlob.centerPositions.back(), existingBlobs[i].predictedNextPosition);
if (dblDistance < dblLeastDistance){
dblLeastDistance = dblDistance;
intIndexOfLeastDistance = i;
}
}
}
if (dblLeastDistance < currentFrameBlob.dblCurrentDiagonalSize * 0.5) //距离小于对角线一半，即：匹配上
addBlobToExistingBlobs(currentFrameBlob, existingBlobs, intIndexOfLeastDistance);
else
addNewBlob(currentFrameBlob, existingBlobs); //新目标
}
for (int i = 0; i < existingBlobs.size(); i++) {
if (existingBlobs[i].blnCurrentMatchFoundOrNewBlob == false)
existingBlobs[i].intNumOfConsecutiveFramesWithoutAMatch++;
if (existingBlobs[i].intNumOfConsecutiveFramesWithoutAMatch >= 5) //连续5帧以上没有匹配上，认为不再跟踪
existingBlobs[i].blnStillBeingTracked = false;
//需要增加对数组的判断，防止数组过大。
if (existingBlobs[i].intNumOfConsecutiveFramesWithoutAMatch >= 50) //连续50帧以上没有匹配上，删除
existingBlobs.erase(existingBlobs.begin() + i);
}
}
void BlobTrack::addBlobToExistingBlobs(target_Blob ¤tFrameBlob, std::vector<target_Blob> &existingBlobs, int &intIndex)
{
existingBlobs[intIndex].currentContour = currentFrameBlob.currentContour;
existingBlobs[intIndex].currentBoundingRect = currentFrameBlob.currentBoundingRect;
existingBlobs[intIndex].dblCurrentDiagonalSize = currentFrameBlob.dblCurrentDiagonalSize;
existingBlobs[intIndex].dblCurrentAspectRatio = currentFrameBlob.dblCurrentAspectRatio;
existingBlobs[intIndex].blnStillBeingTracked = true;
existingBlobs[intIndex].blnCurrentMatchFoundOrNewBlob = true;
existingBlobs[intIndex].centerPositions.push_back(currentFrameBlob.centerPositions.back());
}
void BlobTrack::addNewBlob(target_Blob ¤tFrameBlob, std::vector<target_Blob> &existingBlobs)
{
currentFrameBlob.blnCurrentMatchFoundOrNewBlob = true;
existingBlobs.push_back(currentFrameBlob);
}
double BlobTrack::distanceBetweenPoints(cv::Point point1, cv::Point point2)
{
int intX = abs(point1.x - point2.x);
int intY = abs(point1.y - point2.y);
return(sqrt(pow(intX, 2) + pow(intY, 2)));
}