点云处理【七】(点云配准)

点云处理

第一章点云数据采集

1.点云配准

点云配准是将两个或多个点云数据集融合到一个统一的坐标系统中的过程。这通常是为了创建一个完整的模型或融合从不同视角采集的数据。
点云配准一般分为粗配准和精配准，粗配准指的是在两幅点云之间的变换完全未知的情况下进行较为粗糙的配准，目的主要是为精配准提供较好的变换初值；精配准则是给定一个初始变换，进一步优化得到更精确的变换。这里我们主要介绍精配准。

2.点云粗配准算法

2.1 基于特征匹配的配准算法

SAC-IA

2.2 基于穷举搜索的配准算法

4PCS:四点配准算法(4PCS)基于寻找四个点的一致集合，并尝试找到最佳的变换，使得这些点在源和目标点云中都是一致的。
该方法适用于重叠区域较小或者重叠区域发生较大变化场景点云配准，无需对输入数据进行预滤波和去噪，算法能够快速准确的完成点云配准
Sper4PCS

2.3 基于概率分布的配准算法

NDT

3.点云精配准算法

3.1 基于优化的配准方法

大部分基于优化的方法在于找对应点搜索和变换估计。对应点搜索是在另一个点云中找到每个点的匹配点。变换估计就是利用对应关系来估计变换矩阵。这两个阶段将进行迭代，以找到最佳的变换。
基于优化的配准方法大致可分为4种方法：基于ICP的变种方法、基于图优化的、基于GMM的和半定的配准方法

3.1.1 基于ICP的变种方法

3.1.1.1 ICP

迭代最近点(ICP)通过最小化平移矩阵t和旋转矩阵R，使两个点云重合度最高(每个点到点距离最短)。
ICP需要一个相对好的初始估计，否则容易陷入局部最小值。
open3d：

import open3d as o3d
import numpy as np
import copy
def draw_registration_result(source, target, transformation):"""Visualize the registration result."""source_temp = copy.deepcopy(source)source_temp.transform(transformation)o3d.visualization.draw_geometries([source_temp, target])# 1. 读取两个点云
source = o3d.io.read_point_cloud("peizhun/1697938934952.pcd")
target = o3d.io.read_point_cloud("peizhun/1697938935423.pcd")# 2. 下采样点云 (可选)
source_down = source.voxel_down_sample(voxel_size=5)
target_down = target.voxel_down_sample(voxel_size=5)# 3. 估计法线 (对于某些 ICP 变体可能需要)
# source_down.estimate_normals(search_param=o3d.geometry.KDTreeSearchParamHybrid(radius=100, max_nn=30))
# target_down.estimate_normals(search_param=o3d.geometry.KDTreeSearchParamHybrid(radius=100, max_nn=30))# 4. 执行 ICP
init_guess = np.eye(4)  # 如果你有一个初步的变换猜测，可以替换这里
threshold = 50  # 两点之间的最大距离
reg_p2p = o3d.pipelines.registration.registration_icp(source_down, target_down, threshold, init_guess,o3d.pipelines.registration.TransformationEstimationPointToPoint())# 5. 显示结果
print("Transformation is:")
print(reg_p2p.transformation)
draw_registration_result(source, target, reg_p2p.transformation)

请添加图片描述
pcl

#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/registration/icp.h>
#include <pcl/visualization/pcl_visualizer.h>int main(int argc, char** argv) {pcl::PointCloud<pcl::PointXYZ>::Ptr source(new pcl::PointCloud<pcl::PointXYZ>);pcl::PointCloud<pcl::PointXYZ>::Ptr target(new pcl::PointCloud<pcl::PointXYZ>);pcl::PointCloud<pcl::PointXYZ>::Ptr output(new pcl::PointCloud<pcl::PointXYZ>);// 读取点云数据if (pcl::io::loadPCDFile<pcl::PointXYZ>("../data/1695551473330.pcd", *source) == -1 || pcl::io::loadPCDFile<pcl::PointXYZ>("../data/1695551473719.pcd", *target) == -1) {std::cout << "Failed to read the PCD files!" << std::endl;return -1;}// 创建 ICP 对象并设置参数pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;icp.setInputSource(source);icp.setInputTarget(target);icp.setMaximumIterations(50);icp.setTransformationEpsilon(1e-8);icp.setMaxCorrespondenceDistance(50);  // 可以根据需要调整icp.align(*output);pcl::visualization::PCLVisualizer::Ptr viewer(new pcl::visualization::PCLVisualizer("ICP"));viewer->setBackgroundColor(0, 0, 0);viewer->addPointCloud<pcl::PointXYZ>(target, "target cloud");viewer->addPointCloud<pcl::PointXYZ>(output, "output cloud");viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "target cloud");viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "output cloud");viewer->addCoordinateSystem(1.0);viewer->initCameraParameters();viewer->spin();return 0;
}

请添加图片描述

ICP的改进算法：
（1）Point-to-Plane ICP：将icp中点到点的距离改为点到目标面的距离，这样就不容易陷入局部最优，但也增加了计算量。
（2）Plane-to-Plane ICP：将icp中点到点的距离改为面到面的距离，精度更高，但进一步增加了计算量。
（3）Generalized ICP (GICP)：结合了点到点、点到面、面到面的距离，效果更好。
（4）Normal Iterative Closest Point (NICP)：考虑了法向量、局部曲率信息，效果进一步提高。
mbicp

3.1.1.2 GICP

pcl和open3d都提供了gicp，可直接调用：

Open3d

import open3d as o3d
import numpy as np
import copy
import os
def delete_zero(pcd):# 将点云转为 numpy 数组points = np.asarray(pcd.points)# 找到非0的点non_zero_indices = np.where(np.any(points != 0, axis=1))[0]# 根据这些索引筛选点filtered_points = points[non_zero_indices]# 更新点云对象pcd.points = o3d.utility.Vector3dVector(filtered_points)return pcdpath = "peizhun"
paths = os.listdir(path)
target = o3d.io.read_point_cloud(os.path.join(path,paths[0]))
target = delete_zero(target)for index in paths[:2]:source = o3d.io.read_point_cloud(os.path.join(path,index))print(source)print("---"+index)source = delete_zero(source)print(source)threshold = 100  # 距离阈值trans_init = np.asarray([[1.0, 0.0, 0.0, 0.0],[0.0, 1.0, 0.0, 0.0],[0.0, 0.0, 1.0, 0],[0.0, 0.0, 0.0, 1.0]])  # 初始变换矩阵，一般由粗配准提供# -------------------------------------------------# 计算两个重要指标，fitness计算重叠区域（内点对应关系/目标点数）。越高越好。# inlier_rmse计算所有内在对应关系的均方根误差RMSE。越低越好。evaluation = o3d.pipelines.registration.evaluate_registration(source, target, threshold, trans_init)generalized_icp = o3d.pipelines.registration.registration_generalized_icp(source, target, threshold, trans_init,o3d.pipelines.registration.TransformationEstimationForGeneralizedICP(),o3d.pipelines.registration.ICPConvergenceCriteria(max_iteration=35))  # 设置最大迭代次数source.transform(generalized_icp.transformation)target = target+source# -----------------可视化配准结果--------------------
o3d.visualization.draw_geometries([target])

请添加图片描述

PCL

#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/registration/gicp.h>
#include <pcl/visualization/pcl_visualizer.h>int main()
{pcl::PointCloud<pcl::PointXYZ>::Ptr source(new pcl::PointCloud<pcl::PointXYZ>);pcl::PointCloud<pcl::PointXYZ>::Ptr target(new pcl::PointCloud<pcl::PointXYZ>);pcl::PointCloud<pcl::PointXYZ>::Ptr result(new pcl::PointCloud<pcl::PointXYZ>);// Load point cloudsif (pcl::io::loadPCDFile<pcl::PointXYZ>("1697938951044.pcd", *source) == -1 || pcl::io::loadPCDFile<pcl::PointXYZ>("1697938962416.pcd", *target) == -1){std::cerr << "Failed to load PCD files." << std::endl;return -1;}// Perform GICP alignmentpcl::GeneralizedIterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> gicp;gicp.setInputSource(source);gicp.setInputTarget(target);gicp.align(*result);if (gicp.hasConverged()){std::cout << "GICP has converged. Score: " << gicp.getFitnessScore() << std::endl;std::cout << "Transformation matrix:" << std::endl;std::cout << gicp.getFinalTransformation() << std::endl;}else{std::cerr << "GICP did not converge." << std::endl;return -1;}// Visualizationboost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("GICP Viewer"));viewer->setBackgroundColor(0, 0, 0);viewer->addPointCloud<pcl::PointXYZ>(source, "source cloud");viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 1, 0, 0, "source cloud");viewer->addPointCloud<pcl::PointXYZ>(result, "result cloud");viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 0, 1, 0, "result cloud");viewer->initCameraParameters();viewer->spin();return 0;
}

请添加图片描述

NICP

const转换错误，去const

缺liblz4
https://github.com/yorsh87/nicp/issues/23

3.1.2 基于图优化的配准方法

将点云转化成图结构，像上文中的GICP就是Graph+ICP。

3.1.3 基于GMM的配准方法

也就是基于高斯混合概率模型的方法，如GICP、NDT、CPD等，上面的GICP就是GMM+ICP实现的。

3.1.3.1 NDT

正态分布变换 (NDT)通过将点云表示为一系列的正态分布来工作。然后，通过最大化两个点云之间的概率分布的重叠来寻找最佳的变换。

PCL

#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/registration/ndt.h>
#include <pcl/visualization/pcl_visualizer.h>int main(int argc, char** argv)
{// 加载点云文件pcl::PointCloud<pcl::PointXYZ>::Ptr target_cloud (new pcl::PointCloud<pcl::PointXYZ>);if (pcl::io::loadPCDFile<pcl::PointXYZ> ("1697938951044.pcd", *target_cloud) == -1){PCL_ERROR ("Couldn't read target pcd file\n");return (-1);}pcl::PointCloud<pcl::PointXYZ>::Ptr input_cloud (new pcl::PointCloud<pcl::PointXYZ>);if (pcl::io::loadPCDFile<pcl::PointXYZ> ("1697938962416.pcd", *input_cloud) == -1){PCL_ERROR ("Couldn't read input pcd file\n");return (-1);}// 设置NDT的参数pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ> ndt;ndt.setTransformationEpsilon(10);ndt.setStepSize(1);ndt.setResolution(100);ndt.setMaximumIterations(35);ndt.setInputSource(input_cloud);ndt.setInputTarget(target_cloud);// 配准pcl::PointCloud<pcl::PointXYZ>::Ptr output_cloud (new pcl::PointCloud<pcl::PointXYZ>);ndt.align(*output_cloud);std::cout << "Normal Distributions Transform has converged:" << ndt.hasConverged()<< " score: " << ndt.getFitnessScore() << std::endl;// 可视化结果pcl::visualization::PCLVisualizer viewer("NDT");// 添加目标点云，设置为白色pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> target_color(target_cloud, 255, 255, 255);viewer.addPointCloud<pcl::PointXYZ>(target_cloud, target_color, "target_cloud");// 添加输入点云，设置为绿色pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> input_color(input_cloud, 0, 255, 0);viewer.addPointCloud<pcl::PointXYZ>(input_cloud, input_color, "input_cloud");// 添加输出点云，设置为红色pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> output_color(output_cloud, 255, 0, 0);viewer.addPointCloud<pcl::PointXYZ>(output_cloud, output_color, "output_cloud");viewer.spin();return 0;
}