Camera intrinsic

Get the depth map

depth = cv2.imread(frame1.p_depth, cv2.CV_16UC1)
points_3d = [(u - cx) / fx * float(depth[v, u]) / 100, 
             (v - cy) / fy * float(depth[v, u]) / 100,
             float(depth[v, u]) / 100]

Get the ground true

t = [gt[3]/100, gt[7]/100, gt[11]/100]
                

R = [[-gt[4], -gt[8], gt[0]],
     [-gt[5], -gt[9], gt[1]],
     [-gt[6], -gt[10], gt[2]]