OPEN3D学习笔记(四)
- Global registration
- Extract geometric feature
- Input
- RANSAC
- Local refinement
- Fast global registration
- Input
- Baseline
- Fast global registration
Global registration
这个通常产生不太严格的对齐结果,并用作局部方法的初始化。因为 ICP registration 和Colored point cloud registration都依赖于一个粗略的初始矩阵
Extract geometric feature
提取几何特征
我们向下采样点云,估计法线,然后为每个点计算FPFH特征。FPFH功能是一个33维矢量,它描述了点的局部几何特性。33维空间中最近的邻居查询可以返回具有相似局部几何结构的点。有关详细信息,请参见[Rasu2009]。
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 | def preprocess_point_cloud(pcd, voxel_size): # 传入参数pcd点云数据,voxel_size体素大小 print(":: Downsample with a voxel size %.3f." % voxel_size) pcd_down = pcd.voxel_down_sample(voxel_size) # 降采样 radius_normal = voxel_size * 2 # kdtree参数,用于估计法线的半径,一般设为体素大小的2倍 print(":: Estimate normal with search radius %.3f." % radius_normal) pcd_down.estimate_normals( o3d.geometry.KDTreeSearchParamHybrid(radius=radius_normal, max_nn=30)) # 估计法线的1个参数,使用混合型的kdtree,半径内取最多30个邻居 radius_feature = voxel_size * 5 # kdtree参数,用于估计特征的半径,设为体素大小的5倍 print(":: Compute FPFH feature with search radius %.3f." % radius_feature) pcd_fpfh = o3d.registration.compute_fpfh_feature( pcd_down, o3d.geometry.KDTreeSearchParamHybrid(radius=radius_feature, max_nn=100)) # 计算特征的2个参数,下采样的点云数据,搜索方法kdtree return pcd_down, pcd_fpfh # 返回降采样的点云、fpfh特征 |
Input
准备输入数据集
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 | def prepare_dataset(voxel_size): print(":: Load two point clouds and disturb initial pose.") # 加载2点云并打乱初始的位置 source = o3d.io.read_point_cloud("../../TestData/ICP/cloud_bin_0.pcd") target = o3d.io.read_point_cloud("../../TestData/ICP/cloud_bin_1.pcd") trans_init = np.asarray([[0.0, 0.0, 1.0, 0.0], [1.0, 0.0, 0.0, 0.0], [0.0, 1.0, 0.0, 0.0], [0.0, 0.0, 0.0, 1.0]]) source.transform(trans_init) draw_registration_result(source, target, np.identity(4)) # 可视化用初始矩阵配准结果 source_down, source_fpfh = preprocess_point_cloud(source, voxel_size) # 降采样和计算特征点 target_down, target_fpfh = preprocess_point_cloud(target, voxel_size) # 降采样和计算特征点 return source, target, source_down, target_down, source_fpfh, target_fpfh # 返回两个点云、两个降采样的点云、两个点云的特征点 voxel_size = 0.05 # means 5cm for this dataset,体素大小设定为0.05m source, target, source_down, target_down, source_fpfh, target_fpfh = prepare_dataset(voxel_size) # 调用函数获取这些值 |
RANSAC
我们使用RANSAC进行全球注册。在每个RANSAC迭代中,从源点云中提取ransac_n个随机点。通过查询33维FPFH特征空间中最近的邻居,可以检测到它们在目标点云中的对应点。修剪步骤需要使用快速修剪算法来快速及早拒绝错误匹配。
有以下函数:
CorrespondenceCheckerBasedOnDistance检查对齐的点云是否接近(小于指定的阈值)。
CorrespondenceCheckerBasedOnEdgeLength检查从源和目标对应中分别绘制的任意两个任意边(由两个对应顶点形成的线)的长度是否相似。
CorrespondenceCheckerBasedOnNormal考虑任何对应的顶点法线相似度。它计算两个法线向量的点积。阈值采用弧度值。
RANSACConvergenceCriteria。它定义了RANSAC迭代的最大数量和验证步骤的最大数量
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 | def execute_global_registration(source_down, target_down, source_fpfh, target_fpfh, voxel_size): # 传入2的点云的降采样,两个点云的特征、体素大小 distance_threshold = voxel_size * 1.5 # 设定距离阈值为体素的1.5倍 print(":: RANSAC registration on downsampled point clouds.") print(" Since the downsampling voxel size is %.3f," % voxel_size) print(" we use a liberal distance threshold %.3f." % distance_threshold) # 2个降采样的点云,两个点云的特征,距离阈值,一个函数,4,一个list[0.9的两个对应点的线段长度阈值,两个点的距离阈值],一个函数设定最大迭代次数和最大验证次数 result = o3d.registration.registration_ransac_based_on_feature_matching( source_down, target_down, source_fpfh, target_fpfh, distance_threshold, o3d.registration.TransformationEstimationPointToPoint(False), 4, [ o3d.registration.CorrespondenceCheckerBasedOnEdgeLength(0.9), o3d.registration.CorrespondenceCheckerBasedOnDistance( distance_threshold) ], o3d.registration.RANSACConvergenceCriteria(4000000, 500)) return result # 执行配准 result_ransac = execute_global_registration(source_down, target_down, source_fpfh, target_fpfh, voxel_size) print(result_ransac) draw_registration_result(source_down, target_down, result_ransac.transformation) # 源点云旋转平移到目标点云 |
Local refinement
上面的全局点云配准生成的结果还不够好,需要再精细配准(例如point-to-plane配准)
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 | # 传入参数:2个点云,2个点云的特征,体素大小 def refine_registration(source, target, source_fpfh, target_fpfh, voxel_size): distance_threshold = voxel_size * 0.4 # 距离阈值是体素大小的0.4倍 print(":: Point-to-plane ICP registration is applied on original point") print(" clouds to refine the alignment. This time we use a strict") print(" distance threshold %.3f." % distance_threshold) result = o3d.registration.registration_icp( source, target, distance_threshold, result_ransac.transformation, o3d.registration.TransformationEstimationPointToPlane()) return result result_icp = refine_registration(source, target, source_fpfh, target_fpfh, voxel_size) print(result_icp) draw_registration_result(source, target, result_icp.transformation) ''' 输出 :: Point-to-plane ICP registration is applied on original point clouds to refine the alignment. This time we use a strict distance threshold 0.020. registration::RegistrationResult with fitness=6.210275e-01, inlier_rmse=6.565175e-03, and correspondence_set size of 123482 Access transformation to get result. ''' |
Fast global registration
下面方法没有模型建议和评价,会加快配准,节省时间。
Input
跟上面的一样,调用prepare_dataset
1 2 3 | voxel_size = 0.05 # means 5cm for the dataset source, target, source_down, target_down, source_fpfh, target_fpfh = prepare_dataset(voxel_size) |
Baseline
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 | start = time.time() result_ransac = execute_global_registration(source_down, target_down, source_fpfh, target_fpfh, voxel_size) print("Global registration took %.3f sec.\n" % (time.time() - start)) print(result_ransac) draw_registration_result(source_down, target_down, result_ransac.transformation) ''' :: RANSAC registration on downsampled point clouds. Since the downsampling voxel size is 0.050, we use a liberal distance threshold 0.075. Global registration took 0.085 sec. registration::RegistrationResult with fitness=6.760504e-01, inlier_rmse=2.596653e-02, and correspondence_set size of 3218 Access transformation to get result. ''' |
Fast global registration
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 | def execute_fast_global_registration(source_down, target_down, source_fpfh, target_fpfh, voxel_size): distance_threshold = voxel_size * 0.5 print(":: Apply fast global registration with distance threshold %.3f" \ % distance_threshold) result = o3d.registration.registration_fast_based_on_feature_matching( source_down, target_down, source_fpfh, target_fpfh, o3d.registration.FastGlobalRegistrationOption( maximum_correspondence_distance=distance_threshold)) return result start = time.time() result_fast = execute_fast_global_registration(source_down, target_down, source_fpfh, target_fpfh, voxel_size) print("Fast global registration took %.3f sec.\n" % (time.time() - start)) print(result_fast) draw_registration_result(source_down, target_down, result_fast.transformation) ''' :: Apply fast global registration with distance threshold 0.025 Fast global registration took 0.128 sec. registration::RegistrationResult with fitness=5.054622e-01, inlier_rmse=1.743545e-02, and correspondence_set size of 2406 Access transformation to get result. ''' |
通过适当的配置,快速全局注册的准确性甚至可以与ICP媲美。更多实验结果请参考[Zhou2016]。