のんびりしているエンジニアの日記

ソフトウェアなどのエンジニア的な何かを書きます。

Particle FilterでOpencvを使った物体追跡

Sponsored Links

皆さんこんにちは
お元気ですか。私は元気です。

さて、今日はOpencvを使ってParticle Filterで遊ぼうと思います。
すみません、遅くなりました。忙しくてなかなかこれを勉強する時間がなかったのですが
なんとか実装できました。

因みにParticle Filterは自作で挑みます。

What is Particle Filter

一言で説明すると・・・

粒子フィルタリングと呼ばれることがあるらしい。
大量に粒子ばらまいて、似ている所を活かそうという話です。

Algorithm

今回の物体追跡におけるParticle Filterでは以下を繰り返します
1.Resampling
2.Predict
3.Particleの重み計算
4.Measure(重心の計測)

1.Resampling
結果からリサンプリングを行います。要は寄与が低い値のパーティクルは別のパーティクルで置き換えています。

2.Predict
ばら撒いたParticleを元に予測します。
設定上、等速直線運動を行い、更に雑音を加えることで次の物体の位置を予測します。

3.重み計算
尤度関数を使ってその重心の位置が物体に対してどの程度尤もらしいかを計算します。
今回はエリアのカラーヒストグラムの距離を探索物体と探索エリアで用いましたが、他の実装だとピクセルの色を扱っているところとか色々あるようです。

4.重心計測
尤度の値を元に重み x 位置を計算し、実際の重心を求めています。

この実装だと高速移動を行うと粒子が時々、迷子になっています・・・・・(パラメータが不安)
因みに今回の実装はC++で、Opencvを使った実装を行っています。

現状、輝度しか見てないので光の強弱とか輝度の変化には凄く弱いです。

Source Code


実装にはC++11の機能を使っています。

ParticleFileter.hpp

#pragma once
#include <iostream>
#include <vector>
#include <opencv2/opencv.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <random>
#include "CalcDistance.hpp"

using namespace std;
using namespace cv;

class Particle{
public:
	double weight = 1.0;
	int height;
	int width;
	int height_speed;
	int width_speed;
	Particle(int height,int width,int height_speed,int width_speed);
	void PrintOut();
};

class ParticleFilter{
private:
	int n_sample;//Particleのサンプル数
	vector<Particle> particle_vector; //Particleの保管ベクトル
	Mat search_image; //探索画像
	int image_height; //画像の高さ
	int image_width; //幅
	vector<int> upper; //最大値
	vector<int> lower; //最低値
public:
	ParticleFilter();
	ParticleFilter(int particle_num,int dimension,int height_point,int width_point,Mat &image,vector<int> &upper,vector<int> &lower);
	void Predict();
	void CalcWeight(Mat &input_image);
	double Likelihood(int width,int height,Mat &input_image);
	void Resampling();
	Point Measure();

	vector<Particle> GetParticle();
};

ParticleFilter.hpp

#include "ParticleFilter.hpp"

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;
}

/**
Particle のステータス表示
*/
void Particle::PrintOut(){
	cout << "height:" << height << endl;
	cout << "width:" << width << endl;
	cout << "height_speed:" << height_speed << endl;
	cout << "width_speed:" << width_speed << endl;
}
/*
実装上あまり意味がない
*/
ParticleFilter::ParticleFilter(){

}

/*
パーティクルフィルタの初期化
*/
ParticleFilter::ParticleFilter(int particle_num,int dimension,int height_point,int width_point,Mat &image,vector<int> &vec_upper,vector<int> &vec_lower){
	n_sample = particle_num;
	image_width = image.size().width;
	image_height = image.size().height;

	for(int t = 0; t < n_sample; t++){
		Particle particle(height_point + image_height / 2,width_point + image_width / 2,1,1);
		particle_vector.push_back(particle);
	}

	upper = vec_upper;
	lower = vec_lower;
	search_image = image;
}

/**
物体の位置を予測する。
*/
void ParticleFilter::Predict(){
	for(int index = 0; index < n_sample; index++){
		std::random_device rd;

		std::mt19937 mt(rd());
		std::uniform_real_distribution<double> score(-5.0,5.0);
		int n[4];
		n[0] = score(mt);
		n[1] = score(mt);
		n[2] = score(mt);
		n[3] = score(mt);

		particle_vector[index].width += particle_vector[index].width_speed + n[0];
		particle_vector[index].height += particle_vector[index].height_speed + n[1];
		particle_vector[index].width_speed += n[2];
		particle_vector[index].height_speed += n[3];

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

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

/**
重みを計算する
*/
void ParticleFilter::CalcWeight(Mat &input_image){
	imshow("探索物体",search_image);

	double sum_weight = 0.0;
	//重みを計算する。Likelihoodは自分で定義しても良い
	for(int index = 0; index < particle_vector.size(); index++){
		int width = particle_vector[index].width;
		int height = particle_vector[index].height;

		double weight = Likelihood(width, height, input_image);

		particle_vector[index].weight = weight;
		sum_weight += weight;
	}

	//重みを正規化する。重みの合計を1とする
	for(int index = 0; index < particle_vector.size(); index++){
		particle_vector[index].weight /= sum_weight;
	}
}

/**
尤度もといそれっぽさを計算する
計算方法は基本的に距離など測れるものであればなんでも可。
*/
double ParticleFilter::Likelihood(int width,int height,Mat &input_image){
	//画像の範囲
	int min_width = width - image_width / 2;
	int min_height = height - image_height / 2;
	int max_width = min_width + image_width * 2;
	int max_height = min_height + image_height * 2;

	//todo 修正必要
	int move_image_width = image_width;
	int move_image_height = image_height;
	//範囲の確認、はみ出たら計算しない
	if(min_width < 0 || min_height < 0 || max_width >= input_image.size().width || max_height >= input_image.size().height){
		return 0.0001;
	}

	Rect rt;
	rt.x = min_width;
	rt.y = min_height;
	rt.width = move_image_width;
	rt.height = move_image_height;

	Mat part_image(input_image,rt);

	imshow("解析画像",part_image);

	CalcDistance cd;
	double dist = cd.calcDistance(part_image, search_image);
	return dist;
}

/**
Resamplingの実装
*/
void ParticleFilter::Resampling(){
	vector<double> sum_weights(n_sample);
	sum_weights[0] = particle_vector[0].weight;
	for(int index = 1; index < n_sample; index++){
		sum_weights[index] = sum_weights[index-1] + particle_vector[index].weight;
	}

	vector<Particle> copy_particle_vector(particle_vector);
	cout << "sum_weight "<< sum_weights[n_sample-1] << endl;
	for(int index = 0; index < n_sample; index++){
		double weight_threshold = (double)(rand()%10000) / 10000.0;
		for(int k = 0; k < n_sample; k++){
			if(weight_threshold > sum_weights[k]){
				continue;
			}else{
				particle_vector[index] = copy_particle_vector[k];
				particle_vector[index].weight = 0.0;
				break;
			}
		}
	}
}

/**
Particleの重心を計測
*/
Point ParticleFilter::Measure(){
	double x = 0.0;
	double y = 0.0;
	double weight = 0.0;
	for(int index = 0; index < n_sample; index++){
		x += particle_vector[index].width * particle_vector[index].weight;
		y += particle_vector[index].height * particle_vector[index].weight;
	}

	Point pt;
	pt.x = x;
	pt.y = y;

	cout << pt << endl;

	return pt;
}

vector<Particle> ParticleFilter::GetParticle(){
	return particle_vector;
}

CalcDIstance.hpp

#include "CalcDistance.hpp"

CalcDistance::CalcDistance() {}

/**
2画像間の距離を計測する。
*/
double CalcDistance::calcDistance(Mat &image1, Mat &image2) {
	FeatureDetection fd;
	vector<double> image1_feature = fd.calcFeature(image1);
	vector<double> image2_feature = fd.calcFeature(image2);
	Correlation cl;
	return cl.calcDistance(image1_feature, image2_feature);
}

FeatureDetection::FeatureDetection() {}

int FeatureDetection::calcIndex(int pixel, int index) { return pixel / index; }

vector<double> FeatureDetection::calcFeature(Mat &image) {
	int index = 32;
	int index_number = 255 / index;
	vector<double> color_histogram(pow(index_number + 1, 3.0));

	for (int i = 0; i < color_histogram.size(); i++) {
		color_histogram[i] = 0.0;
	}

	for (int height = 0; height < image.rows; height++) {
		for (int width = 0; width < image.cols; width++) {
			int blue_pixel =
			    image.data[height * image.step +
				       width * image.elemSize() + 0];
			int green_pixel =
			    image.data[height * image.step +
				       width * image.elemSize() + 1];
			int red_pixel =
			    image.data[height * image.step +
				       width * image.elemSize() + 2];

			int color_index =
			    calcIndex(blue_pixel, index) +
			    (index_number + 1) * calcIndex(green_pixel, index) +
			    (index_number + 1) * (index_number + 1) *
				calcIndex(red_pixel, index);
			// cout << "b:" << blue_pixel << "g:" << green_pixel <<
			// "r:" << red_pixel << "index" << color_index << endl;

			color_histogram[color_index]++;
		}
	}

	double sum_hist = 0.0;
	for (int i = 0; i < color_histogram.size(); i++) {
		sum_hist += color_histogram[i];
	}

	for (int i = 0; i < color_histogram.size(); i++) {
		color_histogram[i] /= sum_hist;
	}

	return color_histogram;
}

double Distance::sumVector(vector<double> &histogram) {
	double sum = 0.0;
	for (int i = 0; i < histogram.size(); i++) {
		sum += histogram[i];
	}
	return sum;
}

double Distance::average(vector<double> &histogram) {
	return sumVector(histogram) / histogram.size();
}

vector<double> Distance::normalize(vector<double> &histogram) {
	vector<double> normalize_vector(histogram.size());
	double sum = sumVector(histogram);

	for (int i = 0; i < histogram.size(); i++) {
		normalize_vector[i] = histogram[i] / sum;
	}
	return normalize_vector;
}

double Correlation::calcDistance(vector<double> &histogram1,
				 vector<double> &histogram2) {
	vector<double> normalize_hist1 = normalize(histogram1);
	vector<double> normalize_hist2 = normalize(histogram2);

	double sum = 0.0;
	double sum2 = 0.0;
	double sum3 = 0.0;
	for (int i = 0; i < histogram1.size(); i++) {
		double diff1 = (normalize_hist1[i] - average(normalize_hist1));
		double diff2 = (normalize_hist2[i] - average(normalize_hist2));

		sum += diff1 * diff2;
		sum2 += diff1 * diff1;
		sum3 += diff2 * diff2;
	}
	return sum / sqrt(sum2 * sum3);
}

CalcDistance.cpp

#pragma once
#include <iostream>
#include <vector>
#include <opencv2/opencv.hpp>
#include <opencv2/highgui/highgui.hpp>

using namespace std;
using namespace cv;

class CalcDistance{
public:
	CalcDistance();
	double calcDistance(Mat &image1,Mat &image2);
};

class FeatureDetection{
public:
	FeatureDetection();
	int calcIndex(int pixel,int index);
	vector<double> calcFeature(Mat &image);
};

class Distance{
public:
	double calcDistance(vector<double> &histogram1,vector<double> &histogram2);
	double sumVector(vector<double> &histogram);
	double average(vector<double> &histogram);
	vector<double> normalize(vector<double> &histogram);
};

class Correlation:public Distance{
public:
	double calcDistance(vector<double> &histogram1,vector<double> &histogram2);
};

Capture.cpp

#include <iostream>
#include <opencv2/opencv.hpp>
#include <opencv2/highgui/highgui.hpp>
#include "ParticleFilter.hpp"

using namespace cv;
using namespace std;

Mat image;

bool selectObject = false;
int trackObject = 0;
Point origin;
Rect selection;
Mat searchObject;

//マウスクリック時の行動
void onMouse(int event,int x,int y,int,void*){
    if(selectObject){
        selection.x = MIN(x, origin.x);
        selection.y = MIN(y, origin.y);
        selection.width = std::abs(x - origin.x);
        selection.height = std::abs(y - origin.y);
        selection &= Rect(0, 0, image.cols, image.rows);
    }

    switch(event){
    case EVENT_LBUTTONDOWN:
        origin = Point(x,y);
        selection = Rect(x,y,0,0);
        selectObject = true;
        break;
    case EVENT_LBUTTONUP:
        selectObject = false;
        if( selection.width > 0 && selection.height > 0 )
            trackObject = -1;
        break;
    }
}

int main(void){
	VideoCapture video;
	video.open(0);

	Rect trackWindow;

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

	namedWindow("Camera",0);
	setMouseCallback("Camera",onMouse, 0 );
	Mat frame, hsv, hue, mask, hist, histimg = Mat::zeros(200, 320, CV_8UC3), backproj;
	bool paused = false;

    ParticleFilter *pf = new ParticleFilter();
	while(1){
		Mat frame;
		video >> frame;

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

		frame.copyTo(image);

		if(!paused)
        {
            if(trackObject){
                if(trackObject < 0){
                    //Particle Filterの設定
                    vector<int> upper = {image.size().width,image.size().height,10,10};
                    vector<int> lower = {0,0,-10,-10};
                    Mat roi(image, selection);
                    roi.copyTo(searchObject);
                    cout << selection.x << selection.y << endl;
                    pf = new ParticleFilter(200, 4, selection.y,selection.x,searchObject, upper, lower);

                    trackObject = 1;
                }

                pf->Resampling();
                pf->Predict();
                pf->CalcWeight(image);
                Point pt = pf->Measure();
                double angle = 30;
                //重心を表示
                ellipse(image, pt, Size(10, 10), angle, angle, angle+360, Scalar(0,200,0), 5, 8);

                vector<Particle> particle_vector = pf->GetParticle();
                for(int i = 0; i < particle_vector.size(); i++){
                    ellipse(image, Point(particle_vector[i].width,particle_vector[i].height), Size(1, 1), angle, angle, angle+360, Scalar(0,200,0), 5, 8);
                }
            }
        }
        //trackObjectが存在しない時はこちらへ
        else if(trackObject < 0)
            paused = false;

        //選択オブジェクトがある時に色を変える。
        if(selectObject && selection.width > 0 && selection.height > 0){
            Mat roi(image, selection);
            bitwise_not(roi, roi);
        }

        imshow( "Camera", image );

        char c = (char)waitKey(10);
	}
}

Makefile

CXX = g++
CFLAGS = -O2 -Wall
CXXFLAGS = -O2 -Wall -std=c++11
SOURCE := $(shell ls *.cpp)
HEADER := $(shell ls *.hpp)
OBJS := $(SOURCE:.cpp=.o)
TARGET = test

all: clean $(TARGET)

$(TARGET):$(OBJS)
	$(CXX) `pkg-config --cflags opencv` `pkg-config --libs opencv` -o $@ $(OBJS)

.c.o:
	$(CC) $(CFLAGS) -c $^
.cpp.o:
	$(CXX) $(CXXFLAGS) -c $^

clean:
	rm -f *.o