5 Open3D学习笔记——odometry

里程计：在两个连续的图像对之间找到相机移动

在open3d中实现odometry有两种方式：

• open3d.odometry.RGBDOdometryJacobianFromColorTerm()

• open3d.odometry.RGBDOdometryJacobianFromHybridTerm()

具体示例代码如下：

import open3d as o3d 
import numpy as np 

if __name__ == "__main__":
    #以json方式读取相机内参
    pinhole_camera_intrinsic = o3d.io.read_pinhole_camera_intrinsic(
        "/home/jhon/Apps/Open3D/examples/TestData/camera_primesense.json")
    #打印内参矩阵
    print("相机内参：\n",pinhole_camera_intrinsic.intrinsic_matrix,"\n")
    
    #读取图像
    s_color = o3d.io.read_image("/home/jhon/Apps/Open3D/examples/TestData/RGBD/color/00000.jpg")
    s_depth = o3d.io.read_image("/home/jhon/Apps/Open3D/examples/TestData/RGBD/depth/00000.png")
    t_color = o3d.io.read_image("/home/jhon/Apps/Open3D/examples/TestData/RGBD/color/00001.jpg")
    t_depth = o3d.io.read_image("/home/jhon/Apps/Open3D/examples/TestData/RGBD/depth/00001.png")
    
    #生成RGBD图像
    s_rgbd = o3d.geometry.create_rgbd_image_from_color_and_depth(s_color, s_depth)
    t_rgbd = o3d.geometry.create_rgbd_image_from_color_and_depth(t_color, t_depth)
    #生成点云
    t_pcd = o3d.geometry.create_point_cloud_from_rgbd_image(t_rgbd, pinhole_camera_intrinsic)
     
    option = o3d.odometry.OdometryOption()
    odo_init = np.identity(4)
    print(option)

    #odometry_color
    [success_color_term, trans_color_term, info] = o3d.odometry.compute_rgbd_odometry(
        s_rgbd, t_rgbd, pinhole_camera_intrinsic, odo_init, o3d.odometry.RGBDOdometryJacobianFromColorTerm(),
        option)
    #odometry_Hybrid
    [success_hybrid_term, trans_hybrid_term, info] = o3d.odometry.compute_rgbd_odometry(
        s_rgbd, t_rgbd, pinhole_camera_intrinsic, odo_init, o3d.odometry.RGBDOdometryJacobianFromHybridTerm(),
        option)

    if success_color_term:
        print("Using RGBD Odometry\n")
        print(trans_color_term,"\n")
        s_pcd = o3d.geometry.create_point_cloud_from_rgbd_image(
            s_rgbd, pinhole_camera_intrinsic)
        #变换
        s_pcd.transform(trans_color_term)
        o3d.visualization.draw_geometries([t_pcd,s_pcd],"rgbd",960,540,500,400)      

    if success_hybrid_term:
        print("Using Hybrid rgbd odometry\n")
        print(trans_hybrid_term)
        s_pcd_hybrid = o3d.geometry.create_point_cloud_from_rgbd_image(
            s_rgbd, pinhole_camera_intrinsic)
        s_pcd_hybrid.transform(trans_hybrid_term)
        o3d.visualization.draw_geometries([s_pcd_hybrid, t_pcd])