ofxParticleFilterLocalization.h

#pragma once

// --------------------------------------------------------------
//
// File : ofxParticleFilterLocalization.h
//
// Description : Mobile robot localization sample code with
//   		 ParticleFilterLocalization (PF)
//		 The Robot can get a range data from RFID that its position
//		 known.
//
// Environment : c++, openFrameworks
//
// Author : Atsushi Sakai (ParticleFilterLocalization.m)
//          dotchang
// 
// Copyright (c) : 2014 Atsushi Sakai
//                 2017 dotchang
//
// Licence : Modified BSD Software License Agreement
// --------------------------------------------------------------

#include "ofMath.h"
#include "ofVec2f.h"
#include "ofVec3f.h"
#include "ofMatrix2x2.h" // https://github.com/naokiring/ofMatrix2x2
#include "ofMatrix3x3.h"

#include <math.h> 
#include <vector>
#include <random>
#include <numeric>

#include <omp.h>

class ResultData
{
public:
	float time;
	ofVec3f xTrue;
	ofVec3f xd;
	ofVec3f xEst;
	int z;
	int PEst;
	ofVec2f u;
};

class ofxParticleFilterLocalization
{
public:
	ofxParticleFilterLocalization()
	: engine(seed_gen()), dist(0.f, 1.f) // 標準正規分布: 平均0.0、標準偏差1.0で分布させる
	{}
	~ofxParticleFilterLocalization() {}

	void setup()
	{
		std::cout << "Particle Filter (PF) sample program start!!" << std::endl;

		time = 0.f;
		float endtime = 60.f; // [sec]
		dt = 0.1f; // [sec]

		nSteps = (int)ceil((endtime - time) / dt);

		result.clear();

		// State Vector [x y yaw]'
		xEst = ofVec3f(0.f, 0.f, 0.f);

		// True State
		xTrue = xEst;

		// Dead Reckoning Result
		xd = xTrue;

		// Covariance Matrix for predict
		Q = ofMatrix3x3(powf(0.1f, 2.f), 0.f, 0.f, 0.f, powf(0.1f, 2.f), 0.f, 0.f, 0.f, powf(ofDegToRad(3.f), 2.f));

		// Covariance Matrix for observation
		R = powf(1.f, 2.f); // [range[m]

		// Simulation parameter
		Qsigma = ofMatrix2x2(powf(0.1f, 2.f), 0.0f, 0.0f, powf(ofDegToRad(5.f), 2.f));

		Rsigma = powf(0.1f, 2.f);

		// RFIDタグの位置 [x, y]
		RFID.clear();
		RFID.push_back(ofVec2f(10.f, 0.f));
		RFID.push_back(ofVec2f(10.f, 10.f));
		RFID.push_back(ofVec2f(0.f, 15.f));
		RFID.push_back(ofVec2f(-5.f, 20.f));

		MAX_RANGE = 20.f;
		NP = 100;
		NTh = NP / 2;

		px.reserve(NP);
		pw.reserve(NP);
		for (int i = 0; i < NP; i++) {
			px.emplace_back(xEst);
			pw.emplace_back(1.f / (float)NP);
		}

		cnt = 0;
	}

	void update()
	{
		// Main loop
		if (cnt < nSteps) { //for (int i = 0; i<nSteps; i++) {
			int i = cnt;
			time = time + dt;
			// Input
			ofVec2f u = doControl(time);
			// Observation
			Observation(z, xTrue, xd, u, RFID, MAX_RANGE);

			// ------ Particle Filter --------
// #pragma omp parallel for
			for (int ip = 0; ip < NP; ip++) {
				ofVec3f x = px[ip];
				float w = pw[ip];

				// Dead Reckoning and random sampling
				ofVec3f randn(dist(engine), dist(engine), dist(engine)); // 正規分布で乱数を生成する
				ofMatrix3x3 sqrtq(sqrt(Q.a), sqrt(Q.b), sqrt(Q.c),
					sqrt(Q.d), sqrt(Q.e), sqrt(Q.f),
					sqrt(Q.g), sqrt(Q.h), sqrt(Q.i));
				ofVec3f rs(sqrtq.a*randn.x + sqrtq.b*randn.y + sqrtq.c*randn.z,
					sqrtq.d*randn.x + sqrtq.e*randn.y + sqrtq.f*randn.z,
					sqrtq.g*randn.x + sqrtq.h*randn.y + sqrtq.i*randn.z);
				x = f(x, u) + rs;

				// Calc Inportance Weight
				for (int iz = 0; iz < z.size(); iz++) {
					ofVec2f a(x.x, x.y);
					ofVec2f b(z[iz].y, z[iz].z);
					float pz = (a - b).length();
					float dz = pz - z[iz].x;
					w = w*Gauss(dz, 0.f, sqrt(R));
				}
				px[ip] = x; // 格納
				pw[ip] = w;
			}

			Normalize(pw, NP); // 正規化
			Resampling(px, pw, NTh, NP); // リサンプリング
			// xEst = px * pw';
			xEst = ofVec3f(0.f, 0.f, 0.f);
			for (int is = 0; is < px.size(); is++) {
				xEst += px[is] * pw[is]; // 最終推定値は期待値
			}

			// Simulation Result
			ResultData rd;
			rd.time = time;
			rd.xTrue = xTrue;
			rd.xd = xd;
			rd.xEst = xEst;
			rd.u = u;
			result.push_back(rd);

			cnt++;
		}
	}

	void draw()
	{
		drawGraph(result);
	}

	void drawGraph(std::vector<ResultData>& l_result)
	{
		if (cnt < nSteps) {
			// Animation (remove some flames)
			if (cnt % 1 == 0) {
				ofPushStyle();
				ofPushMatrix();
				ofDrawGrid(1, 24);
				float arrow = 0.5f;
				// パーティクル表示
				for (int ip = 0; ip < NP; ip++) {
					ofPushMatrix();
					ofTranslate(px[ip].x, px[ip].y);
					ofDrawIcoSphere(0.05f);
					ofDrawArrow(ofVec3f(0.f, 0.f, 0.f), ofVec3f(arrow*cos(px[ip].z), arrow*sin(px[ip].z), 0.f), 0.1f);
					ofPopMatrix();
				}
				ofPushMatrix();
				glBegin(GL_LINES);
				for (std::vector<ResultData>::iterator it = l_result.begin(); it != l_result.end(); ++it) {
					glVertex3f(it->xTrue.x, it->xTrue.y, 0.f);
				}
				glEnd();
				ofPopMatrix();

				ofSetColor(ofColor::yellowGreen);
				for (std::vector<ofVec2f>::iterator it = RFID.begin(); it != RFID.end(); ++it) {
					ofPushMatrix();
					ofTranslate(it->x, it->y);
					ofDrawBox(1.f);
					ofPopMatrix();
				}
				// 観測線の表示
				if (!z.empty()) {
					ofSetColor(ofColor::indianRed);
					for (int iz = 0; iz < z.size(); iz++) {
						ofDrawLine(ofVec2f(xTrue.x, xTrue.y), ofVec2f(z[iz].y, z[iz].z));
					}
				}
				ofSetColor(ofColor::greenYellow);
				ofPushMatrix();
				glBegin(GL_LINES);
				for (std::vector<ResultData>::iterator it = l_result.begin(); it != l_result.end(); ++it) {
					glVertex3f(it->xd.x, it->xd.y, 0.f);
				}
				glEnd();
				ofPopMatrix();

				ofSetColor(ofColor::fireBrick);
				ofPushMatrix();
				glBegin(GL_LINES);
				for (std::vector<ResultData>::iterator it = l_result.begin(); it != l_result.end(); ++it) {
					glVertex3f(it->xEst.x, it->xEst.y, 0.f);
				}
				glEnd();
				ofPopMatrix();

				ofPopMatrix();
				ofPopStyle();
			}
		}
		else {
			ofPushMatrix();
			ofDrawGrid(1, 24);

			glBegin(GL_LINES);
			for (int i = 0; i < l_result_.size(); i++) {
				glVertex3f(l_result[i].xTrue.x, l_result[i].xTrue.y, 0.f);
			}
			glEnd();
			ofSetColor(ofColor::fireBrick);
			glBegin(GL_LINES);
			for (int i = 0; i < l_result.size(); i++) {
				glVertex3f(l_result[i].xEst.x, l_result[i].xEst.y, 0.f);
			}
			glEnd();
			ofSetColor(ofColor::greenYellow);
			glBegin(GL_LINES);
			for (int i = 0; i < result_.size(); i++) {
				glVertex3f(l_result[i].xd.x, l_result[i].xd.y, 0.f);
			}
			glEnd();

			ofSetColor(ofColor::red);
			ofPushMatrix();
			ofTranslate(l_result.back().xTrue.x, l_result.back().xTrue.y);
			ofDrawSphere(0.1);
			ofPopMatrix();

			ofSetColor(ofColor::white);
			ofPopMatrix();
		}
	}

	// リサンプリングを実施する関数
	void Resampling(std::vector<ofVec3f>& l_px, std::vector<float>& l_pw, int l_NTh, int l_NP)
	{
		// アルゴリズムはLow Variance Sampling
		float pwpwt = 0.f;
		for (std::vector<float>::iterator it = l_pw.begin(); it != l_pw.end(); ++it) {
			pwpwt += (*it)*(*it);
		}
		float Neff = 1.f / pwpwt;
		if (Neff < l_NTh) // リサンプリング
		{
			std::vector<float> wcum;
			std::partial_sum(l_pw.begin(), l_pw.end(), std::back_inserter(wcum));
			std::vector<float> cumnp(l_pw.size(), 1.f / (float)l_NP), base; // 乱数を加える前のbase
			std::partial_sum(cumnp.begin(), cumnp.end(), std::back_inserter(base));
			for (std::vector<float>::iterator it = base.begin(); it != base.end(); ++it) {
				*it -= 1.f / (float)l_NP;
			}
			std::vector<float> resampleID(base.begin(), base.end()); // ルーレットを乱数分増やす
			float r = ofRandom(1.f);
			for (std::vector<float>::iterator it = resampleID.begin(); it != resampleID.end(); ++it) {
				*it += r / (float)l_NP;
			}
			std::vector<ofVec3f> ppx(l_px.begin(), l_px.end()); // データ格納用
			int ind = 0; // 新しいID
			for (int ip = 0; ip < l_NP; ip++) {
				while (resampleID[ip] > wcum[ind]) {
					ind = ind + 1;
				}
				l_px[ip] = ppx[ind]; // LVSで選ばれたパーティクルに置き換え
				l_pw[ip] = 1.f / (float)l_NP; // 尤度は初期化
			}
		}
	}

	// 重みベクトルを正規化する関数
	void Normalize(std::vector<float>& l_pw, int l_NP)
	{
		float sumw = std::accumulate(l_pw.begin(), l_pw.end(), 0.f);
		if (sumw != 0.f)
			for (std::vector<float>::iterator it = l_pw.begin(); it != l_pw.end(); ++it) {
				*it = *it / sumw; // 正規化
			}
		else
			for (std::vector<float>::iterator it = l_pw.begin(); it != l_pw.end(); ++it) {
				*it = 0.f + 1.f / (float)l_NP;
			}
	}

	// ガウス分布の確率密度を計算する関数
	float Gauss(float x, float u, float sigma)
	{
		return 1.f / sqrt(2.f*PI*powf(sigma, 2.f))*exp(-powf(x - u, 2.f) / (2.f*powf(sigma, 2.f)));
	}

	// Motion Model
	ofVec3f f(ofVec3f x, ofVec2f u)
	{
		return ofVec3f(x.x + dt*cos(x.z)*u.x, x.y + dt*sin(x.z)*u.x, x.z + dt*u.y);
	}

	// Calc Observation from noise parameter
	void Observation(std::vector<ofVec3f>& z, ofVec3f& x, ofVec3f& l_xd, ofVec2f& u, std::vector<ofVec2f>& l_RFID, float l_MAX_RANGE)
	{
		x = f(x, u); // Ground Truth

		// 正規分布で乱数を生成する
		ofVec2f randn(dist(engine), dist(engine));

		u = ofVec2f(u.x + sqrt(Qsigma.a)*randn.x + sqrt(Qsigma.b)*randn.y, u.y + sqrt(Qsigma.c)*randn.x + sqrt(Qsigma.d)*randn.y);
		l_xd = f(l_xd, u); // Dead Reckoning
		// Simulate Observation
		z.clear();
		for (int iz = 0; iz < l_RFID.size(); iz++) {
			float d = (l_RFID[iz] - x).length();
			if (d < l_MAX_RANGE) // 観測範囲内
				z.push_back(ofVec3f(d + sqrt(Rsigma)*dist(engine), l_RFID[iz].x, l_RFID[iz].y));
		}
	}

	ofVec2f doControl(float l_time)
	{
		// Calc Input Parameter
		float T = 10.f; // [sec]

		// [V yawrate]
		float V = 1.f; // [m/s]
		float yawrate = 5.f; // [deg/s]

		return ofVec2f(V*(1.f - exp(-l_time / T)), ofDegToRad(yawrate)*(1.f - exp(-l_time / T)));
	}

protected:
	float time;
	float dt;
	int nSteps;

	ofVec3f xEst; // State Vector [x y yaw]'
	ofVec3f xTrue; // True State
	ofVec3f xd; // Dead Reckoning Result

	std::vector<ResultData> result;

	ofMatrix3x3 Q;
	float R;
	ofMatrix2x2 Qsigma;
	float Rsigma;

	std::vector<ofVec2f> RFID;

	float MAX_RANGE; // 最大観測距離
	int NP; // パーティクル数
	int NTh; // リサンプリングを実施する有効パーティクル数

	vector<ofVec3f> px; // パーティクル格納変数
	vector<float> pw; // 重み変数

	std::vector<ofVec3f> z; // 観測結果

private:
	int cnt;
	
	std::random_device seed_gen;
	std::default_random_engine engine;
	std::normal_distribution<> dist;
};