Google Cartographer: внутренние системы координат ⇐ C++

[Anonymous]

Я изучаю Google Cartographer SLAM. Работая с подкартами в API библиотеки (которая совершенно не документирована), я обнаружил следующие сущности, представляющие координаты:

• Траектория (список поз датчиков с подключенными подкартами)
• Глобальная поза (поза подкарты).
• Локальная поза.
• Срез позы.

Вот строки код, реализующий это в submap_painter.cc:

Код: Выделить всё

void CairoPaintSubmapSlices(
const double scale,
const std::map& submaps,
cairo_t* cr, std::function draw_callback) {
cairo_scale(cr, scale, scale);

for (auto& pair : submaps) {
const auto& submap_slice = pair.second;
if (submap_slice.surface == nullptr) {
return;
}
const Eigen::Matrix4d homo =
ToEigen(submap_slice.pose * submap_slice.slice_pose).matrix();

cairo_save(cr);
cairo_matrix_t matrix;
cairo_matrix_init(&matrix, homo(1, 0), homo(0, 0), -homo(1, 1), -homo(0, 1),
homo(0, 3), -homo(1, 3));
cairo_transform(cr, &matrix);

const double submap_resolution = submap_slice.resolution;
cairo_scale(cr, submap_resolution, submap_resolution);

// Invokes caller's callback to utilize slice data in global cooridnate
// frame. e.g. finds bounding box, paints slices.
draw_callback(submap_slice);
cairo_restore(cr);
}
}

где submap_slice.pose — это глобальная позиция подкарты в глобальных координатах, а submap_slice.slice_pose — это локальная позиция 2D-подкарты на карте, сформулированной ранее. Глобальное положение определяется в метрах, поскольку оно масштабируется в соответствии с разрешением карты, а положение среза затем должно быть определено в пикселях. Функция ToEigen определяется здесь как:

Код: Выделить всё

Eigen::Affine3d ToEigen(const ::cartographer::transform::Rigid3d& rigid3) {
return Eigen::Translation3d(rigid3.translation()) * rigid3.rotation();
}

представляет стандартную ориентацию трехмерного твердого тела на основе кватернионов для Cartographer.
Мне интересно, есть ли способ воспроизвести эти преобразования координат и перестроить карту, созданную Cartographer, но используя OpenCV вместо Cairo. Может ли кто-нибудь помочь мне с этим?

Подробнее здесь: https://stackoverflow.com/questions/736 ... te-systems