实现代码为：
//以中心化点进行旋转
double theta = atan(maindirection.a);//计算的是弧度单位
for (int i = 0; i < origipts.size(); i++)
{
pcl::PointXYZ tempone;
tempone.x = aftercenerlizepts[i].x*cos(theta) + aftercenerlizepts[i].y*sin(theta) + center.x;
tempone.y = aftercenerlizepts[i].y*cos(theta) - aftercenerlizepts[i].x*sin(theta) + center.y;
transpts.push_back(tempone);
}3、测试结果
本程序是在PCL环境下运行，测试工程需要先配置好PCL环境，将点云旋转_test.cpp添加到源文件中即可运行。
3.1 轮廓点检测结果
轮廓点提取主函数如下：
//（1）测试边缘点提取结果
void main()
{
char *filepath = "D:\\testdata\\points.xyz";
char *savepath = "D:\\testdata\\points_boundpts.xyz";
vector<pcl::PointXYZ> origipts = ReadPointXYZIntoVector(filepath);
//假设其z坐标都为0，为平面坐标
for (int i = 0; i < origipts.size(); i++)
{
origipts[i].z = 0;
}
vector<pcl::PointXYZ> boundpts, nonbounpts;
double r = 0.8;
Bounpts(origipts, r, boundpts, nonbounpts);
ofstream outfile(savepath, ios::out);
for (int j = 0; j < boundpts.size(); j++)
{
outfile << fixed << setprecision(3) << boundpts[j].x << " " << boundpts[j].y << " " << boundpts[j].z << " " << fixed << setprecision(0) << 255 << " " << 0 << " " << 0 << endl;
}
for (int j = 0; j < nonbounpts.size(); j++)
{
outfile << fixed << setprecision(3) << nonbounpts[j].x << " " << nonbounpts[j].y << " " << nonbounpts[j].z << " " << fixed << setprecision(0) << 255 << " " << 255 << " " << 255 << endl;
}
outfile.close();
cout << "结束" << endl;
pcl::visualization::PCLVisualizer viewer("点云可视化");
pcl::PointCloud<pcl::PointXYZRGB>::Ptr new_cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
new_cloud->width = origipts.size();
new_cloud->height = 1;
new_cloud->is_dense = false;
new_cloud->points.resize(new_cloud->width*new_cloud->height);
for (int i = 0; i < origipts.size(); i++)
{
if (i < boundpts.size())
{
new_cloud->points[i].x = boundpts[i].x;
new_cloud->points[i].y = boundpts[i].y;
new_cloud->points[i].z = boundpts[i].z;
new_cloud->points[i].r = 255;
new_cloud->points[i].g = 0;
new_cloud->points[i].b = 0;
}
else
{
new_cloud->points[i].x = nonbounpts[i - boundpts.size()].x;
new_cloud->points[i].y = nonbounpts[i - boundpts.size()].y;
new_cloud->points[i].z = nonbounpts[i - boundpts.size()].z;
new_cloud->points[i].r = 255;
new_cloud->points[i].g = 255;
new_cloud->points[i].b = 255;
}
}
pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB>fildColor(new_cloud);
viewer.setBackgroundColor(0, 0, 0);
viewer.addPointCloud<pcl::PointXYZRGB>(new_cloud, fildColor, "inCloud");
viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 5, "inCloud");
while (!viewer.wasStopped())
{
viewer.spinOnce();
}
system("pause");
}
红色点为边缘点，可以看到边缘提取效果比较理想。
3.2 轮廓点分组
轮廓点分组测试结果如下：
//（2）测试边缘点分组
void main()
{
char *filepath = "D:\\testdata\\points.xyz";
char *savepath = "D:\\testdata\\points_boundpts_group.xyz";
vector<pcl::PointXYZ> origipts = ReadPointXYZIntoVector(filepath);
//假设其z坐标都为0，为平面坐标
for (int i = 0; i < origipts.size(); i++)
{
origipts[i].z = 0;
}
vector<pcl::PointXYZ> boundpts, nonbounpts;
double r = 0.8;
Bounpts(origipts, r, boundpts, nonbounpts);
vector<vector<pcl::PointXYZ>> multi_linepoints;
double ds_thres = 0.35;
double linefit_knn = 5;
double growing_knn = 5;
GroupPts(boundpts, ds_thres, linefit_knn, growing_knn, multi_linepoints);
srand((int)time(0));
ofstream outfile(savepath, ios::out);
for (int i = 0; i < multi_linepoints.size(); i++)
{
double R = rand() % 255;
double G = rand() % 255;
double B = rand() % 255;
for (int j = 0; j < multi_linepoints[i].size(); j++)
{
outfile << fixed << setprecision(3) << multi_linepoints[i][j].x << " " << multi_linepoints[i][j].y << " " << multi_linepoints[i][j].z << " " << fixed << setprecision(0) << R << " " << G << " " << B << endl;
}
}
outfile.close();
cout << "结束" << endl;
pcl::visualization::PCLVisualizer viewer("点云可视化");
pcl::PointCloud<pcl::PointXYZRGB>::Ptr new_cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
new_cloud->width = origipts.size();
new_cloud->height = 1;
new_cloud->is_dense = false;
new_cloud->points.resize(new_cloud->width*new_cloud->height);
int sumid = 0;
for (int i = 0; i < multi_linepoints.size(); i++)
{
double R = rand() % 255;
double G = rand() % 255;
double B = rand() % 255;
for (int j = 0; j < multi_linepoints[i].size(); j++)
{
new_cloud->points[sumid].x = multi_linepoints[i][j].x;
new_cloud->points[sumid].y = multi_linepoints[i][j].y;
new_cloud->points[sumid].z = multi_linepoints[i][j].z;
new_cloud->points[sumid].r = R;
new_cloud->points[sumid].g = G;
new_cloud->points[sumid].b = B;
sumid = sumid + 1;
}
}
pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB>fildColor(new_cloud);
viewer.setBackgroundColor(0, 0, 0);
viewer.addPointCloud<pcl::PointXYZRGB>(new_cloud, fildColor, "inCloud");
viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 5, "inCloud");
while (!viewer.wasStopped())
{
viewer.spinOnce();
}
system("pause");
}
属于同一直线的轮廓点，分组结果如上，结果比较理想。
3.3 点云旋转
//（3）原始点云进行旋转
void main()
{
char *filepath = "D:\\testdata\\points.xyz";
char *savepath = "D:\\testdata\\points_boundpts_transformpt.xyz";
vector<pcl::PointXYZ> origipts = ReadPointXYZIntoVector(filepath);
//假设其z坐标都为0，为平面坐标
for (int i = 0; i < origipts.size(); i++)
{
origipts[i].z = 0;
}
vector<pcl::PointXYZ> boundpts, nonbounpts;
double r = 0.8;
Bounpts(origipts, r, boundpts, nonbounpts);
pcl::PointXYZ center;
vector<pcl::PointXYZ> transpts;
double ds_thres = 0.35;
double linefit_knn = 5;
double growing_knn = 5;
TransformPts(origipts, r, ds_thres, linefit_knn, growing_knn, center, transpts);
srand((int)time(0));
ofstream outfile(savepath, ios::out);
for (int i = 0; i < transpts.size(); i++)
{
outfile << fixed << setprecision(3) << transpts[i].x << " " << transpts[i].y << " " << transpts[i].z << " " << endl;
}
outfile.close();
cout << "结束" << endl;
pcl::visualization::PCLVisualizer viewer("点云可视化");
pcl::PointCloud<pcl::PointXYZRGB>::Ptr new_cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
new_cloud->width = origipts.size();
new_cloud->height = 1;
new_cloud->is_dense = false;
new_cloud->points.resize(new_cloud->width*new_cloud->height);
int sumid = 0;
for (int i = 0; i < transpts.size(); i++)
{
new_cloud->points[sumid].x = transpts[i].x;
new_cloud->points[sumid].y = transpts[i].y;
new_cloud->points[sumid].z = transpts[i].z;
new_cloud->points[sumid].r = 255;
new_cloud->points[sumid].g = 255;
new_cloud->points[sumid].b = 255;
}
pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB>fildColor(new_cloud);
viewer.setBackgroundColor(0, 0, 0);
viewer.addPointCloud<pcl::PointXYZRGB>(new_cloud, fildColor, "inCloud");
viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 5, "inCloud");
while (!viewer.wasStopped())
{
viewer.spinOnce();
}
system("pause");
}
旋转前点云与水平方向存在一定旋转角，旋转后点云水平一致，旋转旋转成功。
代码与测试数据下载链接：https://mp.csdn.net/mp_download/manage/download/UpDetailed
