Point cloud projection picture

Use of calibration results

Use calibration_toolkit The calibration file obtained by the tool cannot be used directly , Need to transform

Rotation matrix in camera external parameter matrix , First, transpose , Then the Rodriguez transformation
Translation matrix in camera external parameter matrix , It needs to be transposed
There is no need to change the camera internal parameter matrix and camera deformation matrix

CameraExtrinsicMat = [
			[R[0][0], R[1][0], R[2][0], t[1]],
			[R[0][1], R[1][1], R[2][1], t[2]],
			[R[0][2], R[1][2], R[2][2], -t[0]],
			[0., 0., 0., 1.]]

transfrom.py

import cv2
import numpy as np


#  First do a transpose 
RT = np.transpose(R)
print(RT)

#  Then find the Rodriguez transformation 
rvec = cv2.Rodrigues(RT)[0]
print(rvec)

#  result 
# [[ 1.27379905]
# [-1.17541056]
# [ 1.15989273]]


#  Original translation matrix 
t = np.mat([
    [7.0567131528775830e-03],
    [6.9659648287774212e-02],
    [7.3964965903196039e-02]
])

#  transposition 
# autoware After obtaining the translation matrix, the following operations are carried out ：
# camera_velodyne_translation.x = -camera_velodyne_point.z;
# camera_velodyne_translation.y = camera_velodyne_point.x;
# camera_velodyne_translation.z = camera_velodyne_point.y;
t_true = np.float64([[t[1]], [t[2]], [-t[0]]])
print(t_true)

#  The resulting true translation vector 
# [[-0.07396497],
# [0.00705671],
# [0.06965965]]

Point cloud re projection

Step-by-step instructions ：

1. Use open3d Functions in the Feature Pack , from .pcd Read out the LIDAR point cloud coordinates from the file
2. Declare various internal and external parameter matrices
3. Use opencv Functions in the Feature Pack , Convert world coordinates to pixel coordinates
4. Point cloud re projection

#-*- coding:utf-8 -*-
""" Re projection is realized by using the obtained internal and external parameters of the camera """

import open3d as o3d
import cv2
from PIL import Image
from pylab import *
import matplotlib.pyplot as plt

#  Read pcd Point cloud file , Save as array Array 
cloud = o3d.io.read_point_cloud('1.pcd')  #  You need to prepare your own pcd file 
cloud = np.asarray(cloud.points)  #  Change the data type of the point cloud 

#  Input  projectpoints  The value of each parameter of the function 

#  After matrix transpose , And the rotation matrix obtained by Rodriguez transformation 
rvec = np.float64([1.27379905, -1.17541056, 1.15989273])  

#  The translation matrix obtained after sorting and modification 
tvec = np.float64([-0.07396497, 0.00705671, 0.06965965]) 

#  Camera internal parameters 
camera_matrix = np.float64([[6.0094877060462500e+02, 0,  3.0507696130640221e+02],
                            [0, 6.1174212550675293e+02,  2.5274596287337977e+02],
                            [0, 0, 1]])  #  Camera internal parameters 

#  Camera deformation matrix 
distCoeffs = np.float64([2.3030430710414049e-01, -9.1560321189489913e-01,
                         1.0374975865423207e-02, -8.9662215743119679e-04, 1.3506515085650497e+00]) 


#  The point cloud is generated by 3D To 2D Transformation 
point_2d, _ = cv2.projectPoints(cloud, rvec, tvec, camera_matrix, distCoeffs)

print(point_2d)


#  The re projection is drawn on the image 
im = Image.open('1.jpg')
x = []
y = []

m = -1
for point in point_2d:
    m = m+1
    x_2d = point[0][0]
    y_2d = point[0][1]

    if 0 <= x_2d <= 640 and 0 <= y_2d <= 480:
        x.append(x_2d)
        y.append(y_2d)

x = np.array(x)
y = np.array(y)
plt.scatter(x, y, s=1)
plt.imshow(im)
plt.show()