2d-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{
//判断短直线进行剔除优化。
//保留直线 保存作为待优化。
}