Преобразование координат объекта 3D в 2D мир - PullRequest
Новая
       88

Преобразование координат объекта 3D в 2D мир

1 голос
/ 12 февраля 2020

У меня есть следующий (упрощенный) код, который представляет трехмерную позу робота, оцененную с помощью визуальной одометрии: 

// (r11 r12 r13 tx)
// (r21 r22 r23 ty)
// (r31 r32 r33 tz)
// (0   0   0   1)
Eigen::Matrix4f pose_prev_to_cur;
Eigen::Matrix4f pose_cur_to_world;

while (1) {
    pose_prev_to_cur = ... /* from visual odometry */
    pose_cur_to_world = pose_cur_to_world * pose_prev_to_cur.inverse();
}

Как видите, матрица поз содержит трехмерную матрицу вращения 3х3 и 3х1 вектор перевода. Я строю траекторию робота, создавая точку на карте для каждого кадра: 

// x translation maps to 2D x axis
int x = WINDOW_SCALE * pose_cur_to_world(0, 3) + WINDOW_SIZE / 2;
// z translation maps to 2D y axis
int y = WINDOW_SCALE * pose_cur_to_world(2, 3) + WINDOW_SIZE / 2;
// plotting the point
cv::circle(traj, cv::Point(x, y), 1, cv::Scalar(0, 255, 0), 2);

Это хорошо работает для меня, и траектория строится, как вы можете видеть на изображении: enter image description here Траектория зеленая, текущая позиция робота красная.

Теперь у меня есть показания глубины для каждого кадра. Для каждого пикселя изображения, полученного с камеры, у меня есть измерение глубины в метрах. Я хотел бы также построить расстояние на карте. Моя текущая идея состоит в том, чтобы просто «добавить» значение глубины к значению z перевода матрицы поз, например так: 

Eigen::Matrix4f pose_depth_reading = pose_cur_to_world;

// offset z location by depth reading in meters
pose_depth_reading(2, 3) += 2.0;

Теперь мой вопрос: как построить эту новую точку на карте? Если я просто нанесу точку, используя координаты x / z трехмерных точек, она, очевидно, не будет правильно повернута, поскольку у нее будет только смещение по оси Y.

Я думал о том, чтобы взять вычисленное 2D нанесите на карту точку и сместите ее координату y на глубину, а затем выполните двумерное вращение вокруг начала координат. Но моя проблема в том, что мне нужно получить угол поворота 2D из матрицы вращения 3x3 в pose_cur_to_world, и я не совсем уверен, как это сделать.

Ожидаемый конечный результат показан в следующее изображение: enter image description here Показания глубины помечены синим цветом, и я добавил для краткости серую координатную рамку с осями X и Y.

...