PCL ICP function does not add the second point cloud - c++

Question

I wrote a small code snippet to merge two point clouds with ICP using PCL. However, no matter what I do I end up with the final point cloud containing only the first point cloud. A picture:
#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;
}
As a bonus, I also want to preserve the colors of the points being aligned when using RGB points. How can I do that?
My guess is that it does not find a match and simply discards the second point cloud.
Thank you for your help.

Answer 1

I am not sure if I am stupid or the documentation and example code is misleading, but I managed to figure out what the functions do and how you use it.
The .align() function only applies a transformation to the source point cloud such that it matches the target point cloud. The point cloud it returns is that transformed source cloud so all you have to do is to append it to your global cloud.
Example code:
boost::shared_ptr> cloud1, cloud2, GlobalCloud; //Fill inn the first two with the pointclouds you want to merge.
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.
I hope this helps someone so they don't have to suffer through reading source code to decode what happens. (^-^)
Just ask if there is anything you want to know and I will try to help.