C++ particle code translation

于 2021-07-28 09:56:19 发布
阅读量274 收藏
点赞数
分类专栏： 比较乱的笔记
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.csdn.net/TiffanyXYf/article/details/106018484
版权

C++ particle code translation


是瞎改的笔记，请不要参考
在既有的代码上进行修改：

  1. 图片与模板的输入
  2. 迭代次数与方式的修改

单纯TM

算法步骤

  1. 根据图片大小和particle的个数，随机初始化particle的位置，该位置信息作为固定信息；
  2. 对pre_particle的内部参数根据实际情况进行赋值，即，即使位置信息初始化完成;
  3. 将pre_particle的值赋给particles；
  4. 计算particles中粒子对应的位置的hist，并与template进行比较，由此决定各particle的权重；
  5. 根据权重保留对应particles计算中心，并在中心周围随机产生符合高斯分布的particles，进入下一次迭代；
  6. 达到迭代次数，给出相应的位置

代码修改

  1. 数据结构的修改，现在的代码由于数据结构不够好，导致计算权重等的时候存在很多for循环，需要根据算法的特点和计算特色将数据结构修改一番。
    如果opencv不适合，矩阵的计算就使用EGIN库实现。
//for循环主要存在于：每个粒子的权重计算：包括计算粒子区域的特征、和相应权重的计算
                                   计算完成后归一化权重时，无法直接归一化
                                   解决方法：权重放到一个矩阵（向量）中，这样可以直接归一化
                                                     计算出的distance可以放在一个矩阵中（向量）中，查看能否直接
                                                     计算权重
                                   
//最主要的耗费时间的点在于统计hist，每一个粒子每一次迭代都需要单独统计hist并且计算权重，
由此造成了重复统计
解决方法：如果使用字典，hist的统计耗费时间，如果使用积分图+distance是否会有新的提高呢？积分图的计算opencv有代码，然后使用particle的方法，加上对应Mat，参考very fast template matching的矩特征作为粒子的特征

修改后的代码

距的计算跟随粒子

根据距的特征，我们能够发现，距的计算不太适合粒子滤波，因为区分性太小
384，126

#include <opencv2/opencv.hpp>
#include <iostream>
#include <ctime>
#include <vector>
//#include <Eigen/Dense>
//#include <cmath>
//#include <limits>

//using Eigen::MatrixXd;
//using namespace Eigen;
using namespace cv;
using namespace std;

/****定义使用粒子数目宏****/
const int PARTICLE_NUMBER = 2000;
const double PI = 3.1415926;
const double sigma = 0.01;
void particleTM(const Mat img, const Mat tmp, Mat &result_img)
{
	//参数都写函数体里边，particle算法实现的不好
	int Match_method = 0;
	Mat g_resultImage, src, tempp;
	img.copyTo(src);
	tmp.copyTo(tempp);
	if (src.channels() > 1)
	{
		cvtColor(src, src, CV_RGB2GRAY);
	}
	if (tempp.channels() > 1)
		cvtColor(tempp, tempp, CV_RGB2GRAY);

	int resultImage_cols = img.cols - tmp.cols + 1;
	int resultImage_rows = img.rows - tmp.rows + 1;
	g_resultImage.create(resultImage_cols, resultImage_rows, CV_32FC1);
	matchTemplate(src, tempp, g_resultImage, Match_method);
	normalize(g_resultImage, g_resultImage, 0, 1, NORM_MINMAX, -1, Mat());
	double minValue, maxValue;
	Point minLocation, maxLocation, MatchLocation;
	minMaxLoc(g_resultImage, &minValue, &maxValue, &minLocation, &maxLocation);
	if (Match_method == TM_SQDIFF)
	{
		MatchLocation = minLocation;
	}
	else
	{
		MatchLocation = maxLocation;
	}
	rectangle(src, MatchLocation,
		Point(MatchLocation.x + tempp.cols, MatchLocation.y + tempp.rows),
		Scalar(0, 0, 255), 2, 8, 0);
	/*imshow("input image", src);
	waitKey();*/
	src.copyTo(result_img);
}


void ini_Mat(Mat &input, int width)
{
	for (int i = 0; i < width; i++)
	{
		input.ptr<double>(0)[i] = i + 1;
	}
}
void particleIntTM(Mat img, Mat tmp, Mat &result_img)
{
	Mat src, temp;

	if (img.channels() > 1)
	{
		cvtColor(img, img, COLOR_RGB2GRAY);
	}
	if (tmp.channels() > 1)
	{
		cvtColor(tmp, temp, COLOR_RGB2GRAY);
	}
	/*结合积分图、中心距、Hu距计算作为particle的特征*/
	/***重写matlab程序***/
	temp.convertTo(temp, CV_64F, 1.0 / 255.0, 0);转化为双精度
	img.convertTo(src, CV_64F, 1.0 / 255.0, 0);//转化为双精度

	int boundt_x, boundt_y, bound_x, bound_y;
	boundt_x = temp.cols, boundt_y = temp.rows;
	bound_x = src.cols, bound_y = src.rows;
	int xt0 = floor(boundt_x / 2), yt0 = floor(boundt_y / 2);
	Mat g_resultImage;
	int resultImage_cols = img.cols - tmp.cols + 1;
	int resultImage_rows = img.rows - tmp.rows + 1;
	g_resultImage.create(resultImage_cols, resultImage_rows, CV_32FC1);
	///%% realize the fast template matching
	int d = 2;
	int L = (d + 2)*(d + 1) / 2;//6
	Mat Ut; Ut.create(L, 1, CV_32F);
	Scalar m00 = sum(temp);
	Ut.ptr<float>(0)[0] = m00[0];
	Mat alltx1, allty1, alltx, allty;
	alltx1.create(1, boundt_x, CV_64F);
	allty1.create(1, boundt_y, CV_64F);//1*69
	ini_Mat(alltx1, boundt_x);
	ini_Mat(allty1, boundt_y);
	repeat(alltx1, boundt_y, 1, alltx);
	repeat(allty1.t(), 1, boundt_x, allty);
	Scalar m10 = sum(allty.mul(temp));
	Scalar m01 = sum(alltx.mul(temp));
	Scalar m20 = sum(allty.mul(allty).mul(temp));
	Scalar m02 = sum(alltx.mul(alltx).mul(temp));
	Scalar m11 = sum(alltx.mul(allty).mul(temp));
	Ut.ptr<float>(0)[1] = m10[0] - yt0 * m00[0];
	Ut.ptr<float>(0)[2] = m01[0] - xt0 * m00[0];
	Ut.ptr<float>(0)[3] = m20[0] - 2 * yt0 * m10[0] + pow(yt0, 2)*m00[0];
	Ut.ptr<float>(0)[4] = m11[0] - yt0 * m01[0] - xt0 * m10[0] + yt0 * xt0*m00[0];
	Ut.ptr<float>(0)[5] = m02[0] - 2 * xt0 * m01[0] + pow(xt0, 2)*m00[0];
	/***模板准备完毕，接下来计算图片***/
	Mat allx1, ally1, allx, ally;
	allx1.create(1, bound_x, CV_64F);
	ally1.create(1, bound_y, CV_64F);
	ini_Mat(allx1, bound_x);
	ini_Mat(ally1, bound_y);
	repeat(allx1, bound_y, 1, allx);
	repeat(ally1.t(), 1, bound_x, ally);
	Mat img00, img10, img01, img20, img11, img02;
	integral(src, img00);
	
	integral(ally.mul(src), img10);
	integral(allx.mul(src), img01);
	integral(ally.mul(ally).mul(src), img20);
	integral(allx.mul(allx).mul(src), img02);
	integral(allx.mul(ally).mul(src), img11);	
	Mat i, j, centre_y1, centre_x1, centre_y, centre_x;
	int i_len = bound_y - boundt_y, j_len = bound_x - boundt_x;
	i.create(1, i_len, CV_64F);
	j.create(1, j_len, CV_64F);
	ini_Mat(i, i_len);
	ini_Mat(j, j_len);
	Mat i_centre(1, i_len, CV_64F, Scalar(yt0)),
		j_centre(1, j_len, CV_64F, Scalar(xt0));
	centre_y1 = i + i_centre;
	centre_x1 = j + j_centre;
	repeat(centre_x1, i_len, 1, centre_x);
	repeat(centre_y1.t(), 1, j_len, centre_y);
	
	///粒子滤波作用的地方
	int itra_times = 13;
	Point MatchLocation;
	normalize(Ut,Ut, 1,0, NORM_L2, -1, Mat());
	Mat lctn_weights; //粒子的位置和权重
	lctn_weights.create(5, PARTICLE_NUMBER, CV_32F);
	//weights.create(1, PARTICLE_NUMBER, CV_32F);
	float *lctn0 = lctn_weights.ptr<float>(0), *lctn1 = lctn_weights.ptr<float>(1), //prex,prey
		*lctn2 = lctn_weights.ptr<float>(2), *lctn3 = lctn_weights.ptr<float>(3); //x,y
	float* weight = lctn_weights.ptr<float>(4);
	RNG rng;
	for (int kk = 0; kk < PARTICLE_NUMBER; kk++)
	{
		lctn0[kk] = rng.uniform(0, j_len);
		lctn1[kk] = rng.uniform(0, i_len);
		lctn2[kk] = rng.uniform(0, j_len);
		lctn3[kk] = rng.uniform(0, i_len);
		//weight[kk] = 0;
	}//粒子位置初始化done
	int x, y, prex, prey;//粒子当前、之前位置
	int plusx, plusy;
	vector<double> M(6);//M00,M10,M01,M20,M11,M02
	Mat Uf; Uf.create(L, 1, CV_32F);
	float *Uf_ptr = Uf.ptr<float>(0);
	for (int itra = 0; itra < itra_times; itra++)
	{
		
		double sum_w = 0.0;
		lctn0 = lctn_weights.ptr<float>(0), lctn1 = lctn_weights.ptr<float>(1);
		weight = lctn_weights.ptr<float>(4);
		if (itra == 0)//第一次迭代只需计算粒子weight
		{
			for (int kk = 0; kk < PARTICLE_NUMBER; kk++)//计算初始化位置的权
			{
				
				//计算特征值Uf
				x = lctn0[kk] + 1;
				y = lctn1[kk] + 1;//当前（计算权重的）位置
				plusx = lctn0[kk] + boundt_x;
				plusy = lctn1[kk] + boundt_y;
				
				M[0] = img00.ptr<double>(plusy)[plusx] +
					img00.ptr<double>(y)[x] - img00.ptr<double>(y)[plusx]
					- img00.ptr<double>(plusy)[x];

				M[1] = img10.ptr<double>(plusy)[plusx] + img10.ptr<double>(y)[x]
					- img10.ptr<double>(y)[plusx] - img10.ptr<double>(plusy)[x];

				M[2] = img01.ptr<double>(plusy)[plusx] +
					img01.ptr<double>(y)[x] - img01.ptr<double>(y)[plusx]
					- img01.ptr<double>(plusy)[x];

				M[3] = img20.ptr<double>(plusy)[plusx] +
					img20.ptr<double>(y)[x] - img20.ptr<double>(y)[plusx]
					- img20.ptr<double>(plusy)[x];

				M[4] = img11.ptr<double>(plusy)[plusx] +
					img11.ptr<double>(y)[x] - img11.ptr<double>(y)[plusx]
					- img11.ptr<double>(plusy)[x];

				M[5] = img02.ptr<double>(plusy)[plusx] +
					img02.ptr<double>(y)[x] - img02.ptr<double>(y)[plusx]
					- img02.ptr<double>(plusy)[x];

				double Val_cntx = centre_x.ptr<double>(y - 1)[x - 1],
					Val_cnty = centre_y.ptr<double>(y - 1)[x - 1];
				Uf_ptr[0] = M[0];
				Uf_ptr[1] = M[1] - Val_cnty * M[0];//M10 - centre_y.mul(M00)
				Uf_ptr[2] = M[2] - Val_cntx * M[0];//M01 - centre_x.mul(M00)
				Uf_ptr[3] = M[3] - 2 * Val_cnty * M[1] +
					Val_cnty * Val_cnty * M[0];
				//M20 - 2 * centre_y.mul(M10) +centre_y.mul(centre_y).mul(M00)
				Uf_ptr[4] = M[4] - Val_cnty * M[2] - Val_cntx * M[1] + Val_cntx * Val_cnty*M[0];
				//M11 - centre_y.mul(M01) - centre_x.mul(M10) +
					//	centre_x.mul(centre_y).mul(M00)
				Uf_ptr[5] = M[5] - 2 * Val_cntx*M[2] + Val_cntx * Val_cntx*M[0];//M02 - 2 * centre_x.mul(M01) +
					//	centre_x.mul(centre_x).mul(M00)				
				normalize(Uf, Uf, 1,0, NORM_L2, -1, Mat());		
				/*注意权重计算的方式*/
				//weight[kk] = exp(Uf.dot(Ut) / sqrt(Uf.dot(Uf)));//越大越接近
				weight[kk] = Uf.dot(Ut) / sqrt(Uf.dot(Uf));//越大越接近

				//weight[kk] =(1.0/(sqrt(2*PI)*sigma))* exp(-pow(1-Uf.dot(Ut),2)/(2*sigma*sigma));// / sqrt(Uf.dot(Uf)));//越大越接近
				//sum_w += weight[kk];
			}
			Mat mean;
			Mat stddev;
			meanStdDev(lctn_weights.row(4), mean, stddev);
			lctn_weights.row(4) =lctn_weights.row(4) - mean;
			weight = lctn_weights.row(4).ptr<float>(0);
			for (int leng = 0; leng < lctn_weights.cols; leng++)
			{
				if (weight[leng] < 0) weight[leng] = 0;
				/*if (lctn_weights.row(4).ptr<float>(0)[leng] < 0)
					lctn_weights.row(4).ptr<float>(0)[leng] = 0;*/
			}
			normalize(lctn_weights.row(4), lctn_weights.row(4),  1, 0, NORM_L1, -1, Mat());// = (1.0 / sum_w)*lctn_weights.row(4);

		}
		else//不是第一次迭代
		{
			for (int kk = 0; kk < PARTICLE_NUMBER; kk++)//计算初始化位置的权
			{
				
				lctn0[kk] = cvRound(lctn0[kk] + rng.gaussian(15));//方差取值范围1-5之间（位置变化1-4个像素）
				lctn1[kk] = cvRound(lctn1[kk] + rng.gaussian(15));				
				if (lctn0[kk] >= j_len)
					lctn0[kk] = j_len-1;
				if (lctn0[kk] < 0)lctn0[kk] = 0;
				if (lctn1[kk] >= i_len)
					lctn1[kk] = i_len - 1;
				if (lctn1[kk] < 0)lctn1[kk] = 0;
				//
				x = lctn0[kk] + 1;
				y = lctn1[kk] + 1;
				plusx = lctn0[kk] + boundt_x;
				plusy = lctn1[kk] + boundt_y;
				M[0] = img00.ptr<double>(plusy)[plusx] +
					img00.ptr<double>(y)[x] - img00.ptr<double>(y)[plusx]
					- img00.ptr<double>(plusy)[x];

				M[1] = img10.ptr<double>(plusy)[plusx] + img10.ptr<double>(y)[x]
					- img10.ptr<double>(y)[plusx] - img10.ptr<double>(plusy)[x];

				M[2] = img01.ptr<double>(plusy)[plusx] +
					img01.ptr<double>(y)[x] - img01.ptr<double>(y)[plusx]
					- img01.ptr<double>(plusy)[x];

				M[3] = img20.ptr<double>(plusy)[plusx] +
					img20.ptr<double>(y)[x] - img20.ptr<double>(y)[plusx]
					- img20.ptr<double>(plusy)[x];

				M[4] = img11.ptr<double>(plusy)[plusx] +
					img11.ptr<double>(y)[x] - img11.ptr<double>(y)[plusx]
					- img11.ptr<double>(plusy)[x];

				M[5] = img02.ptr<double>(plusy)[plusx] +
					img02.ptr<double>(y)[x] - img02.ptr<double>(y)[plusx]
					- img02.ptr<double>(plusy)[x];

				double Val_cntx = centre_x.ptr<double>(y - 1)[x - 1],
					Val_cnty = centre_y.ptr<double>(y - 1)[x - 1];
				Uf_ptr[0] = M[0];
				Uf_ptr[1] = M[1] - Val_cnty * M[0];//M10 - centre_y.mul(M00)
				Uf_ptr[2] = M[2] - Val_cntx * M[0];//M01 - centre_x.mul(M00)
				Uf_ptr[3] = M[3] - 2 * Val_cnty * M[1] +
					Val_cnty * Val_cnty * M[0];
				//M20 - 2 * centre_y.mul(M10) +centre_y.mul(centre_y).mul(M00)
				Uf_ptr[4] = M[4] - Val_cnty * M[2] - Val_cntx * M[1] + Val_cntx * Val_cnty*M[0];
				//M11 - centre_y.mul(M01) - centre_x.mul(M10) +
					//	centre_x.mul(centre_y).mul(M00)
				Uf_ptr[5] = M[5] - 2 * Val_cntx*M[2] + Val_cntx * Val_cntx*M[0];//M02 - 2 * centre_x.mul(M01) +
					//	centre_x.mul(centre_x).mul(M00)				
				normalize(Uf, Uf, 1, 0, NORM_L2, -1, Mat());
				/*注意权重计算的方式*/				
				//weight[kk] = exp(Uf.dot(Ut) / sqrt(Uf.dot(Uf)));//越大越接近
				weight[kk] = Uf.dot(Ut) / sqrt(Uf.dot(Uf));//越大越接近

				//weight[kk] = (1.0 / (sqrt(2 * PI)*sigma))* exp(-pow(1 - Uf.dot(Ut), 2)/ (2 * sigma*sigma));
				//sum_w += weight[kk];
			}
			Mat mean;
			Mat stddev;
			meanStdDev(lctn_weights.row(4), mean, stddev);
			lctn_weights.row(4) = lctn_weights.row(4) - mean;//特征小于平均值的粒子直接淘汰
			weight = lctn_weights.row(4).ptr<float>(0);
			for (int leng = 0; leng < lctn_weights.cols; leng++)
			{
				if (weight[leng] < 0) weight[leng] = 0;
				/*if (lctn_weights.row(4).ptr<float>(0)[leng] < 0)
					lctn_weights.row(4).ptr<float>(0)[leng] = 0;*/
			}
			//cout << lctn_weights.row(4) << endl;
			cout << lctn_weights.row(4).ptr<float>(0)[10] << endl;
			normalize(lctn_weights.row(4), lctn_weights.row(4), 1, 0, NORM_L1, -1, Mat());// = (1.0 / sum_w)*lctn_weights.row(4);
			cout << lctn_weights.row(4).ptr<float>(0)[10] << endl;
		}//权重计算完毕(归一化也完毕)
		//cout << lctn_weights.row(4) << endl;

		if (itra < itra_times-1)
		{
			Mat Weights,WeightsIdx;
			Mat Flctn_weights; //缓冲区的粒子的位置和权重
			Flctn_weights.create(2, PARTICLE_NUMBER, CV_32F);
			float *Flctn0 = Flctn_weights.ptr<float>(0), 
				*Flctn1 = Flctn_weights.ptr<float>(1);
			cv::sort(lctn_weights.row(4), Weights, CV_SORT_DESCENDING);
			cv::sortIdx(lctn_weights.row(4), WeightsIdx, CV_SORT_DESCENDING);
			//原来顺序的索引WeightsIdx:32S 访问使用int或者unsigned
			weight = Weights.ptr<float>(0);
			//cout << Weights << endl;
			/*根据权重重采样粒子*/
			lctn0 = lctn_weights.ptr<float>(0), lctn1 = lctn_weights.ptr<float>(1);
			int np=0,nkk=0;
			for (int kk = 0; kk < PARTICLE_NUMBER; kk++)
			{
				np = cvCeil(weight[kk] * PARTICLE_NUMBER);
				//cout << np << endl;
														   ///向上取整为了增加权重大的粒子的个数
				for (int qq = 0; qq < np; qq++)
				{					
					Flctn0[nkk] = lctn0[WeightsIdx.ptr<int>(0)[kk]];
					Flctn1[nkk] = lctn1[WeightsIdx.ptr<int>(0)[kk]];
					nkk++;
					if (nkk == PARTICLE_NUMBER)
						goto EXITOUT;
					///相当于根据权重大小顺序copy粒子
				}
				
			}
			while (nkk < PARTICLE_NUMBER)
			{
				Flctn0[nkk] = lctn0[WeightsIdx.ptr<int>(0)[0]];
				Flctn1[nkk] = lctn1[WeightsIdx.ptr<int>(0)[0]];
				nkk++;
			}
		EXITOUT: {
			Flctn_weights.copyTo(lctn_weights.rowRange(0, 2));//将新的粒子赋值给权威变量
			//Weights.copyTo(lctn_weights.row(4));
			lctn_weights.row(4) = 1.0 / PARTICLE_NUMBER;
			//cout << lctn_weights.row(4);
			//权重1/N
			}
			
		}		
		else//最后一次迭代
		{
			Mat Weights, WeightsIdx;
			Mat Flctn_weights; //缓冲区的粒子的位置和权重
			Flctn_weights.create(2, PARTICLE_NUMBER, CV_32F);
			float *Flctn0 = Flctn_weights.ptr<float>(0),
				*Flctn1 = Flctn_weights.ptr<float>(1);
			cv::sort(lctn_weights.row(4), Weights, CV_SORT_DESCENDING);
			cv::sortIdx(lctn_weights.row(4),WeightsIdx , CV_SORT_DESCENDING);
			MatchLocation.x = 0; MatchLocation.y = 0;
			lctn0 = lctn_weights.ptr<float>(0), lctn1 = lctn_weights.ptr<float>(1);
			//cout << Weights;
			double xL=0, yL=0;
			for (int leng = 0; leng < PARTICLE_NUMBER/5; leng++)
			{
				xL += lctn0[WeightsIdx.ptr<int>(0)[leng]]*
					Weights.ptr<float>(0)[leng];
				yL += lctn1[WeightsIdx.ptr<int>(0)[leng]]*
					Weights.ptr<float>(0)[leng];				
			}
			MatchLocation.x = cvRound(xL);
			MatchLocation.y = cvRound(yL);

		/*	MatchLocation.x = cvRound(lctn_weights.row(0).dot(lctn_weights.row(4)));
			MatchLocation.y = cvRound(lctn_weights.row(1).dot(lctn_weights.row(4)));
		*/	/*cout << lctn_weights.row(0);
			cout << lctn_weights.row(1);
			cout << lctn_weights.row(4);
			cout << "Mx"<<MatchLocation.x << " My"<<MatchLocation.y << endl;*/
		}
	}
	
	normalize(g_resultImage, g_resultImage, 0, 1, NORM_MINMAX, -1, Mat());
	//double minValue, maxValue;
	//Point minLocation, maxLocation, MatchLocation;
	//minMaxLoc(g_resultImage, &minValue, &maxValue, &minLocation, &maxLocation);
	//MatchLocation = maxLocation;
	rectangle(img, MatchLocation,
		Point(MatchLocation.x + temp.cols, MatchLocation.y + temp.rows),
		Scalar(0, 0, 255), 2, 8, 0);
	
	img.copyTo(result_img);
	/*imshow("result ", result_img);
	waitKey();*/
	/*normalize(g_resultImage, g_resultImage, 0, 1, NORM_MINMAX, -1, Mat());
	cout << g_resultImage.ptr<float>(30)[40] << endl;

	imshow("result ", g_resultImage);
	waitKey();*/


}

int main(int argc, unsigned char* argv[])
{
	Mat img = imread("img.jpg");
	/****读取模板****/
	Mat tmp = imread("tmp.jpg");
	//Mat tmp = imread("WebCamTmp.jpg");
	Mat result, result2;
	double start = clock();
	//for(int i=0;i<100;i++)
	particleIntTM(img, tmp, result);
	//particleTM(img, tmp, result);	
	double end = clock();
	///
	/*VideoCapture cap(0);
	while (1)
	{
		if (cap.isOpened())
		{
			cap >> img;
			particleIntTM(img, tmp, result);
			particleTM(img, tmp, result2);

			imshow("searched_target1(IN)", result);
			imshow("searched_target2", result2);

			waitKey(1);
		}
	}*/


	cout << "time is:" <<0.01* (end - start) / CLOCKS_PER_SEC << "s" << endl;
	imshow("Result", result);
	waitKey(0);
	//计算完temp使用4ms
	return 0;
}
模板与图片的所有距一次计算好

计算代价59ms,如果不全计算好，计算代价15ms

修改数据结构前的代码

原来基于hist的参考别人的代码，这里改数据

//#include <opencv2/core/core.hpp>
//#include "opencv2/imgproc/imgproc.hpp"
//#include <opencv2/highgui/highgui.hpp>
#include <opencv2/opencv.hpp>
#include <stdio.h>
#include <iostream>
#include <ctime>

using namespace cv;
using namespace std;

Rect select;



Point origin;
Mat frame, hsv,target_hsv;//待匹配的图片，hsv图片,hsv目标
int after_select_frames = 0;//选择矩形区域完后的帧计数

/****rgb空间用到的变量****/
//int hist_size[]={16,16,16};//rgb空间各维度的bin个数
//float rrange[]={0,255.0};
//float grange[]={0,255.0};
//float brange[]={0,255.0};
//const float *ranges[] ={rrange,grange,brange};//range相当于一个二维数组指针

/****hsv空间用到的变量****/
int hist_size[] = { 16,16,16 };
float hrange[] = { 0,180.0 };
float srange[] = { 0,256.0 };
float vrange[] = { 0,256.0 };

//int hist_size[]={32,32,32};
//float hrange[]={0,359.0.0};
//float srange[]={0,1.0};
//float vrange[]={0,1.0};
const float *ranges[] = { hrange,srange,vrange };

int channels[] = { 0,1,2 };

/****有关粒子窗口变化用到的相关变量****/
int A1 = 2;
int A2 = -1;
int B0 = 1;
double sigmax = 1.0;
double sigmay = 0.5;
double sigmas = 0.001;

/****定义使用粒子数目宏****/
#define PARTICLE_NUMBER 1000 //如果这个数设定太大，经测试这个数字超过25就会报错，则在运行时将会出现错误

/****定义粒子结构体****/
typedef struct particle
{
	int orix, oriy;//原始粒子坐标
	int x, y;//当前粒子的坐标
	double scale;//当前粒子窗口的尺寸
	int prex, prey;//上一帧粒子的坐标
	double prescale;//上一帧粒子窗口的尺寸
	Rect rect;//当前粒子矩形窗口
	//Mat hist;//当前粒子窗口直方图特征
	double weight;//当前粒子权值
}PARTICLE;

PARTICLE pre_particles[PARTICLE_NUMBER];

/****粒子权值降序排列函数****/
int particle_decrease(const void *p1, const void *p2)
{
	PARTICLE* _p1 = (PARTICLE*)p1;
	PARTICLE* _p2 = (PARTICLE*)p2;
	if (_p1->weight < _p2->weight)
		return 1;
	else if (_p1->weight > _p2->weight)
		return -1;
	return 0;//相等的情况下返回0
}

void particleTM(const Mat img, const Mat tmp, Mat &result_img)
{
	//参数都写函数体里边，particle算法实现的不好
	int Match_method = 0;
	Mat g_resultImage, src, tempp;
	img.copyTo(src);
	tmp.copyTo(tempp);
	if (src.channels() > 1)
	{
		cvtColor(src, src, CV_RGB2GRAY);
	}
	if (tempp.channels() > 1)
		cvtColor(tempp,tempp,CV_RGB2GRAY);

	int resultImage_cols = img.cols - tmp.cols + 1;
	int resultImage_rows = img.rows - tmp.rows + 1;
	g_resultImage.create(resultImage_cols, resultImage_rows, CV_32FC1);
	matchTemplate(src, tempp, g_resultImage, Match_method);
	normalize(g_resultImage, g_resultImage, 0, 1, NORM_MINMAX, -1, Mat());
	double minValue, maxValue;
	Point minLocation, maxLocation, MatchLocation;
	minMaxLoc(g_resultImage, &minValue, &maxValue, &minLocation, &maxLocation);
	if (Match_method == TM_SQDIFF)
	{
		MatchLocation = minLocation;
	}
	else
	{
		MatchLocation = maxLocation;
	}
	rectangle(src, MatchLocation,
		Point(MatchLocation.x + tempp.cols, MatchLocation.y + tempp.rows),
		Scalar(0, 0, 255), 2, 8, 0);
	/*imshow("input image", src);
	waitKey();*/
	src.copyTo(result_img);
}

int main(int argc, unsigned char* argv[])
{
	char c;
	int ite_num=3;
	Mat target_img, track_img;//模板，当前粒子圈定的图片区域
	Mat target_hist, track_hist;//模板、粒子圈定区域的hist
	PARTICLE *pParticle,particles[PARTICLE_NUMBER];//代表单个粒子，指向数组中的单个粒子
	RNG rng;//随机数产生器
	int width, height,width_t,height_t;//图片的宽、高
	/***打开摄像头****/
	VideoCapture cam(0);
	if (!cam.isOpened())
		return -1;
	/***采集一帧图片****/
	cam >> frame;
	if (frame.empty())
		return -1;

	frame = imread("img.jpg");
	width = frame.cols;
	height = frame.rows;
	/****建立窗口****/
	namedWindow("camera", 1);//显示视频原图像的窗口	
	//
	/****读取模板****/
	target_img = imread("tmp.jpg");
	//cvtColor(target_img,target_img,CV_RGB2GRAY);
	imwrite("gaga.bmp", target_img);

	if (target_img.empty()) return -1;
	
	width_t = target_img.cols;
	height_t = target_img.rows;
	cvtColor(target_img, target_hsv, CV_BGR2HSV);//模板转hsv
	calcHist(&target_hsv, 1, channels, Mat(), target_hist, 3, hist_size, ranges);
	normalize(target_hist, target_hist);//模板准备完毕
	//
	///
	/***粒子位置随机初始化，内存换时间***/
	pParticle = pre_particles;//指针初始化指向pre_particles数组
	for (int i = 0; i < PARTICLE_NUMBER; i++)
	{				
		pParticle->x = rng.uniform(0,width);
		pParticle->y = rng.uniform(0, height);
		pParticle->orix = pParticle->x;//粒子的原始坐标为选定矩形框(即目标)的中心
		pParticle->oriy = pParticle->y;
		pParticle->prex = pParticle->x;//更新上一次的粒子位置
		pParticle->prey = pParticle->y;		
		pParticle->prescale = 1;
		pParticle->scale = 1;//scale 的作用未知
		pParticle->rect = Rect(pParticle->x - width_t / 2,
			pParticle->y - height / 2, width_t, height_t);
		pParticle->rect &= Rect(0, 0, width, height);//防越界
		/*pParticle->hist = target_hist;
		pParticle->weight = 0;*/
		pParticle++;
	}//pre_particle 的位置分配完毕
	//
	Mat frame_T;
	//particleTM(frame, target_img,frame_T);
	//imshow("resu", frame_T);
	//waitKey();
	while (1)
	{
		double start=clock();
		/****读取一帧图像****/



		/*cam >> frame;
		if (frame.empty())
			return -1;*/



		/****将rgb空间转换为hsv空间****/
		cvtColor(frame, hsv, CV_BGR2HSV);
		///
		/***暂时默认每帧迭代次数为3***/
		for (int i = 0; i < ite_num; i++)
		{
			double sum = 0.0;
			if (i == 0)//初次迭代，将pre_particle赋值给particles（位置）,并为其计算hist
			{
				memcpy(particles,pre_particles,sizeof(particles));//particles位置赋值和rect赋值
				pParticle = particles;
				for (int j = 0; j < PARTICLE_NUMBER; j++)//particles直方图赋值
				{
					track_img = Mat(hsv, pParticle->rect);
					calcHist(&track_img, 1, channels, Mat(), track_hist, 3, hist_size, ranges);
					normalize(track_hist, track_hist);
					pParticle->weight = 1.0 - compareHist(target_hist, track_hist, CV_COMP_BHATTACHARYYA);
					/****累加粒子权值****/
					sum += pParticle->weight;
					pParticle++;
				}//particles 权重赋值结束
				
				
			}//第一次迭代结束
			else//后续迭代
			{
				
				pParticle = particles;
				//RNG rng;//随机数产生器
				int x, y;
				int xpre, ypre;
				double s, pres;
				/****更新粒子结构体的大部分参数****/
				for (int j = 0; j < PARTICLE_NUMBER; j++)
				{
					
					xpre = pParticle->x;
					ypre = pParticle->y;
					pres = pParticle->scale;

					/****更新粒子的矩形区域即粒子中心****/
					x = cvRound(A1*(pParticle->x - pParticle->orix) + A2 * (pParticle->prex - pParticle->orix) +
						B0 * rng.gaussian(sigmax) + pParticle->orix);
					pParticle->x = max(0, min(x, frame.cols - 1));

					y = cvRound(A1*(pParticle->y - pParticle->oriy) + A2 * (pParticle->prey - pParticle->oriy) +
						B0 * rng.gaussian(sigmay) + pParticle->oriy);
					pParticle->y = max(0, min(y, frame.rows - 1));

					s = A1 * (pParticle->scale - 1) + A2 * (pParticle->prescale - 1) + B0 * (rng.gaussian(sigmas)) + 1.0;
					pParticle->scale = max(1.0, min(s, 3.0));

					pParticle->prex = xpre;
					pParticle->prey = ypre;
					pParticle->prescale = pres;
					//        pParticle->orix=pParticle->orix;
					//        pParticle->oriy=pParticle->oriy;

							//注意在c语言中，x-1.0，如果x是int型，则这句语法有错误,但如果前面加了cvRound(x-0.5)则是正确的
					pParticle->rect.x = max(0, min(cvRound(pParticle->x - 0.5*pParticle->scale*pParticle->rect.width), frame.cols));
					pParticle->rect.y = max(0, min(cvRound(pParticle->y - 0.5*pParticle->scale*pParticle->rect.height), frame.rows));
					pParticle->rect.width = min(cvRound(pParticle->rect.width), frame.cols - pParticle->rect.x);
					pParticle->rect.height = min(cvRound(pParticle->rect.height), frame.rows - pParticle->rect.y);
					//        pParticle->rect.width=min(cvRound(pParticle->scale*pParticle->rect.width),frame.cols-pParticle->rect.x);
					//        pParticle->rect.height=min(cvRound(pParticle->scale*pParticle->rect.height),frame.rows-pParticle->rect.y);

							/****计算粒子区域的新的直方图特征****/
					track_img = Mat(hsv, pParticle->rect);
					calcHist(&track_img, 1, channels, Mat(), track_hist, 3, hist_size, ranges);
					normalize(track_hist, track_hist);

					/****更新粒子的权值****/
					//            pParticle->weight=compareHist(target_hist,track_hist,CV_COMP_INTERSECT);
					//采用巴氏系数计算相似度,永远与最开始的那一目标帧相比较
					pParticle->weight = 1.0 - compareHist(target_hist, track_hist, CV_COMP_BHATTACHARYYA);
					/****累加粒子权值****/
					sum += pParticle->weight;
					pParticle++;
				}	

			}
			//
			//以上，粒子权值更新与归一化都结束，接下来进行重采样
			if (i < ite_num - 1) {
				/****归一化粒子权重****/
				pParticle = particles;
				for (int k = 0; k < PARTICLE_NUMBER; k++)
				{
					pParticle->weight /= sum;
					pParticle++;
				}
				/****根据粒子的权值降序排列****/
				pParticle = particles;
				qsort(pParticle, PARTICLE_NUMBER, sizeof(PARTICLE), &particle_decrease);

				/****根据粒子权重重采样粒子****/
				PARTICLE newParticle[PARTICLE_NUMBER];
				int np = 0, k = 0;
				for (int i = 0; i < PARTICLE_NUMBER; i++)
				{
					np = cvRound(pParticle->weight*PARTICLE_NUMBER);
					for (int j = 0; j < np; j++)
					{
						newParticle[k++] = particles[i];
						if (k == PARTICLE_NUMBER)
							goto EXITOUT;
					}
				}
				while (k < PARTICLE_NUMBER)
					newParticle[k++] = particles[0];
			EXITOUT:
				for (int i = 0; i < PARTICLE_NUMBER; i++)
					particles[i] = newParticle[i];
			}
			else//最后一次迭代，只归一化权重和排序即可
			{
				/****归一化粒子权重****/
				pParticle = particles;
				for (int k = 0; k < PARTICLE_NUMBER; k++)
				{
					pParticle->weight /= sum;
					pParticle++;
				}
				/****根据粒子的权值降序排列****/
				pParticle = particles;
				qsort(pParticle, PARTICLE_NUMBER, sizeof(PARTICLE), &particle_decrease);


			}
		}//for end
		double end1 = clock();
		cout << "calculated time is" <<
			(double)(end1 - start) / CLOCKS_PER_SEC << "s"<<endl;

		///****计算最大权重目标的期望位置，作为跟踪结果****/
		/****计算最大权重目标的期望位置，采用所有粒子数作为跟踪结果****/
			Rect rectTrackingTemp(0,0,0,0);
			pParticle=particles;
			for(int i=0;i<PARTICLE_NUMBER;i++)
			{
				rectTrackingTemp.x+=cvRound(pParticle->x*pParticle->weight);
				rectTrackingTemp.y+=cvRound(pParticle->y*pParticle->weight);
				pParticle++;
			}
			pParticle=particles;
			rectTrackingTemp.width = pParticle->rect.width;
			rectTrackingTemp.height = pParticle->rect.height;
		/****显示跟踪结果****/
		Rect tracking_rect(rectTrackingTemp);
		rectangle(frame, tracking_rect, Scalar(0, 0, 255), 3, 8, 0);
		imshow("camera", frame);
		c = (char)waitKey(20);
		if (27 == c)//ESC键
			return -1;
		///		
	}

	return 0;
}
VeraWin
关注
  • 0
    点赞
  • 踩
  • 0
    收藏
    觉得还不错? 一键收藏
  • 0
    评论
专栏目录
评论
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值