5. 2d-Lidar 点云多直线拟合算法

具体步骤：

1. EM+GMM(高斯模糊模型) 点云分割聚类算法的实现。
2. 基于RANSAC单帧lidar数据直线拟合算法实现。
3. 多帧lidar数据实时直线优化算法实现。

算法实现逻辑：

Struct line{
first line,
end line};
std::vector<line> lineVector;
if（EMGMM（）get five custers）{
    for(int i=0, i<five, i++){
        custers[i];
        //对每一个custer进行RANSAC直线拟合。
bool isSuccess = RANSAC_getLines(…lineTemp);
if (isSuccess){
lineVector.push_back(lineTemp);
}
        ……
}
}else{
    //exit(-1);
}

//直线优化算法
If（nowline与pastline 近似相等）{
    If（判断线段两端点到近似直线的距离 && 判断两端点是否在近似线段中间//阈值设置d<0.05）{
        //对直线进行融合优化。
        }else{
            //判断短直线进行剔除优化。
            //保留直线 保存作为待优化。
}
}else{
//判断短直线进行剔除优化。
//保留直线 保存作为待优化。
}

 

图1

 

图2

参考：https://blog.csdn.net/u014568921/article/details/45049541