//跟踪特征点
//在输入点附近查找红色区域，求出重心，作为特征点新的位置
//输入为，1当前图像，2被跟踪特征点上一轮的位置
//返回本次跟踪结果
cv::Point2f tracking(cv::Mat image, const cv::Point2f lastcenter)
{//cv::GaussianBlur(image, image, cv::Size(11, 11), 0);/***********初始化ROI**********/const int r = 100;//检测半径const int r2 = r * 2;int startx = lastcenter.x - r;int starty = lastcenter.y - r;if (startx < 0)startx = 0;if (starty < 0)starty = 0;int width = r2;int height = r2;if (startx + width >= image.size().width)startx = image.size().width - 1 - width;if (starty + height >= image.size().height)starty = image.size().height - 1 - height;cv::Mat roi = image(cv::Rect(startx, starty, width, height));cv::Mat roiHSV;cv::cvtColor(roi, roiHSV, CV_BGR2HSV);//将BGR图像转为HSV图像vector<cv::Mat> hsv;cv::split(roiHSV, hsv);//将hsv三个通道分离cv::Mat h = hsv[0];cv::Mat s = hsv[1];cv::Mat v = hsv[2];cv::Mat roiBinary = cv::Mat::zeros(roi.size(), CV_8U);//二值图像，255的地方表示判断为红色/*************判断颜色****************/const double ts = 0.5 * 255;//s阈值，小于该值不判断const double tv = 0.1 * 255;//v阈值，小于该值不判断const double th = 0 * 180 / 360;//h中心const double thadd = 30 * 180 / 360;//h范围在th±thadd内的才被算作是红色//通过特定阈值，对HSV图像进行二值化for (int i = 0; i < roi.size().height; i++){uchar *ptrh = h.ptr<uchar>(i);uchar *ptrs = s.ptr<uchar>(i);uchar *ptrv = v.ptr<uchar>(i);uchar *ptrbin = roiBinary.ptr<uchar>(i);for (int j = 0; j < roi.size().width; j++){if (ptrs[j] < ts || ptrv[j] < tv)continue;if (th + thadd > 180)if (ptrh[j] < th - thadd && ptrh[j] > th + thadd - 180)continue;if (th - thadd < 0)if (ptrh[j] < th - thadd + 180 && ptrh[j] > th + thadd)continue;ptrbin[j] = 255;//找出红色的像素点，在对应位置标记255}}/*****************对二值化图像求出连通域 重心****************/std::vector<std::vector<cv::Point>> contours;cv::findContours(roiBinary.clone(), contours, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_NONE);//可能会有多个连通域，分别计算出他们的重心std::vector<cv::Point2f> gravityCenters;//重心点集for (int i = 0; i < contours.size(); i++){if (contours[i].size() < 10)//连通域太小continue;int xsum = 0;int ysum = 0;for (int j = 0; j < contours[i].size(); j++){xsum += contours[i][j].x;ysum += contours[i][j].y;}double gpx = xsum / contours[i].size();double gpy = ysum / contours[i].size();gravityCenters.push_back(cv::Point2f(gpx + startx, gpy + starty));}/*********************返回最优点******************///找到重心跟上一轮位置最接近的那个cv::Point ret = lastcenter;double dist = 1000000000;double distX = 1000000000;double distY = 1000000000;for (int i = 0; i < gravityCenters.size(); i++){if (distX > abs(lastcenter.x - gravityCenters[i].x) && distY > abs(lastcenter.y - gravityCenters[i].y)){double newdist = sqrt((lastcenter.x - gravityCenters[i].x)*(lastcenter.x - gravityCenters[i].x) + (lastcenter.y - gravityCenters[i].y)*(lastcenter.y - gravityCenters[i].y));if (dist > newdist){distX = abs(lastcenter.x - gravityCenters[i].x);distY = abs(lastcenter.y - gravityCenters[i].y);dist = newdist;ret = gravityCenters[i];}}}return ret;
}