Как найти вращение и перевод (матрицу преобразования) между соответствующими точками в двух разных системах координат

Я работаю над калибровкой камеры-лидара и некоторое время не могу решить следующую проблему:

Я использую USB-камеру и 2D-лидар. У меня есть координаты соответствующих точек как в кадре лидара, так и в кадре камеры (допустим, у меня есть 3 точки и их координаты в кадре лидара и координаты тех же трех точек в кадре камеры).

Пример для одна точка:
lidar_pt1(xl, yl)
camera_pt1(xc, yc, zc)
...
известны.

Если я жестко запрограммирую матрицу преобразования, я получу ожидаемый результат. Теперь пытаюсь не хардкодить, а автоматически вычислять по известным значениям координат. У меня есть 3 точки в 2D-координатах в кадре лидара и точные 3 точки в 3D-координатах в кадре камеры. Именно здесь я борюсь с математикой, чтобы как-то рассчитать вращение на основе значения координат, которое у меня есть. Есть ли способ получить это вращение?

camera_pt1 = TransformMat * lidarpt1
TransformMat = ?

Я видел несколько примеров использования SVD (http://nghiaho.com/?page_id=671), но я думаю, что они требуют больших наборов данных, и минимум 3 точки не дадут наилучшего результата.


c++ opencv math coordinate-transformation
person Pavel    schedule 03.11.2017    source источник

Ответы (1)


arrow_upward
3
arrow_downward

Если вы возьмете только 3 пары координат из каждой системы, то математика будет довольно простой. Вот простой пример:

    |
  4 |                      (R)
    |                       : ',
    |                       :   ',
    |                       :     ',
  3 |                       :      (P)
    |                       :     ,'
    |                       :   ,'
    |                       : ,'
  2 |      (A).....(B)     (Q)
    |       :     ,'
    |       :   ,'
    |       : ,'
  1 |      (C)
    |
    |
    |
  0 +-------------------------------------
    0       1       2       3       4

Предположим, у вас есть треугольник ABC, который соответствует другому треугольнику PQR. Вы можете представить их вершины в однородных координатах следующим образом:

      .-       -.          .-       -.
      | 1  2  1 |          | 4  3  1 |
ABC = | 2  2  1 |    PQR = | 3  2  1 |
      | 1  1  1 |          | 3  4  1 |
      '-       -'          '-       -'

Вам нужно найти матрицу M, которая отображает ABC на PQR (т. е. ABC × M = PQR). Для этого просто умножьте PQR на величину, обратную ABC:

if    ABC × M  =  PQR,
then  ABC⁻¹ × ABC × M  =  ABC⁻¹ × PQR
so    M  =  ABC⁻¹ × PQR

Существует множество ссылок на то, как инвертировать матрицу 3 × 3. Это должно дать вам следующий результат:

    .-          -.
    | -1  -1   0 |
M = |  1  -1   0 |
    |  3   6   1 |
    '-          -'
person r3mainer    schedule 03.11.2017
comment
Спасибо! Да, в этом есть смысл, но можно ли это сделать, если один известный набор точек в 2D, а другой в 3D, т.е. не будет ли пропадать информация об одном вращении? Мы можем предположить, что координата z для лидарной точки равна нулю, поэтому у нас есть лидарная точка (xl, yl, 0) и соответствующая точка камеры (xc, yc, zc)? - person Pavel; 03.11.2017
comment
Извините, я не заметил, что один из наборов координат был 3D. В этом случае, я думаю, вам просто нужно отбросить координаты z и обрабатывать данные камеры как 2D. Вы не можете установить координаты z лидара равными нулю, потому что это сделает матрицу преобразования необратимой. (Вы не можете инвертировать преобразование 3D⇔2D, потому что оно включает в себя отбрасывание информации в одном направлении и создание ее из ниоткуда в другом направлении.) - person r3mainer; 03.11.2017