LEGO LOAM

LEGO_LOAM学习笔记

legoloam基本框架和loam类似，在loam的上面做了部分改动。
lego_loam特点：

• 一种轻量级的，可疑在嵌入式平台上运行
• 点云分割去除噪音（聚类分割）
• 带有地面优化的实时6自由度估计（在分割和优化过程中，利用了地面的存在）
• 闭环

通过surf点计算pitch、roll、tz，corn点计算yaw，tx，ty。

地面分割

地面分割是根据雷达特性，取lidar下半部分的点云进行分割，地面点云通过，点云投影中的上下两个点的角度差。

void groundRemoval(){
    size_t lowerInd, upperInd;
    float diffX, diffY, diffZ, angle;

    for (size_t j = 0; j < Horizon_SCAN; ++j){
        // groundScanInd 是在 utility.h 文件中声明的线数，groundScanInd=7
        for (size_t i = 0; i < groundScanInd; ++i){

            lowerInd = j + ( i )*Horizon_SCAN;
            upperInd = j + (i+1)*Horizon_SCAN;

            // 初始化的时候用nanPoint.intensity = -1 填充
            // 都是-1 证明是空点nanPoint
            if (fullCloud->points[lowerInd].intensity == -1 ||
                fullCloud->points[upperInd].intensity == -1){
                groundMat.at<int8_t>(i,j) = -1;
                continue;
            }

			// 由上下两线之间点的XYZ位置得到两线之间的俯仰角
			// 如果俯仰角在10度以内，则判定(i,j)为地面点,groundMat[i][j]=1
			// 否则，则不是地面点，进行后续操作
            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;

            if (abs(angle - sensorMountAngle) <= 10){
                groundMat.at<int8_t>(i,j) = 1;
                groundMat.at<int8_t>(i+1,j) = 1;
            }
        }
    }

	// 找到所有点中的地面点或者距离为FLT_MAX(rangeMat的初始值)的点，并将他们标记为-1
	// rangeMat[i][j]==FLT_MAX，代表的含义是什么？ 无效点
    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;
            }
        }
    }

	// 如果有节点订阅groundCloud，那么就需要把地面点发布出来
	// 具体实现过程：把点放到groundCloud队列中去
    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]);
            }
        }
    }
}

聚类分割

通过广度优先搜索进行聚类标记（较少点云簇标记为噪声），剔除数量较少的点云簇。聚类判断如下。所示。

void labelComponents(int row, int col){
    float d1, d2, alpha, angle;
    int fromIndX, fromIndY, thisIndX, thisIndY; 
    bool lineCountFlag[N_SCAN] = {false};

    queueIndX[0] = row;
    queueIndY[0] = col;
    int queueSize = 1;
    int queueStartInd = 0;
    int queueEndInd = 1;

    allPushedIndX[0] = row;
    allPushedIndY[0] = col;
    int allPushedIndSize = 1;
    
    // 标准的BFS
    // BFS的作用是以(row，col)为中心向外面扩散，
    // 判断(row,col)是否是这个平面中一点
    while(queueSize > 0){
        fromIndX = queueIndX[queueStartInd];
        fromIndY = queueIndY[queueStartInd];
        --queueSize;
        ++queueStartInd;
		// labelCount的初始值为1，后面会递增
        labelMat.at<int>(fromIndX, fromIndY) = labelCount;

		// neighbor=[[-1,0];[0,1];[0,-1];[1,0]]
		// 遍历点[fromIndX,fromIndY]边上的四个邻点
        for (auto iter = neighborIterator.begin(); iter != neighborIterator.end(); ++iter){

            thisIndX = fromIndX + (*iter).first;
            thisIndY = fromIndY + (*iter).second;

            if (thisIndX < 0 || thisIndX >= N_SCAN)
                continue;

            // 是个环状的图片，左右连通
            if (thisIndY < 0)
                thisIndY = Horizon_SCAN - 1;
            if (thisIndY >= Horizon_SCAN)
                thisIndY = 0;

			// 如果点[thisIndX,thisIndY]已经标记过
			// labelMat中，-1代表无效点，0代表未进行标记过，其余为其他的标记
			// 如果当前的邻点已经标记过，则跳过该点。
			// 如果labelMat已经标记为正整数，则已经聚类完成，不需要再次对该点聚类
            if (labelMat.at<int>(thisIndX, thisIndY) != 0)
                continue;

            d1 = std::max(rangeMat.at<float>(fromIndX, fromIndY), 
                          rangeMat.at<float>(thisIndX, thisIndY));
            d2 = std::min(rangeMat.at<float>(fromIndX, fromIndY), 
                          rangeMat.at<float>(thisIndX, thisIndY));

			// alpha代表角度分辨率，
			// X方向上角度分辨率是segmentAlphaX(rad)
			// Y方向上角度分辨率是segmentAlphaY(rad)
            if ((*iter).first == 0)
                alpha = segmentAlphaX;
            else
                alpha = segmentAlphaY;

			// 通过下面的公式计算这两点之间是否有平面特征
			// atan2(y,x)的值越大，d1，d2之间的差距越小,越平坦
            angle = atan2(d2*sin(alpha), (d1 -d2*cos(alpha)));

            if (angle > segmentTheta){
				// segmentTheta=1.0472<==>60度
				// 如果算出角度大于60度，则假设这是个平面
                queueIndX[queueEndInd] = thisIndX;
                queueIndY[queueEndInd] = thisIndY;
                ++queueSize;
                ++queueEndInd;

                labelMat.at<int>(thisIndX, thisIndY) = labelCount;
                lineCountFlag[thisIndX] = true;

                allPushedIndX[allPushedIndSize] = thisIndX;
                allPushedIndY[allPushedIndSize] = thisIndY;
                ++allPushedIndSize;
            }
        }
    }


    bool feasibleSegment = false;

	// 如果聚类超过30个点，直接标记为一个可用聚类，labelCount需要递增
    if (allPushedIndSize >= 30)
        feasibleSegment = true;
    else if (allPushedIndSize >= segmentValidPointNum){
		// 如果聚类点数小于30大于等于5，统计竖直方向上的聚类点数
        int lineCount = 0;
        for (size_t i = 0; i < N_SCAN; ++i)
            if (lineCountFlag[i] == true)
                ++lineCount;

		// 竖直方向上超过3个也将它标记为有效聚类
        if (lineCount >= segmentValidLineNum)
            feasibleSegment = true;            
    }

    if (feasibleSegment == true){
        ++labelCount;
    }else{
        for (size_t i = 0; i < allPushedIndSize; ++i){
			// 标记为999999的是需要舍弃的聚类的点，因为他们的数量小于30个
            labelMat.at<int>(allPushedIndX[i], allPushedIndY[i]) = 999999;
        }
    }
}