LiDAR到相机的图像融合

Question

我想融合相机图像{u,v}上的LiDAR {X,Y,Z,1}点，我们有LiDAR点，相机矩阵(K)，失真系数(D)，相机和LiDAR的位置(x,y,z)，相机的旋转和LiDAR (w+xi+yj+zk)。涉及到三个坐标系。车轴坐标系(X：向前、Y：左、Z：向上)、LiDAR坐标(X：右、Y：向前、Z：向上)和相机坐标系(X：右、Y：向下、Z：向前)。我尝试了下面的方法，但是这些点不能正确融合。所有的点都画错了。

坐标系：

对于给定的相机和LiDAR的旋转和位置，我们使用下面的方程计算平移。

t_lidar     = R_lidar  * Position_lidar^T
t_camera    = R_camera  *Position_camera^T

然后，将相对旋转和平移计算为流

R_relative = R_camera^T * R_lidar
t_relative = t_lidar -  t_camera

然后，LiDAR点[X,Y,Z,1]和图像帧[u,v,1]之间的最终变换矩阵和点变换由下式给出：

T =  [ R_relative | t_relative ] 
  [u,v,1]^T = K * T * [X,Y,Z,1]^T

我有什么遗漏的吗？

Answer 1

直接使用opencv投影点

https://docs.opencv.org/2.4/modules/calib3d/doc/camera_calibration_and_3d_reconstruction.html#projectpoints

​

​

C++：void projectPoints(InputArray objectPoints，InputArray rvec，InputArray tvec，InputArray cameraMatrix，InputArray distCoeffs，OutputArray imagePoints，OutputArray jacobian=noArray()，double aspectRatio=0 )

objectPoints -对象点阵列，3xN/Nx3 1通道或1xN/Nx1 3通道(或矢量)，其中N是视图中的点数。

rvec -旋转向量。有关详细信息，请参阅Rodrigues()。

tvec -翻译向量。

cameraMatrix -摄像机矩阵