前面多篇博客都提到過，要善于從官網(wǎng)去熟悉一樣東西。API部分詳細介紹見
Point Cloud Library (PCL): Module registration
這里博主主要借鑒Tutorial里內容（博主整體都有看完）
Introduction — Point Cloud Library 0.0 documentation
接下來主要跑下Registration中的sample例子
一.直接運行下How to use iterative closet point中的代碼（稍微做了變化，打印輸出了Final點云）
#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/registration/icp.h>
int main()
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in(new pcl::PointCloud<pcl::PointXYZ>(5, 1));
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_out(new pcl::PointCloud<pcl::PointXYZ>);
// source->CloudIn data
for (auto& point : *cloud_in)
{
point.x = 1024 * rand() / (RAND_MAX + 1.0f);
point.y = 1024 * rand() / (RAND_MAX + 1.0f);
point.z = 1024 * rand() / (RAND_MAX + 1.0f);
}
std::cout << "source->cloud_in:" << std::endl;
for (auto& point : *cloud_in)
std::cout << point << std::endl;
std::cout << std::endl;
//target->CloudOut data
* cloud_out = *cloud_in;
for (auto& point : *cloud_out)
{
point.x += 1.6f;
point.y += 2.4f;
point.z += 3.2f;
}
std::cout << "target->cloud_out:" << std::endl;
for (auto& point : *cloud_out)
std::cout << point << std::endl;
std::cout << std::endl;
pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
icp.setInputSource(cloud_in);
icp.setInputTarget(cloud_out);
//final->
pcl::PointCloud<pcl::PointXYZ> Final;
icp.align(Final);
std::cout << "Final data points:" << std::endl;
for (auto& point : Final)
std::cout << point << std::endl;
std::cout << std::endl;
//transform info
std::cout << "has converged:" << icp.hasConverged() << " score: " <<
icp.getFitnessScore() << std::endl;
std::cout << icp.getFinalTransformation() << std::endl;
return (0);
}
?輸出結果如下：
?這里需要清楚的一點，是將源點云向目標點云對齊，讓其變換到目標點云的樣子。
從上面結果中也能夠看出，輸出的final點云數(shù)據(jù)是和target是非常近似的，也驗證了上面的表述。同時也能夠看到transform的信息也是對的。
這邊在上面代碼基礎上增加設置迭代次數(shù)，部分代碼如下
//target->CloudOut data
*cloud_out = *cloud_in;
for (auto& point : *cloud_out)
{
point.x += 1.6f;
point.y += 2.4f;
point.z += 3.2f;
}
std::cout << "target->cloud_out:" << std::endl;
for (auto& point : *cloud_out)
std::cout << point << std::endl;
std::cout << std::endl;
pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
icp.setInputSource(cloud_in);
icp.setInputTarget(cloud_out);
std::cout << "default itera times: " << icp.getMaximumIterations() << std::endl;
icp.setMaximumIterations(1);
std::cout << "set itera times: " << icp.getMaximumIterations() << std::endl;
std::cout << std::endl;
//final->可看到設為1時精度并沒有迭代次數(shù)默認值為10高。
這邊可以統(tǒng)計下對齊函數(shù)運行的時間，還是在上面代碼的基礎上
//final->
pcl::PointCloud<pcl::PointXYZ> Final;
clock_t start = clock();
icp.align(Final);
clock_t end = clock();
std::cout << end - start << " ms" << std::endl;
std::cout << std::endl;當?shù)螖?shù)設為30時，耗時9ms.
?當?shù)螖?shù)設為1時，耗時2ms
二. 這里改寫下How to use iterative closest point中的代碼，如下：
// test_vtk63.cpp : 定義控制臺應用程序的入口點。
//
#include <stdio.h>
#include <tchar.h>
#include "vtkAutoInit.h"
//VTK_MODULE_INIT(vtkRenderingOpenGL2);
//VTK_MODULE_INIT(vtkInteractionStyle);
//#define vtkRenderingCore_AUTOINIT 2(vtkRenderingOpenGL2, vtkInteractionStyle)
#include <boost/thread/thread.hpp>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/io/io.h>
#include <pcl/io/pcd_io.h>//pcd 讀寫類相關的頭文件。
#include <pcl/PCLPointCloud2.h>
#include <pcl/visualization/range_image_visualizer.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/registration/icp.h>
#include <iostream>
using namespace pcl;
using namespace pcl::io;
using namespace std;
int testpointcloudToPcd()
{
vtkObject::GlobalWarningDisplayOff();
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in(new pcl::PointCloud<pcl::PointXYZ>);
char strfilepath[256] = "D:\\PCL\\rabbit.pcd";
//第一種讀入方法較多場合如此
if (-1 == pcl::io::loadPCDFile(strfilepath, *cloud_in)) {
cout << "error input!" << endl;
return -1;
}
//輸出源點云點坐標
std::cout << "Saved " << cloud_in->size() << " data points to input:" << std::endl;
//for (auto& point : *cloud_in)
//std::cout << point << std::endl;
//目標點云由輸入點云來構造
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_out(new pcl::PointCloud<pcl::PointXYZ>);
*cloud_out = *cloud_in;
std::cout << "size:" << cloud_out->size() << std::endl;
for (auto& point : *cloud_out)
{
point.x += 0.7f;
point.y += 0.7f;
point.z += 1.0f;
}
//輸出目標點云
std::cout << "Transformed " << cloud_in->size() << " data points:" << std::endl;
//for (auto& point : *cloud_out)
//std::cout << point << std::endl;
pcl::io::savePCDFileASCII("D:\\PCL\\rabbit_trans.pcd", *cloud_out);
pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
icp.setInputSource(cloud_in);
icp.setInputTarget(cloud_out);
pcl::PointCloud<pcl::PointXYZ> Final;
icp.align(Final);
std::cout << "has converged:" << icp.hasConverged() << " score: " <<
icp.getFitnessScore() << std::endl;
std::cout << "Final data points size: " << Final.size() << std::endl;
pcl::io::savePCDFileASCII("D:\\PCL\\Final.pcd", Final);
//輸出變換矩陣
std::cout << icp.getFinalTransformation() << std::endl;
}
int _tmain(int argc, _TCHAR* argv[])
{
testpointcloudToPcd();
return 0;
}
?目標點云由輸入點云變換得到，同時也保存到了本地，方便下次直接加載。運行結果如下：
理論上目標點云是由源點云x平移0.7，y平移0.7，z平移1獲得。實際計算結果是如下矩陣的最后一列。和理論變換值還是有一些差距的。
?輸入源點云，目標點云，F(xiàn)inal點云的部分點坐標做了一個比較。可以看到源點云經(jīng)過變換后，已經(jīng)和目標點云很對齊了。
這里上傳下上面的三份點云文件。
鏈接：https://pan.baidu.com/s/1LUB9jLOG1eIq8JT4wheqHw?
提取碼：pfsy?
?
三. 小實驗1
讀取兩份點云，在窗口中拆分顯示, 測試點云來自于mirrors / pointcloudlibrary / data · GitCode
#include <pcl/memory.h> // for pcl::make_shared
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/point_representation.h>
#include <pcl/io/pcd_io.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/filter.h>
#include <pcl/features/normal_3d.h>
#include <pcl/registration/icp.h>
#include <pcl/registration/icp_nl.h>
#include <pcl/registration/transforms.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <boost/thread/thread.hpp>
using pcl::visualization::PointCloudColorHandlerGenericField;
using pcl::visualization::PointCloudColorHandlerCustom;
//convenient typedefs
typedef pcl::PointXYZ PointT;
typedef pcl::PointCloud<PointT> PointCloud;
typedef pcl::PointNormal PointNormalT;
typedef pcl::PointCloud<PointNormalT> PointCloudWithNormals;
int main()
{
PointCloud::Ptr cloud_src(new PointCloud);
PointCloud::Ptr cloud_tgt(new PointCloud);
pcl::io::loadPCDFile("D://PCL//data-master//tutorials//pairwise//capture0001.pcd", *cloud_src);
pcl::io::loadPCDFile("D://PCL//data-master//tutorials//pairwise//capture0005.pcd", *cloud_tgt);
std::cout << "cloud_src size: " << cloud_src->size() << std::endl;
std::cout << "cloud_target size: " << cloud_tgt->size() << std::endl;
pcl::VoxelGrid<PointT> grid;
PointCloud::Ptr src(new PointCloud);
PointCloud::Ptr tgt(new PointCloud);
grid.setLeafSize(0.05, 0.05, 0.05);
grid.setInputCloud(cloud_src);
grid.filter(*src);
std::cout << "src size: " << src->size() << std::endl;
grid.setInputCloud(cloud_tgt);
grid.filter(*tgt);
std::cout << "target size: " << tgt->size() << std::endl;
pcl::visualization::PCLVisualizer* p;
int vp_1, vp_2;
p = new pcl::visualization::PCLVisualizer("view");
p->createViewPort(0.0, 0, 0.5, 1.0, vp_1);
p->createViewPort(0.5, 0, 1.0, 1.0, vp_2);
PointCloudColorHandlerCustom<PointT> src_h(cloud_src, 255, 0, 0);
PointCloudColorHandlerCustom<PointT> tgt_h(cloud_tgt, 0, 255, 0);
p->addPointCloud(cloud_src, src_h, "vp1_src", vp_1);
p->addPointCloud(cloud_tgt, tgt_h, "vp1_target", vp_1);
p->addPointCloud(cloud_tgt, tgt_h, "vp2_target", vp_2);
p->spin();
return (0);
}
運行結果如下：
四. 小實驗2
上面main函數(shù)中的代碼修改為如下：
PointCloud::Ptr cloud_src(new PointCloud);
PointCloud::Ptr cloud_tgt(new PointCloud);
pcl::io::loadPCDFile("D://PCL//data-master//tutorials//pairwise//capture0001.pcd", *cloud_src);
pcl::io::loadPCDFile("D://PCL//data-master//tutorials//pairwise//capture0005.pcd", *cloud_tgt);
std::cout << "cloud_src size: " << cloud_src->size() << std::endl;
std::cout << "cloud_target size: " << cloud_tgt->size() << std::endl;
pcl::VoxelGrid<PointT> grid;
PointCloud::Ptr src(new PointCloud);
PointCloud::Ptr tgt(new PointCloud);
grid.setLeafSize(0.05, 0.05, 0.05);
grid.setInputCloud(cloud_src);
grid.filter(*src);
std::cout << "src size: " << src->size() << std::endl;
grid.setInputCloud(cloud_tgt);
grid.filter(*tgt);
std::cout << "target size: " << tgt->size() << std::endl;
// Compute surface normals and curvature
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);
pcl::visualization::PCLVisualizer* p;
int vp_1, vp_2;
p = new pcl::visualization::PCLVisualizer("view");
p->createViewPort(0.0, 0, 0.5, 1.0, vp_1);
p->createViewPort(0.5, 0, 1.0, 1.0, vp_2);
PointCloudColorHandlerCustom<PointNormalT> src_h(points_with_normals_src, 255, 0, 0);
PointCloudColorHandlerCustom<PointNormalT> tgt_h(points_with_normals_tgt, 0, 255, 0);
p->addPointCloud(points_with_normals_src, src_h, "vp1_src", vp_1);
p->addPointCloud(points_with_normals_tgt, tgt_h, "vp1_target", vp_1);
p->addPointCloud(points_with_normals_tgt, tgt_h, "vp2_target", vp_2);
p->spin();
return (0);運行結果如下：
?若注釋掉上面代碼中copyPointCloud部分兩句
//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);運行結果如下：
?五. 小實驗3
對上面兩個點云進行配準，迭代次數(shù)設為30。
#include <pcl/memory.h> // for pcl::make_shared
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/point_representation.h>
#include <pcl/io/pcd_io.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/filter.h>
#include <pcl/features/normal_3d.h>
#include <pcl/registration/icp.h>
#include <pcl/registration/icp_nl.h>
#include <pcl/registration/transforms.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <boost/thread/thread.hpp>
using pcl::visualization::PointCloudColorHandlerGenericField;
using pcl::visualization::PointCloudColorHandlerCustom;
//convenient typedefs
typedef pcl::PointXYZ PointT;
typedef pcl::PointCloud<PointT> PointCloud;
typedef pcl::PointNormal PointNormalT;
typedef pcl::PointCloud<PointNormalT> PointCloudWithNormals;
// Define a new point representation for < x, y, z, curvature >
class MyPointRepresentation : public pcl::PointRepresentation < PointNormalT>
{
using pcl::PointRepresentation<PointNormalT>::nr_dimensions_;
public:
MyPointRepresentation()
{
// Define the number of dimensions
nr_dimensions_ = 4;
}
// Override the copyToFloatArray method to define our feature vector
virtual void copyToFloatArray(const PointNormalT & p, float* out) const
{
// < x, y, z, curvature >
out[0] = p.x;
out[1] = p.y;
out[2] = p.z;
out[3] = p.curvature;
}
};
int main()
{
PointCloud::Ptr cloud_src(new PointCloud);
PointCloud::Ptr cloud_tgt(new PointCloud);
pcl::io::loadPCDFile("D://PCL//data-master//tutorials//pairwise//capture0001.pcd", *cloud_src);
pcl::io::loadPCDFile("D://PCL//data-master//tutorials//pairwise//capture0005.pcd", *cloud_tgt);
std::cout << "cloud_src size: " << cloud_src->size() << std::endl;
std::cout << "cloud_target size: " << cloud_tgt->size() << std::endl;
pcl::VoxelGrid<PointT> grid;
PointCloud::Ptr src(new PointCloud);
PointCloud::Ptr tgt(new PointCloud);
grid.setLeafSize(0.05, 0.05, 0.05);
grid.setInputCloud(cloud_src);
grid.filter(*src);
std::cout << "src size: " << src->size() << std::endl;
grid.setInputCloud(cloud_tgt);
grid.filter(*tgt);
std::cout << "target size: " << tgt->size() << std::endl;
// Compute surface normals and curvature
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);
//
// Instantiate our custom point representation (defined above) ...
MyPointRepresentation point_representation;
// ... and weight the 'curvature' dimension so that it is balanced against x, y, and z
float alpha[4] = { 1.0, 1.0, 1.0, 1.0 };
point_representation.setRescaleValues(alpha);
// Align
pcl::IterativeClosestPointNonLinear<PointNormalT, PointNormalT> reg;
reg.setTransformationEpsilon(1e-6);
// Set the maximum distance between two correspondences (src<->tgt) to 10cm
// Note: adjust this based on the size of your datasets
reg.setMaxCorrespondenceDistance(0.1);
// Set the point representation
reg.setPointRepresentation(pcl::make_shared<const MyPointRepresentation>(point_representation));
reg.setInputSource(points_with_normals_src);
reg.setInputTarget(points_with_normals_tgt);
PointCloudWithNormals::Ptr reg_result(new PointCloudWithNormals);
reg.setMaximumIterations(30);
reg.align(*reg_result);
Eigen::Matrix4f transform = reg.getFinalTransformation();
std::cout << "has converaged: " << reg.hasConverged() << " score: " << reg.getFitnessScore() << std::endl;
std::cout << "transform: " << std::endl;
std::cout << transform << std::endl;
std::cout << std::endl;
pcl::io::savePCDFileASCII("D:\\PCL\\trans_method1_final.pcd", *reg_result);
pcl::io::savePCDFileASCII("D:\\PCL\\trans_method1_out.pcd", *points_with_normals_tgt);
PointCloudWithNormals::Ptr transform_src(new PointCloudWithNormals);
pcl::transformPointCloud(*points_with_normals_src, *transform_src, transform);
pcl::io::savePCDFileASCII("D:\\PCL\\trans_method1_tranform_src.pcd", *transform_src);
//
pcl::visualization::PCLVisualizer* p;
int vp_1, vp_2;
p = new pcl::visualization::PCLVisualizer("view");
p->createViewPort(0.0, 0, 0.5, 1.0, vp_1);
p->createViewPort(0.5, 0, 1.0, 1.0, vp_2);
PointCloudColorHandlerCustom<PointNormalT> src_h(points_with_normals_src, 255, 0, 0);
PointCloudColorHandlerCustom<PointNormalT> tgt_h(points_with_normals_tgt, 0, 255, 0);
PointCloudColorHandlerCustom<PointNormalT> final_h(reg_result, 0, 0, 255);
p->addPointCloud(points_with_normals_src, src_h, "vp1_src", vp_1);
p->addPointCloud(reg_result, final_h, "reg_result", vp_1);
p->addPointCloud(points_with_normals_src, src_h, "vp1_src_copy", vp_2);
p->addPointCloud(points_with_normals_tgt, tgt_h, "vp2_target", vp_2);
p->spin();
return (0);
}
運行效果如下（左邊顯示的是對齊后的點云和目標點云，右邊是源點云和目標點云），左邊兩組點云的一致性很好，而右邊兩組由于未對齊，可看到是錯亂分布的。
?源點云到目標點云迭代30次配準的變換矩陣如下：
?由變換矩陣將源點云映射得到的點云和直接通過配準函數(shù)獲得的對齊后點云，部分數(shù)據(jù)如下，很接近。
??
六. 小實驗4
將ICP的最小迭代次數(shù)設為1，外面加一個循環(huán)執(zhí)行此過程30次。
#include <pcl/memory.h> // for pcl::make_shared
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/point_representation.h>
#include <pcl/io/pcd_io.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/filter.h>
#include <pcl/features/normal_3d.h>
#include <pcl/registration/icp.h>
#include <pcl/registration/icp_nl.h>
#include <pcl/registration/transforms.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <boost/thread/thread.hpp>
using pcl::visualization::PointCloudColorHandlerGenericField;
using pcl::visualization::PointCloudColorHandlerCustom;
//convenient typedefs
typedef pcl::PointXYZ PointT;
typedef pcl::PointCloud<PointT> PointCloud;
typedef pcl::PointNormal PointNormalT;
typedef pcl::PointCloud<PointNormalT> PointCloudWithNormals;
// Define a new point representation for < x, y, z, curvature >
class MyPointRepresentation : public pcl::PointRepresentation < PointNormalT>
{
using pcl::PointRepresentation<PointNormalT>::nr_dimensions_;
public:
MyPointRepresentation()
{
// Define the number of dimensions
nr_dimensions_ = 4;
}
// Override the copyToFloatArray method to define our feature vector
virtual void copyToFloatArray(const PointNormalT & p, float* out) const
{
// < x, y, z, curvature >
out[0] = p.x;
out[1] = p.y;
out[2] = p.z;
out[3] = p.curvature;
}
};
int main()
{
PointCloud::Ptr cloud_src(new PointCloud);
PointCloud::Ptr cloud_tgt(new PointCloud);
pcl::io::loadPCDFile("D://PCL//data-master//tutorials//pairwise//capture0001.pcd", *cloud_src);
pcl::io::loadPCDFile("D://PCL//data-master//tutorials//pairwise//capture0005.pcd", *cloud_tgt);
std::cout << "cloud_src size: " << cloud_src->size() << std::endl;
std::cout << "cloud_target size: " << cloud_tgt->size() << std::endl;
pcl::VoxelGrid<PointT> grid;
PointCloud::Ptr src(new PointCloud);
PointCloud::Ptr tgt(new PointCloud);
grid.setLeafSize(0.05, 0.05, 0.05);
grid.setInputCloud(cloud_src);
grid.filter(*src);
std::cout << "src size: " << src->size() << std::endl;
grid.setInputCloud(cloud_tgt);
grid.filter(*tgt);
std::cout << "target size: " << tgt->size() << std::endl;
// Compute surface normals and curvature
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);
//
// Instantiate our custom point representation (defined above) ...
MyPointRepresentation point_representation;
// ... and weight the 'curvature' dimension so that it is balanced against x, y, and z
float alpha[4] = { 1.0, 1.0, 1.0, 1.0 };
point_representation.setRescaleValues(alpha);
// Align
pcl::IterativeClosestPointNonLinear<PointNormalT, PointNormalT> reg;
reg.setTransformationEpsilon(1e-6);
// Set the maximum distance between two correspondences (src<->tgt) to 10cm
// Note: adjust this based on the size of your datasets
reg.setMaxCorrespondenceDistance(0.1);
// Set the point representation
reg.setPointRepresentation(pcl::make_shared<const MyPointRepresentation>(point_representation));
reg.setInputSource(points_with_normals_src);
reg.setInputTarget(points_with_normals_tgt);
//
// Run the same optimization in a loop and visualize the results
Eigen::Matrix4f Ti = Eigen::Matrix4f::Identity(), prev, targetToSource;
PointCloudWithNormals::Ptr reg_result = points_with_normals_src;
reg.setMaximumIterations(1);
pcl::visualization::PCLVisualizer* p;
p = new pcl::visualization::PCLVisualizer("view");
for (int i = 0; i < 30; ++i)
{
PCL_INFO("Iteration Nr. %d.\n", i);
// save cloud for visualization purpose
points_with_normals_src = reg_result;
// Estimate
reg.setInputSource(points_with_normals_src);
reg.align(*reg_result);
//accumulate transformation between each Iteration
Ti = reg.getFinalTransformation() * Ti;
//if the difference between this transformation and the previous one
//is smaller than the threshold, refine the process by reducing
//the maximal correspondence distance
if (std::abs((reg.getLastIncrementalTransformation() - prev).sum()) < reg.getTransformationEpsilon())
reg.setMaxCorrespondenceDistance(reg.getMaxCorrespondenceDistance() - 0.001);
prev = reg.getLastIncrementalTransformation();
// visualize current state
p->removePointCloud("source");
p->removePointCloud("target");
PointCloudColorHandlerGenericField<PointNormalT> src_color_handler(points_with_normals_src, "curvature");
if (!src_color_handler.isCapable())
PCL_WARN("Cannot create curvature color handler!\n");
PointCloudColorHandlerGenericField<PointNormalT> tgt_color_handler(points_with_normals_tgt, "curvature");
if (!tgt_color_handler.isCapable())
PCL_WARN("Cannot create curvature color handler!\n");
p->addPointCloud(points_with_normals_src, src_color_handler, "source");
p->addPointCloud(points_with_normals_tgt, tgt_color_handler, "target");
p->spinOnce();
}
std::cout << "transform: " << std::endl;
std::cout << Ti << std::endl;
std::cout << std::endl;
pcl::io::savePCDFileASCII("D:\\PCL\\trans_method2_final.pcd", *reg_result);
return (0);
}
運行結果如下（可看到和上面一次迭代次數(shù)設為30的方法對比，結果是一致的，只是這樣做的一個好處是可以對齊過程可視化，不像上面黑盒子一樣）：
?七. 兩個點云的融合（相加）
#include <pcl/memory.h> // for pcl::make_shared
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/point_representation.h>
#include <pcl/io/pcd_io.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/filter.h>
#include <pcl/features/normal_3d.h>
#include <pcl/registration/icp.h>
#include <pcl/registration/icp_nl.h>
#include <pcl/registration/transforms.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <boost/thread/thread.hpp>
using pcl::visualization::PointCloudColorHandlerGenericField;
using pcl::visualization::PointCloudColorHandlerCustom;
//convenient typedefs
typedef pcl::PointXYZ PointT;
typedef pcl::PointCloud<PointT> PointCloud;
typedef pcl::PointNormal PointNormalT;
typedef pcl::PointCloud<PointNormalT> PointCloudWithNormals;
int main()
{
PointCloud::Ptr cloud_src(new PointCloud);
PointCloud::Ptr cloud_tgt(new PointCloud);
pcl::io::loadPCDFile("D://PCL//data-master//tutorials//pairwise//capture0001.pcd", *cloud_src);
pcl::io::loadPCDFile("D://PCL//data-master//tutorials//pairwise//capture0005.pcd", *cloud_tgt);
std::cout << "cloud_src size: " << cloud_src->size() << std::endl;
std::cout << "cloud_target size: " << cloud_tgt->size() << std::endl;
pcl::VoxelGrid<PointT> grid;
PointCloud::Ptr src(new PointCloud);
PointCloud::Ptr tgt(new PointCloud);
grid.setLeafSize(0.05, 0.05, 0.05);
grid.setInputCloud(cloud_src);
grid.filter(*src);
std::cout << "src size: " << src->size() << std::endl;
grid.setInputCloud(cloud_tgt);
grid.filter(*tgt);
std::cout << "target size: " << tgt->size() << std::endl;
pcl::visualization::PCLVisualizer* p;
int vp_1, vp_2;
p = new pcl::visualization::PCLVisualizer("view");
p->createViewPort(0.0, 0, 0.5, 1.0, vp_1);
p->createViewPort(0.5, 0, 1.0, 1.0, vp_2);
PointCloudColorHandlerCustom<PointT> src_h(cloud_src, 255, 0, 0);
PointCloudColorHandlerCustom<PointT> tgt_h(cloud_tgt, 0, 255, 0);
p->addPointCloud(cloud_src, src_h, "vp1_src", vp_1);
p->addPointCloud(cloud_tgt, tgt_h, "vp1_target", vp_1);
PointCloud::Ptr total(new PointCloud);
total = cloud_src;
*total += *cloud_tgt;
PointCloudColorHandlerCustom<PointT> total_h(total, 0, 0, 255);
p->addPointCloud(total, total_h, "vp2_target", vp_2);
p->spin();
return (0);
}
運行效果如下：
八.?How to incrementally register pairs of clouds
有了上面的例子后，再去看官網(wǎng)上的這段代碼就很容易了
How to incrementally register pairs of clouds — Point Cloud Library 0.0 documentation文章來源：http://www.zghlxwxcb.cn/news/detail-520356.html
代碼會加載5張點云數(shù)據(jù)，A,B,C,D,E分別代表這5個點云，然后AB,BC,CD,DE分別兩兩配準，這樣就可以把B,C,D,E都變換到A空間中去，?向A對齊，完畢后將這5個點云數(shù)據(jù)做融合（相加），這樣就可以實現(xiàn)一個拼接或者融合的功能，后續(xù)任務就可以基于這份包含更細致信息的點云做文章。文章來源地址http://www.zghlxwxcb.cn/news/detail-520356.html
到了這里，關于PCL - 3D點云配準(registration)介紹的文章就介紹完了。如果您還想了解更多內容，請在右上角搜索TOY模板網(wǎng)以前的文章或繼續(xù)瀏覽下面的相關文章，希望大家以后多多支持TOY模板網(wǎng)！
![[點云配準]LCD(2D-3D特征配準算法)例程align_point_cloud.py解析](https://imgs.yssmx.com/Uploads/2024/02/481623-1.png)
