1 LeGO-LOAM介绍
LeGO-LOAM不仅整合了LOAM的系统结构，同时对LOAM中的特征提取、位姿估计计算都进行了优化改进，此外还加入了闭环检测和全局优化，将LOAM这一LO系统构建为完整的SLAM系统。
- 轻量级：LeGO-LOAM设计得较为轻量，能够在低功耗嵌入式系统上实现实时姿态估计，这使得它非常适合于资源受限的应用场景。
- 地面优化：该算法在分割和优化步骤中充分利用了地面的约束，提高了算法的准确性和鲁棒性。
- 高精度：通过特征提取和两步Levenberg-Marquardt优化方法，LeGO-LOAM能够在连续扫描中解决六个自由度变换的不同分量，从而实现高精度的位姿估计。
2系统架构
与LOAM类似，系统整体分为四个部分，对应四个ROS节点，在四个不同进程中运行。不同的是，LeGO-LOAM将LOAM中负责点云特征提取的scanRegistration节点和负责scan-to-scan匹配的laserOdometry节点整合为featureAssociation节点，增加imageProjection节点，同时在mapOptimization节点中开辟闭环检测与全局优化线程。各节点的具体功能如下：
LeGO-LOAM（Lightweight and Ground-Optimized Lidar Odometry and Mapping）是一个针对地面移动机器人设计的轻量级激光雷达SLAM系统，该系统通过优化和改进LOAM（Lidar Odometry and Mapping）的各个方面，实现了在嵌入式系统上的实时位姿估计和地图构建。LeGO-LOAM的系统架构通常分为几个关键节点，每个节点负责不同的功能。以下是LeGO-LOAM中各节点的主要功能：
（1）ImageProjection
功能描述：该节点主要负责点云的预处理和投影。它接收原始的激光雷达点云数据，并进行一系列的处理，包括点云复制、去除无效点、计算点云的行列信息（即投影到深度图上），以及地面点检测等。这些处理步骤有助于将无序的点云数据有序化，并为后续的特征提取和位姿估计提供基础。
核心功能：
- 点云复制与清理：去除原始数据中的无效点（如NaN值）。
- 点云投影：将点云投影到深度图上，计算每个点的行列索引和距离。
- 地面点检测：通过比较点云中的相邻点来确定地面点。
void projectPointCloud(){
// range image projection
float verticalAngle, horizonAngle, range;
size_t rowIdn, columnIdn, index, cloudSize;
PointType thisPoint;
cloudSize = laserCloudIn->points.size(); // 点云中点个数
for (size_t i = 0; i < cloudSize; ++i){ // 逐点计算
thisPoint.x = laserCloudIn->points[i].x;
thisPoint.y = laserCloudIn->points[i].y;
thisPoint.z = laserCloudIn->points[i].z;
// find the row and column index in the iamge for this point
if (useCloudRing == true){ // 当LiDAR驱动能够输出scan line信息时，直接读取
rowIdn = laserCloudInRing->points[i].ring;
}
else{ // 否则，需要计算俯仰角
verticalAngle = atan2(thisPoint.z, sqrt(thisPoint.x * thisPoint.x + thisPoint.y * thisPoint.y)) * 180 / M_PI; // 单位为度
rowIdn = (verticalAngle + ang_bottom) / ang_res_y; // 计算行号，见公式（1）
}
if (rowIdn < 0 || rowIdn >= N_SCAN)
continue;
horizonAngle = atan2(thisPoint.x, thisPoint.y) * 180 / M_PI; // 计算列号，见公式（2）
// 由于atan2返回的是(-pi,pi)的一个值，为了保证正前方在深度图的中间，需要进行如下角度变换。
columnIdn = -round((horizonAngle-90.0)/ang_res_x) + Horizon_SCAN/2;
if (columnIdn >= Horizon_SCAN)
columnIdn -= Horizon_SCAN;
if (columnIdn < 0 || columnIdn >= Horizon_SCAN)
continue;
//
range = sqrt(thisPoint.x * thisPoint.x + thisPoint.y * thisPoint.y + thisPoint.z * thisPoint.z);
if (range < sensorMinimumRange) // 舍弃深度小于最小阈值的点
continue;
rangeMat.at<float>(rowIdn, columnIdn) = range; // 构建深度图
thisPoint.intensity = (float)rowIdn + (float)columnIdn / 10000.0;
index = columnIdn + rowIdn * Horizon_SCAN; // 深度图像素坐标对应的索引号
fullCloud->points[index] = thisPoint; // 构建点云
fullInfoCloud->points[index] = thisPoint;
fullInfoCloud->points[index].intensity = range; // the corresponding range of a point is saved as "intensity"
}
}
void groundRemoval(){
size_t lowerInd, upperInd;
float diffX, diffY, diffZ, angle;
// 构建地面点深度图groundMat
// -1, 深度图中的无效区域
// 0, 非地面点标记
// 1, 地面点标记
for (size_t j = 0; j < Horizon_SCAN; ++j){
for (size_t i = 0; i < groundScanInd; ++i){
lowerInd = j + ( i )*Horizon_SCAN; // 当前像素点下方像素的索引号
upperInd = j + (i+1)*Horizon_SCAN; // 当前像素点上方像素的索引号
if (fullCloud->points[lowerInd].intensity == -1 ||
fullCloud->points[upperInd].intensity == -1){ // 对应像素无深度值
// no info to check, invalid points
groundMat.at<int8_t>(i,j) = -1;
continue;
}
// 构建向量
diffX = fullCloud->points[upperInd].x - fullCloud->points[lowerInd].x;
diffY = fullCloud->points[upperInd].y - fullCloud->points[lowerInd].y;
diffZ = fullCloud->points[upperInd].z - fullCloud->points[lowerInd].z;
angle = atan2(diffZ, sqrt(diffX*diffX + diffY*diffY) ) * 180 / M_PI; // 计算向量与x-y平面的夹角
if (abs(angle - sensorMountAngle) <= 10){ // sensorMountAngle表示LiDAR安装时与地面的夹角，通俗来说头朝下时夹角为正，头朝上扬起时夹角为负
groundMat.at<int8_t>(i,j) = 1;
groundMat.at<int8_t>(i+1,j) = 1; // 若夹角小于10°，则将构成向量的两个深度图像素点标记为地面点
}
}
}
// extract ground cloud (groundMat == 1)
// mark entry that doesn't need to label (ground and invalid point) for segmentation
// note that ground remove is from 0~N_SCAN-1, need rangeMat for mark label matrix for the 16th scan
for (size_t i = 0; i < N_SCAN; ++i){
for (size_t j = 0; j < Horizon_SCAN; ++j){
if (groundMat.at<int8_t>(i,j) == 1 || rangeMat.at<float>(i,j) == FLT_MAX){
labelMat.at<int>(i,j) = -1;
}
}
}
if (pubGroundCloud.getNumSubscribers() != 0){
for (size_t i = 0; i <= groundScanInd; ++i){
for (size_t j = 0; j < Horizon_SCAN; ++j){
if (groundMat.at<int8_t>(i,j) == 1)
groundCloud->push_back(fullCloud->points[j + i*Horizon_SCAN]); // 插入地面点点云
}
}
}
}
（2）featureAssociation
功能描述：该节点整合了LOAM中原本负责点云特征提取的scanRegistration节点和负责scan-to-scan匹配的laserOdometry节点的功能。它主要负责从预处理后的点云中提取特征点，并进行特征匹配以估计相邻激光帧之间的相对位姿。
核心功能：
- 特征提取：从投影后的点云中提取边缘特征和平面特征。
- 特征匹配：使用提取的特征点进行帧间匹配，以计算相邻激光帧之间的相对位姿。
void updateTransformation(){
if (laserCloudCornerLastNum < 10 || laserCloudSurfLastNum < 100)
return;
for (int iterCount1 = 0; iterCount1 < 25; iterCount1++) { // 第一步： 根据地面特征估计三个自由度
laserCloudOri->clear();
coeffSel->clear();
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;
}
}
bool calculateTransformationCorner(int iterCount){
int pointSelNum = laserCloudOri->points.size();
cv::Mat matA(pointSelNum, 3, CV_32F, cv::Scalar::all(0));
cv::Mat matAt(3, pointSelNum, CV_32F, cv::Scalar::all(0));
cv::Mat matAtA(3, 3, CV_32F, cv::Scalar::all(0));
cv::Mat matB(pointSelNum, 1, CV_32F, cv::Scalar::all(0));
cv::Mat matAtB(3, 1, CV_32F, cv::Scalar::all(0));
cv::Mat matX(3, 1, CV_32F, cv::Scalar::all(0));
float srx = sin(transformCur[0]);
float crx = cos(transformCur[0]);
float sry = sin(transformCur[1]);
float cry = cos(transformCur[1]);
float srz = sin(transformCur[2]);
float crz = cos(transformCur[2]);
float tx = transformCur[3];
float ty = transformCur[4];
float tz = transformCur[5];
float b1 = -crz*sry - cry*srx*srz; float b2 = cry*crz*srx - sry*srz; float b3 = crx*cry; float b4 = tx*-b1 + ty*-b2 + tz*b3;
float b5 = cry*crz - srx*sry*srz; float b6 = cry*srz + crz*srx*sry; float b7 = crx*sry; float b8 = tz*b7 - ty*b6 - tx*b5;
float c5 = crx*srz;
// 利用观测到的线特征构建优化方程的系数矩阵A和残差向量B
for (int i = 0; i < pointSelNum; i++) {
pointOri = laserCloudOri->points[i];
coeff = coeffSel->points[i];
float ary = (b1*pointOri.x + b2*pointOri.y - b3*pointOri.z + b4) * coeff.x
+ (b5*pointOri.x + b6*pointOri.y - b7*pointOri.z + b8) * coeff.z;
float atx = -b5 * coeff.x + c5 * coeff.y + b1 * coeff.z;
float atz = b7 * coeff.x - srx * coeff.y - b3 * coeff.z;
float d2 = coeff.intensity;
matA.at<float>(i, 0) = ary;
matA.at<float>(i, 1) = atx;
matA.at<float>(i, 2) = atz;
matB.at<float>(i, 0) = -0.05 * d2;
}
cv::transpose(matA, matAt);
matAtA = matAt * matA;
matAtB = matAt * matB;
cv::solve(matAtA, matAtB, matX, cv::DECOMP_QR); // QR分解法求解 AtAX = AtB
if (iterCount == 0) {
cv::Mat matE(1, 3, CV_32F, cv::Scalar::all(0));
cv::Mat matV(3, 3, CV_32F, cv::Scalar::all(0));
cv::Mat matV2(3, 3, CV_32F, cv::Scalar::all(0));
cv::eigen(matAtA, matE, matV); // 对AtA这一对称矩阵进行特征值分解
matV.copyTo(matV2); // 特征向量，一行对应一个状态方向
isDegenerate = false;
float eignThre[3] = {10, 10, 10};
for (int i = 2; i >= 0; i--) { // 从最小特征值开始
if (matE.at<float>(0, i) < eignThre[i]) {
for (int j = 0; j < 3; j++) {
matV2.at<float>(i, j) = 0; // 若出现退化，将这一行置零
}
isDegenerate = true;
} else {
break;
}
}
matP = matV.inv() * matV2; // 由于AtA是方阵，根据对称对角化定理V^-1 = Vt
}
if (isDegenerate) {
cv::Mat matX2(3, 1, CV_32F, cv::Scalar::all(0));
matX.copyTo(matX2);
matX = matP * matX2;
}
transformCur[1] += matX.at<float>(0, 0);
transformCur[3] += matX.at<float>(1, 0);
transformCur[5] += matX.at<float>(2, 0);
for(int i=0; i<6; i++){
if(isnan(transformCur[i]))
transformCur[i]=0;
}
float deltaR = sqrt(
pow(rad2deg(matX.at<float>(0, 0)), 2));
float deltaT = sqrt(
pow(matX.at<float>(1, 0) * 100, 2) +
pow(matX.at<float>(2, 0) * 100, 2));
if (deltaR < 0.1 && deltaT < 0.1) { // 判断收敛
return false;
}
return true;
}
（3）mapOptimization
功能描述：该节点负责地图的优化和全局一致性维护。它接收来自featureAssociation节点的位姿估计结果，并进行进一步的优化处理，包括闭环检测和全局优化。
核心功能：
- 闭环检测：通过比较当前帧与关键帧之间的相似度来检测闭环，以消除累积误差。
- 全局优化：在检测到闭环后，使用图优化技术对整个位姿图进行优化，以提高地图的全局一致性。
void saveKeyFramesAndFactor(){
currentRobotPosPoint.x = transformAftMapped[3];
currentRobotPosPoint.y = transformAftMapped[4];
currentRobotPosPoint.z = transformAftMapped[5]; // scan-to-map后的LiDAR位姿
bool saveThisKeyFrame = true; // 关键帧筛选
if (sqrt((previousRobotPosPoint.x-currentRobotPosPoint.x)*(previousRobotPosPoint.x-currentRobotPosPoint.x)
+(previousRobotPosPoint.y-currentRobotPosPoint.y)*(previousRobotPosPoint.y-currentRobotPosPoint.y)
+(previousRobotPosPoint.z-currentRobotPosPoint.z)*(previousRobotPosPoint.z-currentRobotPosPoint.z)) < 0.3){
saveThisKeyFrame = false; // 当平移距离超过0.3m时添加关键帧
}
if (saveThisKeyFrame == false && !cloudKeyPoses3D->points.empty())
return;
previousRobotPosPoint = currentRobotPosPoint;
/**
* update grsam graph
*/
if (cloudKeyPoses3D->points.empty()){
gtSAMgraph.add(PriorFactor<Pose3>(0, Pose3(Rot3::RzRyRx(transformTobeMapped[2], transformTobeMapped[0], transformTobeMapped[1]),
Point3(transformTobeMapped[5], transformTobeMapped[3], transformTobeMapped[4])), priorNoise)); // 先验因子，即明确状态的因子，不随优化而改变
initialEstimate.insert(0, Pose3(Rot3::RzRyRx(transformTobeMapped[2], transformTobeMapped[0], transformTobeMapped[1]),
Point3(transformTobeMapped[5], transformTobeMapped[3], transformTobeMapped[4]))); // 状态量初值
for (int i = 0; i < 6; ++i)
transformLast[i] = transformTobeMapped[i];
}
else{
gtsam::Pose3 poseFrom = Pose3(Rot3::RzRyRx(transformLast[2], transformLast[0], transformLast[1]),
Point3(transformLast[5], transformLast[3], transformLast[4]));
gtsam::Pose3 poseTo = Pose3(Rot3::RzRyRx(transformAftMapped[2], transformAftMapped[0], transformAftMapped[1]),
Point3(transformAftMapped[5], transformAftMapped[3], transformAftMapped[4]));
gtSAMgraph.add(BetweenFactor<Pose3>(cloudKeyPoses3D->points.size()-1, cloudKeyPoses3D->points.size(), poseFrom.between(poseTo), odometryNoise)); // 由相对位姿变换构成的二元因子
initialEstimate.insert(cloudKeyPoses3D->points.size(), Pose3(Rot3::RzRyRx(transformAftMapped[2], transformAftMapped[0], transformAftMapped[1]),
Point3(transformAftMapped[5], transformAftMapped[3], transformAftMapped[4]))); // 状态量初值
}
/**
* update iSAM
*/
isam->update(gtSAMgraph, initialEstimate);
isam->update(); // isam的整体优化
gtSAMgraph.resize(0);
initialEstimate.clear();
/**
* save key poses
*/
PointType thisPose3D;
PointTypePose thisPose6D;
Pose3 latestEstimate;
isamCurrentEstimate = isam->calculateEstimate(); // 整体优化后的位姿
latestEstimate = isamCurrentEstimate.at<Pose3>(isamCurrentEstimate.size()-1);
thisPose3D.x = latestEstimate.translation().y();
thisPose3D.y = latestEstimate.translation().z();
thisPose3D.z = latestEstimate.translation().x();
thisPose3D.intensity = cloudKeyPoses3D->points.size(); // this can be used as index
cloudKeyPoses3D->push_back(thisPose3D);
thisPose6D.x = thisPose3D.x;
thisPose6D.y = thisPose3D.y;
thisPose6D.z = thisPose3D.z;
thisPose6D.intensity = thisPose3D.intensity; // this can be used as index
thisPose6D.roll = latestEstimate.rotation().pitch();
thisPose6D.pitch = latestEstimate.rotation().yaw();
thisPose6D.yaw = latestEstimate.rotation().roll(); // in camera frame
thisPose6D.time = timeLaserOdometry;
cloudKeyPoses6D->push_back(thisPose6D);
/**
* save updated transform
*/
if (cloudKeyPoses3D->points.size() > 1){
transformAftMapped[0] = latestEstimate.rotation().pitch();
transformAftMapped[1] = latestEstimate.rotation().yaw();
transformAftMapped[2] = latestEstimate.rotation().roll();
transformAftMapped[3] = latestEstimate.translation().y();
transformAftMapped[4] = latestEstimate.translation().z();
transformAftMapped[5] = latestEstimate.translation().x();
for (int i = 0; i < 6; ++i){
transformLast[i] = transformAftMapped[i];
transformTobeMapped[i] = transformAftMapped[i];
}
}
pcl::PointCloud<PointType>::Ptr thisCornerKeyFrame(new pcl::PointCloud<PointType>());
pcl::PointCloud<PointType>::Ptr thisSurfKeyFrame(new pcl::PointCloud<PointType>());
pcl::PointCloud<PointType>::Ptr thisOutlierKeyFrame(new pcl::PointCloud<PointType>());
pcl::copyPointCloud(*laserCloudCornerLastDS, *thisCornerKeyFrame);
pcl::copyPointCloud(*laserCloudSurfLastDS, *thisSurfKeyFrame);
pcl::copyPointCloud(*laserCloudOutlierLastDS, *thisOutlierKeyFrame);
cornerCloudKeyFrames.push_back(thisCornerKeyFrame);
surfCloudKeyFrames.push_back(thisSurfKeyFrame);
outlierCloudKeyFrames.push_back(thisOutlierKeyFrame);
}
（4）TransformFusion
功能描述：该节点可能负责将不同传感器（如激光雷达、IMU等）的数据进行融合，以提供更准确、更鲁棒的位姿估计。
扫一扫
438
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
