点云数据三维重建算法(一)：ICP配准算法

代码：

说明：代码和网上的代码略有不同，在经历多次报错后，进行了微调

背景：双轮廓仪对物体进行数据采集，采集到的数据要进行预处理。

第一步进行文件的格式转换，由 .asc 转为 .pcd 格式 ；

第二步将倾斜的点云数据进行旋转，使其水平；

第三步进行直通滤波和统计滤波。

预处理结束后，再进行ICP配准。

#define BOOST_TYPEOF_EMULATION//这里是对比网上的算法额外加的，不然要报错

#include <iostream>
#include <pcl/registration/gicp.h>
#include <pcl/console/time.h>
#include <pcl/features/normal_3d.h>
#include <pcl/io/pcd_io.h>
#include <pcl/io/ply_io.h>
#include <pcl/registration/icp.h>
#include <pcl/registration/transformation_estimation_point_to_plane.h>
#include <pcl/visualization/pcl_visualizer.h>

using namespace pcl;
using namespace std;


//C++版本的ICP部分代码
Eigen::Matrix4f PointToPlane(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud1, pcl::PointCloud<pcl::PointXYZ>::Ptr cloud2)
{
    pcl::PointCloud<pcl::PointNormal>::Ptr src(new pcl::PointCloud<pcl::PointNormal>);
    pcl::copyPointCloud(*cloud1, *src);
    pcl::PointCloud<pcl::PointNormal>::Ptr tgt(new pcl::PointCloud<pcl::PointNormal>);
    pcl::copyPointCloud(*cloud2, *tgt);

    //估算法向量
    pcl::NormalEstimation<pcl::PointNormal, pcl::PointNormal> norm_est;
    norm_est.setSearchMethod(pcl::search::KdTree<pcl::PointNormal>::Ptr(new pcl::search::KdTree<pcl::PointNormal>));
    norm_est.setKSearch(10);
    norm_est.setInputCloud(tgt);
    norm_est.compute(*tgt);

    pcl::IterativeClosestPoint<pcl::PointNormal, pcl::PointNormal> icp;
    typedef pcl::registration::TransformationEstimationPointToPlane<pcl::PointNormal, pcl::PointNormal> PointToPlane;
    boost::shared_ptr<PointToPlane> point_to_plane(new PointToPlane);
    icp.setTransformationEstimation(point_to_plane); // key

    icp.setInputSource(src);
    icp.setInputTarget(tgt);
    icp.setRANSACIterations(50);
    icp.setMaximumIterations(200);
    icp.setTransformationEpsilon(1e-3);
    pcl::PointCloud<pcl::PointNormal> output;
    icp.align(output);
    return icp.getFinalTransformation();
}
//主函数入口
int main(int argc, char** argv)
{

    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_source(new pcl::PointCloud<pcl::PointXYZ>);//源文件
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_target(new pcl::PointCloud<pcl::PointXYZ>);//目标文件

    std::string pcd_source_path, pcd_target_path;
    //对于文件的输入路径  需要要求是pcd或者ply文件格式   对应pcd文件引入对应的包 
    pcd_source_path = "C:/Users/UESTC/Desktop/vs_code/pcl/pcl/1_left_R_lvbo.pcd";
    pcd_target_path = "C:/Users/UESTC/Desktop/vs_code/pcl/pcl/1_right_R_lvbo.pcd";
 cout << "step1"<< endl;
    if (pcl::io::loadPCDFile<pcl::PointXYZ>(pcd_source_path, *cloud_source) == -1)
    {
        PCL_ERROR("Couldn't read file cloud1\n");
        return -1;
    }
cout << "step2"<< endl;
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud2(new pcl::PointCloud<pcl::PointXYZ>);
    if (pcl::io::loadPCDFile<pcl::PointXYZ>(pcd_target_path, *cloud_target) == -1)
    {
        PCL_ERROR("Couldn't read file cloud2\n");
        return -1;
    }
 cout << "step3"<< endl;
    pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloudicp(new pcl::PointCloud<pcl::PointXYZ>);
    //icp的收敛参数
    icp.setTransformationEpsilon(1e-10); //均方误差
    icp.setEuclideanFitnessEpsilon(0.01); //这个是论文部分对应的三次连续变换的差值部分的收敛情况
    icp.setMaximumIterations(200);  //最大迭代次数等
    icp.setInputCloud(cloud_source);
    icp.setInputTarget(cloud_target);
    icp.align(*cloudicp);

    cout << "point to point icp finished" << endl;

    Eigen::Matrix4f trans = PointToPlane(cloudicp, cloud_target);
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_result(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::transformPointCloud(*cloudicp, *cloud_result, trans);

    cout << "point to plane icp finished" << endl;

    //save
    //配准后的保存结果
    pcl::PointCloud<pcl::PointXYZ>::Ptr finalResult(new pcl::PointCloud<pcl::PointXYZ>);
    *finalResult = (*cloud_result) + (*cloud_target);
    //pcl::io::savePCDFileASCII("result1.pcd", *finalResult);
    pcl::io::savePCDFile("C:/Users/UESTC/Desktop/vs_code/pcl/pcl/1_ICPresult.pcd", *finalResult);

    //可视化
    boost::shared_ptr<pcl::visualization::PCLVisualizer>
        viewer_final(new pcl::visualization::PCLVisualizer("3D Viewer"));
    viewer_final->setBackgroundColor(0, 0, 0);

    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> color1(cloud_result, 255, 0, 0);
    viewer_final->addPointCloud<pcl::PointXYZ>(cloud_result, color1, "cloud1");
    viewer_final->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "cloud1");

    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> color2(cloud_target, 0, 255, 0);
    viewer_final->addPointCloud<pcl::PointXYZ>(cloud_target, color2, "cloud2");
    viewer_final->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "cloud2");

    viewer_final->addCoordinateSystem(1.0);
    viewer_final->initCameraParameters();

    while (!viewer_final->wasStopped())
    {
        viewer_final->spinOnce(100);
        boost::this_thread::sleep(boost::posix_time::microseconds(100000));
    }

    return 0;
}

报错处理：

1.错误C3861“pop_t”: 找不到标识符

修改 dish.h 文件，将502行：   typedef unsigned long long pop_t;   移动到480行   #if __GNUC__   代码之前即可！

 注：如果因为权限无法修改，找到 dish.h文件所在位置：C:\Program Files\PCL_1_8_1\3rdParty\FLANN\include\flann\algorithms  （这是我的文件所在位置） ，将 dish.h 文件移到桌面，将其修改，然后保存，在移回原位。

2.报错 pcl::IOException："[pcl::PCDWriter::writeASCII] Could not open file for writing!"

 将读入的两个文件放到项目下的文件夹里，以及输出的文件路径都要对应到项目的文件夹下。