（14）点云数据处理学习——RGBD 里程计

1、主要参考

（1）官网

RGBD Odometry — Open3D 0.16.0 documentation

2、原理和实现

2.1 RGBD Odometry主要作用

RGBD里程计在两个连续的RGBD图像对之间查找相机运动。输入是RGBDImage的两个实例。输出是刚体变换形式的运动。Open3D实现了[Steinbrucker2011]和[Park2017]的方法。

 2.2读取摄像机内参

我们首先从json文件中读取相机内参矩阵

（1）代码

import open3d as o3d
import numpy as np

redwood_rgbd = o3d.data.SampleRedwoodRGBDImages()

pinhole_camera_intrinsic = o3d.io.read_pinhole_camera_intrinsic(
    redwood_rgbd.camera_intrinsic_path)
print(pinhole_camera_intrinsic.intrinsic_matrix)

（2）结果

[[525.    0.  319.5]
 [  0.  525.  239.5]
 [  0.    0.    1. ]]

实际上读取了这个json

（3）json中的内容如下

{
	"width" : 640,
	"height" : 480,
	"intrinsic_matrix" :
	[
		525.0,
		0,
		0,
		0,
		525.0,
		0,
		319.5,
		239.5,
		1
	]
}

 注意：

Open3D中的许多小型数据结构都可以从json文件中读取/写入。这包括相机内参，相机轨迹，姿态图等。

2.3读取RGBD图

（1）描述

该代码块读取两对Redwood格式的RGBD图像。我们参考Redwood数据集进行全面解释。

（2）代码

import open3d as o3d
import numpy as np

redwood_rgbd = o3d.data.SampleRedwoodRGBDImages()

pinhole_camera_intrinsic = o3d.io.read_pinhole_camera_intrinsic(
    redwood_rgbd.camera_intrinsic_path)
print(pinhole_camera_intrinsic.intrinsic_matrix)

source_color = o3d.io.read_image(redwood_rgbd.color_paths[0])
source_depth = o3d.io.read_image(redwood_rgbd.depth_paths[0])
target_color = o3d.io.read_image(redwood_rgbd.color_paths[1])
target_depth = o3d.io.read_image(redwood_rgbd.depth_paths[1])
source_rgbd_image = o3d.geometry.RGBDImage.create_from_color_and_depth(
    source_color, source_depth)
target_rgbd_image = o3d.geometry.RGBDImage.create_from_color_and_depth(
    target_color, target_depth)
target_pcd = o3d.geometry.PointCloud.create_from_rgbd_image(
    target_rgbd_image, pinhole_camera_intrinsic)

注意：Open3D假设彩色图像和深度图像是同步的，并在同一坐标框架中进行配准。这通常可以通过打开RGBD相机设置中的同步和注册功能来实现。

2.4从两个匹配的RGBD图像计算里程数 

（1）代码

import open3d as o3d
import numpy as np

redwood_rgbd = o3d.data.SampleRedwoodRGBDImages()

pinhole_camera_intrinsic = o3d.io.read_pinhole_camera_intrinsic(
    redwood_rgbd.camera_intrinsic_path)
print(pinhole_camera_intrinsic.intrinsic_matrix)

#（一）2.3读取RGBD图
source_color = o3d.io.read_image(redwood_rgbd.color_paths[0])
source_depth = o3d.io.read_image(redwood_rgbd.depth_paths[0])
target_color = o3d.io.read_image(redwood_rgbd.color_paths[1])
target_depth = o3d.io.read_image(redwood_rgbd.depth_paths[1])
source_rgbd_image = o3d.geometry.RGBDImage.create_from_color_and_depth(
    source_color, source_depth)
target_rgbd_image = o3d.geometry.RGBDImage.create_from_color_and_depth(
    target_color, target_depth)
target_pcd = o3d.geometry.PointCloud.create_from_rgbd_image(
    target_rgbd_image, pinhole_camera_intrinsic)


option = o3d.pipelines.odometry.OdometryOption()
odo_init = np.identity(4)
print(option)

#（二）2.4从两个匹配的RGBD图像计算里程数 
[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)
[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)

OdometryOption class.
iteration_number_per_pyramid_level = [ 20, 10, 5, ] 
depth_diff_max = 0.030000
depth_min = 0.000000
depth_max = 4.000000

注意：上述这个代码块调用两个不同的RGBD里程数方法。第一个来自[Steinbrucker2011]。它使对齐图像的照片一致性最小化。第二个来自[Park2017]。除了照片一致性，它还实现了几何约束。这两个函数的运行速度相似，但[Park2017]在我们的基准数据集测试中更准确，因此是推荐的方法。 

 （3）关于OdometryOption()的参数

• minimum_answdence_ratio:对齐后，测量两张RGBD图像的重叠率。如果两个RGBD图像的重叠区域小于规定的比例，则里程表模块认为这是一个失败的情况。
• depth_diff_max:在深度图像域中，如果两个对齐像素的深度差小于指定值，则认为对应。数值越大，搜索越激进，但容易导致搜索结果不稳定。
• depth_min和depth_max:小于或大于指定深度值的像素将被忽略。

2.5 可视化RGBD图像对（Visualize RGBD image pairs）

（1）描述

RGBD图像对被转换为点云并一起渲染。请注意，表示第一个(源)RGBD图像的点云是用里程计估计的转换进行转换的。在这个转换之后，两个点云都对齐了。

（2）代码

if success_color_term:
    print("Using RGB-D Odometry")
    print(trans_color_term)
    source_pcd_color_term = o3d.geometry.PointCloud.create_from_rgbd_image(
        source_rgbd_image, pinhole_camera_intrinsic)
    source_pcd_color_term.transform(trans_color_term)
    o3d.visualization.draw_geometries([target_pcd, source_pcd_color_term],
                                      zoom=0.48,
                                      front=[0.0999, -0.1787, -0.9788],
                                      lookat=[0.0345, -0.0937, 1.8033],
                                      up=[-0.0067, -0.9838, 0.1790])
if success_hybrid_term:
    print("Using Hybrid RGB-D Odometry")
    print(trans_hybrid_term)
    source_pcd_hybrid_term = o3d.geometry.PointCloud.create_from_rgbd_image(
        source_rgbd_image, pinhole_camera_intrinsic)
    source_pcd_hybrid_term.transform(trans_hybrid_term)
    o3d.visualization.draw_geometries([target_pcd, source_pcd_hybrid_term],
                                      zoom=0.48,
                                      front=[0.0999, -0.1787, -0.9788],
                                      lookat=[0.0345, -0.0937, 1.8033],
                                      up=[-0.0067, -0.9838, 0.1790])

 （3）结果

Using RGB-D Odometry
[[ 9.99988286e-01 -7.53983409e-05 -4.83963172e-03  2.74054550e-04]
 [ 1.83909052e-05  9.99930634e-01 -1.17782559e-02  2.29634918e-02]
 [ 4.84018408e-03  1.17780289e-02  9.99918922e-01  6.02121265e-04]
 [ 0.00000000e+00  0.00000000e+00  0.00000000e+00  1.00000000e+00]]

 Using Hybrid RGB-D Odometry
[[ 9.99992973e-01 -2.51084541e-04 -3.74035273e-03 -1.07049775e-03]
 [ 2.07046059e-04  9.99930714e-01 -1.17696227e-02  2.32280983e-02]
 [ 3.74304875e-03  1.17687656e-02  9.99923740e-01  1.40592054e-03]
 [ 0.00000000e+00  0.00000000e+00  0.00000000e+00  1.00000000e+00]]