PCL 点云配准 LM-ICP算法（精配准）

admin管理员组

文章数量:1649174

目录

• 一、概述
• 二、代码
• 三、结果

内容抄自CSDN点云侠：【2024最新版】PCL点云处理算法汇总（C++长期更新版）。质量无忧，永久免费，可放心复制粘贴。

一、概述

  LM-ICP（Levenberg-Marquardt Iterative Closest Point）算法是一种基于 Levenberg-Marquardt 非线性优化的 ICP 点云配准算法。相较于传统的 ICP 算法，LM-ICP 在迭代优化过程中引入了更复杂的误差修正机制，能够更有效地处理带有噪声、局部不规则的点云数据，实现更加精确的点云配准。
LM-ICP 通过最小化源点云与目标点云之间的几何误差（例如点到点或点到面的误差）来计算刚体变换矩阵。Levenberg-Marquardt 是一种结合了梯度下降和高斯牛顿法的非线性优化算法，能够在梯度下降较慢时切换到更为快速的优化路径。
  Levenberg-Marquardt (LM) 结合了两种优化方法：
梯度下降法：在远离最优解时，能够通过学习率较小的更新，慢速地逼近最优解。
高斯牛顿法：在接近最优解时，能通过求解二阶导数信息，快速逼近最优解。
LM-ICP 算法的目标是通过这些优化过程找到源点云和目标点云之间的最佳变换矩阵，使得几何误差最小化。其基本步骤包括：
计算初始对应关系。
使用 Levenberg-Marquardt 优化步骤调整变换矩阵。
迭代更新直到收敛。

二、代码

#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/features/normal_3d_omp.h> // 使用OMP加速法向量计算
#include <pcl/registration/icp_nl.h>    // 非线性ICP算法
#include <pcl/visualization/pcl_visualizer.h>
#include <boost/thread/thread.hpp>
#include <pcl/console/time.h>  // 控制台时间计时器
 
using namespace std;
 
// 计算法线
pcl::PointCloud<pcl::PointNormal>::Ptr compute_normals(pcl::PointCloud<pcl::PointXYZ>::Ptr input_cloud)
{
    pcl::NormalEstimationOMP<pcl::PointXYZ, pcl::Normal> normal_estimator;
    pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>);
    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>);
 
    normal_estimator.setNumberOfThreads(8);  // 使用多线程加速法线计算
    normal_estimator.setInputCloud(input_cloud);  // 设置输入点云
    normal_estimator.setSearchMethod(tree);  // 设置 KD 树搜索
    normal_estimator.setKSearch(10);  // 设置 K 近邻搜索
    normal_estimator.compute(*normals);  // 计算法线
 
    // 拼接点云数据与法线
    pcl::PointCloud<pcl::PointNormal>::Ptr cloud_with_normals(new pcl::PointCloud<pcl::PointNormal>);
    pcl::concatenateFields(*input_cloud, *normals, *cloud_with_normals);
    return cloud_with_normals;
}
 
// 执行 LM-ICP 配准
void run_lm_icp(pcl::PointCloud<pcl::PointNormal>::Ptr& source_normal,
    pcl::PointCloud<pcl::PointNormal>::Ptr& target_normal,
    Eigen::Matrix4f& final_transform, pcl::PointCloud<pcl::PointNormal>::Ptr& icp_cloud)
{
    // 初始化 LM ICP 对象
    pcl::IterativeClosestPointNonLinear<pcl::PointNormal, pcl::PointNormal> lm_icp;
 
    // 设置 ICP 参数
    lm_icp.setInputSource(source_normal);
    lm_icp.setInputTarget(target_normal);
    lm_icp.setTransformationEpsilon(1e-10);    // 设置最小转换差异
    lm_icp.setMaxCorrespondenceDistance(10);   // 设置最大对应点距离
    lm_icp.setEuclideanFitnessEpsilon(0.001);  // 设置均方误差收敛条件
    lm_icp.setMaximumIterations(50);           // 设置最大迭代次数
 
    // 执行 LM-ICP 配准
    lm_icp.align(*icp_cloud);
    final_transform = lm_icp.getFinalTransformation();
    std::cout << "LM-ICP 配准完成，最终分数: " << lm_icp.getFitnessScore() << std::endl;
}
 
// 可视化配准结果
void visualize_registration(pcl::PointCloud<pcl::PointXYZ>::Ptr& source,
    pcl::PointCloud<pcl::PointXYZ>::Ptr& target,
    pcl::PointCloud<pcl::PointXYZ>::Ptr& aligned)
{
    boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("配准结果"));
    viewer->setBackgroundColor(0, 0, 0);  // 设置背景颜色为黑色
 
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> target_color(target, 255, 0, 0);
    viewer->addPointCloud(target, target_color, "target cloud");
 
   /* pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> source_color(source, 0, 255, 0);
    viewer->addPointCloud(source, source_color, "source cloud");*/
 
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> aligned_color(aligned, 0, 0, 255);
    viewer->addPointCloud(aligned, aligned_color, "aligned cloud");
 
    viewer->spin();
}
 
int main()
{
    pcl::console::TicToc time;
    // 读取点云数据
    pcl::PointCloud<pcl::PointXYZ>::Ptr target(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::io::loadPCDFile<pcl::PointXYZ>("1.pcd", *target);
    pcl::PointCloud<pcl::PointXYZ>::Ptr source(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::io::loadPCDFile<pcl::PointXYZ>("2.pcd", *source);
 
    // 计算源点云和目标点云的法线
    pcl::PointCloud<pcl::PointNormal>::Ptr targetNormal = compute_normals(target);
    pcl::PointCloud<pcl::PointNormal>::Ptr sourceNormal = compute_normals(source);
 
    // 执行 LM-ICP
    pcl::PointCloud<pcl::PointNormal>::Ptr icp_cloud(new pcl::PointCloud<pcl::PointNormal>);
    Eigen::Matrix4f final_transform;
    run_lm_icp(sourceNormal, targetNormal, final_transform, icp_cloud);
 
    // 输出变换矩阵
    std::cout << "变换矩阵：\n" << final_transform << std::endl;
 
    // 变换点云并进行可视化
    pcl::PointCloud<pcl::PointXYZ>::Ptr aligned(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::transformPointCloud(*source, *aligned, final_transform);
    visualize_registration(source, target, aligned);
 
    return 0;
}

三、结果

本文标签： 算法点云配准PCLLM精配准