ofxICPSample.h

#pragma once

// --------------------------------------------------------------
//
// File : ofxICPSample.h
//
// Description : Sample code to estimate relative motion with
//   		 Iterative Closest Point (ICP) algorithm 
//
// Environment : c++, openFrameworks
//
// Author : Atsushi Sakai (ICPSample.m)
//          dotchang
// 
// Copyright (c) : 2014 Atsushi Sakai
//                 2017 dotchang
//
// Licence : Modified BSD Software License Agreement
// --------------------------------------------------------------

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

#include <vector>
#include <random>

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

	void setup()
	{
		// Simulation Parameters
		nPoint = 100; // レーザ点の数
		float fieldLength = 5.f; // 点をばら撒く最大距離
		motion = ofVec3f(0.5f, 0.f, 10.f); // 真の移動量[並進x[m],並進y[m],回転[deg]]
		float transitionSigma = 0.01; // 並進方向の移動誤差標準偏差[m]
		float thetaSigma = 1.f; // 回転方向の誤差標準偏差[deg]

		// 点をランダムでばら撒く(t-1の時の点群)
		data1.reserve(nPoint);
		for (int i = 0; i < nPoint; i++) {
			data1.emplace_back(ofVec2f(fieldLength*ofRandom(1.f) - fieldLength / 2.f, fieldLength*ofRandom(1.f) - fieldLength / 2.f));
		}

		// data2= data1を移動させる & ノイズ付加
		// 回転方向　& ノイズ付加
		float theta = ofDegToRad(motion.z) + ofDegToRad(thetaSigma) * ofRandom(1.f);
		// 並進ベクトル & ノイズ付加
		std::vector<ofVec2f> t;
		t.reserve(nPoint);
		std::random_device seed_gen;
		std::default_random_engine engine(seed_gen());
		std::normal_distribution<> dist(0.f, 1.f); // 標準正規分布: 平均0.0、標準偏差1.0で分布させる
		for (int i = 0; i < nPoint; i++) {
			ofVec2f randn(dist(engine), dist(engine)); // 正規分布で乱数を生成する
			t.emplace_back(ofVec2f(motion.x, motion.y) + transitionSigma * randn);
		}
		// 回転行列の作成
		ofMatrix2x2 A(cos(theta), sin(theta), -sin(theta), cos(theta));
		// data1を移動させてdata2を作る
		data2.reserve(nPoint);
		for (int i = 0; i < nPoint; i++) {
			data2.emplace_back(t[i] + A*data1[i]);
		}

		// ICPMatchingの準備
		preError = 0.f; // 一つ前のイタレーションのerror値
		dError = 1000.f; // エラー値の差分
		R = ofMatrix2x2(1.f, 0.f, 0.f, 1.f); // 回転行列
		T = ofVec2f(0.f, 0.f); // 並進ベクトル
		count = -1; // ループカウンタ

		tmp_data1 = data1; // 生データの上書き保護用
	}

	void iteration()
	{
		static bool solved = false;

		// ICPアルゴリズム data2とdata1のMatching
		// R:回転行列 t:並進ベクトル
		// [R, T]=icp(data1,data2)
		if (!solved) {
			solved = ICPMatching(R, T, data2, tmp_data1);
			// 結果の表示
			std::cout << "True Motion [m m deg]:" << motion << endl;
			float theta = acos(R.a) / PI * 180.f;
			ofVec3f Est(T.x, T.y, theta);
			std::cout << "Estimated Motion [m m deg]:" << Est << std::endl;
			ofVec3f Error = Est - motion;
			std::cout << "Error [m m deg]:" << Error << std::endl;
		}
	}

	void update()
	{
		iteration();
	}

	void draw()
	{
		ofPushStyle();
		ofPushMatrix();
		ofDrawGrid(0.5, 8);
		ofSetColor(ofColor::cornflowerBlue);
		for (std::vector<ofVec2f>::iterator it = data1.begin(); it != data1.end(); ++it) {
			ofDrawCircle(*it, 0.05f);
		}
		ofSetColor(ofColor::lightSeaGreen);
		for (std::vector<ofVec2f>::iterator it = data2.begin(); it != data2.end(); ++it) {
			ofDrawCircle(*it, 0.05f);
		}
		ofSetColor(ofColor::mediumVioletRed);
		for (int i = 0; i < nPoint; i++) {
			ofVec2f z = T + R*data1[i];
			ofDrawCircle(z, 0.05f);
		}
		ofPopMatrix();
		ofPopStyle();
	}

protected:
	// ICPアルゴリズムによる、並進ベクトルと回転行列の計算を実施する関数
	bool ICPMatching(ofMatrix2x2& l_R, ofVec2f& t, std::vector<ofVec2f>& l_data1, std::vector<ofVec2f>& l_data2)
	{
		// data1 = [x(t)1 x(t)2 x(t)3 ...]
		// data2 = [x(t+1)1 x(t+1)2 x(t+1)3 ...]
		// x = [x y z]'

		// ICPパラメータ
		float EPS = 0.0001f; // 収束判定値
		int maxIter = 100; // 最大イタレーション数

		if (!(dError < EPS)) { // while (!(dError < EPS)) {
			count = count + 1;

			std::vector<int> ii;
			float error;
			FindNearestPoint(ii, error, l_data1, l_data2); // 最近傍点探索

			ofMatrix2x2 R1;
			ofVec2f t1;
			SVDMotionEstimation(R1, t1, l_data1, l_data2, ii); // 特異値分解による移動量推定
			// 計算したRとtで点群とRとtの値を更新
			for (int i = 0; i < l_data2.size(); i++) {
				l_data2[i] = R1*l_data2[i] + t1;
			}
			l_R = R1*l_R;
			t = R1*t + t1;

			dError = fabs(preError - error); // エラーの改善量
			preError = error; // 一つ前のエラーの総和値を保存

			if (count >= maxIter) { // 収束しなかった
				std::cout << "Max Iteration" << std::endl;
				return true;
			}
			std::cout << "Convergence:" << count << std::endl;
			return false;
		}
		return true;
	}

	// data2に対するdata1の最近傍点のインデックスを計算する関数
	void FindNearestPoint(std::vector<int>& index, float& l_error, std::vector<ofVec2f>& l_data1, std::vector<ofVec2f>& l_data2)
	{
		int m1 = l_data1.size();
		int m2 = l_data2.size();
		index.clear();
		l_error = 0.f;

		for (int i = 0; i < m1; i++) {
			float dist = FLT_MAX;
			int ii = 0;
			for (int j = 0; j < m2; j++) {
				float d = (l_data2[j] - l_data1[i]).lengthSquared();
				if (d < dist) {
					dist = d;
					ii = j;
				}
			}
			index.push_back(ii);
			l_error += sqrt(dist);
		}
	}

	// 特異値分解法による並進ベクトルと、回転行列の計算
	void SVDMotionEstimation(ofMatrix2x2& l_R, ofVec2f& l_t, std::vector<ofVec2f>& l_data1, std::vector<ofVec2f>& l_data2, std::vector<int> index)
	{
		// 各点群の重心の計算
		std::vector<ofVec2f> M(l_data1);
		ofVec2f mm = std::accumulate(M.begin(), M.end(), ofVec2f(0.f, 0.f)) / (float)M.size();
		std::vector<ofVec2f> S;
		S.reserve(index.size());
		for (int i = 0; i < index.size(); i++) {
			S.emplace_back(l_data2[i]);
		}
		ofVec2f ms = std::accumulate(S.begin(), S.end(), ofVec2f(0.f, 0.f)) / (float)S.size();

		// 各点群を重心中心の座標系に変換
		std::vector<ofVec2f> Sshifted;
		Sshifted.reserve(S.size());
		for (int i = 0; i < S.size(); i++) {
			Sshifted.emplace_back(S[i] - ms);
		}
		std::vector<ofVec2f> Mshifted;
		Mshifted.reserve(M.size());
		for (int i = 0; i < M.size(); i++) {
			Mshifted.emplace_back(M[i] - mm);
		}
		ofMatrix2x2 W(0.f, 0.f, 0.f, 0.f);
		for (int i = 0; i < S.size(); i++) {
			W.a += Sshifted[i].x * Mshifted[i].x;
			W.b += Sshifted[i].x * Mshifted[i].y;
			W.c += Sshifted[i].y * Mshifted[i].x;
			W.d += Sshifted[i].y * Mshifted[i].y;
		}

		ofMatrix2x2 U, V;
		ofVec2f A;
		svd22(W, U, A, V); // 特異値分解

		ofMatrix2x2 Vt(V);
		Vt.transpose();
		l_R = U * Vt;
		l_R.transpose();
		l_t = mm - l_R*ms;
	}

private:
	// from here: https ://scicomp.stackexchange.com/questions/8899/robust-algorithm-for-2x2-svd
	void svd22(ofMatrix2x2 a, ofMatrix2x2& u, ofVec2f& s, ofMatrix2x2& v) {
		s.x = (sqrt(powf(a.a - a.d, 2.f) + powf(a.b + a.c, 2.f)) + sqrt(powf(a.a + a.d, 2.f) + powf(a.b - a.c, 2.f))) / 2.f;
		s.y = fabs(s.x - sqrt(pow(a.a - a.d, 2.f) + pow(a.b + a.c, 2.f)));
		v.c = (s.x > s.y) ? sin((atan2(2.f * (a.a * a.b + a.c * a.d), a.a * a.a - a.b * a.b + a.c * a.c - a.d * a.d)) / 2.f) : 0.f;
		v.a = sqrt(1.f - v.c * v.c);
		v.b = -v.c;
		v.d = v.a;
		u.a = (s.x != 0.f) ? (a.a * v.a + a.b * v.c) / s.x : 1.f;
		u.c = (s.x != 0.f) ? (a.c * v.a + a.d * v.c) / s.x : 0.f;
		u.b = (s.y != 0.f) ? (a.a * v.b + a.b * v.d) / s.y : -u.c;
		u.d = (s.y != 0.f) ? (a.c * v.b + a.d * v.d) / s.y : u.a;
	}

protected:
	int nPoint;
	std::vector<ofVec2f> data1, data2;
	ofVec3f motion;

	float preError; // 一つ前のイタレーションのerror値
	float dError; // エラー値の差分
	ofMatrix2x2 R; // 回転行列
	ofVec2f T; // 並進ベクトル
	int count; // ループカウンタ

private:
	std::vector<ofVec2f> tmp_data1;
};