c++ - PCL ICP功能不添加第二点云

Question

我编写了一个小代码片段，使用 PCL 将两个点云与 ICP 合并。然而，无论我做什么，我最终得到的最终点云只包含第一个点云。图片:

#define _CRT_SECURE_NO_DEPRECATE

#include <pcl/point_types.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/registration/icp.h>
#include <pcl/io/pcd_io.h>
#include <boost/make_shared.hpp>
#include "Dot3DReader.h"


int main(int argc, char** argv)
{

pcl::PointCloud<pcl::PointXYZ>::Ptr cloud1(new pcl::PointCloud<pcl::PointXYZ>);

if (pcl::io::loadPCDFile<pcl::PointXYZ>("D:/Imerso/PCL PointCloudExampleData/Frame1.pcd", *cloud1) == -1) //* load the file
{
    PCL_ERROR("Couldn't read file Frame1.pcd \n");
    return (-1);
}

pcl::PointCloud<pcl::PointXYZ>::Ptr cloud2(new pcl::PointCloud<pcl::PointXYZ>);

if (pcl::io::loadPCDFile<pcl::PointXYZ>("D:/Imerso/PCL PointCloudExampleData/Frame2.pcd", *cloud2) == -1) //* load the file
{
    PCL_ERROR("Couldn't read file Frame2.pcd \n");
    return (-1);
}

std::cout << "Read both files." << std::endl;

pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
icp.setInputSource(cloud1);
icp.setInputTarget(cloud2);

boost::shared_ptr<pcl::PointCloud<pcl::PointXYZ>> Final(new    pcl::PointCloud<pcl::PointXYZ>());


std::cout << "Starting aligning." << std::endl;

icp.align(*Final);

std::cout << "Finished aligning." << std::endl;
std::cout << "has converged:" << icp.hasConverged() << " score: " <<
    icp.getFitnessScore() << std::endl;
// CloudViewer を生成
// （表示は別スレッドで実行される)
pcl::visualization::CloudViewer viewer("Final cloud Viewer");


// CloudViewer で点群を表示
viewer.showCloud(Final);

while (true) {
    // ESC キーで終了
    if (GetKeyState(VK_ESCAPE) < 0) {
        return 0;
    }
}
return 0;
}

作为奖励，我还想在使用 RGB 点时保留对齐点的颜色。我该怎么做？

我的猜测是它没有找到匹配项，只是丢弃了第二个点云。

感谢您的帮助。

Answer 1

我不确定是我愚蠢还是文档和示例代码具有误导性，但我设法弄清楚了函数的作用以及如何使用它们。

.align() 函数仅对源点云应用变换，使其与目标点云匹配。它返回的点云是转换后的源云，因此您所要做的就是将其附加到全局云。

示例代码: boost::shared_ptr> cloud1, cloud2, GlobalCloud;//用你想要合并的点云填充前两个。

pcl::IterativeClosestPoint<pcl::PointXYZRGB, pcl::PointXYZRGB> icp;
icp.setInputSource(cloud1); //This cloud will be transformed to match
icp.setInputTarget(cloud2); //this cloud. The result is stored in the cloud provided as input below.


icp.align(*cloud1); //Overwrite the source cloud with the transformed cloud.

*GlobalCloud = *cloud1 + *cloud2; //Merge the two now aligned and matched clouds.

//Do whatever you want with the global cloud.

我希望这对某些人有所帮助，这样他们就不必通过阅读源代码来解码发生的事情。 (^-^)

有什么想知道的就问吧，我会尽力帮忙的。