最近需要进行多张点云图合并+生成mesh,发现Open3D是一个不错的工具。
文中所有用到的代码和数据我上传到了:
百度网盘:
链接:https://pan.baidu.com/s/1ra3bh6ldPVZhv7vfoeHqvQ
提取码:2023
🐰欢迎下载!
文章目录
- 一、介绍
- 二、点云降采样 & 法向量(Normal Estimation)
- 三、点云的基础操作 & 点云聚类(Clustering)
- 四、移除异常点(Outlier Removal)
- 五、RGBD → \rightarrow → Point Cloud (Single)
- 六、自己仅用RGB生成点云
- 七、根据2张点云估计变换矩阵(小范围内finetune, Local Registration)
- 八、根据2张点云估计变换矩阵(较大范围, Global Registration)
- 九、RGB-Video → \rightarrow → A Single Point Cloud (借助Agisoft Metashape软件)
- 十、同时用 color 和 depth 来做 pose estimation (Colored PointCloud Registration)
- 十一、PointCloud → \rightarrow → TriangleMesh (表面重建 , Surface Reconstruction)
- 十二、点云动画
- 十三、RGBD视觉里程计(Visual Odometry)
- 十四、高级一点的ICP
- 十五、PointClouds + Poses → \rightarrow → A single PointCloud
- 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 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
P_A
PA最近的点的距离。这样做可以用于检测我们在 场景点云图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 ) (a,b,c,d) (a,b,c,d): a x + b y + c z + d = 0 ax+by+cz+d=0 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 → \rightarrow → 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 P P和源点云 Q Q Q之间的相关的点集 K = ( p , q ) K={(p,q)} K=(p,q)
-
(2) 不断更新 T T T,直到满足 E ( T ) < t h r e s h o l d E(T)<threshold E(T)<threshold
E ( T ) = ∑ ( p , q ) ∈ K ∣ ∣ p − T q ∣ ∣ 2 E(T) = \sum_{(p,q)\in K} ||p-Tq||^2 E(T)=(p,q)∈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是一个描述一个点的局部几何性质的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算法
-
我们使用
RANSAC
算法实现global registraiton:-
从点云中采样
ransac_n
个点,每个点的邻近点是33维FPFH特征向量空间上的邻近,不是三维空间距离意义上的临近。 -
剪枝算法是丢掉那些匹配错的点们, open3D提供了以下3中
剪枝(prune)
算法:-
(1)
CorrespondenceCheckerBasedOnDistance
: 检查对齐的点的间距是否小于阈值。 -
(2)
CorrespondenceCheckerBasedOnEdgeLength
: 检查任意两条边的长度是否是是否类似:
∣ ∣ e d g e s o u r c e ∣ ∣ > 0.9 ⋅ ∣ ∣ e d g e t a r g e t ∣ ∣ a n d ∣ ∣ e d g e t a r g e t ∣ ∣ > 0.9 ⋅ ∣ ∣ e d g e s o u r c e ∣ ∣ ||edge_{source}|| > 0.9 \cdot ||edge_{target}|| \quad and \quad ||edge_{target}|| > 0.9 \cdot ||edge_{source}|| ∣∣edgesource∣∣>0.9⋅∣∣edgetarget∣∣and∣∣edgetarget∣∣>0.9⋅∣∣edgesource∣∣ -
(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 → \rightarrow → 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_C
EC和
E
G
E_G
EG分别代表颜色项和几何项,
C
p
C_p
Cp是提前计算好的一个网络,
f
f
f是将点投影到切平面的函数;
E
(
T
)
=
(
1
−
δ
)
E
C
(
T
)
+
δ
E
G
(
T
)
E
G
(
T
)
=
∑
(
p
,
q
)
∈
K
(
(
p
−
T
q
)
⋅
n
p
)
2
E
C
(
T
)
=
∑
(
p
,
q
)
∈
K
(
C
p
(
f
(
T
q
)
)
−
C
(
q
)
)
2
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
E(T)=(1−δ)EC(T)+δEG(T)EG(T)=(p,q)∈K∑((p−Tq)⋅np)2EC(T)=(p,q)∈K∑(Cp(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.transformation
draw_registration_result_original_color(source, target ,current_transformation)
十一、PointCloud → \rightarrow → 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
andmax_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 → \rightarrow → 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/