The problem about point cloud projected onto the image

[SUNTAO1963]

Firstly, hank you for creating the devkit and open dataset. This is very helpful for automatic train driving. I want to project the point cloud onto the image by using the internal and external parameters provided by the json file. When the same object is projected onto the image, there will be a large displacement deviation. If you project your own data in this way, the target deviation will be smaller. How can point clouds be accurately projected onto the image on this dataset?

Thanks in advance!

[tklockau]

Thank you for raising this issue. We will look into it!

[tklockau]

Can you please provide more context and images regarding your issue?

[SUNTAO1963]

Can you please provide more context and images regarding your issue?

I try to projetct pointcloud to rgb_highres_center image use the following code.

import json
import open3d as o3d
import numpy as np
import matplotlib
matplotlib.use('Agg')
import matplotlib.pyplot as plt
import matplotlib.image as mpimg
from util import *
import numpy as np
# JSON 文件路径
file_path = "1_calibration_1.1_labels.json"
with open(file_path, "r") as file:
    data = json.load(file)

camera_list = [
    "rgb_center",
    "rgb_highres_center",
    "rgb_highres_left",
    "rgb_highres_right",
    "rgb_left",
    "rgb_right"]

coor_list = [
    "lidar",
    "rgb_center",
    "rgb_highres_center",
    "rgb_highres_left",
    "rgb_highres_right",
    "rgb_left",
    "rgb_right"]

camera_key = {}
coor_key = {}

for coor in coor_list:
    coor_key[coor] = data['openlabel']['coordinate_systems'][coor]['pose_wrt_parent']

for camera_name in camera_list:
    camera_intrinsics = []
    camera_matrix = data['openlabel']['streams'][camera_name]['stream_properties']['intrinsics_pinhole']['camera_matrix']
    distortion_coeffs = data['openlabel']['streams'][camera_name]['stream_properties']['intrinsics_pinhole']['distortion_coeffs']
    width_px = data['openlabel']['streams'][camera_name]['stream_properties']['intrinsics_pinhole']['width_px']
    height_px = data['openlabel']['streams'][camera_name]['stream_properties']['intrinsics_pinhole']['height_px']
    camera_intrinsics.append(camera_matrix)
    camera_intrinsics.append(distortion_coeffs)
    camera_intrinsics.append(width_px)
    camera_intrinsics.append(height_px)
    camera_key[camera_name] = camera_intrinsics

pcd = o3d.io.read_point_cloud("/data/1_1/lidar/012_1631441453.299504000.pcd")
points = np.array(pcd.points)
points = np.insert(points,3,1,axis=1).T
# save_pcd_o3d(points, "/home/st/code/st_p2/pcd/init.pcd")

type_camera = "rgb_highres_center"
camera_coor = coor_key[type_camera]
lidar_coor =  coor_key["lidar"]
camera_m = camera_key[type_camera]

camera_trans = trans_to_matrix(camera_coor["translation"])
camera_roation = quaternion_to_rotation_matrix(camera_coor["quaternion"])
lidar_trans = trans_to_matrix(lidar_coor["translation"])
lidar_roation =  quaternion_to_rotation_matrix(lidar_coor["quaternion"])

points = backward_matrix(lidar_roation, lidar_trans, points)
points = forward_matrix(camera_roation, camera_trans, points)

K = camera_m[0]
temp_k = np.reshape(K, (3, 4))

points = points[0:3, :]
roation = np.matmul(rot_x(-90.0),rot_z(90.0))
points = np.matmul(roation, points).T
points = np.insert(points,3,1,axis=1).T
cam = temp_k.dot(points)
# cam = temp_k.dot(temp_points)
cam = np.delete(cam,np.where(cam[2,:]<0),axis=1)
cam[:2] /= cam[2,:]

png = mpimg.imread("/data/1_1/rgb_highres_center/012_1631441453.300000030.png")
IMG_H,IMG_W,_ = png.shape
plt.axis([0,IMG_W,IMG_H,0])
plt.imshow(png)
# filter point out of canvas
u,v,z = cam
u_out = np.logical_or(u<0, u>IMG_W)
v_out = np.logical_or(v<0, v>IMG_H)
outlier = np.logical_or(u_out, v_out)
cam = np.delete(cam,np.where(outlier),axis=1)
# generate color map from depth
u,v,z = cam
plt.scatter([u],[v],c=[z],cmap='rainbow_r',alpha=0.5,s=2)
plt.title("show")
plt.savefig("1111.png",bbox_inches='tight')
print("input something to continue")

util.py

import numpy as np
import math
import open3d as o3d

def trans_to_matrix(trans):
    trans_inv = np.eye(4)
    trans_inv[0,3] = trans[0]
    trans_inv[1,3] = trans[1]
    trans_inv[2,3] = trans[2]
    return trans_inv

def quaternion_to_rotation_matrix(quaternion):
    quaternion = np.array(quaternion)
    # 归一化四元数
    quaternion = quaternion / np.linalg.norm(quaternion)

    # 提取四元数的分量
    w, x, y, z = quaternion

    # 计算旋转矩阵的元素
    m00 = 1 - 2*y**2 - 2*z**2
    m01 = 2*x*y - 2*w*z
    m02 = 2*x*z + 2*w*y

    m10 = 2*x*y + 2*w*z
    m11 = 1 - 2*x**2 - 2*z**2
    m12 = 2*y*z - 2*w*x

    m20 = 2*x*z - 2*w*y
    m21 = 2*y*z + 2*w*x
    m22 = 1 - 2*x**2 - 2*y**2

    # 构建旋转矩阵
    rotation_matrix = np.array([[m00, m01, m02],
                                [m10, m11, m12],
                                [m20, m21, m22]])

    temp_rotation_matrix = np.eye(4)
    temp_rotation_matrix[0:3, 0:3] = rotation_matrix
    return temp_rotation_matrix

def forward_matrix(r, t, points):
    '''
    正向旋转平移
    '''
    r [:,3] = t[:, 3]
    points = r.dot(points)
    return points

def backward_matrix(r, t, points):
    '''
    反向旋转平移
    '''
    t[0:3,3] = t[0:3,3]*(-1) 
    r[0:3, 0:3] = np.transpose(r[0:3, 0:3])
    points = r.dot(t.dot(points))
    return points

def trans(t, points):
    '''
    反向旋转平移
    '''
    points = t.dot(points)
    return points

def roation(r, points):
    '''
    反向旋转平移
    '''
    points = r.dot(points)
    return points

def save_pcd_o3d(points, path):
    # points = np.array(points)
    pcd = o3d.geometry.PointCloud()
    points = points.T
    pcd.points = o3d.utility.Vector3dVector(points[:, 0:3])
    o3d.io.write_point_cloud(path, pcd)

def rot_x(angle):
    xangle = (angle/180.0)*math.pi
    rx = np.array([
    [1, 0,                   0            ],
    [0, math.cos(xangle),  math.sin(xangle)],
    [0, -math.sin(xangle), math.cos(xangle)]])
    return rx

def rot_y(angle):
    yangle = (angle/180.0)*math.pi
    ry = np.array([
    [math.cos(yangle),  0, math.sin(yangle)],
    [0,                                1, 0],
    [-math.sin(yangle), 0, math.cos(yangle)]])
    return ry

def rot_z(angle):
    zangle = (angle/180.0)*math.pi
    rz = np.array([
    [math.cos(zangle),  -math.sin(zangle), 0],
    [math.sin(zangle),  math.cos(zangle),  0],
    [0,                             0,     1]])
    return rz

I get this result.

Maybe my code have some mistakes in using the coordinate information. Can you give me some advice about my code. Thank you very much!

[egemenkopuz]

The code below is the simplified version of how I calculated the corresponding projection matrices. It may shed light to your problem.

import numpy as np
from scipy.spatial.transform import Rotation as R
import cv2

sensors = {
    "rgb_highres_center" : {
        "intrinsic" : np.array([7267.95450880415, 0.0, 2056.049238502414, 0.0, 0.0, 7267.95450880415, 1232.862908875167, 0.0, 0.0, 0.0, 1.0, 0.0]).reshape(3, 4),
        "quaternion" : np.array([-0.00313306, 0.0562995, 0.00482918, 0.998397]),
        "translation" : np.array([0.0801578, -0.332862, 3.50982]).reshape(3, 1),
        "distortion_coeffs" : np.array([-0.0764891, -0.188057, 0.0, 0.0, 1.78279]),
        "width_px" : 4112,
        "height_px" : 2504
    },
}

details = sensors["rgb_highres_center"]

intrinsic = details["intrinsic"]
quaternion = details["quaternion"]
translation = details["translation"]
distortion_coeffs = details["distortion_coeffs"]
width = details["width_px"]
height = details["height_px"]

# might be an unnecessary step
intrinsics_undistorted, _ = cv2.getOptimalNewCameraMatrix(intrinsic[:,:-1], distortion_coeffs, (width,height), 1, (width,height))
intrinsics_undistorted = np.hstack((intrinsics_undistorted, np.asarray([[0.0], [0.0], [0.0]])))

rotation = R.from_quat(quaternion)
coord_system_rot = R.from_euler('zxy', [-90, 0, 90], degrees=True)
rotation = (rotation * coord_system_rot).as_matrix()

extrinsic = np.vstack((np.hstack((rotation.T, - rotation.T @ translation)), [0.0,0.0,0.0,1.0]))
projection = np.matmul(intrinsics_undistorted, extrinsic)

# then you read the points and project them to 2D
# points_3d = ...
points_2d = np.matmul(projection  points_3d[:4, :])

[SUNTAO1963]

The code below is the simplified version of how I calculated the corresponding projection matrices. It may shed light to your problem.

import numpy as np
from scipy.spatial.transform import Rotation as R
import cv2

sensors = {
    "rgb_highres_center" : {
        "intrinsic" : np.array([7267.95450880415, 0.0, 2056.049238502414, 0.0, 0.0, 7267.95450880415, 1232.862908875167, 0.0, 0.0, 0.0, 1.0, 0.0]).reshape(3, 4),
        "quaternion" : np.array([-0.00313306, 0.0562995, 0.00482918, 0.998397]),
        "translation" : np.array([0.0801578, -0.332862, 3.50982]).reshape(3, 1),
        "distortion_coeffs" : np.array([-0.0764891, -0.188057, 0.0, 0.0, 1.78279]),
        "width_px" : 4112,
        "height_px" : 2504
    },
}

details = sensors["rgb_highres_center"]

intrinsic = details["intrinsic"]
quaternion = details["quaternion"]
translation = details["translation"]
distortion_coeffs = details["distortion_coeffs"]
width = details["width_px"]
height = details["height_px"]

# might be an unnecessary step
intrinsics_undistorted, _ = cv2.getOptimalNewCameraMatrix(intrinsic[:,:-1], distortion_coeffs, (width,height), 1, (width,height))
intrinsics_undistorted = np.hstack((intrinsics_undistorted, np.asarray([[0.0], [0.0], [0.0]])))

rotation = R.from_quat(quaternion)
coord_system_rot = R.from_euler('zxy', [-90, 0, 90], degrees=True)
rotation = (rotation * coord_system_rot).as_matrix()

extrinsic = np.vstack((np.hstack((rotation.T, - rotation.T @ translation)), [0.0,0.0,0.0,1.0]))
projection = np.matmul(intrinsics_undistorted, extrinsic)

# then you read the points and project them to 2D
# points_3d = ...
points_2d = np.matmul(projection  points_3d[:4, :])

Thanks for you code. I run it and try to project points_2d onto the image. But there is no pointcloud point can be project onto the image. Based on this code, did you successfully project it? I'm not sure if this is a problem with my use of your code. Can you hlep me check it?

import numpy as np
from scipy.spatial.transform import Rotation as R
import cv2
import open3d as o3d
import matplotlib
matplotlib.use('Agg')
import matplotlib.pyplot as plt
import matplotlib.image as mpimg

sensors = {
    "rgb_highres_center" : {
        "intrinsic" : np.array([7267.95450880415, 0.0, 2056.049238502414, 0.0, 0.0, 7267.95450880415, 1232.862908875167, 0.0, 0.0, 0.0, 1.0, 0.0]).reshape(3, 4),
        "quaternion" : np.array([-0.00313306, 0.0562995, 0.00482918, 0.998397]),
        "translation" : np.array([0.0801578, -0.332862, 3.50982]).reshape(3, 1),
        "distortion_coeffs" : np.array([-0.0764891, -0.188057, 0.0, 0.0, 1.78279]),
        "width_px" : 4112,
        "height_px" : 2504
    },
}

details = sensors["rgb_highres_center"]

intrinsic = details["intrinsic"]
quaternion = details["quaternion"]
translation = details["translation"]
distortion_coeffs = details["distortion_coeffs"]
width = details["width_px"]
height = details["height_px"]

# might be an unnecessary step
intrinsics_undistorted, _ = cv2.getOptimalNewCameraMatrix(intrinsic[:,:-1], distortion_coeffs, (width,height), 1, (width,height))
intrinsics_undistorted = np.hstack((intrinsics_undistorted, np.asarray([[0.0], [0.0], [0.0]])))

rotation = R.from_quat(quaternion)
coord_system_rot = R.from_euler('zxy', [-90, 0, 90], degrees=True)
rotation = (rotation * coord_system_rot).as_matrix()

extrinsic = np.vstack((np.hstack((rotation.T, - rotation.T @ translation)), [0.0,0.0,0.0,1.0]))
projection = np.matmul(intrinsics_undistorted, extrinsic)

# then you read the points and project them to 2D
pcd = o3d.io.read_point_cloud("./data/1_1/lidar/012_1631441453.299504000.pcd")
points_3d = np.array(pcd.points)
points_3d = np.insert(points_3d,3,1,axis=1).T
points_2d = np.matmul(projection, points_3d[:4, :])
png = mpimg.imread("./data/1_1/rgb_highres_center/012_1631441453.300000030.png")
IMG_H,IMG_W,_ = png.shape
plt.axis([0,IMG_W,IMG_H,0])
plt.imshow(png)
u,v,z = points_2d
u_out = np.logical_or(u<0, u>IMG_W)
v_out = np.logical_or(v<0, v>IMG_H)
outlier = np.logical_or(u_out, v_out)
cam = np.delete(points_2d,np.where(outlier),axis=1)
# generate color map from depth
u,v,z = cam
plt.scatter([u],[v],c=[z],cmap='rainbow_r',alpha=0.5,s=2)
plt.title("show")
plt.savefig("22.png",bbox_inches='tight')

The result look like this:

[egemenkopuz]

@SUNTAO1963 you need to do the following to get the results properly.

# divide x by z if z is not 0
points_2d[0,:] = np.divide(points_2d[0,:],points_2d[2,:],where=points_2d[2,:]!=0)
# divide y by z if z is not 0
points_2d[1,:] = np.divide(points_2d[1,:],points_2d[2,:],where=points_2d[2,:]!=0)

# u,v,z = points_2d
# and the rest of the code

[SUNTAO1963]

Thank you very much. I project success.

[yinheyanxian]

Hello, do you know how to project a 3D bounding box of a point cloud onto an image?