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

大家好，欢迎来到IT知识分享网。

点云处理

1.点云配准

2.点云粗配准算法

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

SAC-IA

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

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

NDT

3.点云精配准算法

3.1 基于优化的配准方法

3.1.1 基于ICP的变种方法

3.1.1.1 ICP

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/52.pcd") target = o3d.io.read_point_cloud("peizhun/23.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/30.pcd", *source) == -1 || pcl::io::loadPCDFile<pcl::PointXYZ>("../data/19.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; } 

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 pcd path = "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 clouds if (pcl::io::loadPCDFile<pcl::PointXYZ>("44.pcd", *source) == -1 || pcl::io::loadPCDFile<pcl::PointXYZ>("16.pcd", *target) == -1) { 
     std::cerr << "Failed to load PCD files." << std::endl; return -1; } // Perform GICP alignment pcl::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; } // Visualization boost::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

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> ("44.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> ("16.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; } 

3.1.4 基于半定规划的配准方法

3.2 特征学习的配准方法

3.3 基于端到端学习的方法

端到端学习方法的基本思想是将配准问题转化为回归问题。

此外还有多视角配准、多源配准等。

截止本文撰写停止之时，采用fast_gicp中的fgicp_mt利用了多线程技术实现了最快算法，teaser++的速度和效果也是可以的，对Maximal Cliques未做进一步探究。