今日嘗試大佬的激光slam代碼，用自己的數(shù)據(jù)時(shí)，遇到了一點(diǎn)問(wèn)題

如代碼所示
將點(diǎn)云從ros的msg格式轉(zhuǎn)換為pcl格式，并計(jì)算各點(diǎn)的仰角分析其線數(shù)，保存在laserCloudScans(N_SCANS)里，這部分使用的是16線的lidar
以我并沒(méi)有看過(guò)多少slam代碼的平平無(wú)奇的大腦，我立馬想到，這不就是aloam里面相似的計(jì)算仰角的代碼么？

大佬的代碼：

//判定各點(diǎn)的線數(shù)，按線數(shù)保存std::vector<pcl::PointCloud<PointType>> laserCloudScans(N_SCANS);for (int i = 0; i < cloudSize; i++){point.x = laserCloudIn.points[i].x;point.y = laserCloudIn.points[i].y;point.z = laserCloudIn.points[i].z;//仰角float angle = atan(point.z / sqrt(point.x * point.x + point.y * point.y)) * 180 / M_PI;int scanID = 0;if (N_SCANS == 16){scanID = int((angle + 15) / 2 + 0.5);if (scanID > (N_SCANS - 1) || scanID < 0){count--;continue;}}laserCloudScans[scanID].push_back(point);}

aloam里面的計(jì)算仰角的代碼：

bool halfPassed = false;int count = cloudSize;PointType point;std::vector<pcl::PointCloud<PointType>> laserCloudScans(N_SCANS);for (int i = 0; i < cloudSize; i++){point.x = laserCloudIn.points[i].x;point.y = laserCloudIn.points[i].y;point.z = laserCloudIn.points[i].z;float angle = atan(point.z / sqrt(point.x * point.x + point.y * point.y)) * 180 / M_PI;//-90�?�90��int scanID = 0;if (N_SCANS == 16){scanID = int((angle + 15) / 2 + 0.5);if (scanID > (N_SCANS - 1) || scanID < 0){count--;continue;}}else if (N_SCANS == 32){scanID = int((angle + 92.0/3.0) * 3.0 / 4.0);if (scanID > (N_SCANS - 1) || scanID < 0){count--;continue;}}else if (N_SCANS == 64){ if (angle >= -8.83)scanID = int((2 - angle) * 3.0 + 0.5);elsescanID = N_SCANS / 2 + int((-8.83 - angle) * 2.0 + 0.5);// use [0 50] > 50 remove outlies if (angle > 2 || angle < -24.33 || scanID > 50 || scanID < 0){count--;continue;}}else{printf("wrong scan number\n");ROS_BREAK();}//printf("angle %f scanID %d \n", angle, scanID);

在一些大佬對(duì)aloam的注釋中，并沒(méi)有詳細(xì)解釋這些參數(shù)是怎么的來(lái)的，但是比較詳細(xì)的解釋就是這句話
//計(jì)算點(diǎn)的仰角(根據(jù)lidar文檔垂直角計(jì)算公式),根據(jù)仰角排列激光線號(hào)，velodyne每?jī)蓚€(gè)scan之間間隔2度

于是我茅塞頓開(kāi)

16線的使用的是velodyne的激光雷達(dá)，VLP-16激光雷達(dá)是Velodyne公司出品的最小型的3維激光雷達(dá)，保留了電機(jī)轉(zhuǎn)速可調(diào)節(jié)的功能。實(shí)時(shí)上傳周圍距離和反射率的測(cè)量值。VLP-16具有100米的遠(yuǎn)量程測(cè)量距離。精巧的外觀設(shè)計(jì)使得安裝非常方便。重量輕，只有830g，非常適合安裝在小型無(wú)人機(jī)和小型移動(dòng)機(jī)器人上。 每秒高達(dá)30萬(wàn)個(gè)點(diǎn)數(shù)據(jù)輸出。±15°的垂直視場(chǎng)，360°水平視場(chǎng)掃描。

重點(diǎn)來(lái)了，它是-15度開(kāi)始掃描的，因此要用我們算出來(lái)的tangle-（-15）才是相對(duì)于第一條掃描線的角度，又因?yàn)樗拿扛鶔呙杈€間隔為[15-（-15）]/（16-1）=2，所以這就是我們算出來(lái)的點(diǎn)在第幾根掃描線上
scanID = int((angle + 15) / 2 + 0.5);
仰角四舍五入(加減0.5截?cái)嘈Ч扔谒纳嵛迦?

OK，現(xiàn)在我們把這個(gè)方法應(yīng)用到我們自己的lidar上，先來(lái)查查lidar使用手冊(cè)，使用手冊(cè)上肯定會(huì)有每一根掃描線的角度的，像這樣：

如果沒(méi)有，那么要它何用？嗬嗬，這就是我們的手冊(cè)上的：


我們的是32線的，可以清楚看見(jiàn)每根線的角度，使用剛才的方法來(lái)計(jì)算
初始角度是2.3125，直接可以得到，而間隔(89.5-2.3125)/(32-1)=2.8125
因此公式為：
scanID = int(((angle - 2.3125)/ 2.8125) + 0.5);

現(xiàn)在我們就學(xué)會(huì)了如何根據(jù)自己的lidar來(lái)修改仰角轉(zhuǎn)換到線數(shù)的公式了吧，現(xiàn)在你可以去看看velodyne32/64的相關(guān)參數(shù)，然后檢驗(yàn)一下自己是否可以算出來(lái)～

總結(jié)

以上是生活随笔為你收集整理的计算激光雷达各点的仰角并分析其线数——根据lidar文档垂直角公式计算自己的激光雷达参数/以aloam为例子的全部?jī)?nèi)容，希望文章能夠幫你解決所遇到的問(wèn)題。

如果覺(jué)得生活随笔網(wǎng)站內(nèi)容還不錯(cuò)，歡迎將生活随笔推薦給好友。