前言

將3維激光點云通過球面投影（Spherical Projection）轉(zhuǎn)換為2維距離圖像（Range Images），是自動駕駛應用場景中一種非常常見的點云處理方式。點云轉(zhuǎn)換為距離圖像后，通常會被輸入給一個2維卷積神經(jīng)網(wǎng)絡去實現(xiàn)目標檢測、語義分割等任務。目前采用這種點云處理方式的典型目標檢測算法有RangeDet，語義分割算法有SqueezeSeg、RangeNet++、SalsaNext等。在這些文章中，都只給出一個通過球面投影轉(zhuǎn)換到距離圖像的最終公式，至于這個公式是怎么來的卻沒有詳細的推導，初看論文的讀者可能會比較困惑。本文將對這個投影公式做一定的推導，可能本人理解的也不是很對，歡迎大家批評指正。

球面投影推導過程

假設有一個m線的旋轉(zhuǎn)掃描式激光雷達，它的垂直視場角FOV被分為上下兩個部分：FOV_up和FOV_down，通常以FOV_up的數(shù)值為正數(shù)而FOV_down數(shù)值為負數(shù)，所以FOV = FOV_up + abs(FOV_down)。激光雷達旋轉(zhuǎn)掃描一周得到的點云相當于是以其自身為中心的空心圓柱體，如果把這個圓柱體展開的話，那么就可以把點云投影到一個圖像平面中去，這個圖像平面就是距離圖像。

對于一個m線的激光雷達，在掃描的某一時刻會得到m個點，如果旋轉(zhuǎn)一周掃描了n次，那么得到的點云就可以用一個的矩陣來表示。那么怎么把3維的點云投影到2維的距離圖像平面呢？這就需要用到球面坐標。

球面坐標用3個參數(shù)來表示：距離，方位角（Azimuth），天頂角（Zenith）。通常使用的激光雷達點云中的每個由3維笛卡爾坐標表示的點實際上是從球面坐標系轉(zhuǎn)換而來：

讓我們再通過下圖來理解一下3維笛卡爾坐標系和球面坐標系之間的關(guān)系。

假設3維笛卡爾坐標系下的點坐標為，那么用球面坐標系可以這樣表示該點：

如果以x軸方向為前視圖的方向把激光雷達旋轉(zhuǎn)掃描一周得到的圓柱體展開后，可以得到一副這樣的圖像：坐標原點在圖像的中心，圖像中像素的縱坐標由pitch角投影得到（范圍為[FOV_down,FOV_up]），橫坐標由yaw角投影得到（范圍為）。

由于圖像坐標系是以左上角作為坐標原點，所以上面得到的前視圖還需要做一下坐標轉(zhuǎn)換，把坐標原點移到左上角去：

把3維點云投影為2維圖像，這種降維操作必然會帶來信息損失。為了盡可能減少投影帶來的信息損失，我們需要選擇合適大小的投影圖像。對于一個64線的激光雷達，一般會設置投影圖像的高為64，那么圖像的寬該如何設置呢？假設激光雷達的水平分辨率為0.35度，那么旋轉(zhuǎn)一周一個激光器最多產(chǎn)生的點數(shù)為。在卷積神經(jīng)網(wǎng)絡中，一般會對輸入特征圖做多次2倍下采樣，所以圖像的寬度需要設置為2的次冪，這里可設置為1024。

由于不同類型激光雷達的視場角、水平分辨率不同，投影圖像的尺寸也會根據(jù)需要設置為不同的值，為了適應這些變化，yaw和pitch還需要進行規(guī)范化：

規(guī)范化后，再乘以投影圖像的寬高，就得到了這個點投影到距離圖像的坐標：

上式中的第二步是將代入得到的。

代碼實現(xiàn)

理解了原理后，我們再用代碼來把這個投影過程實現(xiàn)一遍。在RangeNet++中，點云被轉(zhuǎn)換為5個通道的距離圖像，這5個通道分別代表點云的這5個屬性。下面的代碼將展示如何通過球面投影將點云轉(zhuǎn)換為需要的距離圖像，使用的點云數(shù)據(jù)來源于SemanticKITTI數(shù)據(jù)集。

#include
#include

#include
#include
#include
#include
#include
#include

intmain(intargc,char**argv){
if(argc2){
std::cout<"Usage:"<0]<"
";
return-1;
}

conststd::stringpcd_file(argv[1]);
pcl::PointCloud::Ptrpoint_cloud(
newpcl::PointCloud);

if(pcl::loadPCDFile(pcd_file,*point_cloud)==-1){
std::cout<"Couldn'treadpcdfile!
";
return-1;
}

constexprintwidth=2048;
constexprintheight=64;
constexprfloatfov_up=3*M_PI/180.0;
constexprfloatfov_down=-25*M_PI/180.0;
constexprfloatfov=std::abs(fov_up)+std::abs(fov_down);
conststd::vector<float>image_means{12.12,10.88,0.23,-1.04,0.21};
conststd::vector<float>image_stds{12.32,11.47,6.91,0.86,0.16};
float*range_images=newfloat[5*width*height]();

for(constauto&point:point_cloud->points){
constauto&x=point.x;
constauto&y=point.y;
constauto&z=point.z;
constauto&intensity=point.intensity;
constfloatrange=std::sqrt(x*x+y*y+z*z);
constfloatyaw=-std::atan2(y,x);
constfloatpitch=std::asin(z/range);

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

constintu=std::clamp<int>(static_cast<int>(proj_x),0,width-1);
constintv=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::Matrange=
cv::Mat(height,width,CV_32FC1,static_cast<void*>(range_images));
cv::Matnormalized_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("RangeImage",color_map);
cv::waitKey(0);

delete[]range_images;

return0;
}

對range通道可視化的結(jié)果如下圖所示：

上面的代碼有幾個需要說明的地方：

• fov_up，fov_down，image_means，image_stds這幾個參數(shù)來源于RangeNet++預訓練模型中的arch_cfg.yaml文件。
• std::clamp需要c++17支持，編譯的時候請使用-std=c++17編譯選項。
• 實際使用中width * height的值只需要計算一次，沒必要在循環(huán)里面反復計算，這里這么寫只是為了方便理解。

參考資料

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