564 lines
12 KiB
C++
564 lines
12 KiB
C++
#include <iostream>
|
||
#include <sstream>
|
||
#include <fstream>
|
||
#include <cassert>
|
||
#include <math.h>
|
||
#include <time.h>
|
||
#include <newmat.h>
|
||
#include <newmatap.h>
|
||
#include "ICP_H.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();
|
||
}
|
||
} |