OpenCV3におけるパーティクルフィルタ（粒子フィルタ）

はじめに

• OpenCV2.Xまでは、OpenCV1.X系の書き方でパーティクルフィルタが扱えていたのですが（ここ）、OpenCV3からは無くなっていたので、色々調べてみてC++とOpenCV3.Xシリーズで記述してみました。

• 使用してもらっても構いませんが、保証はしかねます。

• 赤色を追跡するパーティクルフィルタの実装です。

• 実行環境は以下の通りです。

• Visual Studio 2013

• OpenCV 3.1

• OpenCV3のインストールなどはここなどを参考にしてください。

解説のページ

• パーティクルフィルタの解説のページはこのあたりを参考に

パーティクルフィルタによる物体追跡：このページを参考にプログラムを書きました。
Tutorial/Practice0 MIST Project
6.パーティクルフィルタ

プログラムコード

• OpenCVを使うためのヘッダファイル

OPENCV.hpp

/***
* Revised 2016/08/07 for OpenCV-3
***/

# pragma once
# include <iostream>
# include <opencv2/opencv.hpp>
# include <opencv2/core/core.hpp>              // (C++用)
# include <opencv2/imgproc/imgproc.hpp>        // (C++用)
# include <opencv2/highgui/highgui.hpp>        // (C++用)
# include <opencv2/ml/ml.hpp>                  // サポートベクタマシン、ブースティングなどの機械学習
# include <opencv2/features2d/features2d.hpp>  // SURF、FASTなどの特徴抽出
# include <opencv2/xfeatures2d.hpp>
# include <opencv2/xfeatures2d/nonfree.hpp>
# include <opencv2/video/background_segm.hpp>  // 前景/背景分離
# include <opencv2/video/tracking.hpp>         // トラッキング
# include <opencv2/objdetect/objdetect.hpp>    // Haar、LBP、HOGなどのオブジェクト検出器
# include <opencv2/calib3d/calib3d.hpp>        // カメラキャリブレーション、ステレオカメラなど
# include <opencv2/flann/flann.hpp>            // 高速最近傍処理(FLANN)など
# include <opencv2/superres/optical_flow.hpp>  //オプティカルフロー
# include <opencv2/tracking/feature.hpp>

# define CV_VERSION_STR CVAUX_STR(CV_MAJOR_VERSION) CVAUX_STR(CV_MINOR_VERSION) CVAUX_STR(CV_SUBMINOR_VERSION)
# ifdef _DEBUG
# define CV_EXT_STR "d.lib"
# else
# define CV_EXT_STR ".lib"
# endif

# define _X64
# undef _X64

# ifdef _X64
# pragma comment(lib, "C:\\OpenCV3.1.0\\vs2013\\x64\\lib\\opencv_calib3d"	CV_VERSION_STR	CV_EXT_STR)
# pragma comment(lib, "C:\\OpenCV3.1.0\\vs2013\\x64\\lib\\opencv_core"	CV_VERSION_STR	CV_EXT_STR)
# pragma comment(lib, "C:\\OpenCV3.1.0\\vs2013\\x64\\lib\\opencv_features2d"	CV_VERSION_STR	CV_EXT_STR)
# pragma comment(lib, "C:\\OpenCV3.1.0\\vs2013\\x64\\lib\\opencv_flann"	CV_VERSION_STR	CV_EXT_STR)
# pragma comment(lib, "C:\\OpenCV3.1.0\\vs2013\\x64\\lib\\opencv_highgui"	CV_VERSION_STR	CV_EXT_STR)
# pragma comment(lib, "C:\\OpenCV3.1.0\\vs2013\\x64\\lib\\opencv_imgcodecs"	CV_VERSION_STR	CV_EXT_STR)
# pragma comment(lib, "C:\\OpenCV3.1.0\\vs2013\\x64\\lib\\opencv_imgproc"	CV_VERSION_STR	CV_EXT_STR)
# pragma comment(lib, "C:\\OpenCV3.1.0\\vs2013\\x64\\lib\\opencv_ml"	CV_VERSION_STR	CV_EXT_STR)
# pragma comment(lib, "C:\\OpenCV3.1.0\\vs2013\\x64\\lib\\opencv_objdetect"	CV_VERSION_STR	CV_EXT_STR)
# pragma comment(lib, "C:\\OpenCV3.1.0\\vs2013\\x64\\lib\\opencv_photo"	CV_VERSION_STR	CV_EXT_STR)
# pragma comment(lib, "C:\\OpenCV3.1.0\\vs2013\\x64\\lib\\opencv_shape"	CV_VERSION_STR	CV_EXT_STR)
# pragma comment(lib, "C:\\OpenCV3.1.0\\vs2013\\x64\\lib\\opencv_stitching"	CV_VERSION_STR	CV_EXT_STR)
# pragma comment(lib, "C:\\OpenCV3.1.0\\vs2013\\x64\\lib\\opencv_superres"	CV_VERSION_STR	CV_EXT_STR)
# pragma comment(lib, "C:\\OpenCV3.1.0\\vs2013\\x64\\lib\\opencv_ts"	CV_VERSION_STR	CV_EXT_STR)
# pragma comment(lib, "C:\\OpenCV3.1.0\\vs2013\\x64\\lib\\opencv_video"	CV_VERSION_STR	CV_EXT_STR)
# pragma comment(lib, "C:\\OpenCV3.1.0\\vs2013\\x64\\lib\\opencv_videoio"	CV_VERSION_STR	CV_EXT_STR)
# pragma comment(lib, "C:\\OpenCV3.1.0\\vs2013\\x64\\lib\\opencv_videostab"	CV_VERSION_STR	CV_EXT_STR)
# else
# pragma comment(lib, "C:\\OpenCV3.1.0\\vs2013\\x86\\lib\\opencv_calib3d"	CV_VERSION_STR	CV_EXT_STR)
# pragma comment(lib, "C:\\OpenCV3.1.0\\vs2013\\x86\\lib\\opencv_core"	CV_VERSION_STR	CV_EXT_STR)
# pragma comment(lib, "C:\\OpenCV3.1.0\\vs2013\\x86\\lib\\opencv_features2d"	CV_VERSION_STR	CV_EXT_STR)
# pragma comment(lib, "C:\\OpenCV3.1.0\\vs2013\\x86\\lib\\opencv_flann"	CV_VERSION_STR	CV_EXT_STR)
# pragma comment(lib, "C:\\OpenCV3.1.0\\vs2013\\x86\\lib\\opencv_highgui"	CV_VERSION_STR	CV_EXT_STR)
# pragma comment(lib, "C:\\OpenCV3.1.0\\vs2013\\x86\\lib\\opencv_imgcodecs"	CV_VERSION_STR	CV_EXT_STR)
# pragma comment(lib, "C:\\OpenCV3.1.0\\vs2013\\x86\\lib\\opencv_imgproc"	CV_VERSION_STR	CV_EXT_STR)
# pragma comment(lib, "C:\\OpenCV3.1.0\\vs2013\\x86\\lib\\opencv_ml"	CV_VERSION_STR	CV_EXT_STR)
# pragma comment(lib, "C:\\OpenCV3.1.0\\vs2013\\x86\\lib\\opencv_objdetect"	CV_VERSION_STR	CV_EXT_STR)
# pragma comment(lib, "C:\\OpenCV3.1.0\\vs2013\\x86\\lib\\opencv_photo"	CV_VERSION_STR	CV_EXT_STR)
# pragma comment(lib, "C:\\OpenCV3.1.0\\vs2013\\x86\\lib\\opencv_shape"	CV_VERSION_STR	CV_EXT_STR)
# pragma comment(lib, "C:\\OpenCV3.1.0\\vs2013\\x86\\lib\\opencv_stitching"	CV_VERSION_STR	CV_EXT_STR)
# pragma comment(lib, "C:\\OpenCV3.1.0\\vs2013\\x86\\lib\\opencv_superres"	CV_VERSION_STR	CV_EXT_STR)
# pragma comment(lib, "C:\\OpenCV3.1.0\\vs2013\\x86\\lib\\opencv_ts"	CV_VERSION_STR	CV_EXT_STR)
# pragma comment(lib, "C:\\OpenCV3.1.0\\vs2013\\x86\\lib\\opencv_video"	CV_VERSION_STR	CV_EXT_STR)
# pragma comment(lib, "C:\\OpenCV3.1.0\\vs2013\\x86\\lib\\opencv_videoio"	CV_VERSION_STR	CV_EXT_STR)
# pragma comment(lib, "C:\\OpenCV3.1.0\\vs2013\\x86\\lib\\opencv_videostab"	CV_VERSION_STR	CV_EXT_STR)
# endif

/***** 画像のピクセルの直接操作 OpenCV-2.x (MAT* ) *****/

// 汎用マクロ
// IMG: IplImage*, TYPE: 型, X,Y: 座標, ID: インデックス
// 型
// IPL_DEPTH_8U: unsigned char
// IPL_DEPTH_16S: short
// IPL_DEPTH_

# define M_VALUE(MAT, TYPE, X, Y, ID) ((TYPE*)((MAT)->data + (MAT)->step*(Y)))[(X)*(MAT)->channels()+(ID)]
// 汎用マクロ
// IMG: IplImage*, TYPE: 型, X,Y: 座標, ID: インデックス
// M_IMG:Mat ,M_X,M_Y:座標,ID:インデックス
// 型
// IPL_DEPTH_8U: unsigned char
// IPL_DEPTH_16S: short
// IPL_DEPTH_
# define I_VALUE(IMG, TYPE, X, Y, ID) ((TYPE*)((IMG)->imageData + (IMG)->widthStep*(Y)))[(X)*(IMG)->nChannels+(ID)]
# define I_VALUE_FOR_MAT(M_IMG, TYPE,M_X,M_Y,ID) (TYPE)M_IMG.data[(M_X) * M_IMG.channels() + (ID) +  M_IMG.step * (M_Y)]	
// よく使う画像用マクロ
// 8ビット1チャンネル画像 (CV_8UC1)
# define MVC(MAT, X, Y) M_VALUE((MAT), unsigned char, X, Y, 0)

// 8ビット3チャンネル画像 (CV_8UC3)
# define MBC(MAT, X, Y) M_VALUE((MAT), unsigned char, X, Y, 0)
# define MGC(MAT, X, Y) M_VALUE((MAT), unsigned char, X, Y, 1)
# define MRC(MAT, X, Y) M_VALUE((MAT), unsigned char, X, Y, 2)

// 16ビット整数1チャンネル画像 (CV_16U) 符号あり16ビット整数(short) 
# define MVS(IMG, X, Y) M_VALUE(IMG, short, X, Y, 0)

// 16ビット3チャンネル画像 (CV_16SC3) 符号あり16ビット整数(short) 
# define MBS(MAT, X, Y) M_VALUE((MAT), short, X, Y, 0)
# define MGS(MAT, X, Y) M_VALUE((MAT), short, X, Y, 1)
# define MRS(MAT, X, Y) M_VALUE((MAT), short, X, Y, 2)

// 32ビット浮動小数点1チャンネル画像 (CV_32FC1)
# define MVF(MAT, X, Y) M_VALUE((MAT), float, X, Y, 0)

// 32ビット浮動小数点3チャンネル画像 (CV_32FC3)
# define MBF(MAT, X, Y) M_VALUE((MAT), float, X, Y, 0)
# define MGF(MAT, X, Y) M_VALUE((MAT), float, X, Y, 1)
# define MRF(MAT, X, Y) M_VALUE((MAT), float, X, Y, 2)

/***** 画像のピクセルの直接操作 OpenCV-1.x (IplImage* ) *****/

// 汎用マクロ
// IMG: IplImage*, TYPE: 型, X,Y: 座標, ID: インデックス
// 型
// IPL_DEPTH_8U: unsigned char
// IPL_DEPTH_16S: short
// IPL_DEPTH_
# define I_VALUE(IMG, TYPE, X, Y, ID) ((TYPE*)((IMG)->imageData + (IMG)->widthStep*(Y)))[(X)*(IMG)->nChannels+(ID)]

// よく使う画像用マクロ
// 8ビット1チャンネル画像 (IPL_DEPTH_8U, 1)
# define IVC(IMG, X, Y) I_VALUE(IMG, unsigned char, X, Y, 0)

// 8ビット3チャンネル画像 (IPL_DEPTH_8U, 3)
# define IBC(IMG, X, Y) I_VALUE(IMG, unsigned char, X, Y, 0)
# define IGC(IMG, X, Y) I_VALUE(IMG, unsigned char, X, Y, 1)
# define IRC(IMG, X, Y) I_VALUE(IMG, unsigned char, X, Y, 2)

// 16ビット整数1チャンネル画像 (IPL_DEPTH_16S, 1) 符号あり16ビット整数(short) 
# define IVS(IMG, X, Y) I_VALUE(IMG, short, X, Y, 0)

// 16ビット3チャンネル画像 (IPL_DEPTH_16S, 3) 符号あり16ビット整数(short) 
# define IBS(IMG, X, Y) I_VALUE(IMG, short, X, Y, 0)
# define IGS(IMG, X, Y) I_VALUE(IMG, short, X, Y, 1)
# define IRS(IMG, X, Y) I_VALUE(IMG, short, X, Y, 2)

// 32ビット浮動小数点1チャンネル画像 (IPL_DEPTH_32F, 1)
# define IVF(IMG, X, Y) I_VALUE(IMG, float, X, Y, 0)

// 32ビット浮動小数点3チャンネル画像 (IPL_DEPTH_32F, 3)
# define IBF(IMG, X, Y) I_VALUE(IMG, float, X, Y, 0)
# define IGF(IMG, X, Y) I_VALUE(IMG, float, X, Y, 1)
# define IRF(IMG, X, Y) I_VALUE(IMG, float, X, Y, 2)

/***** 行列の要素の直接操作 *****/
// 行:ROW, 列: COL
// 32ビット浮動小数点1チャンネル行列 (CV_32FC1)
# define M1F1(MAT, ROW, COL) ((float*)((MAT)->data.ptr + (MAT)->step*(ROW)))[(COL)]

// 32ビット浮動小数点2チャンネル行列 (CV_32FC2)
# define M2F1(MAT, ROW, COL) ((float*)((MAT)->data.ptr + (MAT)->step*(ROW)))[(COL)*2+0]
# define M2F2(MAT, ROW, COL) ((float*)((MAT)->data.ptr + (MAT)->step*(ROW)))[(COL)*2+1] 

// 32ビット浮動小数点3チャンネル行列 (CV_32FC3)
# define M3F1(MAT, ROW, COL) ((float*)(((MAT)->data).ptr + (MAT)->step*(ROW)))[(COL)*3+0]
# define M3F2(MAT, ROW, COL) ((float*)(((MAT)->data).ptr + (MAT)->step*(ROW)))[(COL)*3+1]
# define M3F3(MAT, ROW, COL) ((float*)(((MAT)->data).ptr + (MAT)->step*(ROW)))[(COL)*3+2]

// 64ビット浮動小数点1チャンネル行列 (CV_32FC1)
# define M1D1(MAT, ROW, COL) ((double*)((MAT)->data.ptr + (MAT)->step*(ROW)))[(COL)]

// 64ビット浮動小数点2チャンネル行列 (CV_64FC2)
# define M2D1(MAT, ROW, COL) ((double*)((MAT)->data.ptr + (MAT)->step*(ROW)))[(COL)*2+0]
# define M2D2(MAT, ROW, COL) ((double*)((MAT)->data.ptr + (MAT)->step*(ROW)))[(COL)*2+1] 

// 64ビット浮動小数点3チャンネル行列 (CV_64FC3)
# define M3D1(MAT, ROW, COL) ((double*)((MAT)->data.ptr + (MAT)->step*(ROW)))[(COL)*3+0]
# define M3D2(MAT, ROW, COL) ((double*)((MAT)->data.ptr + (MAT)->step*(ROW)))[(COL)*3+1]
# define M3D3(MAT, ROW, COL) ((double*)((MAT)->data.ptr + (MAT)->step*(ROW)))[(COL)*3+2]

• パーティクルフィルタのヘッダファイル

Particle.hpp

# pragma once
# include <iostream>
# include <vector>
# include <random>

# include "OPENCV.hpp"

class Particle;       //パーティクルに関するクラス
class ParticleFilter; //パーティクルフィルタ関係のクラス

class Particle{
public:
	//重み
	double weight; // = 1.0;

	//パラメータ
	int height;
	int width;
	int height_speed;
	int width_speed;

	//コンストラクタ
	Particle();
	Particle(int height_point, int width_point,
		 int height_speed_point, int width_speed_point);

	void PrintParameter();
};

class ParticleFilter
{
private:
	int num; //粒子数
	int width; //横
	int height; //縦
	std::vector<int> upper; //最大値
	std::vector<int> lower; //最小値
	std::vector<int> noise; //ノイズ
	std::vector<Particle> particle_vector; //パーティクルの管理 

public:
	ParticleFilter();
	ParticleFilter(int particle_num, int height_point, int width_point,
		std::vector<int> &upper_point, std::vector<int> &lower_point,
		std::vector<int> &noise_point);
	
	Particle Measure(); //重心の計算
	void Predict(); //予測
	void CalcWeight(cv::Mat &input_image); //重みの計算
	void Resampling(); //リサンプリング
	double Likelifood(int x, int y, cv::Mat &input_image); //尤度計算

	std::vector<Particle> GetPaticleVector(); //粒子を返却する
};

• パーティクルフィルタのコード内容

Particle.cpp

# include "ParticleFilter.hpp"

//初期化
Particle::Particle()
{
	Particle(0, 0, 0, 0);
}

//初期化
Particle::Particle(int height_point, int width_point, int height_speed_point, int width_speed_point)
{
	height = height_point;
	width = width_point;
	height_speed = height_speed_point;
	width_speed = width_speed_point;
}

//パラメータの表示
void Particle::PrintParameter()
{
	std::cout << "width = " << width <<
		", height = " << height << 
		", v = " << width_speed <<
		", u = " << height_speed << "\n";
}

//パーティクルフィルタの初期化
ParticleFilter::ParticleFilter()
{
	//ランダム(画像サイズに変更するべき？)
	int rand_point = rand() * 100 + 1;
	std::vector<int> upper = { rand_point, rand_point, 10, 10 };
	std::vector<int> lower = { 0, 0, -10, -10 };
	std::vector<int> noise = { 30, 30, 10, 10 };
	ParticleFilter(100, rand_point, rand_point, upper, lower, noise);
}

//パーティクルフィルタの初期化
ParticleFilter::ParticleFilter(int particle_num, int height_point, int width_point,
	std::vector<int> &upper_point, std::vector<int> &lower_point,
	std::vector<int> &noise_point)
{
	num = particle_num;
	height = height_point;
	width = width_point;

	upper = upper_point;
	lower = lower_point;
	noise = noise_point;

	//パーティクルの初期化を行う
	for (int i = 0; i < num; ++i)
	{
		//ランダムで初期化する
		int n[4];
		for (int i = 0; i < 4; ++i)
		{
			int x = (int)(((double)rand() / RAND_MAX)*INT_MAX);
			n[i] = x % (upper[i] - lower[i]) + lower[i];
		}
		Particle particle(n[0], n[1], n[2], n[3]);

		//粒子の数だけ重みを初期化する
		particle_vector.push_back(particle);
		particle_vector[i].weight = 1.0 / num;		
	}

}

//Particleの重心を計算
Particle ParticleFilter::Measure()
{
	Particle p = Particle(0, 0, 0, 0);

	//初期化
	double n[4];
	for (int i = 0; i < 4; ++i)
		n[i] = 0.0;

	for (int i = 0; i < particle_vector.size(); ++i)
	{
		n[0] += particle_vector[i].width * particle_vector[i].weight;
		n[1] += particle_vector[i].height * particle_vector[i].weight;
		n[2] += particle_vector[i].width_speed * particle_vector[i].weight;
		n[3] += particle_vector[i].height_speed * particle_vector[i].weight;
	}
	p.width        = (int)n[0];
	p.height       = (int)n[1];
	p.width_speed  = (int)n[2];
	p.height_speed = (int)n[3];

	return p;
}

//物体の位置を予測する
void ParticleFilter::Predict()
{
	for (int i = 0; i < num; ++i)
	{
		int n[4];
		for (int i = 0; i < 4; ++i)
			n[i] = (int)(((double)rand() / RAND_MAX) * INT_MAX) % (noise[i] * 2) - noise[i];
		
		particle_vector[i].width += particle_vector[i].width_speed + n[0];
		particle_vector[i].height += particle_vector[i].height_speed + n[1];
		particle_vector[i].width_speed += n[2];
		particle_vector[i].height_speed += n[3];

		//細かい条件はこちら
		//update state
		if (particle_vector[i].width < lower[0]) particle_vector[i].width = lower[0];
		if (particle_vector[i].height < lower[1]) particle_vector[i].height = lower[1];
		if (particle_vector[i].width_speed < lower[2]) particle_vector[i].width_speed = lower[2];
		if (particle_vector[i].height_speed < lower[3]) particle_vector[i].height_speed = lower[3];

		if (particle_vector[i].width >= upper[0]) particle_vector[i].width = upper[0];
		if (particle_vector[i].height >= upper[1]) particle_vector[i].height = upper[1];
		if (particle_vector[i].width_speed >= upper[2]) particle_vector[i].width_speed = upper[2];
		if (particle_vector[i].height_speed >= upper[3]) particle_vector[i].height_speed = upper[3];
	}
}

//リサンプリング
void ParticleFilter::Resampling()
{
	// accumulate weight
	std::vector<double> sum_weight(num);
	sum_weight[0] = particle_vector[0].weight;
	for (int i = 1; i < num; ++i)
	{
		sum_weight[i] = sum_weight[i - 1] + particle_vector[i].weight;
	}

	std::vector<Particle> pre_particle(particle_vector);
	for (int i = 0; i < num; ++i)
	{
		double weight_threshold = (double)(rand() % 10000) / 10000.0;
		for (int j = 0; j < num; ++j)
		{
			if (weight_threshold > sum_weight[j])
			{
				continue;
			}
			else
			{
				particle_vector[i] = pre_particle[j];
				particle_vector[i].weight = 0.0;
				break;
			}
		}
	}
}

//重みの計算
void ParticleFilter::CalcWeight(cv::Mat &input_image)
{
	double sum_weight = 0.0;

	for (int i = 0; i < particle_vector.size(); ++i)
	{
		int x = particle_vector[i].width;
		int y = particle_vector[i].height;

		//尤度の計算
		if (x < 0 || x > input_image.size().width || y < 0 || y > input_image.size().height)
			particle_vector[i].weight = 0.001;
		else
			particle_vector[i].weight = Likelifood(x, y, input_image);

		sum_weight += particle_vector[i].weight;
	}

	//正規化
	for (int i = 0; i < particle_vector.size(); ++i)
	{
		particle_vector[i].weight /= sum_weight;
	}
}

//粒子ベクトルの返却
std::vector<Particle> ParticleFilter::GetPaticleVector()
{
	return particle_vector;
}

//尤度の計算
//尤度の計算を変更すると色々と使える
//今回は赤色を追跡する
double ParticleFilter::Likelifood(int x, int y, cv::Mat &input_image)
{
	float b, g, r;
	float dist = 0.0, sigma = 50.0;

	b = input_image.data[y * input_image.step + x * input_image.elemSize() + 0];	//B
	g = input_image.data[y * input_image.step + x * input_image.elemSize() + 1];	//G
	r = input_image.data[y * input_image.step + x * input_image.elemSize() + 2];	//R

	dist = sqrt(b * b + g * g + (255.0 - r) * (255.0 - r));

	return 1.0 / (sqrt(2.0 * CV_PI) * sigma) * expf(-dist * dist / (2.0 * sigma * sigma));
}

• main関数

main.cpp

# include "ParticleFilter.hpp"

using namespace std;

int main(void){

	cv::VideoCapture video;
	video.open(0);

	if (!video.isOpened()){
		cout << "can't open your video" << endl;
	}

	cv::namedWindow("Camera");

	bool start = false;
	
	ParticleFilter *pf = new ParticleFilter();
	while (1){
		cv::Mat frame;
		video >> frame;

		//終了判定の条件
		if (frame.empty()){
			break;
		}

		if (!start)
		{
			std::vector<int> upper = { frame.size().width, frame.size().height, 10, 10 };
			std::vector<int> lower = { 0, 0, -10, -10 };
			std::vector<int> noise = { 30, 30, 10, 10 };

			pf = new ParticleFilter(300, frame.size().height, frame.size().width, upper, lower, noise);
			start = true;
		}

		pf->Resampling();
		pf->Predict();
		pf->CalcWeight(frame);

		Particle p = pf->Measure();
		p.PrintParameter();

		cv::Point pt = cv::Point(p.width, p.height);

		//全部の点を表示
		std::vector<Particle> particle = pf->GetPaticleVector();
		for (int i = 0; i < particle.size(); ++i)
		{
			cv::Point pp = cv::Point(particle[i].width, particle[i].height);
			cv::circle(frame, pp, 1, cv::Scalar(0, 255, 255), -1);
		}

		//中心を赤色で表示
		cv::circle(frame, pt, 3, cv::Scalar(0, 0, 255), -1);


		cv::imshow("Camera", frame);

		char c = (char)cv::waitKey(1);
		
		if (c == 'q')
			break;
	}

}

結果

• 黄色が粒子全部で、赤色が重心となります。

• 結果として、何もなければそのあたりをうろうろしています。
  

• 赤いものを持ってくると粒子が集まってきます。
  

まとめ

• 今回は参考した場所とOpenCVのページでRGB系で尤度計算を行っていましたが、距離計算できる系であれば利用できるので、たとえばHSV系などで行えばより照明環境に強い追跡もできるかと思います。
• 最後にコードをGitHubにも公開しておきます。

参考：
パーティクルフィルタによる物体追跡