Google Cartographer: Interne Koordinatensysteme ⇐ C++

[Anonymous]

Ich studiere Google Cartographer SLAM. Bei der Arbeit mit Submaps in der Bibliotheks-API (die überhaupt nicht dokumentiert ist) habe ich die folgenden Entitäten entdeckt, die Koordinaten darstellen:

• Trajectory (Liste der Sensorposen mit verbundenen Submaps)
• Globale Pose (Submap-Pose).
• Lokale Pose.
• Slice-Pose.

Hier sind die Linien von Code, der dies in submap_painter.cc implementiert:

Code: Select all

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);
}
}

wobei submap_slice.pose eine globale Pose der Submap in den globalen Koordinaten ist und submap_slice.slice_pose eine lokale Position der 2D-Submap in der zuvor formulierten Karte zu sein scheint. Die globale Position wird in Metern definiert, da sie anhand der Kartenauflösung skaliert wird, und die Slice-Pose sollte dann in Pixeln definiert werden. Die ToEigen-Funktion ist hier definiert als:

Code: Select all

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

Darstellung der Standard-Quaternion-basierten Ausrichtung des 3D-Starrkörpers für Cartographer.
Ich frage mich, ob es eine Möglichkeit gibt, diese Koordinatentransformationen zu reproduzieren und eine von Cartographer erstellte Karte neu zu erstellen, aber mit OpenCV anstelle von Cairo. Könnte mir jemand dabei helfen?