#include<iostream>
#include<opencv2/opencv.hpp>
void draw_line(cv::Mat& image, std::vector<cv::Point2f> pts1, std::vector<cv::Point2f> pts2)
{
std::vector<cv::Scalar>lut;
cv::RNG rng(12345);
for (auto i = 0; i < pts1.size(); ++i) {
int b = rng.uniform(0, 255);
int g = rng.uniform(0, 255);
int r = rng.uniform(0, 255);
lut.push_back(cv::Scalar(b, g, r));
}
for (auto i = 0; i < pts1.size(); ++i) {
cv::line(image, pts1[i], pts2[i], cv::Scalar(255, 0, 0), 2, 8);
}
}
int main()
{
cv::Mat old_frame, old_gray;
old_frame = cv::imread("1403636579763555584.png");
cv::cvtColor(old_frame, old_gray, cv::COLOR_BGR2GRAY);
//角点光源初始化
std::vector<cv::Point2f> feature_pts;
std::chrono::steady_clock::time_point t1 = std::chrono::steady_clock::now();
#if 1
cv::goodFeaturesToTrack(old_gray, feature_pts, 200, 0.01, 10, cv::Mat(), 3, false);
#else
std::vector<cv::KeyPoint> points;
cv::FAST(old_gray, points, 40);
for (auto kv : points)
{
feature_pts.push_back(cv::Point2f(kv.pt.x, kv.pt.y));
}
#endif
std::chrono::steady_clock::time_point t2 = std::chrono::steady_clock::now();
double ttrack= std::chrono::duration_cast<std::chrono::duration<double> >(t2 - t1).count();
std::cout << "goodFeaturesToTrack run time: " << ttrack << std::endl;
std::vector<cv::Point2f> pts_0;
std::vector<cv::Point2f> pts_1;
pts_0.insert(pts_0.end(), feature_pts.begin(), feature_pts.end());
pts_1.insert(pts_1.end(), feature_pts.begin(), feature_pts.end());
cv::Mat frame, gray;
frame = cv::imread("1403636579813555456.png");
cv::cvtColor(frame, gray, cv::COLOR_BGR2GRAY);
std::vector<uchar> status;
std::vector<float> err;
//定义停止条件,当迭代10次不需要计算,两次的计算结果差小于0.01也不需要计算
cv::TermCriteria criteria = cv::TermCriteria(cv::TermCriteria::COUNT + cv::TermCriteria::EPS, 10, 0.01);
int level = 1;
/* 设置opencv为单线程 */
cv::setNumThreads(0);
while(true)
{
std::vector<cv::Mat> prev_pyr;
std::vector<cv::Mat> cur_pyr;
cv::Size window_size = cv::Size(11, 11);
t1 = std::chrono::steady_clock::now();
cv::buildOpticalFlowPyramid(old_gray, prev_pyr, window_size, level);
cv::buildOpticalFlowPyramid(gray, cur_pyr, window_size, level);
t2 = std::chrono::steady_clock::now();
ttrack= std::chrono::duration_cast<std::chrono::duration<double> >(t2 - t1).count();
std::cout << "buildOpticalFlowPyramid run time: " << ttrack << std::endl;
std::cout << "跟踪的点数为:" << pts_0.size() << std::endl;
//光流法函数
std::chrono::steady_clock::time_point t1 = std::chrono::steady_clock::now();
/* 手动构建金字塔,两步到位 */
cv::calcOpticalFlowPyrLK(prev_pyr, cur_pyr, pts_0, pts_1, status, err, window_size, level, criteria, 0);
/* 一步到位 */
//cv::calcOpticalFlowPyrLK(old_gray, gray, pts_0, pts_1, status, err, cv::Size(11, 11), 0, criteria, 0);
std::chrono::steady_clock::time_point t2 = std::chrono::steady_clock::now();
double ttrack= std::chrono::duration_cast<std::chrono::duration<double> >(t2 - t1).count();
std::cout << "calcOpticalFlowPyrLK run time: " << ttrack << std::endl;
//检测是否出错
size_t i = 0, k = 0;
cv::RNG rng(12345);
for (i = 0; i < pts_1.size(); ++i) {
//距离与状态检测
double dist = abs(pts_0[i].x - pts_1[i].x) + abs(pts_0[i].y - pts_1[i].y);
if (status[i] && dist > 2) {
pts_0[k] = pts_0[i];
pts_1[k++] = pts_1[i];
int b = rng.uniform(0, 255);
int g = rng.uniform(0, 255);
int r = rng.uniform(0, 255);
cv::circle(frame, pts_1[i], 3, cv::Scalar(b, g, r), 3, 8);
cv::line(frame, pts_0[i], pts_1[i], cv::Scalar(b, g, r), 3, 8);
}
}
//绘制跟踪线
draw_line(frame, pts_0, pts_1);
//cv::imshow("frame", frame);
//char c = cv::waitKey(0);
}
}