ofxGridMapSample.h

#pragma once

// --------------------------------------------------------------
//
// File : ofxGridMapSample.h
//
// Description : Sample code to build grid map
//               https://github.com/AtsushiSakai/MATLABRobotics/blob/master/Mapping/GridMapSample/GridMapSample.m
//
// Environment : c++, openFrameworks
//
// Author : Atsushi Sakai (GridMapSample.m)
//          dotchang
// 
// Copyright (c) : 2014 Atsushi Sakai
//                 2017 dotchang
//
// Licence : GPL Software License Agreement
// --------------------------------------------------------------


#include "ofVec2f.h"
#include "ofVec3f.h"
#include "ofPixels.h"
#include "ofImage.h"
#include "ofMatrix2x2.h" // https://github.com/naokiring/ofMatrix2x2

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

class ObservationData
{
public:
	float range;
	float rad; // angle
	float x;
	float y;
};

class Observation
{
public:
	Observation()
	{
		ANGLE_TICK = 10.f;
		MAX_RANGE = 50.f;
		MIN_RANGE = 5.f;

		update();
	}

	void update()
	{
		data.clear();
		for (float angle=0.f; angle<360.f; angle+=ANGLE_TICK) {
			ObservationData od;
			od.range = ofRandom(MAX_RANGE - MIN_RANGE) + MIN_RANGE;
			od.rad = ofDegToRad(angle);
			od.x = od.range * cos(od.rad);
			od.y = od.range * sin(od.rad);
			data.push_back(od);
		}
	}

	std::vector<ObservationData>& GetObservationData(){ return data; }

	float getAngleTick(){ return ANGLE_TICK; }

protected:
	std::vector<ObservationData> data; // 観測値[range, angle x y; ...]
	float ANGLE_TICK; // スキャンレーザの角度解像度[deg]
	float MAX_RANGE; // スキャンレーザの最大観測距離[m]
	float MIN_RANGE; // スキャンレーザの最小観測距離[m]
};

class GridData
{
public:
	float range;
	float angle;
	ofVec2f gxy;
	int ig;
};

class RayData
{
public:
	float minAngle; // 最小角度
	float maxAngle; // 最大角度
	std::vector<GridData> grid; // 中に入るgridのデータ
};

class GridMap
{
public:
	ofVec2f CENTER;
	int RESO;
	int WIDTH;
	int HEIGHT;
	int nGrid;
	ofFloatPixels data;

	std::vector<RayData> precasting;
};

class ofxGridMapSample
{
public:
	ofxGridMapSample(){}
	~ofxGridMapSample(){}

	void setup()
	{
		gm.CENTER = ofVec2f(0,0);
		gm.RESO = 1;
		gm.WIDTH = 100;
		gm.HEIGHT = 100;
		gm.nGrid = gm.WIDTH * gm.HEIGHT;

		// dataには物体の存在確率[0,1]を格納する
		// 初期値は不明なので0.5とする
		gm.data.allocate(gm.WIDTH, gm.HEIGHT, OF_IMAGE_GRAYSCALE);
		gm.data.setColor(0.5f);

		pose = ofVec3f(0.f, 0.f, 0.f); // ロボットの現在姿勢[x, y, yaw]

		z.update(); // センサの観測点の取得
	}

	void update()
	{
	}

	void draw()
	{
		if(image.isAllocated()){
			image.draw(-gm.WIDTH/2, -gm.HEIGHT/2);
		}
	}
	
	// シミュレーション1: End Point Update
	void simulation1()
	{
		// 初期値は不明なので0.5とする
		gm.data.setColor(0.5f);
		
		HitGridUpdate(gm, z);

		image.setFromPixels(gm.data);
		image.save("figure1.png");
	}
	
	// シミュレーション2: Likelihood Update
	void simulation2()
	{
		// 初期値は不明なので0.5とする
		gm.data.setColor(0.5f);
		
		LikelihoodUpdate(gm, z);

		image.setFromPixels(gm.data);
		image.save("figure2.png");
	}

	// シミュレーション3: Ray Casting Update
	void simulation3()
	{
		// 初期値は不明なので0.5とする
		gm.data.setColor(0.5f);
		
		RayCastingUpdate(gm, z);

		image.setFromPixels(gm.data);
		image.save("figure3.png");
	}

	// レイキャスティングによるGridの更新
	void RayCastingUpdate(GridMap& l_gm, Observation& l_z)
	{
		// 事前レイキャスティングモデルの作成
		PreCasting(l_gm, l_z.getAngleTick());

		// 事前レイキャスティングモデルに従ってグリッドの確率の更新
		std::vector<ObservationData>& od = l_z.GetObservationData();
		int rayId = -1;
		for(int iz=0; iz<od.size(); iz++){ // それぞれの観測点に対して
			float range = od[iz].range;
			rayId = rayId + 1; // レイキャスティングクラスタにおけるデータID
			// 各観測点はそれぞれのクラスタから取得できるとする。

			// クラスタ内の各gridのデータからビームモデルによるupdate
			for(int ir=0; ir<l_gm.precasting[rayId].grid.size(); ir++){
				float grange = l_gm.precasting[rayId].grid[ir].range;
				int gid = l_gm.precasting[rayId].grid[ir].ig;
				
				if(grange < (range-(float)l_gm.RESO/2.f)) // free
					l_gm.data[gid] = 0.f;
				else if(grange < (range+(float)l_gm.RESO/2.f)) // hit
					l_gm.data[gid] = 1.f;
				// それ以上の距離のgridはunknownなので何もしない
				else break;
			}
		}
	}

	// 事前レイキャスティングモデルの作成
	void PreCasting(GridMap& l_gm, float angleTick)
	{
		l_gm.precasting.clear();
		// 各角度について対応するグリッドを追加していく
		for(float ia=(0.f-angleTick/2.f); ia<(360.f+angleTick/2.f); ia+=angleTick){
			// 角度範囲の保存
			RayData ray;
			ray.minAngle = ia;
			ray.maxAngle = ia+angleTick;
			ray.grid.clear(); // 角度範囲に入ったグリッドのデータ
			for(int ig=0; ig<l_gm.nGrid; ig++){
				// 各グリッドのxy値を取得
				ofVec2f gxy = GetXYFromDataIndex(ig, l_gm);
				float range = gxy.length();
				float angle = atan2(gxy.y, gxy.x);
				if(angle<0.f) // [0 360]度に変換
					angle=angle + 2.f*PI;
				if(ray.minAngle<=ofRadToDeg(angle) && ray.maxAngle>=ofRadToDeg(angle)){
					GridData gd;
					gd.range = range;
					gd.angle = angle;
					gd.gxy = gxy;
					gd.ig = ig;
					ray.grid.push_back(gd);
				}
			}
			// rangeの値でソーティングしておく
			if(!ray.grid.empty()){
				sort(ray.grid.begin(), ray.grid.end(),
					[](const GridData& x, const GridData& y) { return x.range < y.range; });
			}
			l_gm.precasting.push_back(ray);
		}
	}

	// 観測点がヒットしたグリッドの確率を１にする関数
	void HitGridUpdate(GridMap& l_gm, Observation& l_z)
	{
		std::vector<ObservationData>& od = l_z.GetObservationData();
		for(int iz=0; iz<od.size(); iz++)
		{
			float zx = od[iz].x;
			float zy = od[iz].y;
			int ind = GetDBIndexFromXY(zx, zy, gm);
			l_gm.data[ind] = 1.f;
		}
	}

	// 尤度場のGridMapを作る関数
	void LikelihoodUpdate(GridMap& l_gm, Observation& l_z)
	{
		std::vector<ObservationData>& od = l_z.GetObservationData();
		for(int ig=0; ig<l_gm.nGrid; ig++)
		{
			ofVec2f gxy = GetXYFromDataIndex(ig, l_gm); // それぞれのグリッドxyインデックスを取得
			ofVec2f zxy = FindNearest(gxy, l_z); // 最近傍の観測値の取得
			float p = GaussLikelihood(gxy, zxy); // ガウシアン尤度の計算
			l_gm.data[ig]=p*10.f; // グリッドへの格納
		}
	}

private:
	// ガウス分布の尤度を計算する関数
	float GaussLikelihood(ofVec2f gxy, ofVec2f zxy)
	{
		float sigma_2 = 3.f;
		float sigma_cov = 0.f;

		//(Sigma*2*PI).determinant()^(-0.5)*exp(-0.5*(gxy-zxy)*inv(Sigma)*(gxy-zxy)); // 多変量正規分布
		ofMatrix2x2 Sigma = ofMatrix2x2(sigma_2, sigma_cov, sigma_cov, sigma_2); // 正定値行列		
		ofMatrix2x2 invSigma = Sigma.inverse(Sigma);
		ofVec2f d = gxy-zxy; // 行ベクトル
		float e = exp(-0.5f*((d.x*invSigma.a + d.y*invSigma.c)*d.x+(d.x*invSigma.b+d.y*invSigma.d)*d.y));
		return (1.f/sqrt((Sigma.determinant()*powf(2.f*PI,2.f))))*e;
	}

	// Gridのデータインデックスから、そのグリッドのx,y座標を取得する関数
	ofVec2f GetXYFromDataIndex(int ig, GridMap& l_gm)
	{
		// x, yインデックスの取得
		int indy = (int)fmod((float)ig, (float)l_gm.WIDTH) - 1;
		int indx = fix((float)ig/(float)l_gm.WIDTH);

		int x = GetXYPosition(indx, l_gm.WIDTH, gm.RESO);
		int y = GetXYPosition(indy, l_gm.HEIGHT, gm.RESO);
		return ofVec2f(x, y);
	} 

	// x-yインデックスの値から、位置を取得する関数
	int GetXYPosition(int index, int width, int resolution)
	{
		return resolution*(index - width/2)+resolution/2;
	}

	// ある座標値xyに一番近いzの値を返す関数
	ofVec2f FindNearest(ofVec2f xy, Observation& l_z)
	{
		// すべてのzとxyの差を計算
		std::vector<ofVec2f> d;
		std::vector<ObservationData>& od = l_z.GetObservationData();
		for(int i=0; i<od.size(); i++){
			d.push_back(xy - ofVec2f(od[i].x, od[i].y));
		}

		// ノルム距離の最小値のインデックスを取得
		float min = FLT_MAX; // 最小値
		int minid = 0;
		for(int id=0; id<d.size(); id++){
			float nd = d[id].lengthSquared();
			if(min>nd){
				min=nd;
				minid = id;
			}
		}
		return ofVec2f(od[minid].x, od[minid].y);
	}

protected:
	// xyの値から対応するグリッドのベクトルインデックスを取得する関数
	int GetDBIndexFromXY(float x, float y, GridMap& l_gm)
	{
		// グリッドマップ中心座標系に変換
		float gx = x-l_gm.CENTER.x;
		float gy = y-l_gm.CENTER.y;

		// グリッドマップのインデックスを取得
		int indX = GetXYIndex(x, l_gm.WIDTH, l_gm.RESO); // x軸方向のindex
		int indY = GetXYIndex(y, l_gm.HEIGHT, l_gm.RESO); // y軸方向のindex
		int ind = GetDBIndexFromIndex(indX, indY, l_gm); // DBのインデックス

		return ind;
	}
	
	// x-y方向の値から、インデックスを取得する関数
	int GetXYIndex(float position, int width, int resolution)
	{	
		return fix((position/(float)resolution+(float)width/2.f));
	}

	// データベクトルのインデックスを計算する関数
	// -1は無効インデックスを表す。
	int GetDBIndexFromIndex(int indX, int indY, GridMap& l_gm)
	{
		// 全体インデックス
		int ind = l_gm.WIDTH*indX + indY;
		// X方向のインデックスチェック
		if(indX>=l_gm.WIDTH)
			ind=-1;
		// Y方向のインデックスチェック
		else if(indY>=l_gm.HEIGHT)
			ind=-1;
		// 全体のインデックスチェック
		else if(ind>=l_gm.nGrid)
			ind=-1;
		return ind;
	}

private:
	inline int fix(float A)
	{
		int sign = A<0.f ? -1 : 1;
		return floor(A) * sign;
	}

protected:
	GridMap gm;
	ofVec3f pose;
	Observation z;

	ofImage image;
};