如何通过球面投影(Spherical Projection)将点云转换为距离图像(Range Images)

前言

将3维激光点云通过球面投影（Spherical Projection）转换为2维距离图像（Range Images），是自动驾驶应用场景中一种非常常见的点云处理方式。点云转换为距离图像后，通常会被输入给一个2维卷积神经网络去实现目标检测、语义分割等任务。目前采用这种点云处理方式的典型目标检测算法有RangeDet，语义分割算法有SqueezeSeg、RangeNet++、SalsaNext等。在这些文章中，都只给出一个通过球面投影转换到距离图像的最终公式，至于这个公式是怎么来的却没有详细的推导，初看论文的读者可能会比较困惑。本文将对这个投影公式做一定的推导，可能本人理解的也不是很对，欢迎大家批评指正。

图片来源于RangeDet论文

球面投影推导过程

假设有一个m线的旋转扫描式激光雷达，它的垂直视场角FOV被分为上下两个部分：FOV_up和FOV_down，通常以FOV_up的数值为正数而FOV_down数值为负数，所以FOV = FOV_up + abs(FOV_down)。激光雷达旋转扫描一周得到的点云相当于是以其自身为中心的空心圆柱体，如果把这个圆柱体展开的话，那么就可以把点云投影到一个图像平面中去，这个图像平面就是距离图像。

对于一个m线的激光雷达，在扫描的某一时刻会得到m个点，如果旋转一周扫描了n次，那么得到的点云就可以用一个m * n的矩阵来表示。那么怎么把3维的点云投影到2维的距离图像平面呢？这就需要用到球面坐标。

图片来源于RangeDet论文

---

让我们再通过下图来理解一下3维笛卡尔坐标系和球面坐标系之间的关系。

假设3维笛卡尔坐标系下的点坐标为(x1,y1,z1)，那么用球面坐标系可以这样表示该点：

由于图像坐标系是以左上角作为坐标原点，所以上面得到的前视图还需要做一下坐标转换，把坐标原点移到左上角去：

代码实现

理解了原理后，我们再用代码来把这个投影过程实现一遍。在RangeNet++中，点云被转换为5个通道的距离图像，这5个通道分别代表点云的range、x、y、z、intensity这5个属性。下面的代码将展示如何通过球面投影将点云转换为需要的距离图像，使用的点云数据来源于SemanticKITTI数据集。

#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>

#include <cmath>
#include <iostream>
#include <memory>
#include <opencv2/opencv.hpp>
#include <string>
#include <vector>

int main(int argc, char** argv) {
  if (argc < 2) {
    std::cout << "Usage: " << argv[0] << " <pcd_file>\n";
    return -1;
  }

  const std::string pcd_file(argv[1]);
  pcl::PointCloud<pcl::PointXYZI>::Ptr point_cloud(
      new pcl::PointCloud<pcl::PointXYZI>);

  if (pcl::io::loadPCDFile<pcl::PointXYZI>(pcd_file, *point_cloud) == -1) {
    std::cout << "Couldn't read pcd file!\n";
    return -1;
  }

  constexpr int width = 2048;
  constexpr int height = 64;
  constexpr float fov_up = 3 * M_PI / 180.0;
  constexpr float fov_down = -25 * M_PI / 180.0;
  constexpr float fov = std::abs(fov_up) + std::abs(fov_down);
  const std::vector<float> image_means{12.12, 10.88, 0.23, -1.04, 0.21};
  const std::vector<float> image_stds{12.32, 11.47, 6.91, 0.86, 0.16};
  float* range_images = new float[5 * width * height]();

  for (const auto& point : point_cloud->points) {
    const auto& x = point.x;
    const auto& y = point.y;
    const auto& z = point.z;
    const auto& intensity = point.intensity;
    const float range = std::sqrt(x * x + y * y + z * z);
    const float yaw = -std::atan2(y, x);
    const float pitch = std::asin(z / range);

    float proj_x = 0.5f * (yaw / M_PI + 1.0f) * width;
    float proj_y = (1.0f - (pitch + std::abs(fov_down)) / fov) * height;
    proj_x = std::floor(proj_x);
    proj_y = std::floor(proj_y);

    const int u = std::clamp<int>(static_cast<int>(proj_x), 0, width - 1);
    const int v = std::clamp<int>(static_cast<int>(proj_y), 0, height - 1);

    range_images[0 * width * height + v * width + u] =
        (range - image_means.at(0)) / image_stds.at(0);
    range_images[1 * width * height + v * width + u] =
        (x - image_means.at(1)) / image_stds.at(1);
    range_images[2 * width * height + v * width + u] =
        (y - image_means.at(2)) / image_stds.at(2);
    range_images[3 * width * height + v * width + u] =
        (z - image_means.at(3)) / image_stds.at(3);
    range_images[4 * width * height + v * width + u] =
        (intensity - image_means.at(4)) / image_stds.at(4);
  }

  // 对range通道进行可视化
  cv::Mat range =
      cv::Mat(height, width, CV_32FC1, static_cast<void*>(range_images));
  cv::Mat normalized_range, u8_range, color_map;
  cv::normalize(range, normalized_range, 255, 0, cv::NORM_MINMAX);
  normalized_range.convertTo(u8_range, CV_8UC1);
  cv::applyColorMap(u8_range, color_map, cv::COLORMAP_JET);
  cv::imwrite("range_color_map.jpg", color_map);
  cv::imshow("Range Image", color_map);
  cv::waitKey(0);

  delete[] range_images;

  return 0;
}

对range通道可视化的结果如下图所示：

上面的代码有几个需要说明的地方：

1. fov_up，fov_down，image_means，image_stds这几个参数来源于RangeNet++预训练模型中的arch_cfg.yaml文件。
2. std::clamp需要c++17支持，编译的时候请使用-std=c++17编译选项。
3. 实际使用中width * height的值只需要计算一次，没必要在循环里面反复计算，这里这么写只是为了方便理解。

参考资料

RangeDet: In Defense of Range View for LiDAR-based 3D Object Detection
https://towardsdatascience.com/spherical-projection-for-point-clouds-56a2fc258e6c