matlab - 激光雷达到相机图像融合

Question

我要融合激光雷达{X,Y,Z,1}相机图像上的点{u,v}我们有 LiDAR 点、相机矩阵 (K)、畸变系数 (D)、相机和 LiDAR 的位置 (x,y,z)、相机和 LiDAR 的旋转 (w+xi+yj+zk)。涉及三个坐标系。车轴坐标系(X:前，Y:左，Z:上)，激光雷达坐标(X:右，Y:前，Z:上)和相机坐标系( X:向右，Y:向下，Z:向前)。我尝试了以下方法，但这些点没有正确融合。所有点都被错误地绘制了。

坐标系:

对于给定的相机和激光雷达的旋转和位置，我们使用以下等式计算平移。

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 projectpoint
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 channel 或 1xN/Nx1 3 channel (或向量)，其中 N 是 View 中的点数。
rvec - 旋转矢量。有关详细信息，请参阅 Rodrigues()。
tvec——翻译向量。
cameraMatrix - 相机矩阵