LiDAR для камеры слияния изображений

Question

Я хочу соединить LiDAR {X,Y,Z,1} точек на изображении с камеры {u,v}, для которых у нас есть точки 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 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-канальный или 1xN / Nx1 3-канальный (иливектор), где N - количество точек в представлении.

rvec - Вектор вращения.Подробности смотрите в Rodrigues ().

tvec - Вектор перевода.

cameraMatrix - Матрица камеры