ICP （ Iterative Closest Point ）演算法實作（ C 語言）

ICP （ Iterative Closest Point ）演算法是用來將兩群點集合（ point set ）
對齊的演算法，這是我在去年修資工系王傑智老師的課「機器人知覺
與學習」學到的。


簡單解釋：

假設有兩個點集合 P 和 Q ，其中 P 和 Q 裡面的點數目未必一樣。
如果我知道 P 裡面的每個點都能在 Q 裡面找到相對應的點，另外我
假設經過一次平移和旋轉，可以讓 P 和 Q 之間的誤差最小（誤差的
算法：加總 P 裡面的所有點和 Q 中相對應的點之間的距離），那麼
一定可以經由計算得知所需的平移和旋轉量是多少。

##CONTINUE##不過，以上的敘述在現實應用上有個大問題，就是怎樣得知 P 、 Q
裡面的點的對應關係呢？

在這個演算法中，就是武斷的認定 Q 中「最近距離」的點就是對應
點，接著平移旋轉後，用新的點集合 P' 再迭代一次，得到 P'' ，
就這樣迭代到誤差收斂為止。

以下是我自己寫的 C 程式碼，因為學弟想要把它實驗在 DSP 裡面，
用 C 語言是比較直接的作法。

// icp.h

#ifndef _ICP_H
#define _ICP_H

typedef struct Point2D
{
	double x;
	double y;
} Point2D;

double p2p_length(Point2D p, Point2D q);

Point2D find_closest_point(Point2D target, Point2D* refPset, int size);

void icp_step(double* dx, double* dy, double* dth, Point2D* tgtPset, int t_size, Point2D* refPset, int r_size);

double icp(double* dx, double* dy, double* dth, Point2D* tgtPset, int t_size, Point2D* refPset, int r_size);

#endif

// icp.c

#include " icp.h "
#include < math.h >
#include < stdlib.h >
#include < stdio.h >

double p2p_length(Point2D p, Point2D q)
{
	return sqrt(pow(p.x - q.x, 2)+pow(p.y - q.y, 2));
}

Point2D find_closest_point(Point2D target, Point2D* refPset, int size)
{
	Point2D ret;
	double min_distance;
	int i;

	ret = refPset[0];
	min_distance = p2p_length(target, refPset[0]);
	for(i=1; i < size; i++){
		if(p2p_length(target, refPset[i]) < min_distance){
			ret = refPset[i];
			min_distance = p2p_length(target, refPset[i]);
		}
	}
	return ret;
}

void icp_step(double* dx, double* dy, double* dth, Point2D* tgtPset, int t_size, Point2D* refPset, int r_size)
{
	double sum_bx_times_ay;
	double sum_by_times_ax;
	double sum_bx_times_ax;
	double sum_by_times_ay;
	double mean_bx;
	double mean_by;
	double mean_ax;
	double mean_ay;

	Point2D* a = (Point2D*)malloc(t_size*sizeof(Point2D));
	// accroding to paper, a is the closest point set
	// according to paper, b is the target point set (tgtPset in this function)

	int i;
	
	// find closest point set
	for(i=0; i < t_size; i++){
		a[i] = find_closest_point(tgtPset[i], refPset, r_size);
	}
	
	// initialize dx, dy, dth
	*dx = 0;
	*dy = 0;
	*dth = 0;
	mean_bx = 0;
	mean_by = 0;
	mean_ax = 0;
	mean_ay = 0;

	for(i=0; i < t_size; i++){
			mean_bx += tgtPset[i].x;
			mean_by += tgtPset[i].y;
			mean_ax += a[i].x;
			mean_ay += a[i].y;
	}
	
	mean_bx = mean_bx / t_size;
	mean_by = mean_by / t_size;
	mean_ax = mean_ax / t_size;
	mean_ay = mean_ay / t_size;

	sum_bx_times_ay = 0;
	sum_by_times_ax = 0;
	sum_bx_times_ax = 0;
	sum_by_times_ay = 0;

	for(i=0; i < t_size; i++){
		sum_bx_times_ay += (tgtPset[i].x - mean_bx)*(a[i].y - mean_ay);
		sum_by_times_ax += (tgtPset[i].y - mean_by)*(a[i].x - mean_ax);
		sum_bx_times_ax += (tgtPset[i].x - mean_bx)*(a[i].x - mean_ax);
		sum_by_times_ay += (tgtPset[i].y - mean_by)*(a[i].y - mean_ay);
	}

	*dth = atan2(sum_bx_times_ay - sum_by_times_ax, sum_bx_times_ax + sum_by_times_ay);
	*dx = mean_ax - ((mean_bx * cos(*dth)) - (mean_by * sin(*dth)));
	*dy = mean_ay - ((mean_bx * sin(*dth)) + (mean_by * cos(*dth)));

}

double icp(double* dx, double* dy, double* dth, Point2D* tgtPset, int t_size, Point2D* refPset, int r_size)
{
	double error = 0;
	double pre_err = 0;
	double step_dx, step_dy, step_dth;
	double tmp_x, tmp_y;
	Point2D* step_tgtPset = (Point2D*)malloc(t_size*sizeof(Point2D));

	int i, iter_count;

	for(i=0; i < t_size; i++){
		step_tgtPset[i] = tgtPset[i];
	}
	
	*dx = 0;
	*dy = 0;
	*dth = 0;
	iter_count = 0;
	do{
		iter_count++;

		icp_step(&step_dx, &step_dy, &step_dth, step_tgtPset, t_size, refPset, r_size);
		
		pre_err = error;
		error = 0;
		for(i=0; i < t_size; i++){
			tmp_x = (step_tgtPset[i].x * cos(step_dth)) - (step_tgtPset[i].y * sin(step_dth)) + (step_dx);
			tmp_y = (step_tgtPset[i].x * sin(step_dth)) + (step_tgtPset[i].y * cos(step_dth)) + (step_dy);
			step_tgtPset[i].x = tmp_x;
			step_tgtPset[i].y = tmp_y;

			error += (pow(step_tgtPset[i].x - refPset[i].x, 2) + pow(step_tgtPset[i].y - refPset[i].y, 2));
		}
		error /= t_size;

		*dx += step_dx;
		*dy += step_dy;
		*dth += step_dth;

		// just for debug
		printf("iter[%d]: err = %lf, dx = %lf, dy = %lf, dth = %lf\n", iter_count, error, *dx, *dy, *dth);
		system("pause");
		//
	} while(fabs(error - pre_err) > 0.00001);

	return error;
}

// test_main.c
// just for debug and test

#include "icp.h"
#include < stdlib.h >
#include < math.h >
#include < stdio.h >

double randomNumber(int hi)  //the correct random number generator for [0,hi]
{
	// scale in range [0,1)
	double scale;
	scale = (double)rand();
	scale /= (double)(RAND_MAX);
	// return range [0,hi]
	return (scale*hi); // implicit cast and truncation in return
}
void GenerateRandomPointSet(Point2D* pSet, int p_size)
{
	int i;
	for(i=0; i < p_size; i++){
		pSet[i].x = randomNumber(1000);
		pSet[i].y = randomNumber(1000);
	}
}

void MovePointSet(Point2D* pSet, int p_size, double dx, double dy)
{
	int i;
	for(i=0; i < p_size; i++){
		pSet[i].x += dx;
		pSet[i].y += dy;
	}
}

void RotatePointSet(Point2D* pSet, int p_size, double dth)
{
	int i;
	double tmp_x, tmp_y;
	for(i=0; i < p_size; i++){
		tmp_x = pSet[i].x * cos(dth) - pSet[i].y * sin(dth);
		tmp_y = pSet[i].x * sin(dth) + pSet[i].y * cos(dth);
		pSet[i].x = tmp_x;
		pSet[i].y = tmp_y;
	}
}

int main()
{
	Point2D tgtPset[1000];
	Point2D refPset[1000];
	double error;
	double dx, dy, dth;

	int i;

	GenerateRandomPointSet(tgtPset, 1000);

	for(i=0; i < 1000; i++){
		refPset[i] = tgtPset[i];
	}

	MovePointSet(tgtPset, 1000, 10, 12);
	RotatePointSet(tgtPset, 1000, 0.02);

	error = icp(&dx, &dy, &dth, tgtPset, 1000, refPset, 1000);

	printf("final result: err = %lf, dx = %lf, dy = %lf, dth = %lf\n", error, dx, dy, dth);
	system("pause");
	return 0;
}

最後補充幾點值得注意的事情：
1. 這程式碼本身是最單純的 ICP ，完全沒有任何別的加速技巧。
2. 所謂的加速技巧主要有二，其一是如何 down sampling ，因為ICP 需要的運算量很大，如果不做 down sampling ，需要 real time 的 ICP 大概就掛了。
3. 另一加速技巧在於改善點集合的資料結構，其中最有名的適合 ICP 演算法的資料結構是 K-D tree ，因為它的資料格式和距離有關，用 K-D tree 可以在尋找對應點的時候大幅降低運算時間。