PCL迭代最近点算法(ICP)的C++实现

21 篇文章 10 订阅

简介:
在维基百科中是这样介绍迭代最近点算法。迭代最近点(ICP)是一种用于最小化两点云之间差异的算法。给定P、Q两个点集,求解旋转矩阵R和平移矩阵T使得min{distance(P,Q)}.
C++算法流程图:
这里写图片描述
四元数求解方法
这里写图片描述
这里写图片描述
C++代码

/**

** Filename:icp.h

** Copyright (c) 2017-2018 

** Author:Rson

** Date:2018/04/03

** Modifier:

** Date:

** Description: 

**

** Version:

**/

#ifndef _ICP_H
#define _ICP_H

#include <vector>
#include <string>

#include <pcl\point_cloud.h>
#include <pcl\point_types.h>
#include <pcl\visualization\pcl_visualizer.h>

struct Vertex
{
	double coord[3];
};

struct Point
{
	float x;
	float y;
	float z;
};

class ICP
{
public:
	ICP();
	ICP(int controlnum=1000,double thre=0.01,int iter=100);
	virtual ~ICP();

	void readfile(std::string firstname, std::string secondname);
	void run();
	void writefile(std::string name);
	void showcloud(std::string firstname, std::string secondname,std::string thirdname);

private:
	void initransmat();//初始化旋转矩阵和平移矩阵
	void sample();//采样控制点
	double closest();//找出最近点并返回误差
	void getcenter();//获取两个控制点的中心
	void rmcontcenter();//移动两个控制点的中心
	void transform();//将四元数转换成矩阵并更新整个旋转矩阵
	void uprotate();// 更新变换矩阵
	void uptranslate();
	void updata();//更新控制点坐标
	void applyall();

private:
	double distance(Vertex a, Vertex b);
	void printTT();
	void printTR();
	

private:
	int conNum;//控制点数目
	int iterate;//迭代次数
	double threshold;//阈值
	std::vector<Vertex> VarrP;//起始点
	std::vector<Vertex> VarrQ;
	Vertex meanP;//控制点中心
	Vertex meanQ;
	Vertex *contP;//P控制点
	Vertex *contQ;
	Vertex *rmcoP;//移动后的控制点
	Vertex *rmcoQ;

	int *index; //在采样控制点和寻找相应的点索引时使用

	double TT[3];//平移向量
	double TR[3][3];//旋转矩阵
	double Rw[3][3];//旋转的步距
	double Tw[3];//平移的步距
	double quad[4];//四元数


};

#endif  /* ICP_H */
/**

** Filename:icp.cpp

** Copyright (c) 

** Author:Rson

** Date:2018/04/03

** Modifier:

** Date:

** Description:

**

** Version:

**/
#include "stdafx.h"
#include <iostream>
#include <sstream>
#include <fstream>
#include <cassert>
#include <math.h>
#include <time.h>
#include <newmat10/newmat.h>
#include <newmat10/newmatap.h>
#include "ICP.h"


ICP::ICP()
{
}

ICP::ICP(int controlnum, double thre, int iter)
{
	conNum = controlnum;
	threshold = thre;
	iterate = iter;

	contP = new Vertex[conNum];
	assert(contP != NULL);
	contQ = new Vertex[conNum];
	assert(contQ != NULL);
	rmcoP = new Vertex[conNum];
	assert(rmcoP != NULL);
	rmcoQ = new Vertex[conNum];
	assert(rmcoQ != NULL);
	index = new int[conNum];
	assert(index != NULL);
}

ICP::~ICP()
{
	delete[] contP;
	delete[] contQ;
	delete[] rmcoP;
	delete[] rmcoQ;
	delete[] index;
}

void ICP::readfile(std::string firstname, std::string secondname)
{
	std::cout << "读取两个点云文件!!" <<std:: endl;
	ifstream in;
	in.open(firstname.c_str(), std::ios::in);
	if (!in.is_open())
	{
		std::cout << "error open!" <<std:: endl;
		system("pause");
	}
	Vertex v;
	while(in>>v.coord[0]>>v.coord[1]>>v.coord[2])
	{
		VarrP.push_back(v);
	}
	std::cout << "点云A的大小:" << VarrP.size() << std::endl;
	in.close();

	//
	in.open(secondname.c_str(), std::ios::in);
	if (!in.is_open())
	{
		cout << "error open!" << endl;
		system("pause");
	}
	
	//Vertex v;
	while (in >> v.coord[0] >> v.coord[1] >> v.coord[2])
	{
		VarrQ.push_back(v);
	}
	std::cout << "点云B的大小:" << VarrQ.size() << std::endl;
	in.close();
}
void ICP::run()
{
	initransmat();
	sample();
	//
	double err = closest();
	std::cout << "初始误差:error = " << err << std::endl;
	//
	for (int i = 0; i<iterate; i++)
	{
		getcenter();
		rmcontcenter();
		transform();
		uprotate();
		uptranslate();
		updata();
		double newerr = closest();
		std::cout << "迭代次数 times = " << i << std::endl;
		std::cout << "error = " << newerr << std::endl;
		double delta = fabs(err - newerr) / conNum;
		std::cout << "delta = " << delta << std::endl;
		if (delta<threshold)
			break;
		err = newerr;

	}
	printTR();
	printTT();
	applyall();
}
void ICP::writefile(std::string name)
{
	ofstream outobj;
	outobj.open(name.c_str());
	//outobj << "# Geomagic Studio" << endl;
	int num = 1;
	for (vector<Vertex>::const_iterator p = VarrP.begin(); p != VarrP.end(); p++)
	{
		Vertex v;
		v = *p;
		outobj << v.coord[0] << " " << v.coord[1] << " " << v.coord[2] << endl;
		//outobj << "p " << num++ << endl;

		//outobj << "v " << v.coord[0] << " " << v.coord[1] << " " << v.coord[2] << endl;
		//outobj << "p " << num++ << endl;
	}
	//
	outobj.close();
}

//初始化变换矩阵
//    -
//    | 1.0  0.0 0.0 | 0.0  |
//	  |	0.0  1.0 0.0 | 0.0  |
//	  | 0.0  0.0 1.0 | 0.0  |
//    | -------------|----- |
//    | 0.0 0.0 0.0  | 1.0  |
void ICP::initransmat()//初始化变换矩阵
{
	std::cout << "初始化变换矩阵" << endl;
	for (int i = 0; i < 3; i++)
		TT[i] = 0;
	for (int i = 0; i < 3; i++)
	{
		for (int j = 0; j < 3; j++)
		{
			if (i != j)
				TR[i][j] = 0.0;
			else
				TR[i][j] = 1.0;
		}
	}
}


//随机选取控制点,并存储在contP中

void ICP::sample()
{
	std::cout<<"随机选取控制点,并存储在contP中"<<std::endl;
	int N = VarrP.size();
	bool *flag = new bool[N];
	assert(flag != NULL);
	for (int i = 0; i < N; i++)
		flag[i] = false;
	//随机选择一个控制点,并记录其索引
	srand((unsigned)time(NULL));
	for (int i = 0; i < conNum; i++)
	{
		while (true)
		{
			int sam = rand() % N;
			if (!flag[sam])
			{
				index[i] = sam;
				flag[sam] = true;
				break;
			}
		}
	}
	//cout<<"store control points into contP"<<endl;
	for (int i = 0; i<conNum; i++)
	{
		Vertex v = VarrP[index[i]];//
		for (int j = 0; j<3; j++)
		{
			contP[i].coord[j] = v.coord[j];
		}
	}
	delete[] flag;
}

//找出最近点并计算误差之和
double ICP::closest()
{
	//find closest points and error
	double error = 0.0;
	for (int i = 0; i < conNum; i++)
	{
		double mindist = 100.0;
		index[i] = 0;
		for (unsigned int j = 0; j < VarrQ.size(); j++)
		{
			double dist = distance(contP[i], VarrQ[j]);
			if (dist < mindist)
			{
				mindist = dist;
				index[i] = j;
			}
		}
		Vertex v = VarrQ[index[i]];
		for (int j = 0; j < 3; j++)
		{
			contQ[i].coord[j] = v.coord[j];
		}
		error += mindist;
	}
	return error;
}

//求取两个点云控制点的重心
void ICP::getcenter()
{
	//首先初始化重心坐标0,0,0
	for (int i = 0; i < 3; i++)
		meanP.coord[i] = 0.0;
	//求取每个分量之和
	for (int i = 0; i < conNum; i++)
	{
		for (int j = 0; j < 3; j++)
		{
			meanP.coord[j] += contP[i].coord[j];
		}
	}
	//求取平均值
	for (int i = 0; i < 3; i++)
		meanP.coord[i] = meanP.coord[i] / conNum;

	//
	for (int i = 0; i < 3; i++)
		meanQ.coord[i] = 0.0;
	//求取每个分量之和
	for (int i = 0; i < conNum; i++)
	{
		for (int j = 0; j < 3; j++)
		{
			meanQ.coord[j] += contQ[i].coord[j];
		}
	}
	//求取平均值
	for (int i = 0; i < 3; i++)
		meanQ.coord[i] = meanQ.coord[i] / conNum;
}

//点集中心化,生成新的点云数据
void ICP::rmcontcenter()
{
	std::cout << "点集中心化,生成新的点云数据" << std::endl;
	for (int i = 0; i < conNum; i++)
	{
		for (int j = 0; j < 3; j++)
		{
			rmcoP[i].coord[j] = contP[i].coord[j] - meanP.coord[j];
			rmcoQ[i].coord[j] = contQ[i].coord[j] - meanQ.coord[j];
		}
	}
}

void ICP::transform()
{
	std::cout << "获取变换矩阵" << std::endl;
	Matrix B(4, 4);
	B = 0;
	double u[3];//di+di'
	double d[3];//di-di'
	//计算协方差
	for (int i = 0; i < conNum; i++)
	{
		for (int j = 0; j < 3; j++)
		{
			u[j] = rmcoP[i].coord[j] + rmcoQ[i].coord[j];
			d[j] = rmcoP[i].coord[j] - rmcoQ[i].coord[j];
		}
		double uM[16] = { 
			   0, -d[0], -d[1], -d[2],
			d[0],     0, -u[2], -u[1],
			d[1], -u[2],     0,  u[0],
			d[2],  u[1], -u[0],    0 };

		Matrix Ai(4, 4);
		Ai << uM;
		B += Ai * Ai.t();
	}

	Matrix U;
	Matrix V;
	DiagonalMatrix D;
	SVD(B, D, U, V);

	for (int i = 0; i < 4; i++)
	{
		quad[i] = V.element(i, 3);
	}
	B.Release();
	U.Release();
	V.Release();
	D.Release();
}

void ICP::uprotate()
{
	//根据四元数求解选择矩阵
	Rw[0][0] = quad[0] * quad[0] + quad[1] * quad[1] - quad[2] * quad[2] - quad[3] * quad[3];
	Rw[0][1] = 2 * (-quad[0] * quad[3] + quad[1] * quad[2]);
	Rw[0][2] = 2 * (quad[0] * quad[2] + quad[1] * quad[3]);
	Rw[1][0] = 2 * (quad[0] * quad[3] + quad[1] * quad[2]);
	Rw[1][1] = quad[0] * quad[0] - quad[1] * quad[1] + quad[2] * quad[2] - quad[3] * quad[3];
	Rw[1][2] = 2 * (-quad[0] * quad[1] + quad[2] * quad[3]);
	Rw[2][0] = 2 * (-quad[0] * quad[2] + quad[1] * quad[3]);
	Rw[2][1] = 2 * (quad[0] * quad[1] + quad[2] * quad[3]);
	Rw[2][2] = quad[0] * quad[0] - quad[1] * quad[1] - quad[2] * quad[2] + quad[3] * quad[3];

	//Rn+1 = R * Rn
	double tmp[3][3];
	for (int i = 0; i < 3; i++)
	{
		for (int j = 0; j < 3; j++)
		{
			tmp[i][j] = 0;
		}
	}

	for (int i = 0; i < 3; i++)
	{
		for (int j = 0; j < 3; j++)
		{
			for (int k = 0; k < 3; k++)
			{
				tmp[i][j] += Rw[i][k] * TR[k][j];
			}
		}
	}

	for (int i = 0; i < 3; i++)
	{
		for (int j = 0; j < 3; j++)
			TR[i][j] = tmp[i][j];
	}
}

void ICP::uptranslate()
{
	//Tw = P'-Rw * P
	double tmp[3] = { 0, 0, 0 };
	for (int i = 0; i < 3; i++)
	{
		for (int j = 0; j < 3; j++)
		{
			tmp[i] += Rw[i][j] * meanP.coord[j];
		}
	}
	for (int i = 0; i < 3; i++)
	{
		Tw[i] = meanQ.coord[i] - tmp[i];
	}

	double temp[3] = { 0, 0, 0 };
	for (int i = 0; i < 3; i++)
	{
		for (int j = 0; j < 3; j++)
		{
			temp[i] += Rw[i][j] * TT[j];
		}
	}
	for (int i = 0; i < 3; i++)
	{
		TT[i] = temp[i] + Tw[i];
	}
}

void ICP::updata()
{
	for (int i = 0; i < conNum; i++)
	{
		double tmp[3] = { 0, 0, 0 };
		for (int j = 0; j < 3; j++)
		{
			for (int k = 0; k < 3; k++)
			{
				tmp[j] += Rw[j][k] * contP[i].coord[k];
			}
		}
		for (int j = 0; j < 3; j++)
			contP[i].coord[j] = tmp[j] + Tw[j];
	}
}

void ICP::applyall()
{
	for (vector<Vertex>::iterator p = VarrP.begin(); p != VarrP.end(); p++)
	{
		Vertex v = *p;
		double tmp[3] = { 0, 0, 0 };
		for (int i = 0; i < 3; i++)
		{
			for (int k = 0; k < 3; k++)
			{
				tmp[i] += TR[i][k] * v.coord[k];
			}
		}
		for (int i = 0; i < 3; i++)
		{
			v.coord[i] = tmp[i] + TT[i];
		}
		*p = v;
	}
}

double ICP::distance(Vertex a, Vertex b)
{
	double dist = 0.0;
	for (int i = 0; i < 3; i++)
	{
		dist += (a.coord[i] - b.coord[i])*(a.coord[i] - b.coord[i]);
	}
	return dist;
}

void ICP::printTT()
{
	std::cout << "Translate Matrix = " << std::endl;
	for (int i = 0; i < 3; i++)
	{
		std::cout << TT[i] << " ";
	}
	std::cout << std::endl;
}

void ICP::printTR()
{
	std::cout << "Rotate Matrix = " << std::endl;
	for (int i = 0; i < 3; i++)
	{
		for (int j = 0; j < 3; j++)
		{
			std::cout << TR[i][j] << " ";
		}
		std::cout << std::endl;
	}
}

void ICP::showcloud(std::string firstname, std::string secondname,std::string thirdname)
{
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_Target(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_Source(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloudOut(new pcl::PointCloud<pcl::PointXYZ>);

	std::cout << "显示两个点云:" << std::endl;
	//第一个点云数据
	ifstream in;
	in.open(firstname.c_str(), std::ios::in);
	if (!in.is_open())
	{
		std::cout << "error open!" << std::endl;
		system("pause");
	}
	vector<Point> points;
	points.clear();
	Point tmp;
	while (in >> tmp.x >> tmp.y >> tmp.z)
	{
		points.push_back(tmp);
	}
	pcl::PointXYZ cltmp;

	for (size_t i = 0; i != points.size();i++)
	{
		cltmp.x = points[i].x;
		cltmp.y = points[i].y;
		cltmp.z = points[i].z;
		cloud_Target->points.push_back(cltmp);
	}
	std::cout << "点云A的大小:" << cloud_Target->size() << std::endl;
	in.close();

	//第二个点云数据
	in.open(secondname.c_str(), std::ios::in);
	if (!in.is_open())
	{
		std::cout << "error open!" << std::endl;
		system("pause");
	}
	points.clear();
	while (in >> tmp.x >> tmp.y >> tmp.z)
	{
		points.push_back(tmp);
	}
	//pcl::PointXYZ cltmp;

	for (size_t i = 0; i != points.size(); i++)
	{
		cltmp.x = points[i].x;
		cltmp.y = points[i].y;
		cltmp.z = points[i].z;
		cloud_Source->points.push_back(cltmp);
	}
	std::cout << "点云B的大小:" << cloud_Source->size() << std::endl;
	in.close();

	//第三个点云数据
	in.open(thirdname.c_str(), std::ios::in);
	if (!in.is_open())
	{
		std::cout << "error open!" << std::endl;
		system("pause");
	}
	points.clear();
	while (in >> tmp.x >> tmp.y >> tmp.z)
	{
		points.push_back(tmp);
	}
	//pcl::PointXYZ cltmp;

	for (size_t i = 0; i != points.size(); i++)
	{
		cltmp.x = points[i].x;
		cltmp.y = points[i].y;
		cltmp.z = points[i].z;
		cloudOut->points.push_back(cltmp);
	}
	std::cout << "点云C的大小:" << cloudOut->size() << std::endl;
	in.close();


	//可视化初始化
	pcl::visualization::PCLVisualizer viewer;
	viewer.setCameraFieldOfView(0.785398);//fov 45°  视场角
	viewer.setBackgroundColor(0.2, 0.2, 0.2);
	viewer.setCameraPosition(
		0, 0, 0,
		0, 0, -1,
		0, 0, 0);
	//点云可视化
	pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> TargetHandler(cloud_Target, 255, 0, 0);
	pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> SourceHandler(cloud_Source, 0, 0, 255);
	pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> OutHandler(cloudOut, 0, 255, 0);
	viewer.addPointCloud(cloud_Target, TargetHandler, "cloud_Target");
	viewer.addPointCloud(cloud_Source, SourceHandler, "cloud_Source");
	viewer.addCoordinateSystem(0.1, "cloud", 0);


	int v2 = 1;
	viewer.createViewPort(0.5, 0, 1, 1, v2);
	viewer.createViewPortCamera(v2);
	viewer.setCameraFieldOfView(0.785398, v2);//fov 45°
	viewer.setBackgroundColor(0.2, 0.2, 0.2,v2);
	viewer.setCameraPosition(
		0, 0, 0,
		0, 0, -1,
		0, 0, 0,v2);
	//点云可视化
	viewer.addPointCloud(cloud_Target, TargetHandler, "cloud222", v2);
	viewer.addPointCloud(cloudOut, OutHandler, "cloudOut",v2);
	viewer.addCoordinateSystem(0.1, "cloud1", v2);
	while(!viewer.wasStopped())
	{
		viewer.spinOnce();
	}
}

//主程序
#include "stdafx.h"
#include <iostream>
#include <pcl\common\transforms.h>
#include <pcl\io\pcd_io.h>
#include <pcl\visualization\pcl_visualizer.h>
#include <pcl\registration\icp.h>
#include "ICP.h">
#include <iostream>
#include <time.h>
int main()
{
	clock_t start, finish;
	start = clock();
	ICP myicp(1000, 0.00001, 50);
	myicp.readfile("bunny_0.asc", "bunny_1.asc");
	myicp.run();
	myicp.writefile("out.asc");
	finish = clock();
	std::cout << "运行时间:" << (finish - start) / 1000 << "s" << std::endl;
	myicp.showcloud("bunny_0.asc", "bunny_1.asc", "out.asc");
	system("pause");
	return 0;
}
  • 8
    点赞
  • 54
    收藏
    觉得还不错? 一键收藏
  • 4
    评论
评论 4
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值