基于OpenCV复现Google Cartographer内部坐标系地图绘制

背景与坐标实体说明 #

我在研究Google Cartographer SLAM技术时，处理无文档说明的库API子图模块，遇到以下核心坐标实体：

• 轨迹：关联子图的传感器位姿序列
• 全局位姿：子图在全局坐标系中的位姿（单位：米）
• 局部位姿：子图内部的局部位姿
• 切片位姿：2D子图在原始地图中的局部像素位姿

原Cairo实现逻辑 #

Cartographer原本用Cairo实现子图绘制，核心代码如下：

void CairoPaintSubmapSlices(
    const double scale,
    const std::map<::cartographer::mapping::SubmapId, SubmapSlice>& submaps,
    cairo_t* cr, std::function<void(const SubmapSlice&)> 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);

    // 调用回调在全局坐标系中处理切片数据（如计算边界框、绘制切片）
    draw_callback(submap_slice);
    cairo_restore(cr);
  }
}

其中ToEigen函数负责将Cartographer的3D刚体姿态（四元数表示）转换为Eigen齐次矩阵：

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

核心逻辑是：将子图的全局位姿与切片位姿复合，转换为齐次变换矩阵，再适配Cairo的坐标系做轴映射，最后缩放子图分辨率完成绘制。

OpenCV替代实现步骤 #

OpenCV的坐标系为X向右、Y向下，与Cartographer的X向前、Y向左坐标系存在轴映射差异，需对应复现变换逻辑：

1. 核心变换矩阵推导 #

原Cairo的变换本质是将Cartographer的3D姿态投影到2D平面，并适配画布坐标系。我们需要将Eigen齐次矩阵转换为OpenCV的2D仿射变换矩阵（cv::Mat类型，2x3）：

• Cartographer的2D平面变换：X向前，Y向左
• OpenCV的画布变换：X向右，Y向下
• 对应轴映射：OpenCV的X = Cartographer的Y，OpenCV的Y = -Cartographer的X

基于此，从Eigen的4x4齐次矩阵homo提取2D变换参数，构造OpenCV的仿射矩阵：

// 从复合姿态的齐次矩阵中提取2D变换参数
const Eigen::Matrix3d homo_2d = homo.block<3,3>(0,0); // 取前3x3子矩阵（忽略Z轴）

// 构造OpenCV的2x3仿射变换矩阵
cv::Mat cv_transform = cv::Mat::eye(2, 3, CV_64F);
// 旋转缩放部分：适配轴映射
cv_transform.at<double>(0, 0) = homo_2d(1, 1);
cv_transform.at<double>(0, 1) = -homo_2d(1, 0);
cv_transform.at<double>(1, 0) = homo_2d(0, 1);
cv_transform.at<double>(1, 1) = -homo_2d(0, 0);
// 平移部分：适配轴映射与缩放
cv_transform.at<double>(0, 2) = homo_2d(0, 2) * scale;
cv_transform.at<double>(1, 2) = -homo_2d(1, 2) * scale;

2. 子图分辨率缩放 #

原代码中最后对Cairo画布缩放submap_resolution（子图分辨率，单位：米/像素），对应到OpenCV中，我们需要在变换矩阵中整合分辨率缩放：

// 将分辨率缩放整合到变换矩阵
cv_transform.at<double>(0, 0) *= submap_resolution;
cv_transform.at<double>(0, 1) *= submap_resolution;
cv_transform.at<double>(1, 0) *= submap_resolution;
cv_transform.at<double>(1, 1) *= submap_resolution;

3. 绘制子图到全局画布 #

使用OpenCV的cv::warpAffine将子图切片绘制到全局地图画布上，需要先计算子图在全局画布中的边界，确保画布足够大：

// 假设全局地图画布为global_map，子图切片为submap_image（CV_8UC1或CV_8UC3）
cv::Size global_size = global_map.size();
// 执行仿射变换，将子图绘制到全局画布
cv::warpAffine(submap_image, global_map, cv_transform, global_size,
               cv::INTER_LINEAR, cv::BORDER_TRANSPARENT);

完整替代函数示例 #

void OpenCVPaintSubmapSlices(
    const double scale,
    const std::map<::cartographer::mapping::SubmapId, SubmapSlice>& submaps,
    cv::Mat& global_map, std::function<void(const SubmapSlice&, cv::Mat&)> draw_callback) {
  // 全局画布初始化（按需设置尺寸，这里假设已预先初始化）
  if (global_map.empty()) {
    // 可根据子图的全局边界计算合适的画布尺寸
    global_map = cv::Mat(cv::Size(10000, 10000), CV_8UC1, cv::Scalar(0));
  }

  for (const auto& pair : submaps) {
    const auto& submap_slice = pair.second;
    if (submap_slice.surface == nullptr) {
      continue; // 跳过无效子图
    }

    // 将Cairo Surface转换为OpenCV Mat（需根据实际Surface格式调整转换逻辑）
    cv::Mat submap_image;
    // 示例转换逻辑（以RGBA格式Surface为例）
    unsigned char* data = cairo_image_surface_get_data(submap_slice.surface);
    int width = cairo_image_surface_get_width(submap_slice.surface);
    int height = cairo_image_surface_get_height(submap_slice.surface);
    submap_image = cv::Mat(height, width, CV_8UC4, data).clone();
    // 若为灰度图可转成单通道：cv::cvtColor(submap_image, submap_image, cv::COLOR_RGBA2GRAY);

    // 计算复合姿态的齐次矩阵
    const Eigen::Matrix4d homo = ToEigen(submap_slice.pose * submap_slice.slice_pose).matrix();
    const Eigen::Matrix3d homo_2d = homo.block<3,3>(0,0);

    // 构造OpenCV仿射变换矩阵
    cv::Mat cv_transform = cv::Mat::eye(2, 3, CV_64F);
    // 轴映射与旋转缩放
    cv_transform.at<double>(0, 0) = homo_2d(1, 1);
    cv_transform.at<double>(0, 1) = -homo_2d(1, 0);
    cv_transform.at<double>(1, 0) = homo_2d(0, 1);
    cv_transform.at<double>(1, 1) = -homo_2d(0, 0);
    // 平移与全局缩放
    cv_transform.at<double>(0, 2) = homo_2d(0, 2) * scale;
    cv_transform.at<double>(1, 2) = -homo_2d(1, 2) * scale;
    // 子图分辨率缩放
    const double submap_resolution = submap_slice.resolution;
    cv_transform.colRange(0,2) *= submap_resolution;

    // 执行变换绘制子图
    cv::warpAffine(submap_image, global_map, cv_transform, global_map.size(),
                   cv::INTER_LINEAR, cv::BORDER_TRANSPARENT);

    // 调用自定义回调处理子图（如标记边界、添加额外信息）
    draw_callback(submap_slice, global_map);
  }
}

关键注意事项 #

• 坐标系映射：必须处理Cartographer与OpenCV的轴方向差异，否则子图会出现旋转或翻转错误
• 数据格式转换：需将Cairo的Surface正确转换为OpenCV的cv::Mat，注意通道数（RGBA转灰度或RGB）
• 画布尺寸：全局画布需足够大以容纳所有子图，可通过遍历所有子图的全局位姿计算边界后初始化

内容的提问来源于stack exchange，提问作者Andrei Vukolov