前言

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

球面投影推導(dǎo)過(guò)程

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

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

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

讓我們?cè)偻ㄟ^(guò)下圖來(lái)理解一下3維笛卡爾坐標(biāo)系和球面坐標(biāo)系之間的關(guān)系。

假設(shè)3維笛卡爾坐標(biāo)系下的點(diǎn)坐標(biāo)為，那么用球面坐標(biāo)系可以這樣表示該點(diǎn)：

如果以x軸方向?yàn)榍耙晥D的方向把激光雷達(dá)旋轉(zhuǎn)掃描一周得到的圓柱體展開(kāi)后，可以得到一副這樣的圖像：坐標(biāo)原點(diǎn)在圖像的中心，圖像中像素的縱坐標(biāo)由pitch角投影得到（范圍為[FOV_down,FOV_up]），橫坐標(biāo)由yaw角投影得到（范圍為）。

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

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

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

規(guī)范化后，再乘以投影圖像的寬高，就得到了這個(gè)點(diǎn)投影到距離圖像的坐標(biāo)：

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

代碼實(shí)現(xiàn)

理解了原理后，我們?cè)儆么a來(lái)把這個(gè)投影過(guò)程實(shí)現(xiàn)一遍。在RangeNet++中，點(diǎn)云被轉(zhuǎn)換為5個(gè)通道的距離圖像，這5個(gè)通道分別代表點(diǎn)云的這5個(gè)屬性。下面的代碼將展示如何通過(guò)球面投影將點(diǎn)云轉(zhuǎn)換為需要的距離圖像，使用的點(diǎn)云數(shù)據(jù)來(lái)源于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);
}

//對(duì)range通道進(jìn)行可視化
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;
}

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

上面的代碼有幾個(gè)需要說(shuō)明的地方：

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

參考資料

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