[笔记]Open3D基础知识及例程demo

最近需要进行多张点云图合并+生成mesh,发现Open3D是一个不错的工具。
文中所有用到的代码和数据我上传到了:
百度网盘:
链接：https://pan.baidu.com/s/1ra3bh6ldPVZhv7vfoeHqvQ
提取码：2023
🐰欢迎下载!

文章目录

- 一、介绍
- - 1.1 在notebook中可视化
- 二、点云降采样 & 法向量(Normal Estimation)
- - 2.1 点云降采样(Voxel Downsampling)
  - 2.2 点云法向量估计(Voxel Normal Estimation)
- 三、点云的基础操作 & 点云聚类(Clustering)
- - 3.1 裁剪点云
  - 3.2 给点云上色
  - 3.3 计算两张点云图的距离
  - 3.4 点云的边界框(Bounding Box)
  - 3.5 点云的凸包(Convex Hull)
  - 3.6 DBSCAN 聚类(clustering)
  - 3.7 平面分割(Plane Segmentation)
  - 3.8 过滤掉被遮挡的点(Hidden Point Removal)
- 四、移除异常点(Outlier Removal)
- - 4.1 `every_k_points`降采样
  - 4.2 `pcd.select_by_index(ind)`
  - 4.3 移除离群点(based on 平均距离)
  - 4.4 移除离群点(based on 邻近点数量)
- 五、RGBD $\to$ Point Cloud （Single)
- 六、自己仅用RGB生成点云
- 七、根据2张点云估计变换矩阵(小范围内finetune, Local Registration)
- - 7.1 point-to-point ICP
- 八、根据2张点云估计变换矩阵(较大范围, Global Registration)
- - 8.1 Step 1 提取出FPFH特征
  - 8.2 Step 2 RANSAC算法
  - 8.3 Fast Global Registration
- 九、RGB-Video $\to$ A Single Point Cloud (借助Agisoft Metashape软件)
- 十、同时用 color 和 depth 来做 pose estimation (Colored PointCloud Registration)
- 十一、PointCloud $\to$ TriangleMesh (表面重建 , Surface Reconstruction)
- - 11.1 Alpha shapes
  - 11.2 Ball Pivoting (BPA)
  - 11.3 Poisson Surface Reconstruction
  - 11.4 density上色(cmap)
  - 11.5 mask掉density较小的部分
- 十二、点云动画
- 十三、RGBD视觉里程计(Visual Odometry)
- 十四、高级一点的ICP
- - 14.1 Vanilla ICP VS Robust Kernel ICP
  - 14.2 用numpy修改点云的某个属性
- 十五、PointClouds + Poses $\to$ A single PointCloud
- - 15.1 简单叠加版
  - 15.2 官方例程的方案
- Reference

我的版本:

Package	Version
open3d	0.13.0
opencv	4.6.0
numpy	1.21.5

一、介绍

open3D有python和cpp的库，python库就是cpp封装好以后的API。

核心功能：

3D数据结构
3D数据处理
场景重建(scene reconstruction)
表面校准(surface alignment)
PBR 渲染(PBR(Physically Based Rendering))

1.1 在notebook中可视化

https://github.com/seminar2012/Open3D/blob/master/examples/Python/open3d_tutorial.py

import os
import open3d as o3d
import open3d_tutorial as o3dtuto3dtut.interactive = not "CI" in os.environpcd = o3d.io.read_point_cloud('./Vkitti2_Scene06.pcd')
print(np.asarray(pcd.points).shape)
o3d.visualization.draw_geometries([pcd])
'''
o3d.visualization.draw_geometries(...)的参数们:zoom(0~1): 放大倍数 lookat: 视角point_show_normal: 是否显示法向量width (default=1920): 窗口宽height (default=1080): 窗口高mesh_show_wireframe: 是否可视化网格
'''----------------------------------------
(253320, 3)

二、点云降采样 & 法向量(Normal Estimation)

2.1 点云降采样(Voxel Downsampling)

我们有时候有很多点，甚至有一些outliers(异常值)，这时候我们需要降采样。

在交互界面直接 - 和 + 键是可以让点云稀疏 or 稠密的。

降采样算法有两步:

(1) 将 point clouds 分成 voxels

(2) 每一个 occupied voxel 只生成一个点(对voxel内点云求均值)

downpcd = pcd.voxel_down_sample(voxel_size=1)
print(np.asarray(downpcd.points).shape)
o3d.visualization.draw_geometries([downpcd])-----------------------------
(253320, 3) -> (1127, 3)

2.2 点云法向量估计(Voxel Normal Estimation)

经过降采样后，我们可以估计每个点的法向量，这有助于我们根据法向量的朝向进行分割。

使用KDTree Search算法, radius和max_nn分别是搜索半径和近点数量，这个要根据具体情况设置或试出来。

在交互页面 N 就可以看到法向量。

    downpcd = pcd.voxel_down_sample(voxel_size=1)downpcd.estimate_normals(search_param=o3d.geometry.KDTreeSearchParamHybrid(radius=5, max_nn=40))o3d.visualization.draw_geometries([downpcd])

 # 查看具体法向量的值
print(np.asarray(downpcd.points).shape, len(downpcd.normals))downpcd.points[0], downpcd.normals[0]-------------------------------(1127, 3) 1127
(array([ -4.86774654,   9.96530266, -21.93620071]),array([ 0.96354307,  0.25091144, -0.09288806]))

三、点云的基础操作 & 点云聚类(Clustering)

3.1 裁剪点云

json文件要求指定20个点以构成包围被裁剪物体的截面。其内容如下所示:

{"axis_max" : 4.022921085357666,"axis_min" : -0.76341366767883301,"bounding_polygon" : [[ 2.6509309513852526, 0.0, 1.6834473132326844 ],[ 2.5786428246917148, 0.0, 1.6892074266735244 ],[ 2.4625790337552154, 0.0, 1.6665777078297999 ],[ 2.2228544982251655, 0.0, 1.6168160446813649 ],[ 2.166993206001413, 0.0, 1.6115495157201662 ],[ 2.1167895865303286, 0.0, 1.6257706054969348 ],[ 2.0634657721747383, 0.0, 1.623021658624539 ],[ 2.0568612343437236, 0.0, 1.5853892911207643 ],[ 2.1605399001237027, 0.0, 0.96228993255083017 ],[ 2.1956669387205228, 0.0, 0.95572746049785073 ],[ 2.2191318790575583, 0.0, 0.88734449982108754 ],[ 2.2484881847925919, 0.0, 0.87042807267013633 ],[ 2.6891234157295827, 0.0, 0.94140677988967603 ],[ 2.7328692490470647, 0.0, 0.98775740674840251 ],[ 2.7129337547575547, 0.0, 1.0398850034649203 ],[ 2.7592174072415405, 0.0, 1.0692940558509485 ],[ 2.7689216419453428, 0.0, 1.0953914441371593 ],[ 2.6851455625455669, 0.0, 1.6307334122162018 ],[ 2.6714776099981239, 0.0, 1.675524657088997 ],[ 2.6579576128816544, 0.0, 1.6819127849749496 ]],"class_name" : "SelectionPolygonVolume","orthogonal_axis" : "Y","version_major" : 1,"version_minor" : 0
}

具体代码:

# 这里用的是open3D内置的点云数据
# 链接：https://pan.baidu.com/s/1O4s8tFOvExhuKMl2OCv4Kg
# 提取码：82u1pcd = o3d.io.read_point_cloud('ch3/fragment.ply')vol = o3d.visualization.read_selection_polygon_volume('ch3/cropped.json')
chair = vol.crop_point_cloud(pcd)
o3d.visualization.draw_geometries([chair])

3.2 给点云上色

chair.paint_uniform_color([1, 0.706,0]) # [R, G, B]
o3d.visualization.draw_geometries([chair])

3.3 计算两张点云图的距离

这个计算的是点云图B中所有点距离点云图A中点 $P_A$ 最近的点的距离。这样做可以用于检测我们在场景点云图fragment.ply 中是否有椅子这个对象 chair。

# 剔除掉了椅子
pcd = o3d.io.read_point_cloud('ch3/fragment.ply')vol = o3d.visualization.read_selection_polygon_volume('ch3/cropped.json')
chair = vol.crop_point_cloud(pcd)dists = pcd.compute_point_cloud_distance(chair)
dists = np.asarray(dists)
ind = np.where(dists > 0.01)[0]
pcd_without_chair = pcd.select_by_index(ind)o3d.visualization.draw_geometries([pcd_without_chair])

3.4 点云的边界框(Bounding Box)

open3d提供了AxisAlignedBoundingBox和OrientedBoundingBox这两种边界框。

aabb = chair.get_axis_aligned_bounding_box()
aabb.color = (1, 0, 0)
obb = chair.get_oriented_bounding_box()
obb.color = (0, 1, 0)
o3d.visualization.draw_geometries([chair, aabb, obb])

(红色代表AxisAlignedBoundingBox，绿色代表OrientedBoundingBox)

3.5 点云的凸包(Convex Hull)

凸包(Convex Hull)是包住一团点云的最小的triangle mesh网格。

import os
import numpy as np
import open3d as o3d
import open3d_tutorial as o3dtut# 下载斯坦福的经典兔子
# 这里记得修改 open3d_tutorial.py line 196 里面的路径
pcl = o3dtut.get_bunny_mesh().sample_points_poisson_disk(number_of_points=2000)hull, _ = pcl.compute_convex_hull()
hull_ls = o3d.geometry.LineSet.create_from_triangle_mesh(hull)
hull_ls.paint_uniform_color((1,0,0))
o3d.visualization.draw_geometries([pcl, hull_ls])

3.6 DBSCAN 聚类(clustering)

open3D有DBSCAN这种密集聚类算法，需要两个参数:

eps: 一个cluster中邻近点的距离

min_points: 一个cluster中至少有多少个点

import open3d as o3d 
import matplotlib.pyplot as plt
import numpy as nppcd = o3d.io.read_point_cloud('ch1/Vkitti2_Scene06.pcd')
pcd = pcd.voxel_down_sample(voxel_size=0.3)with o3d.utility.VerbosityContextManager(o3d.utility.VerbosityLevel.Debug) as cm:labels = np.array(pcd.cluster_dbscan(eps=0.8, min_points=20, print_progress=True))max_label = labels.max()
print(f"Point clouds has {max_label+1} clusters.")
colors = plt.get_cmap('tab20')(labels / (max_label if max_label > 0 else 1))
colors[labels<0] = 0 # label "-1" 表示噪声pcd.colors = o3d.utility.Vector3dVector(colors[:,:3])
o3d.visualization.draw_geometries([pcd])

(黑色的是噪点)

3.7 平面分割(Plane Segmentation)

open3D也支持基于RANSAC算法的平面分割，平面分割(Plane Segmentation)是指找到一个包含点数最多的平面。

segment_plane函数有3个参数:
- distance_threshold: 一个点距离自己所属于的估计平面的最近距离
- ransac_n: 用于估计一个平面需要随机采样的点数
- num_iterations: 一个估计平面被采样并验证的频率

函数返回 $(a, b, c, d)$ : $a x + b y + cz + d = 0$

pcd = o3d.io.read_point_cloud('ch1/Vkitti2_Scene06.pcd')
pcd = pcd.voxel_down_sample(voxel_size=0.1)plane_model, inliers = pcd.segment_plane(distance_threshold=0.4, ransac_n=3, num_iterations=1000)[a, b, c, d] = plane_model
print(f'{a}x + {b}y + {c}z + d = 0')inlier_cloud = pcd.select_by_index(inliers)
inlier_cloud.paint_uniform_color([1, 0, 0])
outlier_cloud = pcd.select_by_index(inliers, invert=True)
o3d.visualization.draw_geometries([inlier_cloud, outlier_cloud])

3.8 过滤掉被遮挡的点(Hidden Point Removal)

丢掉被挡住的点云。

import open3d as o3d
import numpy as nppcd = o3d.io.read_point_cloud('ch1/Vkitti2_Scene06.pcd')
pcd = pcd.voxel_down_sample(voxel_size=0.2) # len(pcd.points) = 6909diameter = np.linalg.norm(np.asarray(pcd.get_max_bound()) - np.asarray(pcd.get_min_bound()))camera = [0, 0, diameter]
radius = diameter * 1000_, pt_map = pcd.hidden_point_removal(camera, radius)pcd = pcd.select_by_index(pt_map)o3d.visualization.draw_geometries([pcd])

四、移除异常点(Outlier Removal)

点云通常包含: noise, artifacts, remove等导致的异常点。

4.1 `every_k_points`降采样

every_k_points这种降采样方法可以直接指定缩小多少倍的点。

import open3d as o3dpcd = o3d.io.read_point_cloud('ch1/Vkitti2_Scene06.pcd')
print(len(pcd.points))
pcd = pcd.uniform_down_sample(every_k_points=5)
print(len(pcd.points))o3d.visualization.draw_geometries([pcd])
----------------------
253320
50664

4.2 `pcd.select_by_index(ind)`

根据idx过滤点云:

def display_inlier_outlier(pcd, ind):inlier_cloud = pcd.select_by_index(ind)outlier_cloud = pcd.select_by_index(ind, invert=True)outlier_cloud.paint_uniform_color([1, 0, 0])inlier_cloud.paint_uniform_color([0.8, 0.8, 0.8])o3d.visualization.draw_geometries([inlier_cloud, outlier_cloud])return inlier_cloud, outlier_cloud

4.3 移除离群点(based on 平均距离)

如果一个点距离他的临近点的距离>>所有点云距离临近点距离的均值，我们就丢掉这些点。

statiscal_outlier_removal函数有2个参数:
- nb_neighbours: 在计算邻近点距离均值的时候，选取几个邻近点。
- std_ratio: 相对于平均每距离的阈值，这个数越小，滤除的点越少。

import open3d as o3dpcd = o3d.io.read_point_cloud('ch1/Vkitti2_Scene06.pcd')
pcd = pcd.uniform_down_sample(every_k_points=20) # 12666cl, ind = pcd.remove_statistical_outlier(nb_neighbors=20, std_ratio=5.0)display_inlier_outlier(pcd, ind)

4.4 移除离群点(based on 邻近点数量)

radius_outlier_removal有2个参数:
- nb_points: 邻近球体内至少应该有的点数
- radius: 邻近球体的半径

import open3d as o3dpcd = o3d.io.read_point_cloud('ch1/Vkitti2_Scene06.pcd')
pcd = pcd.uniform_down_sample(every_k_points=20) # 12666cl, ind = pcd.remove_radius_outlier(nb_points=20, radius=5.0)display_inlier_outlier(pcd, ind)

五、RGBD $\to$ Point Cloud （Single)

open3D库有内置的图片类，通过read-image,write_image,filter_image可以调用它们，open3D的图片类也是可以直接np.asarray( )的。

一个open3D的RGBD图是这样的: RGBDImage.depth和RGBDImage.color，我们需要 register 相同分辨率的RGB和D才能得到RGBDImage。

import open3d as o3d
import numpy as npcolor_raw = o3d.io.read_image('./ch5/color/0.jpg')
depth_raw = o3d.io.read_image('./ch5/depth/0.png')rgbd_image = o3d.geometry.RGBDImage.create_from_color_and_depth(color_raw, depth_raw,depth_scale=1000.0, depth_trunc=10, convert_rgb_to_intensity=False)
print(np.max(np.asarray(rgbd_image.color)))
print(np.max(np.asarray(rgbd_image.depth)))
# rgbd_image.color 被转化为灰度图 [0,1] float
# rgbd_depth [0,3] 表示实际的米数

这边我们用的是o3d.camera.PinholeCameraIntrinsicParameters.PrimeSenseDefault作为内参矩阵，默认参数为: (fx, fy)=(525, 525)， (cx, cy)=(319.5, 239.5)

pcd = o3d.geometry.PointCloud.create_from_rgbd_image(rgbd_image, o3d.camera.PinholeCameraIntrinsic(o3d.camera.PinholeCameraIntrinsicParameters.PrimeSenseDefault))
# 翻转一下，不然朝向不对
pcd.transform([[1,0,0,0],[0,-1,0,0],[0,0,-1,0],[0,0,0,1]])
o3d.visualization.draw_geometries([pcd])

上面的工作是单张视角的，如果我们有多视角的RGBD，就可以stitch所有的点，得到一个很不错的3D点云模型。

open3D不支持 pgm/ppm 格式的图片儿。

六、自己仅用RGB生成点云

相比于五，自己需要提前根据单目RGB估计出深度才可以。

这个工作是根据RGB估计深度值，捡到宝了: https://github.com/nianticlabs/monodepth2

作者是这样搞的: https://www.youtube.com/watch?v=jid-53uPQr0

Step 1: 根据RGB生成D

#import numpy as np
import cv2
import timedef img_to_depth(img ,model):imgHeight, imgWidth, channels = img.shape# start time to calculate FPSstart = time.time()img = cv2.cvtColor(img, cv2.COLOR_BGR2RGB)# Create Blob from Input Image# MiDaS v2.1 Large ( Scale : 1 / 255, Size : 384 x 384, Mean Subtraction : ( 123.675, 116.28, 103.53 ), Channels Order : RGB )blob = cv2.dnn.blobFromImage(img, 1/255., (384,384), (123.675, 116.28, 103.53), True, False)# MiDaS v2.1 Small ( Scale : 1 / 255, Size : 256 x 256, Mean Subtraction : ( 123.675, 116.28, 103.53 ), Channels Order : RGB )#blob = cv2.dnn.blobFromImage(img, 1/255., (256,256), (123.675, 116.28, 103.53), True, False)# Set input to the modelmodel.setInput(blob)# Make forward pass in modeloutput = model.forward()output = output[0,:,:]output = cv2.resize(output, (imgWidth, imgHeight))# Normalize the outputoutput = cv2.normalize(output, None, 0, 1, norm_type=cv2.NORM_MINMAX, dtype=cv2.CV_32F)return outputif __name__ == '__main__':path_model = "./"# Read Networkmodel_name = "model-f6b98070.onnx"; # MiDaS v2.1 Large# Load the DNN modelmodel = cv2.dnn.readNet(path_model + model_name)if (model.empty()):print("Could not load the neural net! - Check path")model.setPreferableBackend(cv2.dnn.DNN_BACKEND_CUDA)model.setPreferableTarget(cv2.dnn.DNN_TARGET_CUDA)depth_normal = img_to_depth(cv2.imread('./wuke_color.png'), model)depth_normal *= 255depth_normal = depth_normal.astype('uint8')cv2.imwrite('./depth.png', depth_normal)

Step 2: RGBD生成点云

import open3d as o3d
import numpy as npcolor_raw = o3d.io.read_image('./ch6/wuke_color.png')
depth_raw = o3d.io.read_image('./ch6/depth.png')rgbd_image = o3d.geometry.RGBDImage.create_from_color_and_depth(color_raw, depth_raw,depth_scale=10.0, depth_trunc=10, convert_rgb_to_intensity=False)
print(np.max(np.asarray(rgbd_image.color)))
print(np.max(np.asarray(rgbd_image.depth)))
# rgbd_image.color 被转化为灰度图 [0,1] float
# rgbd_depth [0,3] floatpcd = o3d.geometry.PointCloud.create_from_rgbd_image(rgbd_image, o3d.camera.PinholeCameraIntrinsic(o3d.camera.PinholeCameraIntrinsicParameters.PrimeSenseDefault))
# 翻转一下，不然朝向不对
pcd.transform([[1,0,0,0],[0,-1,0,0],[0,0,-1,0],[0,0,0,1]])
o3d.visualization.draw_geometries([pcd])

七、根据2张点云估计变换矩阵(小范围内finetune, Local Registration)

传统SLAM方法有:
- 概率法: EKF, Monte-Carlo, Hough, etc.
- 匹配法(Scan Matching): ICP, RANSAC, Scan-to-Map, etc.
ICP(Iterative Closest Point, 迭代最近点)

原理: 找到2次点云之间的R和T, 使得两张点云的最近邻居之间的距离最小化。

步骤:
- (1) 找到目标点云 $P$ 和源点云 $Q$ 之间的相关的点集 $K={(p,q)}$
- (2) 不断更新 $T$ ,直到满足 $E (T) < t h res h o l d$
  $sum_{(p,q)in K} ||p-Tq||^2$
Geometric registration

7.1 point-to-point ICP

我们用draw_registration_result函数来可视化alignment这两帧点云的过程。

import copy
import open3d as o3ddef draw_registration_result(source, target, transformation):source_temp = copy.deepcopy(source)target_temp = copy.deepcopy(target)source_temp.paint_uniform_color([1, 0.706, 0])target_temp.paint_uniform_color([0, 0.651, 0.929])source_temp.transform(transformation)o3d.visualization.draw_geometries([source_temp, target_temp])

函数evaluate_registration有2个参数:
- fitness: 重叠区域，越大越好
- inlier_rmse: 两组点云的RMSE阈值，终止条件
函数 TransformationEstimationPointToPoint提供了ICP算法的接口。

这个我感觉是在init设置的比较好的时候才行，起到finetune的作用，如果init乱设的，这个不行的

import open3d as o3d
import numpy as npsource = o3d.io.read_point_cloud('ch7/0.pcd')
target = o3d.io.read_point_cloud('ch7/10.pcd')
source = source.uniform_down_sample(every_k_points=10)
target = target.uniform_down_sample(every_k_points=10)# len(source.points), len(target.points) (8044, 8241)threshold = 0.02
trans_init = np.asarray([[1, 0, 0, 0],[0, 1,0, 0],[0, 0, 1, 0.25],[0.0, 0.0, 0.0, 1.0]])# draw_registration_result(source, target, trans_init)
# 算一下 trans_init的误差
# evaluation = o3d.pipelines.registration.evaluate_registration(source, target, threshold, trans_init)
# print(evaluation)

threshold = 0.02
reg_p2p = o3d.pipelines.registration.registration_icp(source, target, threshold, trans_init, o3d.pipelines.registration.TransformationEstimationPointToPoint(),o3d.pipelines.registration.ICPConvergenceCriteria(max_iteration=100000))print(reg_p2p.transformation)

八、根据2张点云估计变换矩阵(较大范围, Global Registration)

Local版本的ICP registration和Colored Point Cloud Registration都很依赖设定的初始条件。实际情况下，没法知道这些初始条件，因此我们需要Global Registration.

8.1 Step 1 提取出FPFH特征

(FPFH是一个描述一个点的局部几何性质的33维的特征向量，即每个点都有一个33维的FPFH。)

import copy
import open3d as o3d
import numpy as npdef draw_registration_result(source, target, transformation):source_temp = copy.deepcopy(source)target_temp = copy.deepcopy(target)source_temp.paint_uniform_color([1, 0.706, 0])target_temp.paint_uniform_color([0, 0.651, 0.929])source_temp.transform(transformation)o3d.visualization.draw_geometries([source_temp, target_temp])def preprocess_point_cloud(pcd, voxel_size):# 降采样pcd_down = pcd.voxel_down_sample(voxel_size)# 估计法向量radius_normal = voxel_size * 2pcd_down.estimate_normals(o3d.geometry.KDTreeSearchParamHybrid(radius=radius_normal, max_nn=30))radius_feature = voxel_size * 5# 计算FPFH特征pcd_fpfh = o3d.pipelines.registration.compute_fpfh_feature(pcd_down, o3d.geometry.KDTreeSearchParamHybrid(radius=radius_feature, max_nn=100))return pcd_down, pcd_fpfhdef prepare_dataset(voxel_size):'''voxel_size的单位是m'''source = o3d.io.read_point_cloud('ch7/0.pcd')target = o3d.io.read_point_cloud('ch7/10.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, 1.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_fph = preprocess_point_cloud(source, voxel_size)target_down, target_fph = preprocess_point_cloud(target, voxel_size)return source, target, source_down, target_down, source_fph, target_fphvoxel_size = 0.2
source, target, source_down, target_down, source_fph, target_fph = prepare_dataset(voxel_size)

8.2 Step 2 RANSAC算法

我们使用RANSAC算法实现global registraiton：
- 从点云中采样ransac_n个点，每个点的邻近点是33维FPFH特征向量空间上的邻近，不是三维空间距离意义上的临近。
- 剪枝算法是丢掉那些匹配错的点们， open3D提供了以下3中剪枝(prune)算法:
  - (1) CorrespondenceCheckerBasedOnDistance: 检查对齐的点的间距是否小于阈值。
  - (2) CorrespondenceCheckerBasedOnEdgeLength: 检查任意两条边的长度是否是是否类似:
    $∣∣edgesource∣∣>0.9⋅∣∣edgetarget∣∣and∣∣edgetarget∣∣>0.9⋅∣∣edgesource∣∣||edge_{source}|| > 0.9 cdot ||edge_{target}|| quad and quad ||edge_{target}|| > 0.9 cdot ||edge_{source}||$
  - (3) CorrespondenceCheckerBasedOnNormal: 计算两个点的法向量之间的夹角，阈值是考控制夹角的。

主要使用的函数是registration_ransac_based_on_feature_matching, 主要参数是RANSACConvergenceCriteria，代表RANSAC算法迭代的最大次数。

def execute_global_registration(source_down, target_down, source_fph, target_fph, voxel_size):distance_threshold = voxel_size * 1.5result = o3d.pipelines.registration.registration_ransac_based_on_feature_matching(source_down, target_down, source_fph, target_fph, True, distance_threshold,o3d.pipelines.registration.TransformationEstimationPointToPoint(False),3,[o3d.pipelines.registration.CorrespondenceCheckerBasedOnEdgeLength(0.9),o3d.pipelines.registration.CorrespondenceCheckerBasedOnDistance(distance_threshold)],o3d.pipelines.registration.RANSACConvergenceCriteria(10000))return resultvoxel_size = 0.2
source, target, source_down, target_down, source_fph, target_fph = prepare_dataset(voxel_size)result_ransac = execute_global_registration(source_down, target_down, source_fph, target_fph, voxel_size)
print(result_ransac)
draw_registration_result(source_down, target_down, result_ransac.transformation)

但是这个效果并不让我满意，因此我们需要用七的方法进行refine:

def refine_registration(source, target, source_fph, target_fph, voxel_size):# distance_threshold = voxel_size * 0.4# result = o3d.pipelines.registration.registration_icp(#     source, target, distance_threshold, #     result_ransac.transformation,#     o3d.pipelines.registration.TransformationEstimationPointToPlane()# )threshold = 0.01result = o3d.pipelines.registration.registration_icp(source, target, threshold, result_ransac.transformation, o3d.pipelines.registration.TransformationEstimationPointToPoint(),o3d.pipelines.registration.ICPConvergenceCriteria(max_iteration=10000))return result result_icp = refine_registration(source.uniform_down_sample(every_k_points=10), target.uniform_down_sample(every_k_points=10), source_fph, target_fph, voxel_size)
print(result_icp)draw_registration_result(source, target, result_icp.transformation)

8.3 Fast Global Registration

# Fast Global Registrationvoxel_size = 0.05
source, target, source_down, target_down, source_fph, target_fph = prepare_dataset(voxel_size)def execute_fast_global_registration(source_down, target_down, source_fph, target_fph):distance_threshold = voxel_size * 0.5result = o3d.pipelines.registration.registration_ransac_based_on_feature_matching(source_down, target_down, source_fph, target_fph, True, voxel_size)return resultresult_fast = execute_fast_global_registration(source_down, target_down, source_fph, target_fph)draw_registration_result(source_down, target_down, result_fast.transformation)

(这个open3d版本会导致这里报错，参考: https://blog.csdn.net/qq_45912037/article/details/127792718)

九、RGB-Video $\to$ A Single Point Cloud (借助Agisoft Metashape软件)

https://www.agisoft.com/downloads/installer/

这个软件不仅有GUI界面，还有Python接口，还是比较叼的，我只试用了30天，有亿点贵。

不过淘宝上好像只要十几块钱。

Python API

安装: https://www.cnblogs.com/ludwig1860/p/14853911.html?ivk_sa=1024320u

文档: https://www.agisoft.com/pdf/metashape_python_api_2_0_0.pdf

十、同时用 color 和 depth 来做 pose estimation (Colored PointCloud Registration)

这个工作的对准效果是真屌! 👍

这项工作参考的是: Colored Point Cloud Registration Revisited, ICCV 2017

还是基于ICP算法,但是同时使用颜色和点云进行估计。

有色点云估计变换矩阵的核心函数是 registration_colored_icp：

$E_C$ 和 $E_G$ 分别代表颜色项和几何项, $C_p$ 是提前计算好的一个网络， $f$ 是将点投影到切平面的函数；
$(1-delta)E_C(T) + delta E_G(T) \ E_G(T) = sum_{(p,q)in K} ((p-Tq) cdot n_p)^2 \ E_C(T) = sum_{(p,q)in K} (C_p(f(Tq))-C(q))^2$

import open3d as o3d 
import numpy as np
import cv2 def draw_registration_result_original_color(source, target, transformation):source_temp = copy.deepcopy(source)source_temp.transform(transformation)o3d.visualization.draw_geometries([source_temp, target])source = o3d.io.read_point_cloud('ch7/0.pcd')
target = o3d.io.read_point_cloud('ch7/10.pcd')current_transformation = np.asarray([[1, 0, 0, 0],[0, 1,0, 0],[0, 0, 1, 0.25],[0.0, 0.0, 0.0, 1.0]])voxel_radius = [0.4, 0.2, 0.1]
max_iter = [500, 300, 140]# current_transformation = np.identity(4)print("3. Colored point cloud registration")
for scale in range(3):iter = max_iter[scale]radius = voxel_radius[scale]print([iter, radius, scale])print("3-1. Downsample with a voxel size %.2f" % radius)source_down = source.voxel_down_sample(radius)target_down = target.voxel_down_sample(radius)print("3-2. Estimate normal.")source_down.estimate_normals(o3d.geometry.KDTreeSearchParamHybrid(radius=radius * 2, max_nn=30))target_down.estimate_normals(o3d.geometry.KDTreeSearchParamHybrid(radius=radius * 2, max_nn=30))print("3-3. Applying colored point cloud registration")result_icp = o3d.pipelines.registration.registration_colored_icp(source_down, target_down, radius, current_transformation,o3d.pipelines.registration.TransformationEstimationForColoredICP(),o3d.pipelines.registration.ICPConvergenceCriteria(relative_fitness=1e-4,relative_rmse=1e-4,max_iteration=iter))current_transformation = result_icp.transformationdraw_registration_result_original_color(source, target ,current_transformation)

十一、PointCloud $\to$ TriangleMesh (表面重建 , Surface Reconstruction)

此处介绍三种算法进行表面重建:
- [1983] Alpha shapes
- [1999] Ball pivoting
- [2006] Poisson Surface Reconstruction(泊松表面重建)

11.1 Alpha shapes

Alpha Shape类似于凸包(Convex Hell), 用一堆四棱锥把所有点云包含起来。函数: create_from_point_cloud_alpha_shape;

# Alpha Shapes
import os
import numpy as np
import open3d as o3d
import open3d_tutorial as o3dtutmesh = o3dtut.get_bunny_mesh()
# 按照泊松分布采样
pcd = mesh.sample_points_poisson_disk(750)
# o3d.visualization.draw_geometries([pcd])alpha = 0.02
mesh = o3d.geometry.TriangleMesh.create_from_point_cloud_alpha_shape(pcd, alpha)
mesh.compute_vertex_normals()
o3d.visualization.draw_geometries([mesh], mesh_show_back_face=True)

11.2 Ball Pivoting (BPA)

这个方法和 Alpha shapes 类似，思路: 扔一个球可能会接触三个点，然后就用这三个点画一个三角形，函数: create_from_point_cloud_ball_pivoting:

mesh = o3dtut.get_bunny_mesh()
# 按照泊松分布采样
pcd = mesh.sample_points_poisson_disk(3000)# 几种球体的半径
radii= [0.005, 0.01, 0.02, 0.04]rec_mesh = o3d.geometry.TriangleMesh.create_from_point_cloud_ball_pivoting(pcd, o3d.utility.DoubleVector(radii)
)o3d.visualization.draw_geometries([pcd, rec_mesh])

11.3 Poisson Surface Reconstruction

这个方法可以得到比较平滑的曲面，函数是: create_from_point_cloud_poisson, 这个函数有一个重要的参数是octree_used

import open3d as o3d
import open3d_tutorial as o3dtutmesh = o3dtut.get_bunny_mesh()
# 按照泊松分布采样
pcd = mesh.sample_points_poisson_disk(3000)with o3d.utility.VerbosityContextManager(o3d.utility.VerbosityLevel.Debug) as cm:mesh, densities = o3d.geometry.TriangleMesh.create_from_point_cloud_poisson(pcd, depth=9)mesh.paint_uniform_color([1, 0.706, 0])o3d.visualization.draw_geometries([mesh])

效果图:

11.4 density上色(cmap)

import open3d as o3d
import open3d_tutorial as o3dtut
import numpy as np 
import matplotlib.pyplot as pltmesh = o3dtut.get_bunny_mesh()
# 按照泊松分布采样
pcd = mesh.sample_points_poisson_disk(3000)
pcd.estimate_normals()with o3d.utility.VerbosityContextManager(o3d.utility.VerbosityLevel.Debug) as cm:mesh, densities = o3d.geometry.TriangleMesh.create_from_point_cloud_poisson(pcd, depth=15)densities = np.asarray(densities)
density_colors = plt.get_cmap('plasma')((densities-densities.min()) / (densities.max()-densities.min())
)density_colors = density_colors[:, :3]
density_mesh = o3d.geometry.TriangleMesh()density_mesh.vertices = mesh.vertices
density_mesh.triangles = mesh.triangles
density_mesh.triangle_normals = mesh.triangle_normals
density_mesh.vertex_colors = o3d.utility.Vector3dVector(density_colors)o3d.visualization.draw_geometries([density_mesh])

11.5 mask掉density较小的部分

# 丢掉低density的部分
vertices_to_remove = densities < np.quantile(densities, 0.01)
mesh.remove_vertices_by_mask(vertices_to_remove)
o3d.visualization.draw_geometries([mesh])

十二、点云动画

import open3d as o3d 
import cv2 
import numpy as np 
import copy 
import time o3d.utility.set_verbosity_level(o3d.utility.VerbosityLevel.Debug)source = o3d.io.read_point_cloud('ch7/0.pcd')
target = o3d.io.read_point_cloud('ch7/10.pcd')
source = source.uniform_down_sample(every_k_points=10)
target = target.uniform_down_sample(every_k_points=10)trans = np.asarray([[1, 0, 0, 0],[0, 1,0, 0.05],[0, 0, 1, 0.2],[0.0, 0.0, 0.0, 1.0]])source.transform(trans)# flip_transform = [[1, 0, 0, 0], [0, -1, 0, 0], [0, 0, -1, 0], [0, 0, 0, 1]]
# source.transform(flip_transform)
# target.transform(flip_transform)vis = o3d.visualization.Visualizer()
vis.create_window()
vis.add_geometry(source)
vis.add_geometry(target)threshold = 0.05 
icp_iteration = 100 
save_image = False # 看到每次iter的过程
for i in range(icp_iteration):result = o3d.pipelines.registration.registration_icp(source, target, threshold, trans, o3d.pipelines.registration.TransformationEstimationPointToPoint(),o3d.pipelines.registration.ICPConvergenceCriteria(max_iteration=80))source.transform(result.transformation) #这一步在变vis.update_geometry(source)vis.poll_events()vis.update_renderer()time.sleep(0.1)vis.destroy_window()

十三、RGBD视觉里程计(Visual Odometry)

之前用的是ICP的方法，这里是VO的方法；

OdometryOption( )函数的参数:
- minimum_correspondence_ratio: 两张RGBD的overlap的阈值
- max_depth_diff: 对于 depth VO 的阈值
- min_depth and max_depth: 只选取这里面范围的深度值

import open3d as o3d 
import cv2 
import numpy as np 
import copy 
import time pinhole_camera_intrinsic = o3d.camera.PinholeCameraIntrinsic(o3d.camera.PinholeCameraIntrinsicParameters.PrimeSenseDefault
)source_color = o3d.io.read_image('./ch5/color/0.jpg')
source_depth = o3d.io.read_image('./ch5/depth/0.png')
target_color = o3d.io.read_image('./ch5/color/10.jpg')
target_depth = o3d.io.read_image('./ch5/depth/10.png')
source_rgbd_image = o3d.geometry.RGBDImage.create_from_color_and_depth(source_color, source_depth, depth_scale=1000.)
source_pcd = o3d.geometry.PointCloud.create_from_rgbd_image(source_rgbd_image, pinhole_camera_intrinsic)
target_rgbd_image = o3d.geometry.RGBDImage.create_from_color_and_depth(target_color, target_depth, depth_scale=1000.)
target_pcd = o3d.geometry.PointCloud.create_from_rgbd_image(target_rgbd_image, pinhole_camera_intrinsic)# source = o3d.io.read_point_cloud('ch7/0.pcd')
# target = o3d.io.read_point_cloud('ch7/10.pcd')# source = source.uniform_down_sample(every_k_points=10)
# target = target.uniform_down_sample(every_k_points=10)def draw_registration_result(source, target, transformation):source_temp = copy.deepcopy(source)target_temp = copy.deepcopy(target)source_temp.paint_uniform_color([1, 0.706, 0])target_temp.paint_uniform_color([0, 0.651, 0.929])source_temp.transform(transformation)o3d.visualization.draw_geometries([source_temp, target_temp])# 计算VO
option = o3d.pipelines.odometry.OdometryOption()
odo_init = np.identity(4)# 下面的是两种VO方案:
method = 'hybrid'
# 方案一: Color
if method == 'color':[success_color_term, trans_color_term, info] = o3d.pipelines.odometry.compute_rgbd_odometry(source_rgbd_image, target_rgbd_image,pinhole_camera_intrinsic, odo_init,o3d.pipelines.odometry.RGBDOdometryJacobianFromColorTerm(), option)
if method == 'hybrid':
# 方案二: Hybrid[success_hybrid_term, trans_hybrid_term, info] = o3d.pipelines.odometry.compute_rgbd_odometry(source_rgbd_image, target_rgbd_image,pinhole_camera_intrinsic, odo_init,o3d.pipelines.odometry.RGBDOdometryJacobianFromHybridTerm(), option)draw_registration_result(source_pcd, target_pcd, trans_hybrid_term)

十四、高级一点的ICP

14.1 Vanilla ICP VS Robust Kernel ICP

⭐https://www.pudn.com/news/6260ca3c090cbf2c4eea24d9.html

import open3d as o3d 
import cv2 
import numpy as np 
import copy 
import time def draw_registration_result(source, target, transformation):source_temp = copy.deepcopy(source)target_temp = copy.deepcopy(target)source_temp.paint_uniform_color([1, 0.706, 0])target_temp.paint_uniform_color([0, 0.651, 0.929])source_temp.transform(transformation)o3d.visualization.draw_geometries([source_temp, target_temp])trans_init = np.array([[ 0.99884836, -0.00300904, -0.04788421,  0.01982128],[ 0.00513161,  0.99900659,  0.04426623, -0.02027177],[ 0.04770344, -0.04446098,  0.99787154,  0.09383609],[ 0.        ,  0.        ,  0.        ,  1.        ]])source = o3d.io.read_point_cloud('ch7/0.pcd')
target = o3d.io.read_point_cloud('ch7/10.pcd')source = source.uniform_down_sample(every_k_points=10)
source_cp = copy.deepcopy(source)
target = target.uniform_down_sample(every_k_points=10)def apply_noise(pcd, mu, sigma):noisy_pcd = copy.deepcopy(pcd)points = np.asarray(pcd.points)points += np.random.normal(mu, sigma, size=points.shape)noisy_pcd.points = o3d.utility.Vector3dVector(points)return noisy_pcdmu, sigma = 0, 0.1
source_noisy = apply_noise(source, mu, sigma)
# o3d.visualization.draw_geometries([source_noisy])# Vanilla ICP VS Robust Kernels ICP# 1. Vanilla ICP 
threshold = 2.0
# p2l = o3d.pipelines.registration.TransformationEstimationPointToPlane()
# target.estimate_normals()
# reg_p2l = o3d.pipelines.registration.registration_icp(source_noisy, target, threshold, trans_init, p2l)# 2. Robust Kernel ICP
loss = o3d.pipelines.registration.TukeyLoss(k=sigma)
p2l = o3d.pipelines.registration.TransformationEstimationPointToPlane(loss)
target.estimate_normals()
reg_p2l = o3d.pipelines.registration.registration_icp(source_noisy, target, threshold, trans_init, p2l)draw_registration_result(source_cp, target, reg_p2l.transformation)

14.2 用numpy修改点云的某个属性

因为之前对VKitti2，如果不对法向量取反，重建出来的表面是烂的，因此我决定对法向量取反做一下表面重建。

import open3d as o3d 
import cv2 
import numpy as np 
import copy 
import time def inverse_normal(pcd):normals = np.asarray(pcd.normals)normals = -1 * normalspcd.normals = o3d.utility.Vector3dVector(normals)source = o3d.io.read_point_cloud('ch7/0.pcd')source.estimate_normals()inverse_normal(source)
# o3d.visualization.draw_geometries([source])# 泊松
with o3d.utility.VerbosityContextManager(o3d.utility.VerbosityLevel.Debug) as cm:mesh, densities = o3d.geometry.TriangleMesh.create_from_point_cloud_poisson(source, depth=10)
o3d.visualization.draw_geometries([mesh])# Ball Pivoting
radii= [0.2, 0.5, 1, 2]rec_mesh = o3d.geometry.TriangleMesh.create_from_point_cloud_ball_pivoting(source, o3d.utility.DoubleVector(radii)
)o3d.visualization.draw_geometries([rec_mesh])

十五、PointClouds + Poses $\to$ A single PointCloud

http://www.open3d.org/docs/latest/python_example/camera/index.html

15.1 简单叠加版

0 0 1
-0.2739592186924325 0.021819345900466677 -0.9614937663021573 -0.31057997014702826
8.33962904204855e-19 -0.9997426093226981 -0.02268733357278151 0.5730122438481298
-0.9617413095492113 -0.006215404179813816 0.27388870414358013 2.1264800183565487
0.0 0.0 0.0 1.0
1 1 2
-0.2693331107603073 0.03329392381977483 -0.9624713970216773 -0.31035872146799454
0.0 -0.9994022291201125 -0.03457143950937459 0.5963093994288613
-0.9630470785211782 -0.009311233346521649 0.2691721112697053 2.126036136362892
0.0 0.0 0.0 1.0
...

# txt转log, 这里用的都是绝对坐标
import json
import open3d as o3d 
import numpy as np
import cv2 pose_path = './ch5/pose/'record = ''for i in range(81):start = str(i) + ' ' + str(i) + ' ' + str(i+1) + 'n'lines = open(pose_path+str(i)+'.txt').readlines()# -114temp = list(map(float, lines[1].strip('n').split(' ')))temp[-1] -= 114new = str(temp[0]) + ' ' + str(temp[1]) +  ' ' + str(temp[2]) + ' ' + str(temp[3]) + 'n'lines[1] = new# 取反temp = list(map(float, lines[0].strip('n').split(' ')))temp[-1] = -temp[-1]new = str(temp[0]) + ' ' + str(temp[1]) +  ' ' + str(temp[2]) + ' ' + str(temp[3]) + 'n'lines[0] = newtemp = list(map(float, lines[1].strip('n').split(' ')))temp[-1] = -temp[-1]new = str(temp[0]) + ' ' + str(temp[1]) +  ' ' + str(temp[2]) + ' ' + str(temp[3]) + 'n'lines[1] = newtemp = list(map(float, lines[2].strip('n').split(' ')))temp[-1] = -temp[-1]new = str(temp[0]) + ' ' + str(temp[1]) +  ' ' + str(temp[2]) + ' ' + str(temp[3]) + 'n'lines[2] = newall_content = start + lines[0] + lines[1] + lines[2] + lines[3] + 'n'record += all_contentfile = open('./Vkitti2.log', 'w')
file.writelines(record)

叠加点云:

import numpy as np
import open3d as o3dprint("Testing camera in open3d ...")
intrinsic = o3d.camera.PinholeCameraIntrinsic(o3d.camera.PinholeCameraIntrinsicParameters.PrimeSenseDefault)
print(intrinsic.intrinsic_matrix)
print(o3d.camera.PinholeCameraIntrinsic())
x = o3d.camera.PinholeCameraIntrinsic(640, 480, 525, 525, 320, 240)
print(x)
print(x.intrinsic_matrix)
o3d.io.write_pinhole_camera_intrinsic("test.json", x)
y = o3d.io.read_pinhole_camera_intrinsic("test.json")
print(y)
print(np.asarray(y.intrinsic_matrix))print("Read a trajectory and combine all the RGB-D images.")
pcds = []
# redwood_rgbd = o3d.data.SampleRedwoodRGBDImages()
trajectory = o3d.io.read_pinhole_camera_trajectory('trajectory.log')'''
redwood_rgbd.trajectory_log_path = 
C:\Users\wuke2/open3d_data/extract/SampleRedwoodRGBDImages/trajectory.log
'''o3d.io.write_pinhole_camera_trajectory("test.json", trajectory)
# print(trajectory)
# print(trajectory.parameters[0].extrinsic)
# print(np.asarray(trajectory.parameters[0].extrinsic))for i in range(81):im1 = o3d.io.read_image('ch5/depth/' + str(i) + '.png')im2 = o3d.io.read_image('ch5/color/' + str(i) + '.jpg')im = o3d.geometry.RGBDImage.create_from_color_and_depth(im2, im1, 1000.0, 10.0, False)pcd = o3d.geometry.PointCloud.create_from_rgbd_image(im, trajectory.parameters[i].intrinsic,trajectory.parameters[i].extrinsic)pcds.append(pcd.voxel_down_sample(voxel_size=0.2))
o3d.visualization.draw_geometries(pcds)

15.2 官方例程的方案

C:/Users/wuke2/AppData/Local/Programs/Python/Python37/Lib/site-packages/open3d/examples/reconstruction_system

https://blog.csdn.net/chencaw/article/details/128337851

Reference

[1] BV1ei4y1Q7sE

[2] https://github.com/isl-org/Open3D

[3] http://www.open3d.org/docs/release/index.html#python-api-index

[4] https://ww2.mathworks.cn/help/vision/ref/pcwrite.html#buph6m1-7

[5] https://github.com/niconielsen32/ComputerVision

[6] http://www.open3d.org/docs/release/