• 大小: 3KB
    文件类型: .rar
    金币: 2
    下载: 1 次
    发布日期: 2021-06-13
  • 语言: 其他
  • 标签: PCL  ICP  

资源简介

利用PCL开源库编写代码FPFH+ICP算法实现点云高精度配准,并计算配准误差!基于PCL库版本1.9!

资源截图

代码片段和文件信息

#include //采样一致性
#include 
#include 
#include 
#include 
#include 
#include 
#include //
#include //
#include //icp配准
#include //可视化
#include //时间

using pcl::NormalEstimation;
using pcl::search::KdTree;
typedef pcl::PointXYZ PointT;
typedef pcl::PointCloud PointCloud;

//点云可视化
void visualize_pcd(PointCloud::Ptr pcd_src PointCloud::Ptr pcd_tgt PointCloud::Ptr pcd_final)
{

//创建初始化目标
pcl::visualization::PCLVisualizer viewer(“registration Viewer“);


pcl::visualization::PointCloudColorHandlerCustom src_h(pcd_src 0 255 0);
pcl::visualization::PointCloudColorHandlerCustom tgt_h(pcd_tgt 255 0 0);
pcl::visualization::PointCloudColorHandlerCustom final_h(pcd_final 0 0 255);
viewer.setBackgroundColor(255 255 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“);

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;
ax = az + dlta;
}
else
{
ay = -M_PI / 2;
ax = -az + dlta;
}
}
else
{
ay = -asin(result_trans(2 0));
ax = atan2(result_trans(2 1) / cos(ay) result_trans(2 2) / cos(ay));
az = atan2(result_trans(1 0) / cos(ay) result_trans(0 0) / cos(ay));
}
result_angle << ax ay az;

cout << “x轴旋转角度:“ << ax << endl;
cout << “y轴旋转角度:“ << ay << endl;
cout << “z轴旋转角度:“ << az << endl;
}

int main(int argc char** argv)
{
//加载点云文件
PointCloud::Ptr cloud_src_o(new PointCloud);//原点云,待配准
pcl::io::loadPCDFile(“E:/vs13/pcldata/bun/rabbit.pcd“ *cloud_src_o);
PointCloud::Ptr cloud_tgt_o(new PointCloud);//目标点云
pcl::io::loadPCDFile(“E:/vs13/pcldata/bun/rabbit_1.pcd“ *cloud_tgt_o);

clock_t start = clock();

//去除NAN点
std::vector indices_src; //保存去除的点的索引
pcl::removeNaNFromPointCloud(*cloud_src_o *cloud_src_o indices_src);
std::cout << “remove *cloud_src_o nan“ << endl;

std::vector indices_tgt;
pcl::removeNaNFromPointCloud(*cloud_tgt_o *cloud_tgt_o indices_tgt);
std::cout << “remove *cloud_tgt_o nan“ << endl;

//下采样滤波
pcl::VoxelGrid voxel_grid;
voxel_grid.setLeafSize(0.012 0.012 0.012);
voxel_grid.setInputCloud(cloud_src_o);
PointCloud::Ptr cloud_src(new PointC

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----

     文件       8813  2019-03-04 20:04  fpfh+icp.cpp

----------- ---------  ---------- -----  ----

                 8813                    1


评论

共有 条评论