目錄
1?a-loam流程簡介
1.1 節(jié)點圖
1.2 代碼整體框架
2 特征點提取及均勻化
2.1論文原理
2.2 代碼詳解
3 異常點篩除
3.1 論文原理
3.2 代碼詳解
4 激光雷達畸變及運動補償
4.1?畸變及補償原理
4.2 代碼詳解
5 幀間里程計運動估計
5.1?幀間里程計原理
5.2 代碼詳解
6 局部地圖構(gòu)建
6.1?局部地圖構(gòu)建原理
6.2 代碼詳解
論文翻譯：SLAM論文翻譯(2) - LeGO-LOAM: Lightweight and Ground-OptimizedLidar Odometry and Mapping on Variab_幾度春風(fēng)里的博客-CSDN博客
1?a-loam流程簡介
參考文章：https://www.cnblogs.com/wellp/p/8877990.html
1.1 節(jié)點圖
1.2 代碼整體框架
LOAM主要包含兩個模塊，一個是Lidar Odometry，即使用激光雷達做里程計計算兩次掃描之間的位姿變換；另一個是Lidar Mapping，利用多次掃描的結(jié)果構(gòu)建地圖，細化位姿軌跡。由于Mapping部分計算量較大，所以計算頻率較低(1Hz)，由Mapping優(yōu)化Odometry過程中計算出來的軌跡。
A-LOAM的代碼清晰簡潔，主要是使用了Ceres函數(shù)庫。整個代碼目錄如下：
 |
docker | 提供了docker環(huán)境，方便開發(fā)者搭建環(huán)境 |
| include | 包含一個簡單的通用頭文件以及一個計時器的類TicToc，計時單位為ms | |
| launch | ros的啟動文件 | |
| out、picture | 預(yù)留的編譯產(chǎn)出目錄、README.md中的圖片 | |
| rviz_cfg | rviz的配置文件 | |
| src | 核心代碼：4個cpp分別對應(yīng)了節(jié)點圖中的4個node；1個hpp為基于Ceres構(gòu)建殘差函數(shù)時使用的各個仿函數(shù)類 |
（1）kittiHelper.cpp
- 對應(yīng)節(jié)點：/kittiHelper
- 功能：讀取kitti odometry的數(shù)據(jù)集的數(shù)據(jù)，具體包括點云、左右相機的圖像、以及pose的groundtruth（訓(xùn)練集才有），然后分成5個topic以10Hz（可修改）的頻率發(fā)布出去，其中真正對算法有用的topic只有點云/velodyne_points，其他四個topic都是在rviz中可視化用。
- 代碼分析：這部分代碼主要是ros的消息發(fā)布，需要注意的是，kitti的訓(xùn)練集真值pose的坐標系和點云的坐標系不相同如下圖所示，A-LOAM為了統(tǒng)一，坐標系均采用點云的坐標系，所以需要將真值pose進行坐標變換，轉(zhuǎn)到點云的坐標系下；真值pose的坐標系實際是左相機的坐標系。
（2）scanRegistration.cpp
- 對應(yīng)節(jié)點：/ascanRegistration
- 功能：對輸入點云進行濾波，提取4種feature，即邊緣點特征sharp、less_sharp，面特征flat、less_flat
（3）laserOdometry.cpp
- 對應(yīng)節(jié)點：/alaserOdometry
- 功能：基于前述的4種feature進行幀與幀的點云特征配準，即簡單的Lidar Odometry
（4）laserMapping.cpp
- 對應(yīng)節(jié)點：/alaserMapping
- 功能：基于前述的corner_less_sharp特征點和surf_less_flat特征點，進行幀與submap的點云特征配準，對Odometry線程計算的位姿進行finetune
2 特征點提取及均勻化
2.1論文原理
為什么需要特征點提??？
Lidar Odometry是通過Lidar的兩次掃描匹配，計算這兩次掃描之間idar的位姿變換，從而用作里程計Odometry。然而LOAM并沒有采用全部的激光點進行匹配，而是篩選出了兩類特征點，分別是角點和平面點。
????????我們從激光雷達云
中提取特征點開始，激光雷達在
中自然生成分布不均勻的點。我們選擇邊緣點和平面表面塊上的特征點。設(shè) i 是
中的一個點，
，設(shè)S是激光掃描儀在同一次掃描中返回的 i 的連續(xù)點的集合。定義一個術(shù)語來評估局部表面的平滑度，
????????根據(jù)c值對掃描點進行排序，然后選擇c值最大的特征點即邊緣點，c值最小的特征點即平面點。為了在環(huán)境中均勻分布特征點，我們將掃描分為四個相同的子區(qū)域。每個子區(qū)域最多可以提供2個邊緣點和4個平面點。當點 i 的c值大于或小于閾值，且所選點個數(shù)不超過最大值時，點 i 才能被選為邊點或平面點（特征點的均勻化）??????。
總結(jié)：LOAM提出了一種簡單而高效的特征點提取方式：根據(jù)點云點的曲率提取特征點。即把特別尖銳的邊線點與特別平坦的平面點作為特征點。
公式看起來比較復(fù)雜，實際上就是同一條掃描線上的取目標點左右兩側(cè)各5個點，分別與目標點的坐標作差，得到的結(jié)果就是目標點的曲率。當目標點處在棱或角的位置時，自然與周圍點的差值較大，得到的曲率較大；反之當目標點在平面上時，周圍點與目標點的坐標相近，得到的曲率自然較小。
2.2 代碼詳解
scanRegistration.cpp文件首先會進行初始化判斷；接著會進行會去除nan點和距離小于閾值的點；然后遍歷每一個點，保存為pcl::PointXYZI格式，便于后續(xù)計算曲率。
void laserCloudHandler(const sensor_msgs::PointCloud2ConstPtr &laserCloudMsg)
{
// 如果系統(tǒng)沒有初始化的話，就等幾幀
if (!systemInited)
{
systemInitCount++;
if (systemInitCount >= systemDelay)
{
systemInited = true;
}
else
return;
}
TicToc t_whole; //registration計時
TicToc t_prepare; //計算曲率前的預(yù)處理計時
// 記錄每個scan有曲率的點的開始和結(jié)束索引
std::vector<int> scanStartInd(N_SCANS, 0);
std::vector<int> scanEndInd(N_SCANS, 0);
// 定義pcl格式的消息類型
pcl::PointCloud<pcl::PointXYZ> laserCloudIn;
// pcl不能直接處理消息類型，將點云從ros格式轉(zhuǎn)到pcl的格式
pcl::fromROSMsg(*laserCloudMsg, laserCloudIn);
std::vector<int> indices;
// 去除掉點云中的nan點
// pcl::removeNaNFromPointCloud(輸入點云, 輸出點云, 對應(yīng)保留的索引);
pcl::removeNaNFromPointCloud(laserCloudIn, laserCloudIn, indices);
// 去除距離小于閾值的點
removeClosedPointCloud(laserCloudIn, laserCloudIn, MINIMUM_RANGE);
// 計算起始點和結(jié)束點的角度，由于激光雷達是順時針旋轉(zhuǎn)，這里取反就相當于轉(zhuǎn)成了逆時針
int cloudSize = laserCloudIn.points.size();
float startOri = -atan2(laserCloudIn.points[0].y, laserCloudIn.points[0].x);
// 每次掃描是一條線，看作者的數(shù)據(jù)集激光x向前，y向左，那么下面就是線一端到另一端
// atan2的輸出為-pi到pi(atan輸出為-pi/2到pi/2)
// 計算旋轉(zhuǎn)角時取負號是因為velodyne是順時針旋轉(zhuǎn)
// atan2范圍是[-Pi,PI]，這里加上2PI是為了保證起始到結(jié)束相差2PI符合實際
float endOri = -atan2(laserCloudIn.points[cloudSize - 1].y,
laserCloudIn.points[cloudSize - 1].x) +
2 * M_PI;
// 總有一些例外，比如這里大于3PI，和小于PI，就需要做一些調(diào)整到合理范圍
if (endOri - startOri > 3 * M_PI)
{
endOri -= 2 * M_PI;
}
else if (endOri - startOri < M_PI)
{
endOri += 2 * M_PI;
}
//printf("end Ori %f\n", endOri);
bool halfPassed = false; //過半記錄標志
int count = cloudSize; //記錄總點數(shù)
PointType point;
// 按線數(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;
// 求仰角atan輸出為-pi/2到pi/2，實際看scanID應(yīng)該每條線之間差距是2度
float angle = atan(point.z / sqrt(point.x * point.x + point.y * point.y)) * 180 / M_PI;
int scanID = 0;
// 計算是第幾根scan
// 16線激光雷達范圍[-15°，15°] -> (+15變?yōu)檎?[0,30] -> (/2每一個線的角度2°) -> (+0.5四舍五入)
if (N_SCANS == 16)
{
scanID = int((angle + 15) / 2 + 0.5);
// scanID不合理時退出
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);
else
scanID = 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;
}
}
// 只有16,32,64線，其他的會報錯
else
{
printf("wrong scan number\n");
ROS_BREAK();
}
//printf("angle %f scanID %d \n", angle, scanID);
// 計算水平角
float ori = -atan2(point.y, point.x);
// 如果水平角沒有超過一半
if (!halfPassed)
{
// 確保起始角-終止角（ori-startOri）的范圍在[-PI/2 , 3/2*PI]
if (ori < startOri - M_PI / 2)
{
ori += 2 * M_PI;
}
else if (ori > startOri + M_PI * 3 / 2)
{
ori -= 2 * M_PI;
}
// 如果超過180度，就說明過了一半了
if (ori - startOri > M_PI)
{
halfPassed = true;
}
}
// 如果水平角沒有超過一半
else
{
// 確保起始角-終止角（ori-startOri）的范圍在[-PI * 3/2 , PI/2]
ori += 2 * M_PI; // 先補償2PI
if (ori < endOri - M_PI * 3 / 2)
{
ori += 2 * M_PI;
}
else if (ori > endOri + M_PI / 2)
{
ori -= 2 * M_PI;
}
}
// 角度的計算是為了計算相對的起始時刻的時間
float relTime = (ori - startOri) / (endOri - startOri);
// 整數(shù)部分是scan的索引，小數(shù)部分是相對起始時刻的時間
point.intensity = scanID + scanPeriod * relTime;
// 根據(jù)scan的idx送入各自數(shù)組
laserCloudScans[scanID].push_back(point);
}
// cloudSize是有效的點云的數(shù)目
cloudSize = count;
printf("points size %d \n", cloudSize);
pcl::PointCloud<PointType>::Ptr laserCloud(new pcl::PointCloud<PointType>());
// 全部集合到一個點云里面去，但是使用兩個數(shù)組標記其實和結(jié)果，這里分別+5和-6是為了計算曲率方便
for (int i = 0; i < N_SCANS; i++)
{
scanStartInd[i] = laserCloud->size() + 5;
*laserCloud += laserCloudScans[i];
scanEndInd[i] = laserCloud->size() - 6;
}
printf("prepare time %f \n", t_prepare.toc());
...
}接著開始計算曲率，計算曲率時代碼和論文中的公式有一些差異，代碼中省略了分母的計算；在計算曲率后根據(jù)閾值判斷4類角點和面點。
{
...
// 開始計算曲率，角點曲率比較大，面點曲率比較小
// 曲率 = 左邊5個點+右邊5個點-10*當前點
for (int i = 5; i < cloudSize - 5; i++)
{
float diffX = laserCloud->points[i - 5].x + laserCloud->points[i - 4].x + laserCloud->points[i - 3].x + laserCloud->points[i - 2].x + laserCloud->points[i - 1].x - 10 * laserCloud->points[i].x + laserCloud->points[i + 1].x + laserCloud->points[i + 2].x + laserCloud->points[i + 3].x + laserCloud->points[i + 4].x + laserCloud->points[i + 5].x;
float diffY = laserCloud->points[i - 5].y + laserCloud->points[i - 4].y + laserCloud->points[i - 3].y + laserCloud->points[i - 2].y + laserCloud->points[i - 1].y - 10 * laserCloud->points[i].y + laserCloud->points[i + 1].y + laserCloud->points[i + 2].y + laserCloud->points[i + 3].y + laserCloud->points[i + 4].y + laserCloud->points[i + 5].y;
float diffZ = laserCloud->points[i - 5].z + laserCloud->points[i - 4].z + laserCloud->points[i - 3].z + laserCloud->points[i - 2].z + laserCloud->points[i - 1].z - 10 * laserCloud->points[i].z + laserCloud->points[i + 1].z + laserCloud->points[i + 2].z + laserCloud->points[i + 3].z + laserCloud->points[i + 4].z + laserCloud->points[i + 5].z;
// 存儲曲率，索引，2個標志位
cloudCurvature[i] = diffX * diffX + diffY * diffY + diffZ * diffZ;
cloudSortInd[i] = i;
cloudNeighborPicked[i] = 0;
cloudLabel[i] = 0;
}
TicToc t_pts; //計時用
// 角點，降采樣角點，面點，降采樣面點
pcl::PointCloud<PointType> cornerPointsSharp;
pcl::PointCloud<PointType> cornerPointsLessSharp;
pcl::PointCloud<PointType> surfPointsFlat;
pcl::PointCloud<PointType> surfPointsLessFlat;
float t_q_sort = 0;
// 遍歷每個scan
for (int i = 0; i < N_SCANS; i++)
{
// 沒有有效的點了，就continue
if( scanEndInd[i] - scanStartInd[i] < 6)
continue;
// 用來存儲不太平整的點
pcl::PointCloud<PointType>::Ptr surfPointsLessFlatScan(new pcl::PointCloud<PointType>);
// 將每個scan的曲率點分成6等份處理,確保周圍都有點被選作特征點,或者說每兩行都有
for (int j = 0; j < 6; j++)
{
// 六等份起點：sp = scanStartInd + (scanEndInd - scanStartInd)*j/6
// 六等份終點：ep = scanStartInd - 1 + (scanEndInd - scanStartInd)*(j+1)/6
int sp = scanStartInd[i] + (scanEndInd[i] - scanStartInd[i]) * j / 6;
int ep = scanStartInd[i] + (scanEndInd[i] - scanStartInd[i]) * (j + 1) / 6 - 1;
TicToc t_tmp;
// 對點云按照曲率進行排序，小的在前，大的在后
std::sort (cloudSortInd + sp, cloudSortInd + ep + 1, comp);
t_q_sort += t_tmp.toc();
int largestPickedNum = 0;
// 挑選曲率比較大的部分
for (int k = ep; k >= sp; k--)
{
// 排序后順序就亂了，這個時候索引的作用就體現(xiàn)出來了
int ind = cloudSortInd[k];
// 看看這個點是否是有效點，同時曲率是否大于閾值
if (cloudNeighborPicked[ind] == 0 &&
cloudCurvature[ind] > 0.1)
{
// 曲率大的點記數(shù)一下
largestPickedNum++;
// 每段選2個曲率大的點
if (largestPickedNum <= 2)
{
// label為2是曲率大的標記
cloudLabel[ind] = 2;
// cornerPointsSharp存放大曲率的點
cornerPointsSharp.push_back(laserCloud->points[ind]);
cornerPointsLessSharp.push_back(laserCloud->points[ind]);
}
// 以及20個曲率稍微大一些的點
else if (largestPickedNum <= 20)
{
// label置1表示曲率稍微大
cloudLabel[ind] = 1;
cornerPointsLessSharp.push_back(laserCloud->points[ind]);
}
// 超過20個就算了
else
{
break;
}
// 這個點被選中后 pick標志位置1
cloudNeighborPicked[ind] = 1;
// 為了保證特征點不過度集中，將選中的點周圍5個點都置1,避免后續(xù)會選到
for (int l = 1; l <= 5; l++)
{
// 查看相鄰點距離是否差異過大，如果差異過大說明點云在此不連續(xù)，是特征邊緣，就會是新的特征，因此就不置位了
float diffX = laserCloud->points[ind + l].x - laserCloud->points[ind + l - 1].x;
float diffY = laserCloud->points[ind + l].y - laserCloud->points[ind + l - 1].y;
float diffZ = laserCloud->points[ind + l].z - laserCloud->points[ind + l - 1].z;
if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05)
{
break;
}
// 最后就是直接篩過的
cloudNeighborPicked[ind + l] = 1;
}
// 下面同理
for (int l = -1; l >= -5; l--)
{
float diffX = laserCloud->points[ind + l].x - laserCloud->points[ind + l + 1].x;
float diffY = laserCloud->points[ind + l].y - laserCloud->points[ind + l + 1].y;
float diffZ = laserCloud->points[ind + l].z - laserCloud->points[ind + l + 1].z;
if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05)
{
break;
}
cloudNeighborPicked[ind + l] = 1;
}
}
}
// 下面開始挑選面點
int smallestPickedNum = 0;
for (int k = sp; k <= ep; k++)
{
int ind = cloudSortInd[k];
// 確保這個點沒有被pick且曲率小于閾值
if (cloudNeighborPicked[ind] == 0 &&
cloudCurvature[ind] < 0.1)
{
// -1認為是平坦的點
cloudLabel[ind] = -1;
surfPointsFlat.push_back(laserCloud->points[ind]);
smallestPickedNum++;
// 這里不區(qū)分平坦和比較平坦，因為剩下的點label默認是0,就是比較平坦
if (smallestPickedNum >= 4)
{
break;
}
// 同樣防止特征點聚集
cloudNeighborPicked[ind] = 1;
for (int l = 1; l <= 5; l++)
{
float diffX = laserCloud->points[ind + l].x - laserCloud->points[ind + l - 1].x;
float diffY = laserCloud->points[ind + l].y - laserCloud->points[ind + l - 1].y;
float diffZ = laserCloud->points[ind + l].z - laserCloud->points[ind + l - 1].z;
if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05)
{
break;
}
cloudNeighborPicked[ind + l] = 1;
}
for (int l = -1; l >= -5; l--)
{
float diffX = laserCloud->points[ind + l].x - laserCloud->points[ind + l + 1].x;
float diffY = laserCloud->points[ind + l].y - laserCloud->points[ind + l + 1].y;
float diffZ = laserCloud->points[ind + l].z - laserCloud->points[ind + l + 1].z;
if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05)
{
break;
}
cloudNeighborPicked[ind + l] = 1;
}
}
}
// 遍歷所有的點，只要不是角點，則認為是面點
for (int k = sp; k <= ep; k++)
{
// 這里可以看到，剩下來的點都是一般平坦，這個也符合實際
if (cloudLabel[k] <= 0)
{
surfPointsLessFlatScan->push_back(laserCloud->points[k]);
}
}
}
pcl::PointCloud<PointType> surfPointsLessFlatScanDS;
pcl::VoxelGrid<PointType> downSizeFilter;
// 一般平坦的點比較多，所以這里做一個體素濾波
downSizeFilter.setInputCloud(surfPointsLessFlatScan);
downSizeFilter.setLeafSize(0.2, 0.2, 0.2);
downSizeFilter.filter(surfPointsLessFlatScanDS);
// less flat點匯總
surfPointsLessFlat += surfPointsLessFlatScanDS;
}
printf("sort q time %f \n", t_q_sort);
printf("seperate points time %f \n", t_pts.toc());
...
}最后將提取到的角點和面點發(fā)布出去。
// 分別將當前點云、四種特征的點云發(fā)布出去
sensor_msgs::PointCloud2 laserCloudOutMsg;
pcl::toROSMsg(*laserCloud, laserCloudOutMsg);
laserCloudOutMsg.header.stamp = laserCloudMsg->header.stamp;
laserCloudOutMsg.header.frame_id = "/camera_init";
pubLaserCloud.publish(laserCloudOutMsg);
sensor_msgs::PointCloud2 cornerPointsSharpMsg;
pcl::toROSMsg(cornerPointsSharp, cornerPointsSharpMsg);
cornerPointsSharpMsg.header.stamp = laserCloudMsg->header.stamp;
cornerPointsSharpMsg.header.frame_id = "/camera_init";
pubCornerPointsSharp.publish(cornerPointsSharpMsg);
sensor_msgs::PointCloud2 cornerPointsLessSharpMsg;
pcl::toROSMsg(cornerPointsLessSharp, cornerPointsLessSharpMsg);
cornerPointsLessSharpMsg.header.stamp = laserCloudMsg->header.stamp;
cornerPointsLessSharpMsg.header.frame_id = "/camera_init";
pubCornerPointsLessSharp.publish(cornerPointsLessSharpMsg);
sensor_msgs::PointCloud2 surfPointsFlat2;
pcl::toROSMsg(surfPointsFlat, surfPointsFlat2);
surfPointsFlat2.header.stamp = laserCloudMsg->header.stamp;
surfPointsFlat2.header.frame_id = "/camera_init";
pubSurfPointsFlat.publish(surfPointsFlat2);
sensor_msgs::PointCloud2 surfPointsLessFlat2;
pcl::toROSMsg(surfPointsLessFlat, surfPointsLessFlat2);
surfPointsLessFlat2.header.stamp = laserCloudMsg->header.stamp;
surfPointsLessFlat2.header.frame_id = "/camera_init";
pubSurfPointsLessFlat.publish(surfPointsLessFlat2);
// 可以按照每個scan發(fā)出去，不過這里是false
if(PUB_EACH_LINE)
{
for(int i = 0; i< N_SCANS; i++)
{
sensor_msgs::PointCloud2 scanMsg;
pcl::toROSMsg(laserCloudScans[i], scanMsg);
scanMsg.header.stamp = laserCloudMsg->header.stamp;
scanMsg.header.frame_id = "/camera_init";
pubEachScan[i].publish(scanMsg);
}
}
printf("scan registration time %f ms *************\n", t_whole.toc());
if(t_whole.toc() > 100)
ROS_WARN("scan registration process over 100ms");
...
}3 異常點篩除
3.1 論文原理
????????如圖所示（a）實線段代表局部表面斑塊。點A位于與激光束有一定角度的表面上(橙色虛線段)。點B位于與激光束大致平行的表面上。我們認為B是一個不可靠的特征點，不選擇它作為特征點。（b）實線段是激光可觀測的物體。點A在被遮擋區(qū)域的邊界上(橙色虛線段)，可以檢測到邊緣點。然而，如果從不同的角度觀察，被遮擋的區(qū)域可以改變并變得可觀察到。我們不把A作為顯著邊點，也不選擇A可靠的特征點。
可以定義這兩類異常點為平行點和遮擋點
平行點：指的就是圖中的（a）點，就是激光的射線幾乎和物體的平面平行了，剔除這種點的原因有兩個:
- 激光的數(shù)據(jù)會不準,射線被拉長
- 這種點被視為特征點后會非常不穩(wěn)定,下一幀可能就沒了,無法做配對了例如圖片中的lidar向左移一點,那么B點就消失了,形成對比的就是A點,極短時間內(nèi)不會消失
遮擋點：遮擋點就是圖中的（b）點，lidar一條sacn上，相鄰的兩個或幾個點的距離偏差很大，被視為遮擋點，剔除這種點的原因是：
- 這種點被視為特征點后會非常不穩(wěn)定，下一幀可能就沒了，無法做配對了，例如圖片中的lidar向右移一點,那么A點就消失了,形成對比的就是B點,極短時間內(nèi)不會消失此時最后把A點的左邊幾個點,都剔除掉。
3.2 代碼詳解
在a-loam的代碼中，沒有集成論文異常點篩除的部分，從vio-sam學(xué)習(xí)一下代碼。
// 標記一下遮擋的點
void markOccludedPoints()
{
int cloudSize = extractedCloud->points.size();
// 遍歷兩種情況，遮擋的點和平行的點
for (int i = 5; i < cloudSize - 6; ++i)
{
// occluded points
// 取出相鄰兩個點距離信息
float depth1 = cloudInfo.pointRange[i];
float depth2 = cloudInfo.pointRange[i+1];
// 計算兩個有效點之間的列id差
int columnDiff = std::abs(int(cloudInfo.pointColInd[i+1] - cloudInfo.pointColInd[i]));
// 只有比較靠近才有意義
if (columnDiff < 10){
// 10 pixel diff in range image
// 對應(yīng)圖b中B點的情況
// 這樣depth1容易被遮擋，因此其之前的5個點為不穩(wěn)定的點
if (depth1 - depth2 > 0.3){
cloudNeighborPicked[i - 5] = 1;
cloudNeighborPicked[i - 4] = 1;
cloudNeighborPicked[i - 3] = 1;
cloudNeighborPicked[i - 2] = 1;
cloudNeighborPicked[i - 1] = 1;
cloudNeighborPicked[i] = 1;
}else if (depth2 - depth1 > 0.3){ // 同理，對應(yīng)圖b中B點的情況
cloudNeighborPicked[i + 1] = 1;
cloudNeighborPicked[i + 2] = 1;
cloudNeighborPicked[i + 3] = 1;
cloudNeighborPicked[i + 4] = 1;
cloudNeighborPicked[i + 5] = 1;
cloudNeighborPicked[i + 6] = 1;
}
}
// 圖a平行的情況，通過判斷左右兩點距Lidar的差進行判斷
float diff1 = std::abs(float(cloudInfo.pointRange[i-1] - cloudInfo.pointRange[i]));
float diff2 = std::abs(float(cloudInfo.pointRange[i+1] - cloudInfo.pointRange[i]));
// 如果兩點距離比較大 就很可能是平行的點，也很可能失去觀測
if (diff1 > 0.02 * cloudInfo.pointRange[i] && diff2 > 0.02 * cloudInfo.pointRange[i])
cloudNeighborPicked[i] = 1;
}
}4 激光雷達畸變及運動補償
參考文章：3D激光SLAM:ALOAM:激光雷達的運動畸變補償代碼解析_eigen::quaterniond::identity().slerp_月照銀海似蛟龍的博客-CSDN博客
4.1?畸變及補償原理
什么是激光雷達的運動畸變 ?
我們知道激光雷達的一幀數(shù)據(jù)是過去一段時間而非某個時刻的數(shù)據(jù)，因此在這一幀時間內(nèi)的激光雷達或者其載體通常會發(fā)生運動。因此，這一幀的原點不一致會導(dǎo)致運動畸變，所以需要畸變校正。
如何進行運動補償?
運動補償?shù)哪康木褪?strong>把所有的點云補償?shù)侥骋粫r刻，這樣就可以把本身在過去100ms內(nèi)收集的點云統(tǒng)一到一個時間點上去，這個時間點可以是起始時刻，也可以是結(jié)束時刻，也可以是中間的任意時刻。
畸變校準方法
因此運動補償需要知道每個點時刻對應(yīng)的位姿
通常有幾種做法：
- 如果有高頻里程計，可以比較方便的獲取每個點相對起始掃描時刻的位姿。
- 如果有imu，可以方便的求出每個點相對起始點的旋轉(zhuǎn)。
- 如果沒有其它傳感器，可以使用勻速模型假設(shè)，使用上一幀間里程計的結(jié)果，作為當前兩幀之間的運動，同時假設(shè)當前幀也是勻速運動，也可以估計出每個點相對起始時刻的位姿k-1 到 k 幀 和 k到k+1幀的運動是一至的，用k-1到k幀的位姿變換當做k到k+1幀的位姿變換, 可以求到k到k+1幀的每個點的位姿變換。
ALOAM是使用的純lidar的方式，所以使用的是第3種方式。做一個勻速模型假設(shè)，即上一幀的位姿變換，就是這幀的位姿變換以此來求出輸入點坐標系到起始時刻點坐標系的位姿變換，通過求的時間占比s，這里把位姿變換分解成了旋轉(zhuǎn) + 平移的方式。
線性插值和球面線性插值的對比如下圖。
為什么選擇球面插值？
兩個單位四元數(shù)之間進行插值，如左圖的線性插值，得到的四元數(shù)一定不是單位四元數(shù)，我們期望對于旋轉(zhuǎn)的插值應(yīng)該是不改變長度的，所以顯然右圖球面（Slerp）插值更為合理。?由于四元數(shù)是一個超復(fù)數(shù)，不是簡單的乘法，求它的占比用的 Eigen的slerp函數(shù)(球面線性插值)
4.2 代碼詳解
在laserOdometry.cpp文件中進行畸變運動補償。代碼中通過反變換的思想,首先把點統(tǒng)一到起始時刻坐標系下,再通過反變換,得到結(jié)束時刻坐標系下的點。
/*
* @brief lidar去畸變,所有點補償?shù)狡鹗紩r刻
* @param pi 輸入點的點云
* @param po 轉(zhuǎn)換后的輸出點的點云
*/
void TransformToStart(PointType const *const pi, PointType *const po)
{
// interpolation ratio
double s;
// 由于kitti數(shù)據(jù)集上的lidar已經(jīng)做過了運動補償，因此這里就不做具體補償了
if (DISTORTION)
// intensity：實數(shù)部分存的是 scan上點的 id
// pi->intensity - int(pi->intensity)：虛數(shù)部分存的這一點相對這一幀起始點的時間差
// SCAN_PERIOD：一幀的時間，10HZ的lidar,周期是0.1s
// s：計算當前點在一幀中所占的時間比例
s = (pi->intensity - int(pi->intensity)) / SCAN_PERIOD;
else
s = 1.0; // s = 1s說明全部補償?shù)近c云結(jié)束的時刻
// 這里相當于是一個勻速模型的假設(shè)
// 把位姿變換分解成平移和旋轉(zhuǎn)
// 由于四元數(shù)是一個超復(fù)數(shù), 不是簡單的乘法, 求它的占比用的 Eigen的slerp函數(shù)(球面線性插值)
Eigen::Quaterniond q_point_last = Eigen::Quaterniond::Identity().slerp(s, q_last_curr);
Eigen::Vector3d t_point_last = s * t_last_curr;
Eigen::Vector3d point(pi->x, pi->y, pi->z); //把當前點的坐標取出
Eigen::Vector3d un_point = q_point_last * point + t_point_last; //通過旋轉(zhuǎn)和平移將當前點轉(zhuǎn)到幀起始時刻坐標系下的坐標
// 將求得的轉(zhuǎn)換后的坐標賦值給輸出點
po->x = un_point.x();
po->y = un_point.y();
po->z = un_point.z();
po->intensity = pi->intensity; // 賦值原的intensity
}
// 所有點補償?shù)浇Y(jié)束時刻
// 這個是通過反變換的思想,首先把點統(tǒng)一到起始時刻坐標系下,再通過反變換,得到結(jié)束時刻坐標系下的點
void TransformToEnd(PointType const *const pi, PointType *const po)
{
// 首先先做畸變校正,都轉(zhuǎn)到起始時刻
pcl::PointXYZI un_point_tmp;
TransformToStart(pi, &un_point_tmp); // 先統(tǒng)一到起始時刻
//再通過反變換的方式,將起始時刻坐標系下的點轉(zhuǎn)到結(jié)束時刻坐標系下
Eigen::Vector3d un_point(un_point_tmp.x, un_point_tmp.y, un_point_tmp.z); // 取出起始時刻坐標系下的點
//q_last_curr/t_last_curr 是結(jié)束時刻坐標系轉(zhuǎn)到起始時刻坐標系的旋轉(zhuǎn)和平移
Eigen::Vector3d point_end = q_last_curr.inverse() * (un_point - t_last_curr); // //通過反變換,求得轉(zhuǎn)到結(jié)束時刻坐標系下的點坐標
//將求得的轉(zhuǎn)換后的坐標賦值給輸出點
po->x = point_end.x();
po->y = point_end.y();
po->z = point_end.z();
//移除掉 intensity 相對時間的信息
po->intensity = int(pi->intensity);
}5 幀間里程計運動估計
5.1?幀間里程計原理
幀間里程計估計激光雷達在一次掃描中的運動。設(shè)
為一次掃描 k 的開始時間，每次掃描結(jié)束時，掃描過程中感知到的點云
，重投影到時間戳
，如圖6所示。我們將重新投影的點云標記為
。在下一個掃描 k+1 中，
與新接收到的點云
一起使用，來估計激光雷達的運動。
?如圖所示。重新投影點云到掃描結(jié)束。藍色線段表示掃描k中感知到的點云。在掃描k
結(jié)束時，被重投影到時間戳，以獲得(綠色線段)。然后，在掃描k+1期間，和新感知的
點云(橙色線段)一起估計激光雷達的運動。
????????我們假設(shè)和現(xiàn)在都是可用的，然后從尋找兩個激光雷達云之間的對應(yīng)關(guān)系開始。對于，我們使用上一節(jié)討論的方法從激光雷達云中找到角點和面點。設(shè)
為角點集合，
為面點集合。我們將的邊線作為中的點的對應(yīng)點，平面集合作為中的點的對應(yīng)點。
????????請注意，在掃描k+1開始時，是一個空集，在掃描過程中，隨著接收到更多的點，它會增長。激光雷達里程計遞歸估計掃描過程中的 6-DOF 運動，并隨著的增加逐漸包含更多的點。在每次迭代中，和使用當前估計的變換重新投影到掃描的開始。讓
和
為重投影點集。對于?和
?中的每個點，我們將找到中最近的鄰居點。在這里，存儲在3D-kd樹中，用于快速索引。
特征點匹配分為scan-to-scan以及scan-to-map兩種，這兩種類型的匹配基本原理都是一致的，都是轉(zhuǎn)到相同坐標系下采用最近鄰的方式獲取，不同的是scan-to-scan更加簡單粗暴，而scan-to-map會更加復(fù)雜魯棒，具體如下：
scan-to-scan如何進行特征點匹配
在scan-to-scan的匹配過程中，首先是將所有特征點轉(zhuǎn)到當前幀其實時刻坐標系下，再將前后幀特征點轉(zhuǎn)到同一坐標系下，后者可以通過IMU或者勻速運動模型獲得初始位姿：
- 對于corner sharp點O來說就是尋找匹配幀中，corner less sharp點中最近的點以及和點位于相鄰線束（不一定是相鄰一幀）最近的點B，殘差即點O到直線AB的距離；
- 對于surf flat點P來說就是就是尋找匹配幀中，surf less flat點中最近的點 ，以及和??位于同一線束的點，以及和A位于相鄰線束的點，殘差即點 P 到平面的距離；
5.2 代碼詳解
代碼按照上面的原理進行復(fù)現(xiàn)，調(diào)用ceres求解器求解得到幀間位姿，最后將求解的位姿發(fā)布。
int main(int argc, char **argv)
{
// 初始化節(jié)點 laserOdometry
ros::init(argc, argv, "laserOdometry");
// 聲明句柄
ros::NodeHandle nh;
//從服務(wù)器讀取參數(shù) mapping_skip_frame 下采樣的頻率
nh.param<int>("mapping_skip_frame", skipFrameNum, 2);
printf("Mapping %d Hz \n", 10 / skipFrameNum);
// 訂閱一些點云的topic,100是隊列
ros::Subscriber subCornerPointsSharp = nh.subscribe<sensor_msgs::PointCloud2>("/laser_cloud_sharp", 100, laserCloudSharpHandler);
ros::Subscriber subCornerPointsLessSharp = nh.subscribe<sensor_msgs::PointCloud2>("/laser_cloud_less_sharp", 100, laserCloudLessSharpHandler);
ros::Subscriber subSurfPointsFlat = nh.subscribe<sensor_msgs::PointCloud2>("/laser_cloud_flat", 100, laserCloudFlatHandler);
ros::Subscriber subSurfPointsLessFlat = nh.subscribe<sensor_msgs::PointCloud2>("/laser_cloud_less_flat", 100, laserCloudLessFlatHandler);
ros::Subscriber subLaserCloudFullRes = nh.subscribe<sensor_msgs::PointCloud2>("/velodyne_cloud_2", 100, laserCloudFullResHandler);
ros::Publisher pubLaserCloudCornerLast = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_corner_last", 100);
ros::Publisher pubLaserCloudSurfLast = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_surf_last", 100);
ros::Publisher pubLaserCloudFullRes = nh.advertise<sensor_msgs::PointCloud2>("/velodyne_cloud_3", 100);
ros::Publisher pubLaserOdometry = nh.advertise<nav_msgs::Odometry>("/laser_odom_to_init", 100);
ros::Publisher pubLaserPath = nh.advertise<nav_msgs::Path>("/laser_odom_path", 100);
nav_msgs::Path laserPath;
int frameCount = 0;
ros::Rate rate(100);
while (ros::ok())
{
ros::spinOnce(); // 觸發(fā)一次回調(diào)，參考https://www.cnblogs.com/liu-fa/p/5925381.html
// 首先確保訂閱的五個消息都有，有一個隊列為空都不行
if (!cornerSharpBuf.empty() && !cornerLessSharpBuf.empty() &&
!surfFlatBuf.empty() && !surfLessFlatBuf.empty() &&
!fullPointsBuf.empty())
{
// 分別求出隊列第一個時間
timeCornerPointsSharp = cornerSharpBuf.front()->header.stamp.toSec();
timeCornerPointsLessSharp = cornerLessSharpBuf.front()->header.stamp.toSec();
timeSurfPointsFlat = surfFlatBuf.front()->header.stamp.toSec();
timeSurfPointsLessFlat = surfLessFlatBuf.front()->header.stamp.toSec();
timeLaserCloudFullRes = fullPointsBuf.front()->header.stamp.toSec();
// 因為同一幀的時間戳都是相同的，因此這里比較是否是同一幀
if (timeCornerPointsSharp != timeLaserCloudFullRes ||
timeCornerPointsLessSharp != timeLaserCloudFullRes ||
timeSurfPointsFlat != timeLaserCloudFullRes ||
timeSurfPointsLessFlat != timeLaserCloudFullRes)
{
printf("unsync messeage!");
ROS_BREAK();
}
// 分別將五個點云消息取出來，同時轉(zhuǎn)成pcl的點云格式
mBuf.lock();
cornerPointsSharp->clear();
pcl::fromROSMsg(*cornerSharpBuf.front(), *cornerPointsSharp); //轉(zhuǎn)成pcl的點云格式
cornerSharpBuf.pop(); //去掉隊列里面的第一個
cornerPointsLessSharp->clear();
pcl::fromROSMsg(*cornerLessSharpBuf.front(), *cornerPointsLessSharp);
cornerLessSharpBuf.pop();
surfPointsFlat->clear();
pcl::fromROSMsg(*surfFlatBuf.front(), *surfPointsFlat);
surfFlatBuf.pop();
surfPointsLessFlat->clear();
pcl::fromROSMsg(*surfLessFlatBuf.front(), *surfPointsLessFlat);
surfLessFlatBuf.pop();
laserCloudFullRes->clear();
pcl::fromROSMsg(*fullPointsBuf.front(), *laserCloudFullRes);
fullPointsBuf.pop();
mBuf.unlock();
TicToc t_whole;
// 判斷系統(tǒng)初始化
if (!systemInited)
{
systemInited = true;
std::cout << "Initialization finished \n";
}
else
{
// 取出當前幀明顯的角點和面點和上一幀所有的角點和面點約束建立
int cornerPointsSharpNum = cornerPointsSharp->points.size();
int surfPointsFlatNum = surfPointsFlat->points.size();
TicToc t_opt;
// 進行兩次迭代
for (size_t opti_counter = 0; opti_counter < 2; ++opti_counter)
{
corner_correspondence = 0; //初始化角點匹配的點對的數(shù)量
plane_correspondence = 0; //初始化面點匹配的點對的數(shù)量
//ceres::LossFunction *loss_function = NULL;
// 定義一下ceres的核函數(shù)，0.1代表殘差大于0.1的點 ,則權(quán)重降低；小于0.1則認為正常,不做特殊的處理
ceres::LossFunction *loss_function = new ceres::HuberLoss(0.1);
// 由于旋轉(zhuǎn)不滿足一般意義的加法，因此這里使用ceres自帶的local param
ceres::LocalParameterization *q_parameterization =
new ceres::EigenQuaternionParameterization();
ceres::Problem::Options problem_options;
// 聲明ceres 的 Problem::Options
ceres::Problem problem(problem_options);
// 待優(yōu)化的變量是幀間位姿，平移和旋轉(zhuǎn)，這里旋轉(zhuǎn)使用四元數(shù)來表示
// para_q是一個數(shù)組的指針，后面跟參數(shù)的的長度，不符合普通加法則再設(shè)置local Param
problem.AddParameterBlock(para_q, 4, q_parameterization); //添加四元數(shù)的參數(shù)塊
problem.AddParameterBlock(para_t, 3); //添加平移的參數(shù)塊
pcl::PointXYZI pointSel; //畸變校正后的點云
std::vector<int> pointSearchInd; //kdtree找到的最近鄰點的id
std::vector<float> pointSearchSqDis; //kdtree找到的最近鄰點的距離
TicToc t_data;
// 尋找角點的約束
for (int i = 0; i < cornerPointsSharpNum; ++i)
{
// 運動補償
TransformToStart(&(cornerPointsSharp->points[i]), &pointSel);
// 在上一幀所有角點構(gòu)成的kdtree中尋找距離當前幀最近的一個點
kdtreeCornerLast->nearestKSearch(pointSel, 1, pointSearchInd, pointSearchSqDis);
int closestPointInd = -1, minPointInd2 = -1;
// 只有小于給定閾值才認為是有效約束
if (pointSearchSqDis[0] < DISTANCE_SQ_THRESHOLD)
{
// 對應(yīng)的最近距離的索引取出來
closestPointInd = pointSearchInd[0];
// 找到其所在線束id，線束信息藏在intensity的整數(shù)部分
int closestPointScanID = int(laserCloudCornerLast->points[closestPointInd].intensity);
double minPointSqDis2 = DISTANCE_SQ_THRESHOLD;
// 尋找角點，在剛剛角點id上下分別繼續(xù)尋找，目的是找到最近的角點，由于其按照線束進行排序，所以就是向上找
for (int j = closestPointInd + 1; j < (int)laserCloudCornerLast->points.size(); ++j)
{
// 不找同一根線束的
if (int(laserCloudCornerLast->points[j].intensity) <= closestPointScanID)
continue;
// 要求找到的線束距離當前線束不能太遠
if (int(laserCloudCornerLast->points[j].intensity) > (closestPointScanID + NEARBY_SCAN))
break;
// 計算和當前找到的角點之間的距離
double pointSqDis = (laserCloudCornerLast->points[j].x - pointSel.x) *
(laserCloudCornerLast->points[j].x - pointSel.x) +
(laserCloudCornerLast->points[j].y - pointSel.y) *
(laserCloudCornerLast->points[j].y - pointSel.y) +
(laserCloudCornerLast->points[j].z - pointSel.z) *
(laserCloudCornerLast->points[j].z - pointSel.z);
// 尋找距離最小的角點及其索引
if (pointSqDis < minPointSqDis2)
{
// 記錄其索引
minPointSqDis2 = pointSqDis;
minPointInd2 = j;
}
}
// 同樣另一個方向?qū)ふ覍?yīng)角點
for (int j = closestPointInd - 1; j >= 0; --j)
{
// if in the same scan line, continue
if (int(laserCloudCornerLast->points[j].intensity) >= closestPointScanID)
continue;
// if not in nearby scans, end the loop
if (int(laserCloudCornerLast->points[j].intensity) < (closestPointScanID - NEARBY_SCAN))
break;
double pointSqDis = (laserCloudCornerLast->points[j].x - pointSel.x) *
(laserCloudCornerLast->points[j].x - pointSel.x) +
(laserCloudCornerLast->points[j].y - pointSel.y) *
(laserCloudCornerLast->points[j].y - pointSel.y) +
(laserCloudCornerLast->points[j].z - pointSel.z) *
(laserCloudCornerLast->points[j].z - pointSel.z);
if (pointSqDis < minPointSqDis2)
{
// find nearer point
minPointSqDis2 = pointSqDis;
minPointInd2 = j;
}
}
}
// 如果這個角點是有效的角點
if (minPointInd2 >= 0) // both closestPointInd and minPointInd2 is valid
{
// 取出當前點和上一幀的兩個角點
// 當前幀的角點
Eigen::Vector3d curr_point(cornerPointsSharp->points[i].x,
cornerPointsSharp->points[i].y,
cornerPointsSharp->points[i].z);
// 上一幀的a點
Eigen::Vector3d last_point_a(laserCloudCornerLast->points[closestPointInd].x,
laserCloudCornerLast->points[closestPointInd].y,
laserCloudCornerLast->points[closestPointInd].z);
// 上一幀的b點
Eigen::Vector3d last_point_b(laserCloudCornerLast->points[minPointInd2].x,
laserCloudCornerLast->points[minPointInd2].y,
laserCloudCornerLast->points[minPointInd2].z);
double s;
if (DISTORTION)
s = (cornerPointsSharp->points[i].intensity - int(cornerPointsSharp->points[i].intensity)) / SCAN_PERIOD;
else
s = 1.0;
// 添加ceres的約束項就是定義CostFuction代價函數(shù)
ceres::CostFunction *cost_function = LidarEdgeFactor::Create(curr_point, last_point_a, last_point_b, s);
// 給problem添加殘差項
problem.AddResidualBlock(cost_function, loss_function, para_q, para_t);
// 角點的約束次數(shù)加1
corner_correspondence++;
}
}
// 進行面點的約束
for (int i = 0; i < surfPointsFlatNum; ++i)
{
TransformToStart(&(surfPointsFlat->points[i]), &pointSel);
// 先尋找上一幀距離這個面點最近的面點
kdtreeSurfLast->nearestKSearch(pointSel, 1, pointSearchInd, pointSearchSqDis);
int closestPointInd = -1, minPointInd2 = -1, minPointInd3 = -1;
// 距離必須小于給定閾值
if (pointSearchSqDis[0] < DISTANCE_SQ_THRESHOLD)
{
// 取出找到的上一幀面點的索引
closestPointInd = pointSearchInd[0];
// 取出最近的面點在上一幀的第幾根scan上面
int closestPointScanID = int(laserCloudSurfLast->points[closestPointInd].intensity);
double minPointSqDis2 = DISTANCE_SQ_THRESHOLD, minPointSqDis3 = DISTANCE_SQ_THRESHOLD;
// 額外在尋找兩個點，要求，一個點和最近點同一個scan，另一個是不同scan
// 按照增量方向?qū)ふ移渌纥c
for (int j = closestPointInd + 1; j < (int)laserCloudSurfLast->points.size(); ++j)
{
// 不能和當前找到的上一幀面點線束距離太遠
if (int(laserCloudSurfLast->points[j].intensity) > (closestPointScanID + NEARBY_SCAN))
break;
// 計算和當前幀該點距離
double pointSqDis = (laserCloudSurfLast->points[j].x - pointSel.x) *
(laserCloudSurfLast->points[j].x - pointSel.x) +
(laserCloudSurfLast->points[j].y - pointSel.y) *
(laserCloudSurfLast->points[j].y - pointSel.y) +
(laserCloudSurfLast->points[j].z - pointSel.z) *
(laserCloudSurfLast->points[j].z - pointSel.z);
// 如果是同一根scan且距離最近
if (int(laserCloudSurfLast->points[j].intensity) <= closestPointScanID && pointSqDis < minPointSqDis2)
{
minPointSqDis2 = pointSqDis;
minPointInd2 = j;
}
// 如果是其他線束點
else if (int(laserCloudSurfLast->points[j].intensity) > closestPointScanID && pointSqDis < minPointSqDis3)
{
minPointSqDis3 = pointSqDis;
minPointInd3 = j;
}
}
// 同樣的方式，去按照降序方向?qū)ふ疫@兩個點
for (int j = closestPointInd - 1; j >= 0; --j)
{
// if not in nearby scans, end the loop
if (int(laserCloudSurfLast->points[j].intensity) < (closestPointScanID - NEARBY_SCAN))
break;
double pointSqDis = (laserCloudSurfLast->points[j].x - pointSel.x) *
(laserCloudSurfLast->points[j].x - pointSel.x) +
(laserCloudSurfLast->points[j].y - pointSel.y) *
(laserCloudSurfLast->points[j].y - pointSel.y) +
(laserCloudSurfLast->points[j].z - pointSel.z) *
(laserCloudSurfLast->points[j].z - pointSel.z);
// if in the same or higher scan line
if (int(laserCloudSurfLast->points[j].intensity) >= closestPointScanID && pointSqDis < minPointSqDis2)
{
minPointSqDis2 = pointSqDis;
minPointInd2 = j;
}
else if (int(laserCloudSurfLast->points[j].intensity) < closestPointScanID && pointSqDis < minPointSqDis3)
{
// find nearer point
minPointSqDis3 = pointSqDis;
minPointInd3 = j;
}
}
// 如果另外找到的兩個點是有效點，就取出他們的3d坐標
if (minPointInd2 >= 0 && minPointInd3 >= 0)
{
Eigen::Vector3d curr_point(surfPointsFlat->points[i].x,
surfPointsFlat->points[i].y,
surfPointsFlat->points[i].z);
Eigen::Vector3d last_point_a(laserCloudSurfLast->points[closestPointInd].x,
laserCloudSurfLast->points[closestPointInd].y,
laserCloudSurfLast->points[closestPointInd].z);
Eigen::Vector3d last_point_b(laserCloudSurfLast->points[minPointInd2].x,
laserCloudSurfLast->points[minPointInd2].y,
laserCloudSurfLast->points[minPointInd2].z);
Eigen::Vector3d last_point_c(laserCloudSurfLast->points[minPointInd3].x,
laserCloudSurfLast->points[minPointInd3].y,
laserCloudSurfLast->points[minPointInd3].z);
double s;
if (DISTORTION)
s = (surfPointsFlat->points[i].intensity - int(surfPointsFlat->points[i].intensity)) / SCAN_PERIOD;
else
s = 1.0;
// 構(gòu)建點到面的約束
ceres::CostFunction *cost_function = LidarPlaneFactor::Create(curr_point, last_point_a, last_point_b, last_point_c, s);
problem.AddResidualBlock(cost_function, loss_function, para_q, para_t);
plane_correspondence++;
}
}
}
//printf("coner_correspondance %d, plane_correspondence %d \n", corner_correspondence, plane_correspondence);
printf("data association time %f ms \n", t_data.toc());
// 如果總的約束太少，就打印一下
if ((corner_correspondence + plane_correspondence) < 10)
{
printf("less correspondence! *************************************************\n");
}
// 調(diào)用ceres求解器求解
TicToc t_solver;
ceres::Solver::Options options;
options.linear_solver_type = ceres::DENSE_QR;
options.max_num_iterations = 4;
options.minimizer_progress_to_stdout = false;
ceres::Solver::Summary summary;
ceres::Solve(options, &problem, &summary);
printf("solver time %f ms \n", t_solver.toc());
}
printf("optimization twice time %f \n", t_opt.toc());
// 這里的w_curr 實際上是 w_last
t_w_curr = t_w_curr + q_w_curr * t_last_curr;
q_w_curr = q_w_curr * q_last_curr;
}
TicToc t_pub;
// 發(fā)布lidar里程記結(jié)果
// publish odometry
nav_msgs::Odometry laserOdometry;
laserOdometry.header.frame_id = "/camera_init";
laserOdometry.child_frame_id = "/laser_odom";
laserOdometry.header.stamp = ros::Time().fromSec(timeSurfPointsLessFlat);
// 以四元數(shù)和平移向量發(fā)出去
laserOdometry.pose.pose.orientation.x = q_w_curr.x();
laserOdometry.pose.pose.orientation.y = q_w_curr.y();
laserOdometry.pose.pose.orientation.z = q_w_curr.z();
laserOdometry.pose.pose.orientation.w = q_w_curr.w();
laserOdometry.pose.pose.position.x = t_w_curr.x();
laserOdometry.pose.pose.position.y = t_w_curr.y();
laserOdometry.pose.pose.position.z = t_w_curr.z();
pubLaserOdometry.publish(laserOdometry);
geometry_msgs::PoseStamped laserPose;
laserPose.header = laserOdometry.header;
laserPose.pose = laserOdometry.pose.pose;
laserPath.header.stamp = laserOdometry.header.stamp;
laserPath.poses.push_back(laserPose);
laserPath.header.frame_id = "/camera_init";
pubLaserPath.publish(laserPath);
// transform corner features and plane features to the scan end point
if (0)
{
int cornerPointsLessSharpNum = cornerPointsLessSharp->points.size();
for (int i = 0; i < cornerPointsLessSharpNum; i++)
{
TransformToEnd(&cornerPointsLessSharp->points[i], &cornerPointsLessSharp->points[i]);
}
int surfPointsLessFlatNum = surfPointsLessFlat->points.size();
for (int i = 0; i < surfPointsLessFlatNum; i++)
{
TransformToEnd(&surfPointsLessFlat->points[i], &surfPointsLessFlat->points[i]);
}
int laserCloudFullResNum = laserCloudFullRes->points.size();
for (int i = 0; i < laserCloudFullResNum; i++)
{
TransformToEnd(&laserCloudFullRes->points[i], &laserCloudFullRes->points[i]);
}
}
pcl::PointCloud<PointType>::Ptr laserCloudTemp = cornerPointsLessSharp;
cornerPointsLessSharp = laserCloudCornerLast;
laserCloudCornerLast = laserCloudTemp;
laserCloudTemp = surfPointsLessFlat;
surfPointsLessFlat = laserCloudSurfLast;
laserCloudSurfLast = laserCloudTemp;
laserCloudCornerLastNum = laserCloudCornerLast->points.size();
laserCloudSurfLastNum = laserCloudSurfLast->points.size();
// std::cout << "the size of corner last is " << laserCloudCornerLastNum << ", and the size of surf last is " << laserCloudSurfLastNum << '\n';
// kdtree設(shè)置當前幀，用來下一幀lidar odom使用
kdtreeCornerLast->setInputCloud(laserCloudCornerLast);
kdtreeSurfLast->setInputCloud(laserCloudSurfLast);
// 一定降頻后給后端發(fā)送
if (frameCount % skipFrameNum == 0)
{
frameCount = 0;
sensor_msgs::PointCloud2 laserCloudCornerLast2;
pcl::toROSMsg(*laserCloudCornerLast, laserCloudCornerLast2);
laserCloudCornerLast2.header.stamp = ros::Time().fromSec(timeSurfPointsLessFlat);
laserCloudCornerLast2.header.frame_id = "/camera";
pubLaserCloudCornerLast.publish(laserCloudCornerLast2);
sensor_msgs::PointCloud2 laserCloudSurfLast2;
pcl::toROSMsg(*laserCloudSurfLast, laserCloudSurfLast2);
laserCloudSurfLast2.header.stamp = ros::Time().fromSec(timeSurfPointsLessFlat);
laserCloudSurfLast2.header.frame_id = "/camera";
pubLaserCloudSurfLast.publish(laserCloudSurfLast2);
sensor_msgs::PointCloud2 laserCloudFullRes3;
pcl::toROSMsg(*laserCloudFullRes, laserCloudFullRes3);
laserCloudFullRes3.header.stamp = ros::Time().fromSec(timeSurfPointsLessFlat);
laserCloudFullRes3.header.frame_id = "/camera";
pubLaserCloudFullRes.publish(laserCloudFullRes3);
}
printf("publication time %f ms \n", t_pub.toc());
printf("whole laserOdometry time %f ms \n \n", t_whole.toc());
if(t_whole.toc() > 100)
ROS_WARN("odometry process over 100ms");
frameCount++;
}
rate.sleep();
}
return 0;
}6 局部地圖構(gòu)建
6.1?局部地圖構(gòu)建原理
????????不同于前端的 scan-to-scan 的過程， LOAM 的后端是 scan-to-map 的算法，具體來說就是把當前幀和地圖進行匹配，得到更準的位姿同時也可以構(gòu)建更好的地圖。由于是scan-to-map 的算法，因此計算量會明顯高于 scan-to-scan 的前端，所以，后端通常處于一個低頻的運行頻率，但是由于 scan-to-map 的精度往往優(yōu)于 scan-to-scan，因此后端也有著比起前端來說更高的精度。
????????前端里程記會定期向后端發(fā)送位姿
，但是在 mapping 模塊中，我們需要的得到的位姿是
，因此， mapping 模塊就是需要估計出 odom 坐標系和 map坐標系之間的相對位姿變換
????????首先需要了解的一件事情就是地圖的構(gòu)成，地圖通常是當前幀通過匹配得到在地圖標系下的準確位姿之后拼接而成，如果我們保留所有的拼接的點云，此時隨著時間的運行，內(nèi)存很容易就吃不消了，因此考慮存儲離當前幀比較近的部分地圖，同時，為了便于地圖更新和調(diào)整，在原始 LOAM 中，使用的是基于珊格的地圖存儲方式。具體來說，將整個地圖分成 21×21×11 個珊格，每個珊格是一個邊長 50m 的正方體，當?shù)貓D逐漸累加時，珊格之外的部分就被舍棄，這樣可以保證內(nèi)存空間不會隨著程序的運行而爆炸。
laserMapping的運行頻率比laserOdometry低，并且每次掃描只調(diào)用一次。在掃描k+1結(jié)束時，激光雷達里程計生成一個未變形的點云
，同時生成一個姿態(tài)變換
，包含掃描過程中在
之間激光雷達運動。laserMapping匹配并在世界坐標 {W} 中注冊，如圖(8)所示。為了解釋這個過程，我們定義
為地圖上的點云，累積到掃描k，讓
為掃描k結(jié)束時激光雷達在地圖上的姿態(tài)
。根據(jù)激光雷達測程的輸出，laserMapping將從
擴展到
，得到
，并將投影到世界坐標{W}，記為
。接下來，算法通過優(yōu)化激光雷達姿態(tài)來匹配與。
圖8所示。laserMapping過程的說明。藍色曲線表示掃描k處映射算法生成的激光雷達
在地圖上的姿態(tài)。橙色曲線表示用里程計算法計算的掃描k+1, 期間
激光雷達的運動。使用和，將里程計算法發(fā)布的未失真點云投影到
地圖上，記為(綠 色線段)，并與地圖上已有的云(黑色線段)進行匹
配。
總結(jié)：laserMapping中使用scan to map的匹配方法，即最新的關(guān)鍵幀scan（綠色線）與其他所有幀組成的全部地圖（黑色線）進行匹配，因此laserMapping中的位姿估計方法聯(lián)系了所有幀的信息，而不是像laserOdometry中僅僅只利用了兩個關(guān)鍵幀的信息，所以位姿估計更準確。
顯然如果完全使用所有區(qū)域的點云這樣做的效率很低，因此只截取scan附近的全部地圖中的一個10m3的cube，用這兩個cube的匹配的結(jié)果代替全局匹配結(jié)果。顯然，在submap的cube與全部地圖的cube進行匹配時，在laserOdomerty中幀與幀之間的匹配方法就不太適用了（實際上cube中已經(jīng)沒有了幀的概念）。cube的匹配方法如下：文章來源：http://www.zghlxwxcb.cn/news/detail-734803.html
- 取當前幀的特征點（邊線點/平面點）
- 找到全部地圖特征點中，當前特征點的5個最近鄰點
- 如果是邊線點，則以這五個點的均值點為中心，以5個點的主方向向量（類似于PCA方法）為方向，作直線，令該邊線點與直線距離最短，構(gòu)建給非線性優(yōu)化問題
- 如果是平面點，則尋找五個點的法方向（反向的PCA），令這個平面點在法方向上與五個近鄰點的距離最小，構(gòu)建給非線性優(yōu)化問題
- 求解能夠讓非線性問題代價函數(shù)最小的LiDAR位姿?
6.2 代碼詳解
// 主處理線程
void process()
{
while(1)
{
// 確保需要的buffer里都有值
while (!cornerLastBuf.empty() && !surfLastBuf.empty() &&
!fullResBuf.empty() && !odometryBuf.empty())
{
mBuf.lock();
// 以cornerLastBuf為基準，把時間戳小于其的全部pop出去
while (!odometryBuf.empty() && odometryBuf.front()->header.stamp.toSec() < cornerLastBuf.front()->header.stamp.toSec())
odometryBuf.pop();
// 如果里程計信息為空跳出本次循環(huán)
if (odometryBuf.empty())
{
mBuf.unlock();
break;
}
// 如果面特征信息不為空，面特征時間戳小于特征時間戳則取出面特征數(shù)據(jù)
while (!surfLastBuf.empty() && surfLastBuf.front()->header.stamp.toSec() < cornerLastBuf.front()->header.stamp.toSec())
surfLastBuf.pop();
// 如果面特征信息為空跳出本次循環(huán)
if (surfLastBuf.empty())
{
mBuf.unlock();
break;
}
// 如果全部點信息不為空，全部點云時間戳小于角特征時間戳則取出全部點云信息
while (!fullResBuf.empty() && fullResBuf.front()->header.stamp.toSec() < cornerLastBuf.front()->header.stamp.toSec())
fullResBuf.pop();
// 全部點云信息為空則跳出
if (fullResBuf.empty())
{
mBuf.unlock();
break;
}
// 記錄時間戳
timeLaserCloudCornerLast = cornerLastBuf.front()->header.stamp.toSec();
timeLaserCloudSurfLast = surfLastBuf.front()->header.stamp.toSec();
timeLaserCloudFullRes = fullResBuf.front()->header.stamp.toSec();
timeLaserOdometry = odometryBuf.front()->header.stamp.toSec();
// 原則上取出來的時間戳都是一樣的，如果不一定說明有問題了
if (timeLaserCloudCornerLast != timeLaserOdometry ||
timeLaserCloudSurfLast != timeLaserOdometry ||
timeLaserCloudFullRes != timeLaserOdometry)
{
printf("time corner %f surf %f full %f odom %f \n", timeLaserCloudCornerLast, timeLaserCloudSurfLast, timeLaserCloudFullRes, timeLaserOdometry);
printf("unsync messeage!");
mBuf.unlock();
break;
}
// 點云全部轉(zhuǎn)成pcl的數(shù)據(jù)格式
// 清空上次全部點云，并接收新的
laserCloudCornerLast->clear();
pcl::fromROSMsg(*cornerLastBuf.front(), *laserCloudCornerLast);
cornerLastBuf.pop();
laserCloudSurfLast->clear();
pcl::fromROSMsg(*surfLastBuf.front(), *laserCloudSurfLast);
surfLastBuf.pop();
laserCloudFullRes->clear();
pcl::fromROSMsg(*fullResBuf.front(), *laserCloudFullRes);
fullResBuf.pop();
// lidar odom的結(jié)果轉(zhuǎn)成eigen數(shù)據(jù)格式
// 接收里程計坐標系下的四元數(shù)與位移
q_wodom_curr.x() = odometryBuf.front()->pose.pose.orientation.x;
q_wodom_curr.y() = odometryBuf.front()->pose.pose.orientation.y;
q_wodom_curr.z() = odometryBuf.front()->pose.pose.orientation.z;
q_wodom_curr.w() = odometryBuf.front()->pose.pose.orientation.w;
t_wodom_curr.x() = odometryBuf.front()->pose.pose.position.x;
t_wodom_curr.y() = odometryBuf.front()->pose.pose.position.y;
t_wodom_curr.z() = odometryBuf.front()->pose.pose.position.z;
odometryBuf.pop();
// 角特征不為空，堆入角特征，輸出目前運行實時
// 考慮到實時性，就把隊列里其他的都pop出去，不然可能出現(xiàn)處理延時的情況
while(!cornerLastBuf.empty())
{
cornerLastBuf.pop();
printf("drop lidar frame in mapping for real time performance \n");
}
mBuf.unlock();
TicToc t_whole;
// 根據(jù)前端結(jié)果，得到后端的一個初始估計值
transformAssociateToMap();
TicToc t_shift;
// 根據(jù)初始估計值計算尋找當前位姿在地圖中的索引，一個各自邊長是50m
// 后端的地圖本質(zhì)上是一個以當前點為中心，一個珊格地圖
int centerCubeI = int((t_w_curr.x() + 25.0) / 50.0) + laserCloudCenWidth;
int centerCubeJ = int((t_w_curr.y() + 25.0) / 50.0) + laserCloudCenHeight;
int centerCubeK = int((t_w_curr.z() + 25.0) / 50.0) + laserCloudCenDepth;
// 如果小于25就向下去整，相當于四舍五入的一個過程
if (t_w_curr.x() + 25.0 < 0)
centerCubeI--;
if (t_w_curr.y() + 25.0 < 0)
centerCubeJ--;
if (t_w_curr.z() + 25.0 < 0)
centerCubeK--;
// 如果當前珊格索引小于3,就說明當前點快接近地圖邊界了，需要進行調(diào)整，相當于地圖整體往x正方向移動
while (centerCubeI < 3)
{
for (int j = 0; j < laserCloudHeight; j++)
{
for (int k = 0; k < laserCloudDepth; k++)
{
int i = laserCloudWidth - 1;
// 從x最大值開始
pcl::PointCloud<PointType>::Ptr laserCloudCubeCornerPointer =
laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
pcl::PointCloud<PointType>::Ptr laserCloudCubeSurfPointer =
laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
// 整體右移
for (; i >= 1; i--)
{
laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
laserCloudCornerArray[i - 1 + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
laserCloudSurfArray[i - 1 + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
}
// 此時i = 0,也就是最左邊的格子賦值了之前最右邊的格子
laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
laserCloudCubeCornerPointer;
laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
laserCloudCubeSurfPointer;
// 該點云清零，由于是指針操作，相當于最左邊的格子清空了
laserCloudCubeCornerPointer->clear();
laserCloudCubeSurfPointer->clear();
}
}
// 索引右移
centerCubeI++;
laserCloudCenWidth++;
}
// 同理x如果抵達右邊界，就整體左移
while (centerCubeI >= laserCloudWidth - 3)
{
for (int j = 0; j < laserCloudHeight; j++)
{
for (int k = 0; k < laserCloudDepth; k++)
{
int i = 0;
pcl::PointCloud<PointType>::Ptr laserCloudCubeCornerPointer =
laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
pcl::PointCloud<PointType>::Ptr laserCloudCubeSurfPointer =
laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
// 整體左移
for (; i < laserCloudWidth - 1; i++)
{
laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
laserCloudCornerArray[i + 1 + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
laserCloudSurfArray[i + 1 + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
}
laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
laserCloudCubeCornerPointer;
laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
laserCloudCubeSurfPointer;
laserCloudCubeCornerPointer->clear();
laserCloudCubeSurfPointer->clear();
}
}
centerCubeI--;
laserCloudCenWidth--;
}
// y和z的操作同理
while (centerCubeJ < 3)
{
for (int i = 0; i < laserCloudWidth; i++)
{
for (int k = 0; k < laserCloudDepth; k++)
{
int j = laserCloudHeight - 1;
pcl::PointCloud<PointType>::Ptr laserCloudCubeCornerPointer =
laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
pcl::PointCloud<PointType>::Ptr laserCloudCubeSurfPointer =
laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
for (; j >= 1; j--)
{
laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
laserCloudCornerArray[i + laserCloudWidth * (j - 1) + laserCloudWidth * laserCloudHeight * k];
laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
laserCloudSurfArray[i + laserCloudWidth * (j - 1) + laserCloudWidth * laserCloudHeight * k];
}
laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
laserCloudCubeCornerPointer;
laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
laserCloudCubeSurfPointer;
laserCloudCubeCornerPointer->clear();
laserCloudCubeSurfPointer->clear();
}
}
centerCubeJ++;
laserCloudCenHeight++;
}
while (centerCubeJ >= laserCloudHeight - 3)
{
for (int i = 0; i < laserCloudWidth; i++)
{
for (int k = 0; k < laserCloudDepth; k++)
{
int j = 0;
pcl::PointCloud<PointType>::Ptr laserCloudCubeCornerPointer =
laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
pcl::PointCloud<PointType>::Ptr laserCloudCubeSurfPointer =
laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
for (; j < laserCloudHeight - 1; j++)
{
laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
laserCloudCornerArray[i + laserCloudWidth * (j + 1) + laserCloudWidth * laserCloudHeight * k];
laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
laserCloudSurfArray[i + laserCloudWidth * (j + 1) + laserCloudWidth * laserCloudHeight * k];
}
laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
laserCloudCubeCornerPointer;
laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
laserCloudCubeSurfPointer;
laserCloudCubeCornerPointer->clear();
laserCloudCubeSurfPointer->clear();
}
}
centerCubeJ--;
laserCloudCenHeight--;
}
while (centerCubeK < 3)
{
for (int i = 0; i < laserCloudWidth; i++)
{
for (int j = 0; j < laserCloudHeight; j++)
{
int k = laserCloudDepth - 1;
pcl::PointCloud<PointType>::Ptr laserCloudCubeCornerPointer =
laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
pcl::PointCloud<PointType>::Ptr laserCloudCubeSurfPointer =
laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
for (; k >= 1; k--)
{
laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * (k - 1)];
laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * (k - 1)];
}
laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
laserCloudCubeCornerPointer;
laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
laserCloudCubeSurfPointer;
laserCloudCubeCornerPointer->clear();
laserCloudCubeSurfPointer->clear();
}
}
centerCubeK++;
laserCloudCenDepth++;
}
while (centerCubeK >= laserCloudDepth - 3)
{
for (int i = 0; i < laserCloudWidth; i++)
{
for (int j = 0; j < laserCloudHeight; j++)
{
int k = 0;
pcl::PointCloud<PointType>::Ptr laserCloudCubeCornerPointer =
laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
pcl::PointCloud<PointType>::Ptr laserCloudCubeSurfPointer =
laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
for (; k < laserCloudDepth - 1; k++)
{
laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * (k + 1)];
laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * (k + 1)];
}
laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
laserCloudCubeCornerPointer;
laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
laserCloudCubeSurfPointer;
laserCloudCubeCornerPointer->clear();
laserCloudCubeSurfPointer->clear();
}
}
centerCubeK--;
laserCloudCenDepth--;
}
// 以上操作相當于維護了一個局部地圖，保證當前幀不在這個局部地圖的邊緣，這樣才可以從地圖中獲取足夠的約束
int laserCloudValidNum = 0;
int laserCloudSurroundNum = 0;
// 從當前格子為中心，選出地圖中一定范圍的點云
for (int i = centerCubeI - 2; i <= centerCubeI + 2; i++)
{
for (int j = centerCubeJ - 2; j <= centerCubeJ + 2; j++)
{
for (int k = centerCubeK - 1; k <= centerCubeK + 1; k++)
{
if (i >= 0 && i < laserCloudWidth &&
j >= 0 && j < laserCloudHeight &&
k >= 0 && k < laserCloudDepth)
{
// 把各自的索引記錄下來
laserCloudValidInd[laserCloudValidNum] = i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k;
laserCloudValidNum++;
laserCloudSurroundInd[laserCloudSurroundNum] = i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k;
laserCloudSurroundNum++;
}
}
}
}
laserCloudCornerFromMap->clear();
laserCloudSurfFromMap->clear();
// 開始構(gòu)建用來這一幀優(yōu)化的小的局部地圖
for (int i = 0; i < laserCloudValidNum; i++)
{
*laserCloudCornerFromMap += *laserCloudCornerArray[laserCloudValidInd[i]];
*laserCloudSurfFromMap += *laserCloudSurfArray[laserCloudValidInd[i]];
}
int laserCloudCornerFromMapNum = laserCloudCornerFromMap->points.size();
int laserCloudSurfFromMapNum = laserCloudSurfFromMap->points.size();
// 為了減少運算量，對點云進行下采樣
pcl::PointCloud<PointType>::Ptr laserCloudCornerStack(new pcl::PointCloud<PointType>());
downSizeFilterCorner.setInputCloud(laserCloudCornerLast);
downSizeFilterCorner.filter(*laserCloudCornerStack);
int laserCloudCornerStackNum = laserCloudCornerStack->points.size();
pcl::PointCloud<PointType>::Ptr laserCloudSurfStack(new pcl::PointCloud<PointType>());
downSizeFilterSurf.setInputCloud(laserCloudSurfLast);
downSizeFilterSurf.filter(*laserCloudSurfStack);
int laserCloudSurfStackNum = laserCloudSurfStack->points.size();
printf("map prepare time %f ms\n", t_shift.toc());
printf("map corner num %d surf num %d \n", laserCloudCornerFromMapNum, laserCloudSurfFromMapNum);
// 最終的有效點云數(shù)目進行判斷
if (laserCloudCornerFromMapNum > 10 && laserCloudSurfFromMapNum > 50)
{
TicToc t_opt;
TicToc t_tree;
// 送入kdtree便于最近鄰搜索
kdtreeCornerFromMap->setInputCloud(laserCloudCornerFromMap);
kdtreeSurfFromMap->setInputCloud(laserCloudSurfFromMap);
printf("build tree time %f ms \n", t_tree.toc());
// 建立對應(yīng)關(guān)系的迭代次數(shù)不超2次
for (int iterCount = 0; iterCount < 2; iterCount++)
{
//ceres::LossFunction *loss_function = NULL;
// 建立ceres問題
ceres::LossFunction *loss_function = new ceres::HuberLoss(0.1);
ceres::LocalParameterization *q_parameterization =
new ceres::EigenQuaternionParameterization();
ceres::Problem::Options problem_options;
ceres::Problem problem(problem_options);
problem.AddParameterBlock(parameters, 4, q_parameterization);
problem.AddParameterBlock(parameters + 4, 3);
TicToc t_data;
int corner_num = 0;
// 構(gòu)建角點相關(guān)的約束
for (int i = 0; i < laserCloudCornerStackNum; i++)
{
pointOri = laserCloudCornerStack->points[i];
//double sqrtDis = pointOri.x * pointOri.x + pointOri.y * pointOri.y + pointOri.z * pointOri.z;
// 把當前點根據(jù)初值投到地圖坐標系下去
pointAssociateToMap(&pointOri, &pointSel);
// 地圖中尋找和該點最近的5個點
kdtreeCornerFromMap->nearestKSearch(pointSel, 5, pointSearchInd, pointSearchSqDis);
//如果五個點中最遠的那個還小于1米，則求解協(xié)方差矩陣
if (pointSearchSqDis[4] < 1.0)
{
std::vector<Eigen::Vector3d> nearCorners;
Eigen::Vector3d center(0, 0, 0);
for (int j = 0; j < 5; j++)
{
Eigen::Vector3d tmp(laserCloudCornerFromMap->points[pointSearchInd[j]].x,
laserCloudCornerFromMap->points[pointSearchInd[j]].y,
laserCloudCornerFromMap->points[pointSearchInd[j]].z);
center = center + tmp;
nearCorners.push_back(tmp);
}
// 計算這五個點的均值
center = center / 5.0;
Eigen::Matrix3d covMat = Eigen::Matrix3d::Zero();
// 構(gòu)建協(xié)方差矩陣
for (int j = 0; j < 5; j++)
{
Eigen::Matrix<double, 3, 1> tmpZeroMean = nearCorners[j] - center;
covMat = covMat + tmpZeroMean * tmpZeroMean.transpose();
}
// 進行特征值分解
Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> saes(covMat);
// 根據(jù)特征值分解情況看看是不是真正的線特征
// 特征向量就是線特征的方向
Eigen::Vector3d unit_direction = saes.eigenvectors().col(2);
Eigen::Vector3d curr_point(pointOri.x, pointOri.y, pointOri.z);
// 最大特征值大于次大特征值的3倍認為是線特征
if (saes.eigenvalues()[2] > 3 * saes.eigenvalues()[1])
{
Eigen::Vector3d point_on_line = center;
Eigen::Vector3d point_a, point_b;
// 根據(jù)擬合出來的線特征方向，以平均點為中心構(gòu)建兩個虛擬點
point_a = 0.1 * unit_direction + point_on_line;
point_b = -0.1 * unit_direction + point_on_line;
// 構(gòu)建約束，和lidar odom約束一致
ceres::CostFunction *cost_function = LidarEdgeFactor::Create(curr_point, point_a, point_b, 1.0);
problem.AddResidualBlock(cost_function, loss_function, parameters, parameters + 4);
corner_num++;
}
}
/*
else if(pointSearchSqDis[4] < 0.01 * sqrtDis)
{
Eigen::Vector3d center(0, 0, 0);
for (int j = 0; j < 5; j++)
{
Eigen::Vector3d tmp(laserCloudCornerFromMap->points[pointSearchInd[j]].x,
laserCloudCornerFromMap->points[pointSearchInd[j]].y,
laserCloudCornerFromMap->points[pointSearchInd[j]].z);
center = center + tmp;
}
center = center / 5.0;
Eigen::Vector3d curr_point(pointOri.x, pointOri.y, pointOri.z);
ceres::CostFunction *cost_function = LidarDistanceFactor::Create(curr_point, center);
problem.AddResidualBlock(cost_function, loss_function, parameters, parameters + 4);
}
*/
}
int surf_num = 0;
// 構(gòu)建面點約束
for (int i = 0; i < laserCloudSurfStackNum; i++)
{
pointOri = laserCloudSurfStack->points[i];
//double sqrtDis = pointOri.x * pointOri.x + pointOri.y * pointOri.y + pointOri.z * pointOri.z;
pointAssociateToMap(&pointOri, &pointSel);
kdtreeSurfFromMap->nearestKSearch(pointSel, 5, pointSearchInd, pointSearchSqDis);
Eigen::Matrix<double, 5, 3> matA0;
Eigen::Matrix<double, 5, 1> matB0 = -1 * Eigen::Matrix<double, 5, 1>::Ones();
// 構(gòu)建平面方程Ax + By +Cz + 1 = 0
// 通過構(gòu)建一個超定方程來求解這個平面方程
if (pointSearchSqDis[4] < 1.0)
{
for (int j = 0; j < 5; j++)
{
matA0(j, 0) = laserCloudSurfFromMap->points[pointSearchInd[j]].x;
matA0(j, 1) = laserCloudSurfFromMap->points[pointSearchInd[j]].y;
matA0(j, 2) = laserCloudSurfFromMap->points[pointSearchInd[j]].z;
//printf(" pts %f %f %f ", matA0(j, 0), matA0(j, 1), matA0(j, 2));
}
// find the norm of plane
// 調(diào)用eigen接口求解該方程，解就是這個平面的法向量
Eigen::Vector3d norm = matA0.colPivHouseholderQr().solve(matB0);
double negative_OA_dot_norm = 1 / norm.norm();
// 法向量歸一化
norm.normalize();
// Here n(pa, pb, pc) is unit norm of plane
bool planeValid = true;
// 根據(jù)求出來的平面方程進行校驗，看看是不是符合平面約束
for (int j = 0; j < 5; j++)
{
// if OX * n > 0.2, then plane is not fit well
// 這里相當于求解點到平面的距離
if (fabs(norm(0) * laserCloudSurfFromMap->points[pointSearchInd[j]].x +
norm(1) * laserCloudSurfFromMap->points[pointSearchInd[j]].y +
norm(2) * laserCloudSurfFromMap->points[pointSearchInd[j]].z + negative_OA_dot_norm) > 0.2)
{
planeValid = false; // 點如果距離平面太遠，就認為這是一個擬合的不好的平面
break;
}
}
Eigen::Vector3d curr_point(pointOri.x, pointOri.y, pointOri.z);
// 如果平面有效就構(gòu)建平面約束
if (planeValid)
{
// 利用平面方程構(gòu)建約束，和前端構(gòu)建形式稍有不同
ceres::CostFunction *cost_function = LidarPlaneNormFactor::Create(curr_point, norm, negative_OA_dot_norm);
problem.AddResidualBlock(cost_function, loss_function, parameters, parameters + 4);
surf_num++;
}
}
/*
else if(pointSearchSqDis[4] < 0.01 * sqrtDis)
{
Eigen::Vector3d center(0, 0, 0);
for (int j = 0; j < 5; j++)
{
Eigen::Vector3d tmp(laserCloudSurfFromMap->points[pointSearchInd[j]].x,
laserCloudSurfFromMap->points[pointSearchInd[j]].y,
laserCloudSurfFromMap->points[pointSearchInd[j]].z);
center = center + tmp;
}
center = center / 5.0;
Eigen::Vector3d curr_point(pointOri.x, pointOri.y, pointOri.z);
ceres::CostFunction *cost_function = LidarDistanceFactor::Create(curr_point, center);
problem.AddResidualBlock(cost_function, loss_function, parameters, parameters + 4);
}
*/
}
//printf("corner num %d used corner num %d \n", laserCloudCornerStackNum, corner_num);
//printf("surf num %d used surf num %d \n", laserCloudSurfStackNum, surf_num);
printf("mapping data assosiation time %f ms \n", t_data.toc());
// 調(diào)用ceres求解
TicToc t_solver;
ceres::Solver::Options options;
options.linear_solver_type = ceres::DENSE_QR;
options.max_num_iterations = 4;
options.minimizer_progress_to_stdout = false;
options.check_gradients = false;
options.gradient_check_relative_precision = 1e-4;
ceres::Solver::Summary summary;
ceres::Solve(options, &problem, &summary);
printf("mapping solver time %f ms \n", t_solver.toc());
//printf("time %f \n", timeLaserOdometry);
//printf("corner factor num %d surf factor num %d\n", corner_num, surf_num);
//printf("result q %f %f %f %f result t %f %f %f\n", parameters[3], parameters[0], parameters[1], parameters[2],
// parameters[4], parameters[5], parameters[6]);
}
printf("mapping optimization time %f \n", t_opt.toc());
}
else
{
ROS_WARN("time Map corner and surf num are not enough");
}
transformUpdate();
TicToc t_add;
// 將優(yōu)化后的當前幀角點加到局部地圖中去
for (int i = 0; i < laserCloudCornerStackNum; i++)
{
// 該點根據(jù)位姿投到地圖坐標系
pointAssociateToMap(&laserCloudCornerStack->points[i], &pointSel);
// 算出這個點所在的格子的索引
int cubeI = int((pointSel.x + 25.0) / 50.0) + laserCloudCenWidth;
int cubeJ = int((pointSel.y + 25.0) / 50.0) + laserCloudCenHeight;
int cubeK = int((pointSel.z + 25.0) / 50.0) + laserCloudCenDepth;
// 同樣四舍五入一下
if (pointSel.x + 25.0 < 0)
cubeI--;
if (pointSel.y + 25.0 < 0)
cubeJ--;
if (pointSel.z + 25.0 < 0)
cubeK--;
// 如果超過邊界的話就算了
if (cubeI >= 0 && cubeI < laserCloudWidth &&
cubeJ >= 0 && cubeJ < laserCloudHeight &&
cubeK >= 0 && cubeK < laserCloudDepth)
{
// 根據(jù)xyz的索引計算在一位數(shù)組中的索引
int cubeInd = cubeI + laserCloudWidth * cubeJ + laserCloudWidth * laserCloudHeight * cubeK;
laserCloudCornerArray[cubeInd]->push_back(pointSel);
}
}
// 面點也做同樣的處理
for (int i = 0; i < laserCloudSurfStackNum; i++)
{
pointAssociateToMap(&laserCloudSurfStack->points[i], &pointSel);
int cubeI = int((pointSel.x + 25.0) / 50.0) + laserCloudCenWidth;
int cubeJ = int((pointSel.y + 25.0) / 50.0) + laserCloudCenHeight;
int cubeK = int((pointSel.z + 25.0) / 50.0) + laserCloudCenDepth;
if (pointSel.x + 25.0 < 0)
cubeI--;
if (pointSel.y + 25.0 < 0)
cubeJ--;
if (pointSel.z + 25.0 < 0)
cubeK--;
if (cubeI >= 0 && cubeI < laserCloudWidth &&
cubeJ >= 0 && cubeJ < laserCloudHeight &&
cubeK >= 0 && cubeK < laserCloudDepth)
{
int cubeInd = cubeI + laserCloudWidth * cubeJ + laserCloudWidth * laserCloudHeight * cubeK;
laserCloudSurfArray[cubeInd]->push_back(pointSel);
}
}
printf("add points time %f ms\n", t_add.toc());
TicToc t_filter;
// 把當前幀涉及到的局部地圖的珊格做一個下采樣
for (int i = 0; i < laserCloudValidNum; i++)
{
int ind = laserCloudValidInd[i];
pcl::PointCloud<PointType>::Ptr tmpCorner(new pcl::PointCloud<PointType>());
downSizeFilterCorner.setInputCloud(laserCloudCornerArray[ind]);
downSizeFilterCorner.filter(*tmpCorner);
laserCloudCornerArray[ind] = tmpCorner;
pcl::PointCloud<PointType>::Ptr tmpSurf(new pcl::PointCloud<PointType>());
downSizeFilterSurf.setInputCloud(laserCloudSurfArray[ind]);
downSizeFilterSurf.filter(*tmpSurf);
laserCloudSurfArray[ind] = tmpSurf;
}
printf("filter time %f ms \n", t_filter.toc());
TicToc t_pub;
//publish surround map for every 5 frame
// 每隔5幀對外發(fā)布一下
if (frameCount % 5 == 0)
{
laserCloudSurround->clear();
// 把該當前幀相關(guān)的局部地圖發(fā)布出去
for (int i = 0; i < laserCloudSurroundNum; i++)
{
int ind = laserCloudSurroundInd[i];
*laserCloudSurround += *laserCloudCornerArray[ind];
*laserCloudSurround += *laserCloudSurfArray[ind];
}
sensor_msgs::PointCloud2 laserCloudSurround3;
pcl::toROSMsg(*laserCloudSurround, laserCloudSurround3);
laserCloudSurround3.header.stamp = ros::Time().fromSec(timeLaserOdometry);
laserCloudSurround3.header.frame_id = "/camera_init";
pubLaserCloudSurround.publish(laserCloudSurround3);
}
// 每隔20幀發(fā)布全量的局部地圖
if (frameCount % 20 == 0)
{
pcl::PointCloud<PointType> laserCloudMap;
// 21 × 21 × 11 = 4851
for (int i = 0; i < 4851; i++)
{
laserCloudMap += *laserCloudCornerArray[i];
laserCloudMap += *laserCloudSurfArray[i];
}
sensor_msgs::PointCloud2 laserCloudMsg;
pcl::toROSMsg(laserCloudMap, laserCloudMsg);
laserCloudMsg.header.stamp = ros::Time().fromSec(timeLaserOdometry);
laserCloudMsg.header.frame_id = "/camera_init";
pubLaserCloudMap.publish(laserCloudMsg);
}
int laserCloudFullResNum = laserCloudFullRes->points.size();
// 把當前幀發(fā)布出去
for (int i = 0; i < laserCloudFullResNum; i++)
{
pointAssociateToMap(&laserCloudFullRes->points[i], &laserCloudFullRes->points[i]);
}
sensor_msgs::PointCloud2 laserCloudFullRes3;
pcl::toROSMsg(*laserCloudFullRes, laserCloudFullRes3);
laserCloudFullRes3.header.stamp = ros::Time().fromSec(timeLaserOdometry);
laserCloudFullRes3.header.frame_id = "/camera_init";
pubLaserCloudFullRes.publish(laserCloudFullRes3);
printf("mapping pub time %f ms \n", t_pub.toc());
printf("whole mapping time %f ms +++++\n", t_whole.toc());
// 發(fā)布當前位姿
nav_msgs::Odometry odomAftMapped;
odomAftMapped.header.frame_id = "/camera_init";
odomAftMapped.child_frame_id = "/aft_mapped";
odomAftMapped.header.stamp = ros::Time().fromSec(timeLaserOdometry);
odomAftMapped.pose.pose.orientation.x = q_w_curr.x();
odomAftMapped.pose.pose.orientation.y = q_w_curr.y();
odomAftMapped.pose.pose.orientation.z = q_w_curr.z();
odomAftMapped.pose.pose.orientation.w = q_w_curr.w();
odomAftMapped.pose.pose.position.x = t_w_curr.x();
odomAftMapped.pose.pose.position.y = t_w_curr.y();
odomAftMapped.pose.pose.position.z = t_w_curr.z();
pubOdomAftMapped.publish(odomAftMapped);
// 發(fā)布當前軌跡
geometry_msgs::PoseStamped laserAfterMappedPose;
laserAfterMappedPose.header = odomAftMapped.header;
laserAfterMappedPose.pose = odomAftMapped.pose.pose;
laserAfterMappedPath.header.stamp = odomAftMapped.header.stamp;
laserAfterMappedPath.header.frame_id = "/camera_init";
laserAfterMappedPath.poses.push_back(laserAfterMappedPose);
pubLaserAfterMappedPath.publish(laserAfterMappedPath);
// 發(fā)布tf
static tf::TransformBroadcaster br;
tf::Transform transform;
tf::Quaternion q;
transform.setOrigin(tf::Vector3(t_w_curr(0),
t_w_curr(1),
t_w_curr(2)));
q.setW(q_w_curr.w());
q.setX(q_w_curr.x());
q.setY(q_w_curr.y());
q.setZ(q_w_curr.z());
transform.setRotation(q);
br.sendTransform(tf::StampedTransform(transform, odomAftMapped.header.stamp, "/camera_init", "/aft_mapped"));
frameCount++;
}
std::chrono::milliseconds dura(2);
std::this_thread::sleep_for(dura);
}
}
文章來源地址http://www.zghlxwxcb.cn/news/detail-734803.html
到了這里，關(guān)于a-loam源碼圖文詳解的文章就介紹完了。如果您還想了解更多內(nèi)容，請在右上角搜索TOY模板網(wǎng)以前的文章或繼續(xù)瀏覽下面的相關(guān)文章，希望大家以后多多支持TOY模板網(wǎng)！
