ICP算法二维实现

ICP算法实现自动配准，基于opencv及vs实现，是二维的 ICP算法实现自动配准，基于opencv及vs实现，是二维的

// icp.cpp : 定义控制台应用程序的入口点。
//

#include “stdafx.h“
#include “icp.h“

#define M_PI 3.1415926

Point3DSet *GetPoint3DSet（unsigned int *dset int number） {
	int i;
	double step = M_PI*3/1536;

	#define POINT_NUMBER 682
	Point3DSet *pset = （Point3DSet*）malloc（sizeof（Point3DSet））;
	pset->number = POINT_NUMBER;
	pset->point = （Point3D*）malloc（POINT_NUMBER*sizeof（Point3D））;
	pset->invalid_point_number = 0;

	#define MIN_DISTANCE_MM 20
	/* -135 to +135 degree */
	double theta;
	for （i=44; i<384; i++） {
		if （dset[i] < MIN_DISTANCE_MM） {
			pset->point[i-44].x = 0;
			pset->point[i-44].y = 0;
			pset->point[i-44].z = 0;
			pset->point[i-44].r = 0;
			pset->point[i-44].theta = 0;
			pset->invalid_point_number ++;
			//printf（“uninitialized point #%d\n“ （i-44））;
			continue;
		}
		theta = （-M_PI*3/4） + i*step;
		pset->point[i-44].x = cos（theta）*dset[i];
		pset->point[i-44].y = sin（theta）*dset[i];
		pset->point[i-44].z = 0;
		pset->point[i-44].r = dset[i];
		pset->point[i-44].theta = theta;
	}
	/* [0135） degree */
	for （i=384; i<=725; i++） {
		if （dset[i] < MIN_DISTANCE_MM） {
			pset->point[i-44].x =0;
			pset->point[i-44].y =0;
			pset->point[i-44].z =0;
			pset->point[i-44].r = 0;
			pset->point[i-44].theta = 0;
			pset->invalid_point_number ++;
			//printf（“uninitialized point #%d\n“ （i-44））;
			continue;
		}
		theta = （i-384）*step;
		pset->point[i-44].x = cos（theta）*dset[i];
		pset->point[i-44].y = sin（theta）*dset[i];
		pset->point[i-44].z =0;
		pset->point[i-44].r = dset[i];
		pset->point[i-44].theta = theta;
	}
	//printf（“pset invalid point number is %d\n“ pset->invalid_point_number）;
	return pset;
}

void PlotTwoPoint3DSet（Point3DSet *dset Point3DSet *mset） {
	#define DIAMETER_PIXEL 300
	#define MAX_DISTANCE_MM 4094

	IplImage *mapImg;
	mapImg = cvCreateImage（ cvSize（DIAMETER_PIXEL*2DIAMETER_PIXEL*2） IPL_DEPTH_8U 3）;

	/* Reset the map */
	/*for （i=0; i < mapImg->width; i++） {
		for （j=0; j < mapImg->height; j++） {
			cvSet2D（mapImg j i cvScalar（0000））; } }*/

	double x y;
	int i;
	for （i=0; i < dset->number; i++） {
		if （（dset->point[i].x ==0） && （dset->point[i].y == 0）） {continue;}
		x = dset->point[i].x*DIAMETER_PIXEL/MAX_DISTANCE_MM;
		y = dset->point[i].y*DIAMETER_PIXEL/MAX_DISTANCE_MM;
		x += DIAMETER_PIXEL;
		y += DIAMETER_PIXEL;
		//printf（“（xy） = （%f%f）\n“ x y）;
		cvSet2D（mapImg y x cvScalar（255000））;
	}
	for （i=0; i < mset->number; i++） {
		if （（mset->point[i].x ==0） && （mset->point[i].y == 0）） {continue;}
		x = mset->point[i].x*DIAMETER_PIXEL/MAX_DISTANCE_MM;
		y = mset->point[i].y*DIAMETER_PIXEL/MAX_DISTANCE_MM;
		x += DIAMETER_PIXEL;
		y += DIAMETER_PIXEL;
		//printf（“（xy） = （%f%f）\n“ x y）;
		cvSet2D（mapImg y x cvScalar（002550））;
	}
	/* Plot Center */
	cvSet2D（mapImg DIAMETER_PIXEL DIAMETER_PIXEL cvScalar（025500））;
	cvSet2D（mapImg DIAMETER_PIXEL+1 DIAMETER_PIXEL cvScalar（025500））;
	cvSet2D（mapImg

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----

     文件      90624  2018-05-29 16:29  icp\Debug\icp.exe

     文件     588136  2018-05-29 16:29  icp\Debug\icp.ilk

     文件    1715200  2018-05-29 16:29  icp\Debug\icp.pdb

     文件       2622  2018-05-29 16:29  icp\icp\Debug\cl.command.1.tlog

     文件      21878  2018-05-29 16:29  icp\icp\Debug\CL.read.1.tlog

     文件       1152  2018-05-29 16:29  icp\icp\Debug\CL.write.1.tlog

     文件        565  2018-05-29 15:27  icp\icp\Debug\icp.Build.CppClean.log

     文件        406  2018-05-29 15:39  icp\icp\Debug\icp.exe.embed.manifest

     文件        472  2018-05-29 15:39  icp\icp\Debug\icp.exe.embed.manifest.res

     文件        381  2018-05-29 16:29  icp\icp\Debug\icp.exe.intermediate.manifest

     文件         63  2018-05-29 16:29  icp\icp\Debug\icp.lastbuildstate

     文件       8711  2018-05-29 16:29  icp\icp\Debug\icp.log

     文件     331500  2018-05-29 16:29  icp\icp\Debug\icp.obj

     文件    1179648  2018-05-29 15:27  icp\icp\Debug\icp.pch

     文件        713  2018-05-29 16:29  icp\icp\Debug\icp.vcxprojResolveAssemblyReference.cache

     文件          0  2018-05-29 15:27  icp\icp\Debug\icp.write.1.tlog

     文件        196  2018-05-29 15:39  icp\icp\Debug\icp_manifest.rc

     文件          2  2018-05-29 16:29  icp\icp\Debug\link-cvtres.read.1.tlog

     文件          2  2018-05-29 16:29  icp\icp\Debug\link-cvtres.write.1.tlog

     文件          2  2018-05-29 16:29  icp\icp\Debug\link.12420-cvtres.read.1.tlog

     文件          2  2018-05-29 16:29  icp\icp\Debug\link.12420-cvtres.write.1.tlog

     文件          2  2018-05-29 16:29  icp\icp\Debug\link.12420.read.1.tlog

     文件          2  2018-05-29 16:29  icp\icp\Debug\link.12420.write.1.tlog

     文件          2  2018-05-29 16:29  icp\icp\Debug\link.2516-cvtres.read.1.tlog

     文件          2  2018-05-29 16:29  icp\icp\Debug\link.2516-cvtres.write.1.tlog

     文件          2  2018-05-29 16:29  icp\icp\Debug\link.2516.read.1.tlog

     文件          2  2018-05-29 16:29  icp\icp\Debug\link.2516.write.1.tlog

     文件       3228  2018-05-29 16:29  icp\icp\Debug\link.command.1.tlog

     文件       7468  2018-05-29 16:29  icp\icp\Debug\link.read.1.tlog

     文件        750  2018-05-29 16:29  icp\icp\Debug\link.write.1.tlog

............此处省略33个文件信息