1. 项目概述从地图上的“折线”到机器人的“理解”在地图导航、机器人轨迹规划甚至是手绘图形数字化的场景里我们常常会碰到一条由成百上千个点构成的路径。对于人眼来说这条路径的“形状”一目了然但对于计算机尤其是资源有限的嵌入式系统或需要实时处理的机器人来说处理这么多点是个沉重的负担。更关键的是这些点里有很多是冗余的——它们对描述这条路径的“灵魂”也就是其整体形状贡献微乎其微。这就引出了我们今天要深入探讨的核心问题如何用最少的点最精确地保留一条复杂路径的原始特征答案就是道格拉斯-普克算法。这个以两位开发者姓氏命名的算法自上世纪70年代被提出以来因其思想直观、效果稳定已成为地理信息系统、计算机图形学和机器人学中路径/轮廓简化的基石算法之一。简单来说Douglas-Peucker算法简称DP算法干的是这样一件事它像一位苛刻的雕塑家面对一块原始石材原始路径不断剔除那些不影响整体轮廓的冗余部分最终留下一尊线条精炼、神韵俱在的雕像简化路径。在机器人领域这个“雕像”可能就是机器人的行动轨迹、环境轮廓线或者激光雷达扫描的边界简化后的数据不仅减少了计算和存储开销更能提升后续路径跟踪、定位与建图算法的效率和稳定性。在ROS中处理路径nav_msgs/Path和轮廓如从激光扫描数据中提取的边界点是家常便饭。无论是让移动机器人平滑地跟随一条全局路径还是基于实时感知的环境轮廓进行避障对点集进行高效、保形的简化都至关重要。接下来我将结合自己在实际机器人项目中的应用经验拆解DP算法的原理并给出可直接在ROS中使用的C和Python实现分享其中踩过的坑和优化技巧。2. 算法核心思想与几何直观理解DP算法是一种递归的分治算法其核心思想可以用一句话概括用一条线段去近似一条曲线并找出偏离这条线段最远的点如果该点的偏离距离超过预设的阈值则以该点为界将曲线分成两段并分别对这两段重复此过程否则就用这条线段作为整段曲线的最终简化结果。这个描述可能有点抽象我们用一个具体的例子来可视化这个过程。假设我们有一条由A到E五个点构成的路径如下图示意请想象一条曲线A --- B --- C --- D --- E第一步连接首尾。算法首先用一条直线连接起点A和终点E。第二步寻找最大偏离点。计算中间所有点B, C, D到直线AE的垂直距离即垂足在线段AE上的垂直距离。找出其中距离最大的点假设是C点其距离为d_max。第三步判断与递归。将d_max与用户设定的阈值epsilon进行比较。如果 d_max epsilon认为点C对于描述路径形状至关重要不能被舍弃。此时原路径被点C分割成两段A-C和C-E。算法会分别对这两段路径递归地执行第一步到第三步。如果 d_max epsilon则认为所有中间点B, C, D都可以被舍弃因为用直线AE来近似整段路径其误差已经在可接受的epsilon范围之内。递归终止这一段的简化结果就是起点A和终点E。第四步合并结果。所有递归过程终止后将保留下来的关键点按原始顺序连接起来就得到了简化后的路径。这个算法的精妙之处在于它的自适应特性在路径平直的区域它会大刀阔斧地舍弃大量点而在弯曲剧烈高曲率的区域它会保留更多的点来刻画细节。最终的结果是一个点集这个点集是原始路径点集的子集并且其连接线能够以不超过epsilon的误差来逼近原始路径。注意这里提到的“距离”通常是垂直距离点到线段的垂足距离这是最常用的度量。在具体实现时需要确保垂足落在线段范围内如果垂足在线段延长线上则应计算点到线段端点的欧氏距离。2.1 阈值Epsilon的选择艺术与科学的平衡阈值epsilon是DP算法中唯一的参数也是控制简化程度和精度的“旋钮”。它的选择直接决定了最终结果epsilon值很大算法容忍度高保留的点非常少路径被极度简化可能会丢失重要特征比如一个尖锐的拐角可能被平滑成一条斜线。epsilon值很小算法非常“挑剔”会保留很多点简化效果不明显但能高度还原原始路径。如何选择这个值这没有绝对的金科玉律需要结合你的应用场景基于物理尺度如果你的路径坐标是有实际物理意义的如米那么epsilon可以设置为一个物理长度。例如在机器人导航中如果机器人的定位精度在5厘米左右那么将epsilon设为0.05米5厘米是合理的这样简化引入的误差不会显著影响定位和跟踪性能。基于相对比例如果路径坐标是归一化的或者像素坐标可以根据路径的包围盒大小最大宽度或高度按比例设置。例如epsilon 0.01 * path_width。实验与可视化最直接的方法是尝试几个不同的值将简化结果与原始路径叠加显示直观地观察哪些特征被保留、哪些被抹去直到找到一个在简化效率和形状保真度之间令你满意的平衡点。在我的机器人建图项目中处理2D激光雷达扫描生成的障碍物轮廓时我通常将epsilon设置为激光雷达距离噪声的2-3倍。例如雷达在10米处的噪声约为0.05米那么我会从0.1米开始尝试。这样既能滤除噪声引起的微小凹凸又能确保大型障碍物如墙壁、桌腿的轮廓清晰可辨。3. 算法实现细节与代码剖析理解了原理我们来看实现。一个健壮的实现需要处理好几何计算、递归控制以及结果收集。下面我将分别用C和Python给出两种风格的实现并融入ROS中常用的数据类型。3.1 基础几何计算点到线段的距离这是DP算法的基石。我们需要一个函数来计算点p到由点p1和p2构成的线段的距离。// C 实现 #include cmath #include vector struct Point { double x; double y; }; double perpendicularDistance(const Point pt, const Point lineStart, const Point lineEnd) { // 计算线段长度的平方 double dx lineEnd.x - lineStart.x; double dy lineEnd.y - lineStart.y; double lenSquared dx * dx dy * dy; // 如果线段两个端点重合则直接返回点到端点的距离 if (lenSquared 0.0) { dx pt.x - lineStart.x; dy pt.y - lineStart.y; return std::sqrt(dx * dx dy * dy); } // 计算投影比例 t [(p - lineStart) · (lineEnd - lineStart)] / |lineEnd - lineStart|^2 double t ((pt.x - lineStart.x) * dx (pt.y - lineStart.y) * dy) / lenSquared; // 将 t 钳制到 [0, 1] 区间确保投影点在线段上 t std::max(0.0, std::min(1.0, t)); // 计算投影点的坐标 double projectionX lineStart.x t * dx; double projectionY lineStart.y t * dy; // 返回点 pt 到投影点的欧氏距离 dx pt.x - projectionX; dy pt.y - projectionY; return std::sqrt(dx * dx dy * dy); }# Python 实现 import math from typing import List, Tuple Point Tuple[float, float] # 或者使用 namedtuple/ dataclass def perpendicular_distance(pt: Point, line_start: Point, line_end: Point) - float: # 计算线段向量和长度平方 dx_line line_end[0] - line_start[0] dy_line line_end[1] - line_start[1] len_squared dx_line * dx_line dy_line * dy_line # 处理线段长度为0的情况 if math.isclose(len_squared, 0.0): dx pt[0] - line_start[0] dy pt[1] - line_start[1] return math.hypot(dx, dy) # 计算投影比例 t dx_pt pt[0] - line_start[0] dy_pt pt[1] - line_start[1] t (dx_pt * dx_line dy_pt * dy_line) / len_squared # 钳制 t 到 [0, 1] t max(0.0, min(1.0, t)) # 计算投影点坐标 projection_x line_start[0] t * dx_line projection_y line_start[1] t * dy_line # 返回点到投影点的距离 return math.hypot(pt[0] - projection_x, pt[1] - projection_y)实操心得钳制t到[0, 1]区间这一步至关重要。它确保了计算的是点到线段的距离而不是点到直线的距离。如果忽略这一步对于线段延长线外的点计算出的距离会小于实际点到线段的距离实际应是到端点的距离导致算法在递归判断时可能错误地保留或丢弃点。3.2 递归实现与迭代实现DP算法天然适合用递归描述代码简洁易懂。但深度递归在路径点非常多时可能导致栈溢出。因此一个用栈模拟递归的迭代版本是更稳健的生产级选择。这里我给出两种版本的C实现并对比其优劣。版本一经典递归实现易于理解void douglasPeuckerRecursive(const std::vectorPoint pointList, size_t startIdx, size_t endIdx, double epsilon, std::vectorbool keepFlag) { if (endIdx startIdx 1) { // 起点和终点之间没有其他点直接保留 return; } // 找到离线段 (pointList[startIdx], pointList[endIdx]) 最远的点 double maxDistance 0.0; size_t maxIndex startIdx; for (size_t i startIdx 1; i endIdx; i) { double distance perpendicularDistance(pointList[i], pointList[startIdx], pointList[endIdx]); if (distance maxDistance) { maxDistance distance; maxIndex i; } } // 如果最大距离大于阈值则保留该点并递归处理左右两段 if (maxDistance epsilon) { keepFlag[maxIndex] true; douglasPeuckerRecursive(pointList, startIdx, maxIndex, epsilon, keepFlag); douglasPeuckerRecursive(pointList, maxIndex, endIdx, epsilon, keepFlag); } // 否则中间所有点都不保留仅依靠首尾点 } std::vectorPoint douglasPeucker(const std::vectorPoint points, double epsilon) { if (points.size() 3) { return points; // 点太少无需简化 } std::vectorbool keepFlag(points.size(), false); keepFlag.front() true; // 始终保留起点 keepFlag.back() true; // 始终保留终点 douglasPeuckerRecursive(points, 0, points.size() - 1, epsilon, keepFlag); // 收集被标记保留的点 std::vectorPoint result; for (size_t i 0; i points.size(); i) { if (keepFlag[i]) { result.push_back(points[i]); } } return result; }版本二迭代实现推荐用于生产环境迭代版本使用一个栈来存储需要处理的线段用起止索引表示。std::vectorPoint douglasPeuckerIterative(const std::vectorPoint points, double epsilon) { if (points.size() 3) { return points; } std::vectorbool keepFlag(points.size(), false); keepFlag.front() true; keepFlag.back() true; // 使用栈存储待处理的线段索引对 std::stackstd::pairsize_t, size_t indexStack; indexStack.push({0, points.size() - 1}); while (!indexStack.empty()) { auto [startIdx, endIdx] indexStack.top(); indexStack.pop(); if (endIdx startIdx 1) { continue; } double maxDistance 0.0; size_t maxIndex startIdx; for (size_t i startIdx 1; i endIdx; i) { double distance perpendicularDistance(points[i], points[startIdx], points[endIdx]); if (distance maxDistance) { maxDistance distance; maxIndex i; } } if (maxDistance epsilon) { keepFlag[maxIndex] true; // 将右半段先压栈左半段后压栈保证处理顺序可选 indexStack.push({maxIndex, endIdx}); indexStack.push({startIdx, maxIndex}); } // 如果 maxDistance epsilon则不保留中间点该线段处理完毕 } // 收集结果 std::vectorPoint result; for (size_t i 0; i points.size(); i) { if (keepFlag[i]) { result.push_back(points[i]); } } return result; }为什么推荐迭代版本递归版本虽然直观但在最坏情况下例如一个“之”字形路径递归深度可能达到O(n)对于上万点的路径可能导致栈溢出。迭代版本显式管理栈内存避免了这个问题并且通常更容易进行性能分析和优化。在ROS节点中处理实时传感器数据流时我强烈建议使用迭代版本。3.3 Python实现简洁与实用Python的实现同样可以采用迭代方式利用列表作为栈代码非常简洁。def douglas_peucker(points: List[Point], epsilon: float) - List[Point]: if len(points) 3: return points[:] # 返回副本 # 初始化保留标记首尾点必留 keep [False] * len(points) keep[0] keep[-1] True # 使用栈存储待处理线段 (start_index, end_index) stack [(0, len(points) - 1)] while stack: start_idx, end_idx stack.pop() if end_idx - start_idx 2: # 中间无点 continue # 寻找最大距离点 max_dist 0.0 max_idx start_idx for i in range(start_idx 1, end_idx): dist perpendicular_distance(points[i], points[start_idx], points[end_idx]) if dist max_dist: max_dist dist max_idx i # 判断并分割 if max_dist epsilon: keep[max_idx] True # 先处理右段再左段保证最终点序与递归顺序一致 stack.append((max_idx, end_idx)) stack.append((start_idx, max_idx)) # 否则该线段内所有中间点均不保留 # 收集结果 return [points[i] for i, k in enumerate(keep) if k]4. ROS中的集成与应用实战在ROS中路径通常以nav_msgs/Path消息类型存在其核心是poses数组每个pose包含位置和姿态。DP算法通常只关心路径点的位置pose.position。此外从sensor_msgs/LaserScan转换为PointCloud或提取边界后得到的点集也是常见的输入。4.1 C ROS节点实现示例假设我们有一个订阅原始路径的节点处理后在RViz中发布简化后的路径。// douglas_peucker_node.cpp #include ros/ros.h #include nav_msgs/Path.h #include geometry_msgs/Point.h #include vector // 假设上述 Point 结构体和 douglasPeuckerIterative 函数已定义 class PathSimplifier { public: PathSimplifier() { // 订阅原始路径话题例如 “/global_plan” sub_ nh_.subscribe(/global_plan, 1, PathSimplifier::pathCallback, this); // 发布简化后的路径话题例如 “/global_plan_simplified” pub_ nh_.advertisenav_msgs::Path(/global_plan_simplified, 1); // 从参数服务器读取简化阈值默认值 0.1 米 nh_.param(simplification_epsilon, epsilon_, 0.1); ROS_INFO(Path simplifier initialized with epsilon: %.3f m, epsilon_); } void pathCallback(const nav_msgs::Path::ConstPtr msg) { if (msg-poses.empty()) { ROS_WARN(Received empty path.); return; } // 1. 提取路径点 (x, y) std::vectorPoint rawPoints; rawPoints.reserve(msg-poses.size()); for (const auto pose_stamped : msg-poses) { Point pt; pt.x pose_stamped.pose.position.x; pt.y pose_stamped.pose.position.y; rawPoints.push_back(pt); } // 2. 应用DP算法简化 std::vectorPoint simplifiedPoints douglasPeuckerIterative(rawPoints, epsilon_); // 3. 将简化结果包装回 nav_msgs/Path nav_msgs::Path simplifiedPath; simplifiedPath.header msg-header; // 保持时间戳和坐标系 for (const auto pt : simplifiedPoints) { geometry_msgs::PoseStamped pose_stamped; pose_stamped.header msg-header; pose_stamped.pose.position.x pt.x; pose_stamped.pose.position.y pt.y; pose_stamped.pose.position.z 0.0; // 假设是2D路径 pose_stamped.pose.orientation.w 1.0; // 无旋转 simplifiedPath.poses.push_back(pose_stamped); } // 4. 发布简化后的路径 pub_.publish(simplifiedPath); ROS_DEBUG(Simplified path from %zu to %zu points., rawPoints.size(), simplifiedPoints.size()); } private: ros::NodeHandle nh_; ros::Subscriber sub_; ros::Publisher pub_; double epsilon_; }; int main(int argc, char** argv) { ros::init(argc, argv, path_simplifier_node); PathSimplifier node; ros::spin(); return 0; }对应的CMakeLists.txt和package.xml需要添加对roscpp和nav_msgs的依赖。4.2 Python ROS节点实现示例使用rospy实现类似功能。#!/usr/bin/env python3 # douglas_peucker_node.py import rospy from nav_msgs.msg import Path from geometry_msgs.msg import PoseStamped, Point # 导入之前定义的 douglas_peucker 和 perpendicular_distance 函数 class PathSimplifierPy: def __init__(self): rospy.init_node(path_simplifier_py) self.epsilon rospy.get_param(~simplification_epsilon, 0.1) rospy.loginfo(fPath simplifier (Python) initialized with epsilon: {self.epsilon} m) self.pub rospy.Publisher(/global_plan_simplified, Path, queue_size1) rospy.Subscriber(/global_plan, Path, self.path_callback) def path_callback(self, msg): if not msg.poses: rospy.logwarn(Received empty path.) return # 提取点 raw_points [(p.pose.position.x, p.pose.position.y) for p in msg.poses] # 简化 simplified_tuples douglas_peucker(raw_points, self.epsilon) # 重建Path消息 simplified_path Path() simplified_path.header msg.header for x, y in simplified_tuples: pose_stamped PoseStamped() pose_stamped.header msg.header pose_stamped.pose.position.x x pose_stamped.pose.position.y y pose_stamped.pose.position.z 0.0 pose_stamped.pose.orientation.w 1.0 simplified_path.poses.append(pose_stamped) self.pub.publish(simplified_path) rospy.logdebug(fSimplified path from {len(raw_points)} to {len(simplified_tuples)} points.) def run(self): rospy.spin() if __name__ __main__: node PathSimplifierPy() node.run()4.3 在launch文件中配置参数你可以轻松地在launch文件中调整简化阈值以适应不同的场景或机器人。!-- simplify_path.launch -- launch node namepath_simplifier_cpp pkgyour_package typedouglas_peucker_node outputscreen param namesimplification_epsilon value0.05 / !-- 5厘米阈值 -- /node !-- 或者运行Python版本 -- !-- node namepath_simplifier_py pkgyour_package typedouglas_peucker_node.py outputscreen param namesimplification_epsilon value0.05 / /node -- /launch5. 性能优化、边界情况与进阶话题基础的DP算法时间复杂度为O(n²)最坏情况因为每一层递归都需要遍历当前线段内的所有点来寻找最远点。对于非常大的点集例如高分辨率地图轮廓这可能成为瓶颈。5.1 性能优化技巧提前计算距离在递归/迭代过程中同一个点到不同线段的距离会被重复计算。一种优化是预先计算并存储点集的一些几何属性但实现较复杂通常收益有限。使用空间索引对于特别大的点集可以考虑使用R-tree等空间索引结构来加速“寻找距离线段最远的点”这一步但这会大大增加实现复杂度且仅对点集极度密集且分布特殊的场景有显著提升。对于大多数机器人应用路径点数在几百到几千朴素的O(n²)算法完全够用。设定最小点数如果某段路径的点数已经少于一个阈值例如3个点可以直接返回而不进行进一步简化避免无谓的计算。并行化递归的左右分支是独立的理论上可以并行处理。但在实际ROS节点中由于数据量通常不至于大到需要并行且并行会引入同步复杂度所以较少使用。我的经验是在Intel i7处理器上用C简化一条包含5000个点的路径即使是最朴素的O(n²)实现耗时也通常在几毫秒以内完全满足10Hz甚至更高的实时性要求。过早优化是万恶之源除非性能分析明确显示这里是瓶颈否则建议先从清晰可靠的实现开始。5.2 常见边界情况与处理闭合路径多边形DP算法处理闭合路径时需要小心对待首尾点。一种常见做法是在闭合路径上任意选取一个起点将其重复添加到点列末尾使其“打开”应用标准DP算法后再去掉重复的终点。更严谨的做法是修改算法将首尾点视为线段连接并在递归过程中正确处理环形索引。共线点如果大量点共线算法会高效地剔除几乎所有中间点只保留首尾这是符合预期的。阈值Epsilon为0如果设置epsilon0算法理论上会保留所有点。但在浮点数计算中由于精度问题一些本应共线的点可能因极微小的误差而被保留。如果确实需要保留所有点直接返回原路径即可无需调用算法。Z轴3D路径标准DP算法是定义在二维平面的。对于3D路径一种做法是投影到主要平面如XY平面上计算距离另一种是计算点到3D线段的垂直距离原理类似但计算更复杂。在机器人导航中许多路径本质是2D的高度信息单独处理所以2D简化通常足够。5.3 与其它简化算法的对比DP算法不是唯一的路径简化算法。了解它的替代方案有助于你在不同场景做出选择。算法核心思想优点缺点适用场景Douglas-Peucker递归分治保留偏离大的点保形性极好参数意义直观结果稳定最坏时间复杂度O(n²)对噪声敏感需要高保真形状的场合如地图轮廓、矢量图形Visvalingam-Whyatt迭代移除面积贡献最小的点在视觉感知上有时更自然复杂度O(n log n)参数面积阈值不如距离阈值直观地图制图综合视觉简化Ramer与DP算法完全相同是同一算法的不同名称同DP同DP同DP垂距限值法遍历所有点若当前点到前两个保留点连线的垂距超限则保留单次遍历O(n)复杂度快保形性较差尤其在拐角处可能过度简化对速度要求极高、保形要求不高的实时滤波角度阈值法根据连续三点形成的转角大小决定是否保留中间点计算简单O(n)复杂度对角度噪声敏感可能保留抖动点初步的、快速的去噪如何选择在机器人领域如果你简化的是全局规划器产生的路径或激光雷达提取的精确环境轮廓对形状保真度要求高那么DP算法通常是首选。它的结果可预测且通过调整一个物理意义的参数epsilon可以很方便地在计算开销和路径精度之间取得平衡。如果你处理的是高频的机器人实时轨迹需要进行在线平滑和简化垂距法或角度法可能因其速度优势而被考虑但需要接受其保形能力的下降。6. 实战在ROS导航栈中的应用与问题排查让我们看一个具体的集成案例在move_base全局规划器生成的路径后添加一个简化节点。场景move_base使用global_planner如navfn或global_planner规划出一条从起点到目标点的路径/move_base/NavfnROS/plan类型为nav_msgs/Path。这条路径可能包含很多紧密排列的点直接发给局部规划器如dwa_local_planner可能不是最优的。做法我们编写一个节点如上文的PathSimplifier订阅/move_base/NavfnROS/plan应用DP算法简化后发布到新话题例如/move_base_simplified/plan。然后我们可以配置move_base的global_plan话题指向这个简化后的路径。launch文件配置示例launch node pkgmove_base typemove_base respawnfalse namemove_base outputscreen remap frommove_base_simplified/plan to/move_base/NavfnROS/plan / !-- 让move_base订阅简化后的路径 -- ... !-- 其他move_base参数 -- /node node pkgyour_simplifier_pkg typepath_simplifier_node namepath_simplifier outputscreen remap from/global_plan to/move_base/NavfnROS/plan / !-- 订阅原始路径 -- remap from/global_plan_simplified to/move_base_simplified/plan / !-- 发布简化路径 -- param namesimplification_epsilon value0.1 / /node /launch6.1 常见问题与排查技巧在实际集成中你可能会遇到以下问题简化后路径出现“跳跃”或穿过障碍物原因epsilon值设置过大在急转弯处丢弃了关键点导致连接关键点的线段穿过了障碍物。排查在RViz中同时显示原始路径绿色和简化路径红色观察“跳跃”发生的位置。通常发生在狭窄走廊的拐角处。解决减小epsilon值。一个经验法则是epsilon应小于机器人半径与障碍物间最小安全距离的一半。也可以考虑采用自适应epsilon在路径曲率大的区域使用更小的阈值。简化效果不明显点数没有显著减少原因epsilon值设置过小或者原始路径本身噪声很大很多微小抖动。排查检查原始路径。如果是规划器输出的路径通常本身比较平滑。如果是传感器提取的轮廓可能包含大量噪声。解决适当增大epsilon。对于含噪声的路径务必先进行平滑滤波如高斯滤波、滑动平均再进行简化。DP算法对噪声敏感一个噪声点可能导致其所在线段被不必要的分割。算法在处理长路径时卡顿原因点集规模极大10000且路径形状复杂触发了最坏的O(n²)复杂度。排查使用ros::Time或Python的time模块对算法函数进行计时。解决降采样预处理先对原始路径进行均匀降采样减少点数后再用DP算法。分段处理将长路径分成若干段分别简化后再拼接。切换到迭代版本确保你使用的是迭代栈版本避免递归深度过大。简化后路径的朝向orientation问题现象nav_msgs/Path中的pose包含朝向四元数但我们的简化只处理了位置。简化后中间点的朝向信息丢失了。解决这是常见情况。局部规划器通常更关注位置序列。如果需要朝向可以在简化后根据简化路径上相邻点的连线重新计算每个点的偏航角yaw。for (size_t i 0; i simplifiedPath.poses.size(); i) { double yaw 0.0; if (i simplifiedPath.poses.size() - 1) { // 计算指向下一个点的方向角 double dx simplifiedPath.poses[i1].pose.position.x - simplifiedPath.poses[i].pose.position.x; double dy simplifiedPath.poses[i1].pose.position.y - simplifiedPath.poses[i].pose.position.y; yaw atan2(dy, dx); } else { // 最后一个点朝向与倒数第二个点相同 yaw ... // 取自前一个点或设为0 } // 将yaw转换为四元数并赋值 tf2::Quaternion q; q.setRPY(0, 0, yaw); simplifiedPath.poses[i].pose.orientation tf2::toMsg(q); }一个实用的调试技巧在RViz中创建两个Path显示分别订阅原始和简化后的路径话题用不同颜色区分。同时开启Pose显示来查看关键点的位置。这能让你最直观地评估简化效果并快速调整epsilon参数。记住好的简化应该像给路径做“减法”减去冗余保留灵魂让机器人走得更“聪明”而不是更“冒险”。