文章目录
- 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, &QAction::triggered, this, &MainWindow::doActionPCLICP_Reg);//ICP配准
connect(m_UI->actionMany_Reg, &QAction::triggered, this, &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 &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 &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 = &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].