CloudCompare二次开发之如何通过PCL进行点云配准?

文章目录
0.引言
1.CloudCompare界面设计配准(registrate)按钮
2.ICP配准(ICP_Reg)
3.多幅点云逐步配准(Many_Reg)

0.引言

  因笔者课题涉及点云处理,需要通过PCL进行点云数据一系列处理分析,查阅现有网络资料,对常用PCL点云配准进行代码实现,本文记录配准实现过程。

1.CloudCompare界面设计配准(registrate)按钮

  (1)设计.ui文件

  ①设计按钮

②编译.ui

(2)修改mainwindow.h文件

//点云配准
void doActionPCLICP_Reg(); // ICP配准  
void doActionPCLMany_Reg(); // 多幅点云逐步配准

(3)修改mainwindow.cpp文件

 ①添加头文件

#include <pcl/registration/icp.h>   // ICP配准类相关的头文件
#include <pcl/registration/icp_nl.h>  
#include <pcl/registration/transforms.h>  
#include <pcl/registration/ndt.h>   //NDT配准类对应头文件  
#include <pcl/features/normal_3d.h>

  ②添加实现代码

//ICP配准
void MainWindow::doActionPCLICP_Reg()  
{  
}  
//多幅点云逐步配准  
void MainWindow::doActionPCLMany_Reg()  
{  
}

 ③添加信号槽函数

connect(m_UI->actionICP_Reg, &amp;QAction::triggered, this, &amp;MainWindow::doActionPCLICP_Reg);//ICP配准
connect(m_UI->actionMany_Reg, &amp;QAction::triggered, this, &amp;MainWindow::doActionPCLMany_Reg);//多幅点云逐步配准

(4)生成

2.ICP配准(ICP_Reg)

  (1)实现代码

//ICP配准
void MainWindow::doActionPCLICP_Reg()  
{  
    if (getSelectedEntities().size() != 2)  
    {  
        ccLog::Print(QStringLiteral("需选择两个点云实体"));  
    return;  
    }  
    ccHObject* entity1 = getSelectedEntities()[0];  
    ccHObject* entity2 = getSelectedEntities()[1];  
    ccPointCloud* ccCloud1 = ccHObjectCaster::ToPointCloud(entity1);  
    ccPointCloud* ccCloud2 = ccHObjectCaster::ToPointCloud(entity2);  
    // ---------------------------读取数据到PCL----------------------------------  
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud1(new pcl::PointCloud<pcl::PointXYZ>);  
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud2(new pcl::PointCloud<pcl::PointXYZ>);  
    cloud1->resize(ccCloud1->size());  
    cloud2->resize(ccCloud2->size());  
    for (int i = 0; i < cloud1->size(); ++i)  
    {  
        const CCVector3* point = ccCloud1->getPoint(i);  
        cloud1->points[i].x = point->x;  
        cloud1->points[i].y = point->y;  
        cloud1->points[i].z = point->z;  
    }  
    for (int i = 0; i < cloud2->size(); ++i)  
    {  
        const CCVector3* point = ccCloud2->getPoint(i);  
        cloud2->points[i].x = point->x;  
        cloud2->points[i].y = point->y;  
        cloud2->points[i].z = point->z;  
    }  
    // ----------------------------ICP配准--------------------------------------  
    pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;  
    icp.setInputCloud(cloud1);  
    icp.setInputTarget(cloud2);  
    pcl::PointCloud<pcl::PointXYZ>::Ptr filtered(new pcl::PointCloud<pcl::PointXYZ>);  
    icp.align(*filtered);  
    // ------------------------PCL->CloudCompare--------------------------------  
    if (!filtered->empty())  
    {  
        ccPointCloud* newPointCloud = new ccPointCloud(QString(ccCloud2->getName()+".registered"));  
        for (int i = 0; i < filtered->size(); ++i)  
        {  
            double x = filtered->points[i].x;  
            double y = filtered->points[i].y;  
            double z = filtered->points[i].z;  
            newPointCloud->addPoint(CCVector3(x, y, z));  
        }  
        newPointCloud->setRGBColor(ccColor::Rgba(255, 255, 255, 255));  
        newPointCloud->showColors(true);  
        ccCloud2->setEnabled(false);  
        //创建一个文件夹来放点云  
        ccHObject* CloudGroup = new ccHObject(QString("RegisteredGroup"));  
        CloudGroup->addChild(newPointCloud);  
        m_ccRoot->addElement(CloudGroup);  
        addToDB(newPointCloud);  
        refreshAll();  
        updateUI();  
        QString str = QStringLiteral("变换矩阵:\n");  
        Eigen::Matrix4f m = icp.getFinalTransformation();  
        float* arr = m.data();  
        for (size_t i = 0; i < 16; i++)  
        {  
            str += QString::number(arr[i],'f',6);  
            if ((i+1) % 4 == 0) {  
                str += "\n";  
            }  
            else {  
                str += ",";  
            }  
        }  
        dispToConsole(str, ccMainAppInterface::STD_CONSOLE_MESSAGE);  
    }  
    else  
    {  
        ccCloud2->setEnabled(true);  
        // Display a warning message in the console  
        dispToConsole("Warning: example shouldn't be used as is", ccMainAppInterface::WRN_CONSOLE_MESSAGE);  
    }  
}

(2)配准结果

  ①采样前

②配准后

3.多幅点云逐步配准(Many_Reg)

  (1)实现代码

typedef pcl::PointXYZ PointT;
typedef pcl::PointCloud<PointT> PointCloud;  
typedef pcl::PointNormal PointNormalT;  
typedef pcl::PointCloud<PointNormalT> PointCloudWithNormals;  

/* 以< x, y, z, curvature >形式定义一个新的点 */  
class MyPointRepresentation : public pcl::PointRepresentation <PointNormalT>  
{  
    using pcl::PointRepresentation<PointNormalT>::nr_dimensions_;  
public:  
    MyPointRepresentation()  
    {  
        nr_dimensions_ = 4;   //定义尺寸值  
    }  
    /* 覆盖copyToFloatArray方法来定义我们的特征矢量 */  
    virtual void copyToFloatArray(const PointNormalT &amp;p, float * out) const  
    {  
        /* < x, y, z, curvature > */  
        out[0] = p.x;  
        out[1] = p.y;  
        out[2] = p.z;  
        out[3] = p.curvature;  
    }  
};  

/**匹配一对点云数据集并且返回结果  
*cloud_src:源点云  
*cloud_tgt:目标点云  
*output:输出的配准结果的源点云  
*final_transform:源点云和目标点云之间的转换矩阵  
*/  
void pairAlign(const PointCloud::Ptr cloud_src, const PointCloud::Ptr cloud_tgt, PointCloud::Ptr output,Eigen::Matrix4f &amp;final_transform, bool downsample = false)  
{  
    /* 下采样 */  
    PointCloud::Ptr src(new PointCloud);  
    PointCloud::Ptr tgt(new PointCloud);  
    pcl::VoxelGrid<PointT> grid;  
    if (downsample)  
    {  
        grid.setLeafSize(0.05, 0.05, 0.05);  
        grid.setInputCloud(cloud_src);  
        grid.filter(*src);  
        grid.setInputCloud(cloud_tgt);  
        grid.filter(*tgt);  
    }  
    else  
    {  
        src = cloud_src;  
        tgt = cloud_tgt;  
    }  
    /* 计算曲面法线和曲率 */  
    PointCloudWithNormals::Ptr points_with_normals_src(new PointCloudWithNormals);  
    PointCloudWithNormals::Ptr points_with_normals_tgt(new PointCloudWithNormals);  
    pcl::NormalEstimation<PointT, PointNormalT> norm_est;  
    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>());  
    norm_est.setSearchMethod(tree);  
    norm_est.setKSearch(30);  
    norm_est.setInputCloud(src);  
    norm_est.compute(*points_with_normals_src);  
    pcl::copyPointCloud(*src, *points_with_normals_src);  
    norm_est.setInputCloud(tgt);  
    norm_est.compute(*points_with_normals_tgt);  
    pcl::copyPointCloud(*tgt, *points_with_normals_tgt);  
    MyPointRepresentation point_representation;  
    //调整'curvature'尺寸权重以便使它和x, y, z平衡  
    float alpha[4] = { 1.0, 1.0, 1.0, 1.0 };  
    point_representation.setRescaleValues(alpha);  
    /* 配准 */  
    pcl::IterativeClosestPointNonLinear<PointNormalT, PointNormalT> reg;  
    reg.setTransformationEpsilon(1e-6);  
    reg.setMaxCorrespondenceDistance(0.1); //将两个对应关系之间的(src<->tgt)最大距离设置为10厘米,需要根据数据集大小来调整  
    reg.setPointRepresentation(boost::make_shared<const MyPointRepresentation>(point_representation));  //设置点表示  
    reg.setInputCloud(points_with_normals_src);  
    reg.setInputTarget(points_with_normals_tgt);  
    /* 在一个循环中运行相同的最优化并且使结果可视化 */  
    Eigen::Matrix4f Ti = Eigen::Matrix4f::Identity(), prev, targetToSource;  
    PointCloudWithNormals::Ptr reg_result = points_with_normals_src;  
    reg.setMaximumIterations(2);  
    for (int i = 0; i < 30; ++i)  
    {  
        //PCL_INFO("Iteration Nr. %d.\n", i);  
        points_with_normals_src = reg_result;         //为了可视化的目的保存点云  
        reg.setInputCloud(points_with_normals_src);  
        reg.align(*reg_result);  
        Ti = reg.getFinalTransformation() * Ti;      //在每一个迭代之间累积转换  
    /* 如果这次转换和之前转换之间的差异小于阈值,则通过减小最大对应距离来改善程序 */  
    if (fabs((reg.getLastIncrementalTransformation() - prev).sum()) < reg.getTransformationEpsilon())  
    {  
        reg.setMaxCorrespondenceDistance(reg.getMaxCorrespondenceDistance() - 0.001);  
        prev = reg.getLastIncrementalTransformation();  
    }  
    }  
    targetToSource = Ti.inverse();  //得到目标点云到源点云的变换  
    /* 把目标点云转换回源框架 */  
    pcl::transformPointCloud(*cloud_tgt, *output, targetToSource);  
    /* 添加源点云到转换目标 */  
    *output += *cloud_src;  
    final_transform = targetToSource;  
}  

//多幅点云逐步配准  
void MainWindow::doActionPCLMany_Reg()  
{  
    if (getSelectedEntities().size() < 2)  
    {  
        ccLog::Print(QStringLiteral("需至少选择两个点云实体"));  
    return;  
    }  
    //加个进度条  
    ccProgressDialog pDlg(true, 0);  
    CCLib::GenericProgressCallback* progressCb = &amp;pDlg;  
    if (progressCb) {  
        if (progressCb->textCanBeEdited()) {  
            progressCb->setMethodTitle("compute");  
            progressCb->setInfo(qPrintable(QString("waiting...")));  
        }  
        progressCb->update(0);  
        progressCb->start();  
    }  
    PointCloud::Ptr result(new PointCloud), source, target;  
    Eigen::Matrix4f GlobalTransform = Eigen::Matrix4f::Identity(), pairTransform;  
    for (size_t i = 1; i < getSelectedEntities().size(); ++i)  
    {  
        ccHObject* entity1 = getSelectedEntities()[i-1];  
        ccHObject* entity2 = getSelectedEntities()[i];  
        ccPointCloud* ccCloud1 = ccHObjectCaster::ToPointCloud(entity1);  
        ccPointCloud* ccCloud2 = ccHObjectCaster::ToPointCloud(entity2);  
        // ---------------------------读取数据到PCL----------------------------------  
        pcl::PointCloud<pcl::PointXYZ>::Ptr cloud1(new pcl::PointCloud<pcl::PointXYZ>);  
        pcl::PointCloud<pcl::PointXYZ>::Ptr cloud2(new pcl::PointCloud<pcl::PointXYZ>);  
        cloud1->resize(ccCloud1->size());  
        cloud2->resize(ccCloud2->size());  
        for (int i = 0; i < cloud1->size(); ++i)  
        {  
            const CCVector3* point = ccCloud1->getPoint(i);  
            cloud1->points[i].x = point->x;  
            cloud1->points[i].y = point->y;  
            cloud1->points[i].z = point->z;  
        }  
        for (int i = 0; i < cloud2->size(); ++i)  
        {  
            const CCVector3* point = ccCloud2->getPoint(i);  
            cloud2->points[i].x = point->x;  
            cloud2->points[i].y = point->y;  
            cloud2->points[i].z = point->z;  
        }  
        source = cloud1;  
        target = cloud2;  
    /* 添加可视化数据 */  
    PointCloud::Ptr temp(new PointCloud);  
    pairAlign(source, target, temp, pairTransform, true);  
    pcl::transformPointCloud(*temp, *result, GlobalTransform); //把当前的两两配对转换到全局变换  
    GlobalTransform = pairTransform * GlobalTransform;          //更新全局变换  
    ccCloud1->setEnabled(false);  
    ccCloud2->setEnabled(false);  
    progressCb->update((float)100.0*i / getSelectedEntities().size());  
    }  
    if (progressCb) {  
        progressCb->stop();  
    }  
    // ------------------------PCL->CloudCompare--------------------------------  
    if (!result->empty())  
    {  
        ccPointCloud* newPointCloud = new ccPointCloud(QString("FinalRegistered"));  
        for (int i = 0; i < result->size(); ++i)  
        {  
            double x = result->points[i].x;  
            double y = result->points[i].y;  
            double z = result->points[i].z;  
            newPointCloud->addPoint(CCVector3(x, y, z));  
        }  
        newPointCloud->setRGBColor(ccColor::Rgba(255, 255, 255, 255));  
        newPointCloud->showColors(true);  
        //创建一个文件夹来放点云  
        ccHObject* CloudGroup = new ccHObject(QString("RegisteredGroup"));  
        CloudGroup->addChild(newPointCloud);  
        m_ccRoot->addElement(CloudGroup);  
        addToDB(newPointCloud);  
        refreshAll();  
        updateUI();  
        QString str = QStringLiteral("最终的全局变换矩阵:\n");  
        Eigen::Matrix4f m = GlobalTransform;  
        float* arr = m.data();  
        for (size_t i = 0; i < 16; i++)  
        {  
            str += QString::number(arr[i], 'f', 6);  
            if ((i + 1) % 4 == 0) {  
                str += "\n";  
            }  
            else {  
                str += ",";  
            }  
        }  
        dispToConsole(str, ccMainAppInterface::STD_CONSOLE_MESSAGE);  
    }  
    else  
    {  
        //ccCloud2->setEnabled(true);  
        // Display a warning message in the console  
        dispToConsole("Warning: example shouldn't be used as is", ccMainAppInterface::WRN_CONSOLE_MESSAGE);  
    }  
}

(2)配准结果

  ①配准前

②配准后

参考资料:
[1] 来吧!我在未来等你!. CloudCompare二次开发之如何配置PCL点云库?; 2023-05-15 [accessed 2023-05-16].
[2] Roar冷颜. PCL中点云配准(非常详细,建议收藏); 2021-10-20 [accessed 2023-05-16].
[3] MSTIFIY. 点云配准(PCL+ICP); 2022-07-25 [accessed 2023-05-16].
[4] 滑了丝的螺丝钉. PCL点云配准官方教程; 2020-06-08 [accessed 2023-05-16].
[5] 悠缘之空. PCL函数库摘要——点云配准; 2021-11-07 [accessed 2023-05-16].
[6] 令狐少侠、. 3D点云系列——pcl:点云平滑及法线估计; 2022-04-05 [accessed 2023-05-16].