| 编辑推荐: |
本文主要介绍迭代最近点算法 ,c++代码来实现 ，并附加代码示例，希望可以为您的学习带来收获。
文章来自于csdn，由火龙果Alice编辑推荐。 |
|
简介：
在维基百科中是这样介绍迭代最近点算法。迭代最近点（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;
}
|
|
