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();
|
|||
|
|
}
|
|||
|
|
}
|