Particle滤波 粒子滤波跟踪算法

#include"findCircularMarker.h" //利用梯度的椭圆检测 bool CoreAlgorithm::findEllipses(const Mat img, const cv::Rect mask, vector<RotatedRect>& findResults, const double precisionlevel, bool multi, int kenelsize) { //step-1 将图像转化成灰度图 Mat ImageGray; if (img.channels() == 1) { ImageGray = img; } else { cvtColor(img, ImageGray, CV_BGR2GRAY); } ImageGray = Mat(ImageGray, mask); //namedWindow("window",1); //imshow("window",ImageGray); //waitKey(); //step-2 计算图像梯度信息 //step-2-1 生成高斯滤波器模版 //判断滤波片模版大小是不是奇数 if (kenelsize % 2 == 0) return false; Mat X, Y; cv::Range xgv = cv::Range(-abs((kenelsize - 1) / 2), abs((kenelsize - 1) / 2)); cv::Range ygv = xgv; std::vector<int> t_x, t_y; for (int i = xgv.start; i <= xgv.end; i++) t_x.push_back(i); for (int j = ygv.start; j <= ygv.end; j++) t_y.push_back(j); cv::repeat(cv::Mat(t_x), 1, t_y.size(), X); cv::repeat(cv::Mat(t_y).t(), t_x.size(), 1, Y); Mat GaussianKenelx(kenelsize, kenelsize, CV_64F); Mat GaussianKenely(kenelsize, kenelsize, CV_64F); for (int i = 0; i < kenelsize; i++) { for (int j = 0; j < kenelsize; j++) { GaussianKenelx.at<double>(i, j) = double(-X.at<int>(i, j)*exp(pow(X.at<int>(i, j), 2) / -2)*exp(pow(Y.at<int>(i, j), 2) / -2)); GaussianKenely.at<double>(i, j) = double(-Y.at<int>(i, j)*exp(pow(X.at<int>(i, j), 2) / -2)*exp(pow(Y.at<int>(i, j), 2) / -2)); } } //step-2-2 二维滤波器操作 Mat dx, dy; filter2D(ImageGray, dx, CV_64F, GaussianKenelx); filter2D(ImageGray, dy, CV_64F, GaussianKenely); //step-2-3 梯度范数计算 Mat gradientnorm, gradientnormBinary;; magnitude(dx, dy, gradientnorm);//计算梯度范数 double minvalue, maxvalue; cv::minMaxLoc(gradientnorm, &minvalue, &maxvalue); //step-3 高置信梯度区域的选择 //step-3-1 针对求出的梯度矩阵进行二值化 int thresholdvalue = int(minvalue + maxvalue /4); //尝试直接二值化的方法 //imwrite("F:/test3.jpg",ImageGray); //imwrite("F:/test2.jpg",gradientnorm); gradientnorm.convertTo(gradientnorm, CV_32F); double value = threshold(gradientnorm, gradientnormBinary, thresholdvalue, 255, CV_THRESH_BINARY); gradientnormBinary.convertTo(gradientnormBinary, CV_8UC1); //namedWindow("show", 2); //imshow("show", gradientnormBinary); //waitKey(); //以上部分是不是可以通过先高斯滤波处理再使用canny算子进行边缘检测？？？？？？？？？ //step-3-2 联通区域标识 Mat contoursMask; int contoursNum = connectedComponents(gradientnormBinary, contoursMask, 8); contoursMask.convertTo(contoursMask, CV_16UC1); int type = contoursMask.type(); contoursNum--; //step-3-3 数据整理 vector<vector<Point2d>> conectAreasPos; vector<vector<Point2d>> conectAreasGrad; conectAreasGrad.resize(contoursNum); conectAreasPos.resize(contoursNum); for (int i = 0; i < contoursMask.rows; i++) { for (int j = 0; j < contoursMask.cols; j++) { int tempnum = contoursMask.at<unsigned short>(i, j) - 1; if (contoursMask.at<unsigned short>(i, j) != 0) { conectAreasPos[tempnum].push_back(Point2d(double(i), double(j))); conectAreasGrad[tempnum].push_back(Point2d(dx.at<double>(i, j), dy.at<double>(i, j))); } } } //step-4 利用对偶椭圆算子 vector<Mat> dCVec, precisionVec; vector<double> angleIncertitudeVec; if (!multi) { for (int i = 0; i < contoursNum; i++) { Mat dC, precision; double AngleIncertitude; if (conectAreasPos[i].size()<10) continue; if (!DualConicFitting(conectAreasGrad[i], conectAreasPos[i], dC, precision, AngleIncertitude)) continue; if (precision.at<double>(0, 0) == -1 || precision.at<double>(0, 0)>precisionlevel) continue; double num1 = precision.at<double>(0, 0); dCVec.push_back(dC); precisionVec.push_back(precision); angleIncertitudeVec.push_back(AngleIncertitude); } } else { if (!MultiEllipseFitting(conectAreasGrad, conectAreasPos, dCVec, precisionVec, angleIncertitudeVec)) { return false; } } //step-5 椭圆参数计算 Ax^2+Bxy+Cy^2+Dx+Ey+F=0 vector<Mat> EllipsesparaVec; for (unsigned int i = 0; i < dCVec.size(); i++) { Mat Ellipsespara = Mat(6, 1, CV_64F); Mat _C = dCVec[i].inv(); _C = _C / _C.at<double>(2, 2); Ellipsespara.at<double>(0, 0) = _C.at<double>(1, 1); Ellipsespara.at<double>(1, 0) = _C.at<double>(0, 1) * 2; Ellipsespara.at<double>(2, 0) = _C.at<double>(0, 0); Ellipsespara.at<double>(3, 0) = _C.at<double>(1, 2) * 2; Ellipsespara.at<double>(4, 0) = _C.at<double>(2, 0) * 2; Ellipsespara.at<double>(5, 0) = _C.at<double>(2, 2); EllipsesparaVec.push_back(Ellipsespara); } //step-6 由椭圆一般方程求解椭圆标准方程参数 vector<RotatedRect> findResultstemp; for (unsigned int i = 0; i < EllipsesparaVec.size(); i++) { RotatedRect temppara; if (!conicaEllipseTostd(EllipsesparaVec[i], temppara)) continue; findResultstemp.push_back(temppara); } for (unsigned int i = 0; i < findResultstemp.size(); i++) { findResultstemp[i].center.x += mask.x; findResultstemp[i].center.y += mask.y; } findResults = findResultstemp; return true; } bool CoreAlgorithm::DualConicFitting(vector<Point2d>areaGrad, vector<Point2d>areaPos, Mat& dC, Mat& precision, double& angleIncertitude) { precision = Mat::zeros(1, 2, CV_64F); Mat a, b, c, _M; Mat areaGradmat = Mat(areaGrad).reshape(1); Mat areaPosmat = Mat(areaPos).reshape(1); a = areaGradmat.col(0); b = areaGradmat.col(1); Mat multitemp = areaGradmat.mul(areaPosmat); addWeighted(multitemp.col(0), -1, multitemp.col(1), -1, 0, c); //为了高精度检测，对数据进行了线性归一化 Mat M = Mat(a.rows, 2, CV_64F); Mat tempb = -1 * b; tempb.copyTo(M.col(0)); a.copyTo(M.col(1)); Mat B = -1 * c; Mat mpts, Lnorm, Lnormt, Minvert; if (!solve(M, B, mpts, DECOMP_SVD)) return false; Mat H = Mat::eye(3, 3, CV_64F); H.at<double>(0, 2) = mpts.at<double>(0, 0); H.at<double>(1, 2) = mpts.at<double>(1, 0); Mat abc = Mat(a.rows, 3, CV_64F); a.copyTo(abc.col(0)); b.copyTo(abc.col(1)); c.copyTo(abc.col(2)); Lnorm = H.t()*abc.t(); Lnormt = Lnorm.t(); a = Lnormt.col(0).clone(); b = Lnormt.col(1).clone(); c = Lnormt.col(2).clone(); Mat AA = Mat(5, 5, CV_64F); Mat BB = Mat(5, 1, CV_64F); Mat a2 = a.mul(a); Mat ab = a.mul(b); Mat b2 = b.mul(b); Mat ac = a.mul(c); Mat bc = b.mul(c); Mat c2 = c.mul(c); //solution par least-square //AA*THITA=BB; //求AA Mat aaaa = a2.mul(a2); Mat aaab = a2.mul(ab); Mat aabb = a2.mul(b2); Mat aaac = a2.mul(ac); Mat aabc = a2.mul(bc); Mat abab = ab.mul(ab); Mat abbb = ab.mul(b2); Mat abac = ab.mul(ac); Mat abbc = ab.mul(bc); Mat bbbb = b2.mul(b2); Mat bbac = b2.mul(ac); Mat bbbc = b2.mul(bc); Mat acac = ac.mul(ac); Mat acbc = ac.mul(bc); Mat bcbc = bc.mul(bc); AA.at<double>(0, 0) = sum(aaaa).val[0]; AA.at<double>(0, 1) = sum(aaab).val[0]; AA.at<double>(0, 2) = sum(aabb).val[0]; AA.at<double>(0, 3) = sum(aaac).val[0]; AA.at<double>(0, 4) = sum(aabc).val[0]; AA.at<double>(1, 0) = sum(aaab).val[0]; AA.at<double>(1, 1) = sum(abab).val[0]; AA.at<double>(1, 2) = sum(abbb).val[0]; AA.at<double>(1, 3) = sum(abac).val[0]; AA.at<double>(1, 4) = sum(abbc).val[0]; AA.at<double>(2, 0) = sum(aabb).val[0]; AA.at<double>(2, 1) = sum(abbb).val[0]; AA.at<double>(2, 2) = sum(bbbb).val[0]; AA.at<double>(2, 3) = sum(bbac).val[0]; AA.at<double>(2, 4) = sum(bbbc).val[0]; AA.at<double>(3, 0) = sum(aaac).val[0]; AA.at<double>(3, 1) = sum(abac).val[0]; AA.at<double>(3, 2) = sum(bbac).val[0]; AA.at<double>(3, 3) = sum(acac).val[0]; AA.at<double>(3, 4) = sum(acbc).val[0]; AA.at<double>(4, 0) = sum(aabc).val[0]; AA.at<double>(4, 1) = sum(abbc).val[0]; AA.at<double>(4, 2) = sum(bbbc).val[0]; AA.at<double>(4, 3) = sum(acbc).val[0]; AA.at<double>(4, 4) = sum(bcbc).val[0]; //求BB Mat _ccaa = -1 * (c2.mul(a2)); Mat _ccab = -1 * (c2.mul(ab)); Mat _ccbb = -1 * (c2.mul(b2)); Mat _ccac = -1 * (c2.mul(ac)); Mat _ccbc = -