RGBD 里程计
RGBD 里程计（odometry）用于估计两对连续 RGBD 图像之间的相机运动。输入是两个 RGBDImage 实例，输出是刚性变换形式的运动。Open3D 实现了 [Steinbrucker2011] 与 [Park2017] 的方法。
读取相机内参
Section titled “读取相机内参”我们首先从 json 文件中读取相机内参矩阵。
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)输出：
[[525. 0. 319.5] [ 0. 525. 239.5] [ 0. 0. 1. ]]读取 RGBD 图像
Section titled “读取 RGBD 图像”下面的代码读取两对 Redwood 格式的 RGBD 图像。关于格式的详细说明，请参阅 Redwood 数据集。
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)从两对 RGBD 图像计算里程计
Section titled “从两对 RGBD 图像计算里程计”option = o3d.pipelines.odometry.OdometryOption()odo_init = np.identity(4)print(option)
[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( 20, 10, 5, ] ,)该代码块调用了两种不同的 RGBD 里程计方法。第一种来自 [Steinbrucker2011]，它最小化对齐图像之间的光度一致性（photo consistency）。第二种来自 [Park2017]，在光度一致性之外还加入了几何约束。两种函数的运行速度相近，但在我们的基准数据集测试中，[Park2017] 更为精确，因此是推荐的方法。
OdometryOption() 中的若干参数：
minimum_correspondence_ratio：在对齐之后度量两幅 RGBD 图像的重叠比例。如果两幅 RGBD 图像的重叠区域小于指定比例，里程计模块即判定本次估计失败。depth_diff_max：在深度图像域中，如果两个对齐像素的深度差小于指定值，则将它们视为一对对应关系。该值越大搜索越激进，但结果越不稳定。depth_min和depth_max：深度值小于或大于指定范围的像素将被忽略。
可视化 RGBD 图像对
Section titled “可视化 RGBD 图像对”RGBD 图像对被转换为点云并一起渲染。注意，代表第一幅（源）RGBD 图像的点云使用了里程计估计出的变换进行了变换。经过该变换后，两个点云彼此对齐。
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])
输出：
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]][Open3D WARNING] GLFW Error: Failed to detect any supported platform[Open3D WARNING] GLFW initialized for headless rendering.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]][Open3D WARNING] GLFW initialized for headless rendering.