Как получить матрицу проекции из данных одометрии/tf?

Вы уверены, что вам нужна проекционная матрица? Проекционная матрица камеры обычно представляет собой матрицу 3x4, которая проецирует (однородные) точки в R³ в (однородные) точки в R² на изображении. плоскости до некоторого масштаба (см. запись в Википедии). Похоже, вы заинтересованы в сравнении вычисленной визуальной одометрии с истинной одометрией, представленной на веб-сайте KITTI; в этом случае вы будете сравнивать жесткие матрицы преобразования из вашей оценки VO с преобразованием истинности KITTI.

Если вы используете «сырые» наборы данных, «наземная правда» предоставляется в виде записей данных OXTS, то есть объединенных данных IMU и GPS. Эти данные находятся в глобальном фрейме, и потребуется немного больше работы, чтобы сравнить их с вашими данными. Однако похоже, что вы используете данные эталона одометрии; трансформация земли уже находится в кадре левой камеры, что должно немного облегчить задачу (и это то, что я буду обсуждать).

Поскольку вы не указали язык, я буду говорить здесь более широко, но ROS предоставляет инструменты на C++ (tf и Eigen) и Python (transformations.py) для выполнения таких задач, как преобразование из кватерниона в матрицы вращения...

У вас есть t и q, перевод и вращение представлены в виде кватерниона. Вы можете преобразовать кватернион в матрицу вращения (обычно в форме «sxyz») и наоборот. Данные KITTI указаны в виде матрицы 3x4, и это матрица вращения, объединенная с вектором перевода (т.е. 4-й столбец — t_gt).

r₁₁ r₁₂ r₁₃ t₁
r₂₁ r< sub>22 r₂₃ t₂
r₃₁ r₃₂ r ₃₃ т₃

Вы можете просто вычислить ошибку перевода, вычислив норму L2: ||t - t_gt||. Вычисление ошибки вращения немного сложнее; один из способов сделать это — использовать такую ​​функцию, как QuaternionBase::angularDistance() из Eigen, поскольку оба измерения должны быть в одной и той же системе координат. Для этого вам нужно преобразовать матрицу вращения наземной истины в кватернион, используя либо Eigen, либо библиотеку transforms.py.

Имейте в виду, что это ошибка в кадре одометрии, т. е. ошибка вашей i-й расчетной позы относительно i-й наземной истинной позы в кадре исходной позы. Иногда также полезно сравнивать среднюю ошибку от кадра к кадру, особенно потому, что одометрия имеет тенденцию значительно дрейфовать со временем, даже если алгоритм в среднем относительно точен между кадрами.

В итоге:

• преобразовать матрицы вращения в кватернионы для вычисления угловой ошибки (обратите внимание на системы координат),
• и используйте формулу ||t - t_gt|| вычислить ошибку перевода.
• опять же, обратите внимание на свои системы координат.

Я не очень разбираюсь в компьютерном зрении, но взгляните на Sensor_msgs/CameraInfo Message и image_geometry::PinholeCameraModel Class< /a> ссылка.

void imageCallback([...], const sensor_msgs::CameraInfoConstPtr& info_msg) {
  // initialize cam_model with sensor_msgs::CameraInfo
  image_geometry::PinholeCameraModel cam_model_;
  cam_model_.fromCameraInfo(info_msg);

  // get the projection matrix
  cv::Matx34d projection_matrix;
  projection_matrix = cam_model_.projectionMatrix();
}