Program examples select C++ PCL point cloud registration source code examples. For example, if you need to install a running environment or remote debugging, see your personal QQ business card at the bottom of the article for remote assistance from professional technicians!
Preface
This blog writes code for "C++ PCL Point Cloud Registration Source Code Example". The code is clean, regular and easy to read. Recommended for learning and application.
operation result
Article directory
1. Required tools and software
2. Usage steps
1. Main code
2. Operation results
3. Online assistance
1. Required tools and software
1. VS2019
2. C++
2. Usage steps
The code is as follows (example):
#include <pcl/registration/ia_ransac.h>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/features/normal_3d.h>
#include <pcl/features/fpfh.h>
#include <pcl/search/kdtree.h>
#include <pcl/io/pcd_io.h>
#include <pcl/filters/voxel_grid.h>
#include <time.h>
using namespace std;
//点云可视化
void visualize_pcd(PointCloud::Ptr pcd_src, PointCloud::Ptr pcd_tgt, PointCloud::Ptr pcd_final)
{
pcl::visualization::PCLVisualizer viewer("registration Viewer");
pcl::visualization::PointCloudColorHandlerCustom<PointT> final_h(pcd_final, 0, 0, 255);
viewer.addPointCloud(pcd_src, src_h, "source cloud");
viewer.addPointCloud(pcd_tgt, tgt_h, "tgt cloud");
viewer.addPointCloud(pcd_final, final_h, "final cloud");
viewer.addCoordinateSystem(1.0);
while (!viewer.wasStopped())
{
viewer.spinOnce(100);
//boost::this_thread::sleep(boost::posix_time::microseconds(100000));
}
}
//由旋转平移矩阵计算旋转角度
void matrix2angle(Eigen::Matrix4f& result_trans, Eigen::Vector3f& result_angle)
{
double ax, ay, az;
if (result_trans(2, 0) == 1 || result_trans(2, 0) == -1)
{
az = 0;
double dlta;
dlta = atan2(result_trans(0, 1), result_trans(0, 2));
if (result_trans(2, 0) == -1)
{
ay = M_PI / 2;
}
else
{
ay = -M_PI / 2;
}
}
else
{
ay = -asin(result_trans(2, 0));
ax = atan2(result_trans(2, 1) / cos(ay), result_trans(2, 2) / cos(ay));
}
result_angle << ax, ay, az;
}
int
main(int argc, char** argv)
{
//1. 加载点云文件
PointCloud::Ptr cloud_src_o(new PointCloud); //源点云，待配准
pcl::io::loadPCDFile("rabbit_source.pcd", *cloud_src_o);
PointCloud::Ptr cloud_tgt_o(new PointCloud);//目标点云
pcl::io::loadPCDFile("rabbit_target.pcd", *cloud_tgt_o);
cout << "读入点云完成！" << endl;
PointCloud::Ptr cloud_src(new PointCloud);
pcl::VoxelGrid<PointT> voxel_grid;
voxel_grid.setLeafSize(0.3, 0.3, 0.3);
voxel_grid.setInputCloud(cloud_src_o);
cout << "down size *cloud_src_o from " << cloud_src_o->size() << "to" << cloud_src->size() << endl;
pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne_src;
ne_src.setInputCloud(cloud_src);
pcl::search::KdTree< pcl::PointXYZ>::Ptr tree_src(new pcl::search::KdTree< pcl::PointXYZ>());
ne_src.setSearchMethod(tree_src);
PointCloud::Ptr cloud_tgt(new PointCloud);
pcl::VoxelGrid<pcl::PointXYZ> voxel_grid_2;
voxel_grid_2.setLeafSize(0.3, 0.3, 0.3);
voxel_grid_2.setInputCloud(cloud_tgt_o);
voxel_grid_2.filter(*cloud_tgt);
cout << "down size *cloud_tgt_o.pcd from " << cloud_tgt_o->size() << "to" << cloud_tgt->size() << endl;
//6.对目标点云进行法线估计
pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne_tgt;
ne_tgt.setInputCloud(cloud_tgt);
pcl::search::KdTree< pcl::PointXYZ>::Ptr tree_tgt(new pcl::search::KdTree< pcl::PointXYZ>());
ne_tgt.setSearchMethod(tree_tgt);
pcl::PointCloud<pcl::Normal>::Ptr cloud_tgt_normals(new pcl::PointCloud< pcl::Normal>);
//ne_tgt.setKSearch(20);
ne_tgt.setRadiusSearch(0.02);
ne_tgt.compute(*cloud_tgt_normals);
pcl::FPFHEstimation<pcl::PointXYZ, pcl::Normal, pcl::FPFHSignature33> fpfh_tgt;
fpfh_tgt.setInputCloud(cloud_tgt);
fpfh_tgt.setInputNormals(cloud_tgt_normals);
pcl::search::KdTree<PointT>::Ptr tree_tgt_fpfh(new pcl::search::KdTree<PointT>);
//SAC配准
pcl::SampleConsensusInitialAlignment<pcl::PointXYZ, pcl::PointXYZ, pcl::FPFHSignature33> scia;
scia.setInputSource(cloud_src);
scia.setInputTarget(cloud_tgt);
//scia.setMinSampleDistance(1);
//scia.setNumberOfSamples(2);
//scia.setCorrespondenceRandomness(20);
PointCloud::Ptr sac_result(new PointCloud);
scia.align(*sac_result);
cout << "sac has converged:" << scia.hasConverged() << " score: " << scia.getFitnessScore() << endl;
Eigen::Matrix4f sac_trans;
sac_trans = scia.getFinalTransformation();
cout << sac_trans << endl;
//icp配准
PointCloud::Ptr icp_result(new PointCloud);
pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
//Set the max correspondence distance to 4cm (e.g., correspondences with higher distances will be ignored)
icp.setMaxCorrespondenceDistance(0.04);
// 最大迭代次数
icp.setMaximumIterations(50);
// 两次变化矩阵之间的差值
icp.setTransformationEpsilon(1e-10);
cout << "ICP has converged:" << icp.hasConverged()
<< " score: " << icp.getFitnessScore() << endl;
Eigen::Matrix4f icp_trans;
icp_trans = icp.getFinalTransformation();
cout << icp_trans << endl;
//使用创建的变换对未过滤的输入点云进行变换
pcl::transformPointCloud(*cloud_src_o, *icp_result, icp_trans);
//计算误差
Eigen::Vector3f ANGLE_origin;
ANGLE_origin << 0, 0, M_PI / 5;
double error_x, error_y, error_z;
matrix2angle(icp_trans, ANGLE_result);
error_x = fabs(ANGLE_result(0)) - fabs(ANGLE_origin(0));
error_y = fabs(ANGLE_result(1)) - fabs(ANGLE_origin(1));
error_z = fabs(ANGLE_result(2)) - fabs(ANGLE_origin(2));
//可视化
visualize_pcd(cloud_src_o, cloud_tgt_o, icp_result);
return (0);
}
operation result
3. Online assistance:
If you need to install the operating environment or remote debugging, please see your personal QQ business card at the bottom of the article for remote assistance from professional technicians!
1) Remote installation and running environment, code debugging
2) Visual Studio, Qt, C++, Python programming language introductory guide
3) Interface beautification
4) Software production 5
) Cloud server application
6) Website production
Current article link: https://blog.csdn.net/alicema1111/article/details/132666851
Personal blog homepage : https://blog.csdn.net/alicema1111?type=
All articles by blogger click here: https:/ /blog.csdn.net/alicema1111?type=blog
Recommended by bloggers:
Python face recognition attendance punching system:
https://blog.csdn.net/alicema1111/article/details/133434445
Python fruit tree fruit recognition : https://blog.csdn.net/alicema1111/article/details/ 130862842
Python+Yolov8+Deepsort entrance traffic statistics: https://blog.csdn.net/alicema1111/article/details/130454430
Python+Qt face recognition access management system: https://blog.csdn.net/alicema1111/ article/details/130353433
Python+Qt fingerprint entry recognition attendance system: https://blog.csdn.net/alicema1111/article/details/129338432
Python Yolov5 flame smoke recognition source code sharing: https://blog.csdn.net/alicema1111 /article/details/128420453
Python+Yolov8 road bridge wall crack identification: https://blog.csdn.net/alicema1111/article/details/133434445