#include "opticalFlow.h"
opticalFlow::opticalFlow(float diff1, float diff2, int points) {
diff_threshold_1 = diff1;
diff_threshold_2 = diff2;
maxCount = points;
centers = new cv::Mat[nClusters];
}
void opticalFlow::tracking(Mat& frame)
{
cvtColor(frame, gray, COLOR_BGR2GRAY);
goodFeaturesToTrack(gray, points[0], maxCount, qLevel, minDist);
initial.insert(initial.end(), points[0].begin(), points[0].end());
if (gray_prev.empty()) gray.copyTo(gray_prev);
calcOpticalFlowPyrLK(gray_prev, gray, points[0], points[1], status, err);
int k = 0;
for (size_t i = 0; i < points[1].size(); i++)
{
if (i >= status.size()) break;
if (acceptTrackedPoint(i))
{
initial[k] = initial[i];
points[1][k] = points[1][i];
k++;
}
}
initial.resize(k);
points[1].resize(k);
for (size_t i = 0; i < k; i++)
{
float len = (pow(points[1][i].x - initial[i].x, 2.0) + pow(points[1][i].y - initial[i].y, 2.0));
len = pow(len, 0.5);
lens.push_back(len);
m.push_back({ initial[i].x, initial[i].y });
}
int sampleCount = lens.size();
Mat points_mat(sampleCount, 1, CV_32F, Scalar(0));
Mat labels;
Mat center(nClusters, 1, points_mat.type());
for (int i = 0; i < sampleCount; i++)
{
points_mat.at<float>(i, 0) = static_cast<float>(lens[i]);
}
TermCriteria criteria = TermCriteria(TermCriteria::EPS + TermCriteria::COUNT, 10, 0.1);
if (lens.size() > 3) kmeans(points_mat, nClusters, labels, criteria, 3, KMEANS_PP_CENTERS, center);
cv::sort(center, center, SORT_EVERY_COLUMN + SORT_ASCENDING);
if ((center.at<float>(2, 0) > diff_threshold_1 * center.at<float>(1, 0)) && (center.at<float>(2, 0) > center.at<float>(1, 0) + diff_threshold_2))
{
for (int i = 0; i < lens.size(); i++)
{
if (lens[i] > center.at<float>(2, 0))
{
int x = (int)m[i][0];
int y = (int)m[i][1];
rectangle(frame, cv::Point(x - 50, y - 50), cv::Point(x + 50, y + 50), (0, 0, 255), 1);
}
}
}
points->clear();
initial.clear();
gray_prev = gray.clone();
lens.clear();
m.clear();
}
bool opticalFlow::acceptTrackedPoint(int i)
{
return (status[i] == 1) && ((abs(initial[i].x - points[1][i].x) + abs(initial[i].y - points[1][i].y)) > 2);
}
opticalFlow::~opticalFlow() {
centers->release();
}
评论0