参考资料
- 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();
}
}
}
参考资料
