參考資料
- LOAM等內容的博客 需要好好研讀
- LeGO-LOAM 源碼閱讀筆記(imageProjecion.cpp)
- LeGO-LOAM 源碼閱讀筆記(featureAssociation.cpp)
- LeGO-LOAM 源碼閱讀筆記（mapOptmization.cpp）
代碼理解（ongoing）
- image Projection 的原因之一是， 必須找到所有激光點對應的位置。需要將所有激光點投射到16個channel中去，並且每個channel中的激光點總數應該保持一致。激光雷達得到的數據看成一個16x1800的點雲陣列
PCL_ADD_POINT4D
145 union EIGEN_ALIGN16 { \
146 float data[4]; \
147 struct { \
148 float x; \
149 float y; \
150 float z; \
151 }; \
152 }; \
144 #define PCL_ADD_POINT4D \
145 union EIGEN_ALIGN16 { \
146 float data[4]; \
147 struct { \
148 float x; \
149 float y; \
150 float z; \
151 }; \
152 }; \
194 #define PCL_ADD_INTENSITY \
195 struct \
196 { \
197 float intensity; \
198 }; \
pcl::PointCloud::Ptr reset
pcl::PointCloud<PointType>::Ptr fullCloud;
laserCloudIn.reset(new pcl::PointCloud<PointType>());
ImageProjection cloudHandler
void cloudHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg){
// 1. Convert ros message to pcl point cloud
copyPointCloud(laserCloudMsg);
// 2. Start and end angle of a scan
// 根據節點第一個點和最後一個殿的 x,y,z 推測雷達角度差異
findStartEndAngle();
// 3. Range image projection
// 計算豎直角度，並投射到16個channel中
projectPointCloud();
// 4. Mark ground points
// 由上下兩線之間點的XYZ位置得到兩線之間的俯仰角，如果俯仰角在10度以內，則判定(i,j)爲地面點,
//
groundRemoval();
// 5. Point cloud segmentation
// 將不同類型的點雲放到不同的點雲塊中去，例如outlierCloud，segmentedCloudPure等
cloudSegmentation();
// 6. Publish all clouds
publishCloud();
// 7. Reset parameters for next iteration
resetParameters();
}
可以借鑑的 Publish結構
void publishCloud(){
// 1. Publish Seg Cloud Info
segMsg.header = cloudHeader;
pubSegmentedCloudInfo.publish(segMsg);
// 2. Publish clouds
sensor_msgs::PointCloud2 laserCloudTemp;
pcl::toROSMsg(*outlierCloud, laserCloudTemp);
laserCloudTemp.header.stamp = cloudHeader.stamp;
laserCloudTemp.header.frame_id = "base_link";
pubOutlierCloud.publish(laserCloudTemp);
// segmented cloud with ground
pcl::toROSMsg(*segmentedCloud, laserCloudTemp);
laserCloudTemp.header.stamp = cloudHeader.stamp;
laserCloudTemp.header.frame_id = "base_link";
pubSegmentedCloud.publish(laserCloudTemp);
// projected full cloud
if (pubFullCloud.getNumSubscribers() != 0){
pcl::toROSMsg(*fullCloud, laserCloudTemp);
laserCloudTemp.header.stamp = cloudHeader.stamp;
laserCloudTemp.header.frame_id = "base_link";
pubFullCloud.publish(laserCloudTemp);
}
// original dense ground cloud
if (pubGroundCloud.getNumSubscribers() != 0){
pcl::toROSMsg(*groundCloud, laserCloudTemp);
laserCloudTemp.header.stamp = cloudHeader.stamp;
laserCloudTemp.header.frame_id = "base_link";
pubGroundCloud.publish(laserCloudTemp);
}
// segmented cloud without ground
if (pubSegmentedCloudPure.getNumSubscribers() != 0){
pcl::toROSMsg(*segmentedCloudPure, laserCloudTemp);
laserCloudTemp.header.stamp = cloudHeader.stamp;
laserCloudTemp.header.frame_id = "base_link";
pubSegmentedCloudPure.publish(laserCloudTemp);
}
// projected full cloud info
if (pubFullInfoCloud.getNumSubscribers() != 0){
pcl::toROSMsg(*fullInfoCloud, laserCloudTemp);
laserCloudTemp.header.stamp = cloudHeader.stamp;
laserCloudTemp.header.frame_id = "base_link";
pubFullInfoCloud.publish(laserCloudTemp);
}
}
featureAssociation.cpp
- 它分爲特徵點的提取與匹配兩部分
void runFeatureAssociation()
{
//判斷分割點雲和異常值的實時性
if (newSegmentedCloud && newSegmentedCloudInfo && newOutlierCloud &&
std::abs(timeNewSegmentedCloudInfo - timeNewSegmentedCloud) < 0.05 &&
std::abs(timeNewOutlierCloud - timeNewSegmentedCloud) < 0.05)
{
newSegmentedCloud = false;
newSegmentedCloudInfo = false;
newOutlierCloud = false;
}
else
{
return;
}
//進行形狀特徵的調整
adjustDistortion();
calculateSmoothness();
markOccludedPoints();
//提取特徵
extractFeatures();
publishCloud();
//在配準之前，檢驗LM法是否初始化，接下來就是里程計部分
if (!systemInitedLM)
{
checkSystemInitialization();
return;
}
//提供粗配準的先驗以供優化
updateInitialGuess();
//優化併發送里程計信息
updateTransformation();
integrateTransformation();
publishOdometry();
//發送點雲以供建圖使用
publishCloudsLast();
}
void adjustDistortion()
將點雲數據進行座標變換，進行插補等工作
- 莫名其妙的座標軸變換
point.x = segmentedCloud->points[i].y;
point.y = segmentedCloud->points[i].z;
point.z = segmentedCloud->points[i].x;
void calculateSmoothness()
用於計算光滑性
void calculateSmoothness()
{
int cloudSize = segmentedCloud->points.size();
for (int i = 5; i < cloudSize - 5; i++) {
float diffRange = segInfo.segmentedCloudRange[i-5] + segInfo.segmentedCloudRange[i-4]
+ segInfo.segmentedCloudRange[i-3] + segInfo.segmentedCloudRange[i-2]
+ segInfo.segmentedCloudRange[i-1] - segInfo.segmentedCloudRange[i] * 10
+ segInfo.segmentedCloudRange[i+1] + segInfo.segmentedCloudRange[i+2]
+ segInfo.segmentedCloudRange[i+3] + segInfo.segmentedCloudRange[i+4]
+ segInfo.segmentedCloudRange[i+5];
cloudCurvature[i] = diffRange*diffRange;
// 在markOccludedPoints()函數中對該參數進行重新修改
cloudNeighborPicked[i] = 0;
// 在extractFeatures()函數中會對標籤進行修改，
// 初始化爲0，surfPointsFlat標記爲-1，surfPointsLessFlatScan爲不大於0的標籤
// cornerPointsSharp標記爲2，cornerPointsLessSharp標記爲1
cloudLabel[i] = 0;
cloudSmoothness[i].value = cloudCurvature[i];
cloudSmoothness[i].ind = i;
}
}
void extractFeatures()
進行特徵抽取，然後分別保存到cornerPointsSharp等等隊列中去。 保存到不同的隊列是不同類型的點雲，進行了標記的工作，這一步中減少了點雲數量，使計算量減少。 函數首先清空了cornerPointsSharp,cornerPointsLessSharp,surfPointsFlat,surfPointsLessFlat 然後對cloudSmoothness隊列中sp到ep之間的點的平滑數據進行從小到大的排列。 然後按照不同的要求，將點的索引放到不同的隊列中去。 另外還對點進行了標記。 最後，因爲點雲太多時，計算量過大，因此需要對點雲進行下采樣，減少計算量。 代碼如下：
void extractFeatures()
{
cornerPointsSharp->clear();
cornerPointsLessSharp->clear();
surfPointsFlat->clear();
surfPointsLessFlat->clear();
for (int i = 0; i < N_SCAN; i++) {
surfPointsLessFlatScan->clear();
for (int j = 0; j < 6; j++) {
// sp和ep的含義是什麼???startPointer,endPointer?
int sp = (segInfo.startRingIndex[i] * (6 - j) + segInfo.endRingIndex[i] * j) / 6;
int ep = (segInfo.startRingIndex[i] * (5 - j) + segInfo.endRingIndex[i] * (j + 1)) / 6 - 1;
if (sp >= ep)
continue;
// 按照cloudSmoothness.value從小到大排序
std::sort(cloudSmoothness.begin()+sp, cloudSmoothness.begin()+ep, by_value());
int largestPickedNum = 0;
for (int k = ep; k >= sp; k--) {
// 每次ind的值就是等於k??? 有什麼意義?
// 因爲上面對cloudSmoothness進行了一次從小到大排序，所以ind不一定等於k了
int ind = cloudSmoothness[k].ind;
if (cloudNeighborPicked[ind] == 0 &&
cloudCurvature[ind] > edgeThreshold &&
segInfo.segmentedCloudGroundFlag[ind] == false) {
largestPickedNum++;
if (largestPickedNum <= 2) {
// 論文中nFe=2,cloudSmoothness已經按照從小到大的順序排列，
// 所以這邊只要選擇最後兩個放進隊列即可
// cornerPointsSharp標記爲2
cloudLabel[ind] = 2;
cornerPointsSharp->push_back(segmentedCloud->points[ind]);
cornerPointsLessSharp->push_back(segmentedCloud->points[ind]);
} else if (largestPickedNum <= 20) {
// 塞20個點到cornerPointsLessSharp中去
// cornerPointsLessSharp標記爲1
cloudLabel[ind] = 1;
cornerPointsLessSharp->push_back(segmentedCloud->points[ind]);
} else {
break;
}
cloudNeighborPicked[ind] = 1;
for (int l = 1; l <= 5; l++) {
// 從ind+l開始後面5個點，每個點index之間的差值，
// 確保columnDiff<=10,然後標記爲我們需要的點
int columnDiff = std::abs(int(segInfo.segmentedCloudColInd[ind + l] - segInfo.segmentedCloudColInd[ind + l - 1]));
if (columnDiff > 10)
break;
cloudNeighborPicked[ind + l] = 1;
}
for (int l = -1; l >= -5; l--) {
// 從ind+l開始前面五個點，計算差值然後標記
int columnDiff = std::abs(int(segInfo.segmentedCloudColInd[ind + l] - segInfo.segmentedCloudColInd[ind + l + 1]));
if (columnDiff > 10)
break;
cloudNeighborPicked[ind + l] = 1;
}
}
}
int smallestPickedNum = 0;
for (int k = sp; k <= ep; k++) {
int ind = cloudSmoothness[k].ind;
// 平面點只從地面點中進行選擇???爲什麼要這樣做???
if (cloudNeighborPicked[ind] == 0 &&
cloudCurvature[ind] < surfThreshold &&
segInfo.segmentedCloudGroundFlag[ind] == true) {
cloudLabel[ind] = -1;
surfPointsFlat->push_back(segmentedCloud->points[ind]);
// 論文中nFp=4，將4個最平的平面點放入隊列中
smallestPickedNum++;
if (smallestPickedNum >= 4) {
break;
}
cloudNeighborPicked[ind] = 1;
for (int l = 1; l <= 5; l++) {
// 從前面往後判斷是否是需要的鄰接點，是的話就進行標記
int columnDiff = std::abs(int(segInfo.segmentedCloudColInd[ind + l] - segInfo.segmentedCloudColInd[ind + l - 1]));
if (columnDiff > 10)
break;
cloudNeighborPicked[ind + l] = 1;
}
for (int l = -1; l >= -5; l--) {
// 從後往前開始標記
int columnDiff = std::abs(int(segInfo.segmentedCloudColInd[ind + l] - segInfo.segmentedCloudColInd[ind + l + 1]));
if (columnDiff > 10)
break;
cloudNeighborPicked[ind + l] = 1;
}
}
}
for (int k = sp; k <= ep; k++) {
if (cloudLabel[k] <= 0) {
surfPointsLessFlatScan->push_back(segmentedCloud->points[k]);
}
}
}
// surfPointsLessFlatScan中有過多的點雲，如果點雲太多，計算量太大
// 進行下采樣，可以大大減少計算量
surfPointsLessFlatScanDS->clear();
downSizeFilter.setInputCloud(surfPointsLessFlatScan);
downSizeFilter.filter(*surfPointsLessFlatScanDS);
*surfPointsLessFlat += *surfPointsLessFlatScanDS;
}
}
void updateTransformation()
中主要是兩個部分，一個是找特徵平面，通過面之間的對應關係計算出變換矩陣。 另一個部分是通過角、邊特徵的匹配，計算變換矩陣。 該函數主要由其他四個部分組成：findCorrespondingSurfFeatures,calculateTransformationSurf findCorrespondingCornerFeatures, calculateTransformationCorner 這四個函數分別是對應於尋找對應面、通過面對應計算變換矩陣、尋找對應角/邊特徵、通過角/邊特徵計算變換矩陣。
void updateTransformation(){
if (laserCloudCornerLastNum < 10 || laserCloudSurfLastNum < 100)
return;
for (int iterCount1 = 0; iterCount1 < 25; iterCount1++) {
laserCloudOri->clear();
coeffSel->clear();
// 找到對應的特徵平面
// 然後計算協方差矩陣，保存在coeffSel隊列中
// laserCloudOri中保存的是對應於coeffSel的未轉換到開始時刻的原始點雲數據
findCorrespondingSurfFeatures(iterCount1);
if (laserCloudOri->points.size() < 10)
continue;
// 通過面特徵的匹配，計算變換矩陣
if (calculateTransformationSurf(iterCount1) == false)
break;
}
for (int iterCount2 = 0; iterCount2 < 25; iterCount2++) {
laserCloudOri->clear();
coeffSel->clear();
// 找到對應的特徵邊/角點
// 尋找邊特徵的方法和尋找平面特徵的很類似，過程可以參照尋找平面特徵的註釋
findCorrespondingCornerFeatures(iterCount2);
if (laserCloudOri->points.size() < 10)
continue;
// 通過角/邊特徵的匹配，計算變換矩陣
if (calculateTransformationCorner(iterCount2) == false)
break;
}
}
void integrateTransformation()
計算了旋轉角的累積變化量。 這個函數首先通過AccumulateRotation()將局部旋轉左邊切換至全局旋轉座標。 然後同坐變換轉移到世界座標系下。 再通過PluginIMURotation(rx, ry, rz, imuPitchStart, imuYawStart, imuRollStart, imuPitchLast, imuYawLast, imuRollLast, rx, ry, rz);插入imu旋轉，更新姿態。
mapOptmization.cpp
mapOptmization.cpp進行的內容主要是地圖優化，將得到的局部地圖信息融合到全局地圖中去。
publishGlobalMap()
- publishGlobalMap()主要進行了3個步驟：
- 通過KDTree進行最近鄰搜索;
- 通過搜索得到的索引放進隊列;
- 通過兩次下采樣，減小數據量;
通過儲存的 cloudKeyPoses3D 關鍵點的位置， 找到鄰近的關鍵幀，默認顯示500m範圍內的關鍵位置。
void publishGlobalMap(){
if (pubLaserCloudSurround.getNumSubscribers() == 0)
return;
if (cloudKeyPoses3D->points.empty() == true)
return;
std::vector<int> pointSearchIndGlobalMap;
std::vector<float> pointSearchSqDisGlobalMap;
mtx.lock();
kdtreeGlobalMap->setInputCloud(cloudKeyPoses3D);
// 通過KDTree進行最近鄰搜索
kdtreeGlobalMap->radiusSearch(currentRobotPosPoint, globalMapVisualizationSearchRadius, pointSearchIndGlobalMap, pointSearchSqDisGlobalMap, 0);
mtx.unlock();
for (int i = 0; i < pointSearchIndGlobalMap.size(); ++i)
globalMapKeyPoses->points.push_back(cloudKeyPoses3D->points[pointSearchIndGlobalMap[i]]);
// 對globalMapKeyPoses進行下采樣
downSizeFilterGlobalMapKeyPoses.setInputCloud(globalMapKeyPoses);
downSizeFilterGlobalMapKeyPoses.filter(*globalMapKeyPosesDS);
for (int i = 0; i < globalMapKeyPosesDS->points.size(); ++i){
int thisKeyInd = (int)globalMapKeyPosesDS->points[i].intensity;
*globalMapKeyFrames += *transformPointCloud(cornerCloudKeyFrames[thisKeyInd],&cloudKeyPoses6D->points[thisKeyInd]);
*globalMapKeyFrames += *transformPointCloud(surfCloudKeyFrames[thisKeyInd],&cloudKeyPoses6D->points[thisKeyInd]);
*globalMapKeyFrames += *transformPointCloud(outlierCloudKeyFrames[thisKeyInd],&cloudKeyPoses6D->points[thisKeyInd]);
}
// 對globalMapKeyFrames進行下采樣
downSizeFilterGlobalMapKeyFrames.setInputCloud(globalMapKeyFrames);
downSizeFilterGlobalMapKeyFrames.filter(*globalMapKeyFramesDS);
sensor_msgs::PointCloud2 cloudMsgTemp;
pcl::toROSMsg(*globalMapKeyFramesDS, cloudMsgTemp);
cloudMsgTemp.header.stamp = ros::Time().fromSec(timeLaserOdometry);
cloudMsgTemp.header.frame_id = "/camera_init";
pubLaserCloudSurround.publish(cloudMsgTemp);
globalMapKeyPoses->clear();
globalMapKeyPosesDS->clear();
globalMapKeyFrames->clear();
globalMapKeyFramesDS->clear();
}
void run()
void run(){
if (newLaserCloudCornerLast &&
std::abs(timeLaserCloudCornerLast - timeLaserOdometry) < 0.005 &&
newLaserCloudSurfLast &&
std::abs(timeLaserCloudSurfLast - timeLaserOdometry) < 0.005 &&
newLaserCloudOutlierLast &&
std::abs(timeLaserCloudOutlierLast - timeLaserOdometry) < 0.005 &&
newLaserOdometry)
{
newLaserCloudCornerLast = false;
newLaserCloudSurfLast = false;
newLaserCloudOutlierLast = false;
newLaserOdometry = false;
std::lock_guard<std::mutex> lock(mtx);
if (timeLaserOdometry - timeLastProcessing >= mappingProcessInterval) {
timeLastProcessing = timeLaserOdometry;
transformAssociateToMap();
extractSurroundingKeyFrames();
downsampleCurrentScan();
scan2MapOptimization();
saveKeyFramesAndFactor();
correctPoses();
publishTF();
publishKeyPosesAndFrames();
clearCloud();
}
}
}
參考資料
