Ramer–Douglas–Peucker算法在简化路径上的具体实现

Ramer–Douglas–Peucker是一种用于减少曲线中的点的算法，即能够用较少的点来描述类似的一条曲线，这里尝试使用这个算法简化三维空间中的路径点

参考博客

1.https://blog.csdn.net/m0_57236802/article/details/132896429
2.https://blog.csdn.net/rover2002/article/details/107026278
3.https://blog.csdn.net/qq_27093891/article/details/120676829

基本思想

1. 起始和结束点：算法从一组点的起始点和结束点开始。
2. 找到最远的点：找到距离起始点和结束点连成的直线段最远的一个点。
3. 设置阈值：如果这个点到直线的距离小于某个给定的阈值，那么所有的中间点都会被移除。
4. 递归：如果距离大于阈值，这个点会被保留，然后算法会递归地应用于起始点到这个点，以及这个点到结束点的两个子集。

算法流程

对每一条曲线的首末点虚连一条直线,求所有点与直线的距离,
并找出最大距离值dmax ,
用dmax与限差D相比:
若dmax < D ,这条曲线上的中间点所有舍去;
若dmax ≥D ,保留dmax 相应的坐标点,并以该点为界,把曲线分为两部分,对这两部分反复使用该方法。
控制限差值的大小可以控制抽稀的粒度。

伪代码

来源于高飞老师深蓝学院路径规划课程大作业project说明md文件

function DouglasPeucker(PointList[], epsilon)
    // Find the point with the maximum distance
    dmax = 0
    index = 0
    end = length(PointList)
    for i = 2 to (end - 1) {
        d = perpendicularDistance(PointList[i], Line(PointList[1], PointList[end])) 
        if (d > dmax) {
            index = i
            dmax = d
        }
    }
    
    ResultList[] = empty;
    
    // If max distance is greater than epsilon, recursively simplify
    if (dmax > epsilon) {
        // Recursive call
        recResults1[] = DouglasPeucker(PointList[1...index], epsilon)
        recResults2[] = DouglasPeucker(PointList[index...end], epsilon)

        // Build the result list
        ResultList[] = {recResults1[1...length(recResults1) - 1], recResults2[1...length(recResults2)]}
    } else {
        ResultList[] = {PointList[1], PointList[end]}
    }
    // Return the result
    return ResultList[]
end

具体代码实现

double getDistance(Vector3d mid_point, Vector3d start_point, 
                                                        Vector3d end_point){
    Vector3d line_vector = end_point - start_point;
    Vector3d start_to_mid = mid_point - start_point;

    double t = start_to_mid.dot(line_vector) / line_vector.dot(line_vector);
    Vector3d projection = start_point + t * line_vector;
    double distance = (mid_point - projection).norm();
    
    return distance;
}

vector<Vector3d> DouglasPeucker(const vector<Vector3d> &PointList,
                                               double epsilon){
     // Find the point with the maximum distance
    double d_max = 0;
    double d = 0;
    int index = 0;
    int end = PointList.size();
    int i;
    for(i = 1; i < (end - 1); i++)
    {
        d = getDistance(PointList[i], PointList[0], PointList[end - 1]);
        if(d > d_max)
        {
            d_max = d;
            index = i;
        }
    }
    // ROS_INFO("start a new DouglasPeucker!");
    if(d_max > epsilon)
    {   
        vector<Vector3d> PointList1;
        vector<Vector3d> PointList2;

        for (int i = 0; i <= index; i++) {
            PointList1.push_back(PointList[i]);
        }
        for (int i = index ; i < PointList.size(); i++) {
            PointList2.push_back(PointList[i]);
        }
        //递归
        vector<Vector3d> ResultList1 = DouglasPeucker(PointList1, epsilon);
        vector<Vector3d> ResultList2 = DouglasPeucker(PointList2, epsilon);

        ResultList = ResultList1;
        ResultList.insert(ResultList.end(), ResultList2.begin(), ResultList2.end());

    }
    else
    {
        ResultList = {PointList[0],PointList[end - 1]};
    }
    //std::cout << "Simplified path长度为：" << ResultList.size() << endl;

    return ResultList;
}

vector<Vector3d> pathSimplify(const vector<Vector3d> &path,
                                               double path_resolution) {
    vector<Vector3d> subPath;
    vector<Vector3d> tempPath;
	//以这种写法生成的简化路径vector中会有重复的三维向量元素，因此需要再次对tempPath进行简化
    tempPath = DouglasPeucker(path, path_resolution);
    subPath = {tempPath[0]};
    for(int i = 1; i < tempPath.size(); i++)
    {
        if(tempPath[i-1] != tempPath[i])
        {
            subPath.push_back(tempPath[i]);
        }
            
    }
    std::cout << "简化后的路径点数目为：" << subPath.size() << endl;
    ROS_WARN("success simplify grid path!");

    return subPath;
}
//全局变量
std::vector<Eigen::Vector3d> ResultList;
int main()
{
	double epsilon = 0.3;//阈值
	pathSimplify(grid_path, epsilon);//假设grid_path已知
}

代码效果

此处的grid_path是由A*算法进行图搜索得到，假设已知
图中绿色点组成原始路径，红色点代表简化路径