kitti depth complement

代码
运行环境：windows10
open3d版本：0.12.0

import cv2
import numpy as np
import os 
import math 
import open3d as o3d

basic_path = "D:/dataset/kitti_depth/data_depth_selection/depth_selection/val_selection_cropped"
image_path = "/image/2011_09_26_drive_0002_sync_image_0000000008_image_03.png"
depth_path = "/velodyne_raw/2011_09_26_drive_0002_sync_velodyne_raw_0000000008_image_03.png"
GT_path = "/groundtruth_depth/2011_09_26_drive_0002_sync_groundtruth_depth_0000000008_image_03.png"

image_file = cv2.imread(basic_path+image_path,cv2.IMREAD_GRAYSCALE)   # shape (352,1216) type numpy
depth_file = cv2.imread(basic_path+depth_path, -1)             # shape (352,1216) type numpy
GT_file = cv2.imread(basic_path+GT_path, -1)
print(type(depth_file))  #uint16

def gaussian_filter(distance, sigma):
    return np.exp(-distance ** 2 / (2 * sigma ** 2)) / (math.sqrt(2*math.pi)*sigma)
    
def bilateral_filter(depth, image, kernel_size, sigma_depth, sigma_color):
    padding = (kernel_size-1)/2
    padding = int(padding)
    h, w = image.shape

    img = np.zeros((h+2*padding, w+2*padding), dtype=np.float)
    img[padding:h+padding, padding:w+padding] = image
    dep = np.zeros((h+2*padding, w+2*padding), dtype=np.float)
    dep[padding:h+padding, padding:w+padding] = depth
    output = np.zeros((h+2*padding, w+2*padding), dtype=np.float)

    
    k = np.zeros((kernel_size, kernel_size), dtype=np.float)
    for kernel_center_x in range(padding, h+padding):
        for kernel_center_y in range(padding, w+padding):
            sum = 0
            bf = 0
            for i in range(0, kernel_size):
                for j in range(0, kernel_size):
                    dis0 = pow((dep[kernel_center_x-padding+i,kernel_center_y-padding+j]-dep[kernel_center_x, kernel_center_y]), 2)
                    dis1 = pow((img[kernel_center_x-padding+i,kernel_center_y-padding+j]-img[kernel_center_x, kernel_center_y]), 2)

                    k[i,j] = gaussian_filter(dis0,sigma_depth)*gaussian_filter(dis1,sigma_color)
                    bf = bf + k[i,j]*dep[kernel_center_x-padding+i,kernel_center_y-padding+j]
                    sum = sum + k[i,j]
            bf = bf/sum
            output[kernel_center_x, kernel_center_y] = bf
    return output[padding:h+padding, padding:w+padding]

depth_bf = bilateral_filter(depth_file, image_file, 12, 2, 5)
#print(depth_bf.dtype)  #float64
depth_bf = depth_bf.astype(np.uint16)
#print(type(depth_bf[0][0]))  #uint16

display_dbf = o3d.geometry.Image(depth_bf)
o3d.visualization.draw_geometries([display_dbf])
o3d.visualization.draw_geometries([depth_file])
o3d.visualization.draw_geometries([image_file])

结果图