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

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

我的版本:

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 o3dtut

o3dtut.interactive = not "CI" in os.environ

pcd = 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
# 提取码：82u1

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)
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 np

pcd = 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)$: $ax+by+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 np

pcd = o3d.io.read_point_cloud('ch1/Vkitti2_Scene06.pcd')
pcd = pcd.voxel_down_sample(voxel_size=0.2) # len(pcd.points) = 6909

diameter = 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 o3d

pcd = 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 o3d

pcd = o3d.io.read_point_cloud('ch1/Vkitti2_Scene06.pcd')
pcd = pcd.uniform_down_sample(every_k_points=20) # 12666

cl, 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 o3d

pcd = o3d.io.read_point_cloud('ch1/Vkitti2_Scene06.pcd')
pcd = pcd.uniform_down_sample(every_k_points=20) # 12666

cl, 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 np

color_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 time



 
def img_to_depth(img ,model):


    imgHeight, imgWidth, channels = img.shape

    # start time to calculate FPS
    start = 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 model
    model.setInput(blob)

    # Make forward pass in model
    output = model.forward()
    
    output = output[0,:,:]
    output = cv2.resize(output, (imgWidth, imgHeight))

    # Normalize the output
    output = cv2.normalize(output, None, 0, 1, norm_type=cv2.NORM_MINMAX, dtype=cv2.CV_32F)



    return output

 

if __name__ == '__main__':

    path_model = "./"

    # Read Network
    model_name = "model-f6b98070.onnx"; # MiDaS v2.1 Large

    # Load the DNN model
    model = 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 *= 255
    depth_normal = depth_normal.astype('uint8')

    cv2.imwrite('./depth.png', depth_normal)

Step 2: RGBD生成点云

import open3d as o3d
import numpy as np

color_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] float

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])

七、根据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)<threshold$
    $$E(T)=\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 o3d

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])

• 函数evaluate_registration有2个参数:

  • fitness: 重叠区域，越大越好
  • inlier_rmse: 两组点云的RMSE阈值，终止条件

• 函数 TransformationEstimationPointToPoint提供了ICP算法的接口。

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

import open3d as o3d
import numpy as np

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)

# 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特征

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

import copy
import open3d as o3d
import numpy as np

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])

def preprocess_point_cloud(pcd, voxel_size):
    # 降采样
    pcd_down = pcd.voxel_down_sample(voxel_size)
    # 估计法向量
    radius_normal = voxel_size * 2
    pcd_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_fpfh

def 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_fph
    
voxel_size = 0.2
source, target, source_down, target_down, source_fph, target_fph = prepare_dataset(voxel_size)

8.2 Step 2 RANSAC算法

match

prune

• 我们使用RANSAC算法实现global registraiton：

  • 从点云中采样ransac_n个点，每个点的邻近点是33维FPFH特征向量空间上的邻近，不是三维空间距离意义上的临近。

  • 剪枝算法是丢掉那些匹配错的点们， open3D提供了以下3中剪枝(prune)算法:

    • (1) CorrespondenceCheckerBasedOnDistance: 检查对齐的点的间距是否小于阈值。

    • (2) CorrespondenceCheckerBasedOnEdgeLength: 检查任意两条边的长度是否是是否类似:
      $$||edg{e}_{source}||>0.9\cdot ||edg{e}_{target}||and||edg{e}_{target}||>0.9\cdot ||edg{e}_{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.5

    result = 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 result




voxel_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.01
    result = 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 Registration

voxel_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.5

    result = o3d.pipelines.registration.registration_ransac_based_on_feature_matching(
            source_down, target_down, source_fph, target_fph, True, voxel_size)

    return result


result_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$是将点投影到切平面的函数；
$$\begin{gathered} E(T)=(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 \end{gathered}$$

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.transformation


draw_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 o3dtut

mesh = 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 o3dtut

mesh = 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 plt

mesh = 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_pcd

mu, 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 * normals
    pcd.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()

    # -114
    temp = list(map(float, lines[1].strip('\n').split(' ')))
    temp[-1] -= 114
    new = 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] = new

    temp = 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] = new

    temp = 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] = new

    
    all_content = start + lines[0] + lines[1] + lines[2] + lines[3] + '\n'
    record += all_content

file = open('./Vkitti2.log', 'w')
file.writelines(record)

叠加点云:

import numpy as np
import open3d as o3d


print("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/