costmap 中点与多边形的相对位置关系

一、背景

近期在阅读move_base源码costmap部分(感觉想玩转movebase导航，costmap必须理解呀)。读到两处点与多边形的相对位置关系。在此总结一下，分别是:

• intersects: 利用待测点向右引出的射线与多边形的交点数来确定相对位置。有的称 射线交叉算法，也有称PNPoly算法；
• ptInPolygon: 利用点和多边形的边的方向关系来确定相对位置；

二、intersects

1. 原理思路

以待测点引出射线穿过多边形，若点在多边形外，则射线与多边形交点数目一定为偶数(不考虑射线仅过多边形角点)；若点在多边形内，则射线与多边形交点数目一定为奇数；

2、代码分析

前提条件: 从待测点引出 向右的水平射线，与多边形相交进行交点数目判断。
函数位置: navigation/costmap_2d/src/costmap_math.cpp

/*** @brief 确定一个点 (testx,testy)是否在一个多边形顶点集polygon 内部。该函数使用的是射线交叉算法（Ray-Casting Algorithm）或PNPoly算法，该算法通过计算从点到无穷远的射线与多边形边的交点数量来判断点的内部性* @date 2025/01/14* @return 返回该点是否与多边形相交
**/
bool intersects(std::vector<geometry_msgs::Point>& polygon, float testx, float testy)
{// 记录交点的奇偶性，初始值为 false 此时表示无交点(交叉判断后也可以理解为偶数个交点)， true为奇数个交点bool c = false;int i, j, nvert = polygon.size();  // nvert 是多边形顶点的数量// 循环遍历多边形的每一条边，j 被初始化为最后一个顶点的索引，以便在循环中连接最后一个顶点和第一个顶点for (i = 0, j = nvert - 1; i < nvert; j = i++){// yi 和 yj 分别是当前顶点和前一个顶点的 y 坐标// xi 和 xj 分别是当前顶点和前一个顶点的 x 坐标float yi = polygon[i].y, yj = polygon[j].y, xi = polygon[i].x, xj = polygon[j].x;// 判断当前边是否与测试点的 向右 引出的水平射线相交if (((yi > testy) != (yj > testy)) && (testx < (xj - xi) * (testy - yi) / (yj - yi) + xi))// 若以上条件都满足，说明射线与多边形的当前边相交，因此通过 c = !c 切换 c 的值，追踪交点的数量c = !c;}return c;
}

代码重点：

    // 判断当前边是否与测试点的 向右 引出的水平射线相交if (((yi > testy) != (yj > testy)) && (testx < (xj - xi) * (testy - yi) / (yj - yi) + xi))// 若以上条件都满足，说明射线与多边形的当前边相交，因此通过 c = !c 切换 c 的值，追踪交点的数量c = !c;

首先，(yi > testy) != (yj > testy) 表示检查当前边的两个端点是否分别在测试点的上下方(即两个端点的 y坐标相对于 testy 的位置是否不同)。如果返回 true，说明一端在上方，一端在下方，可能会有交点；若为false，则在线段的上方或者下方，说明不存在交点；
接着，分析条件判断 testx < (xj - xi) * (testy - yi) / (yj - yi) + xi

如下图所示，待测点$(x_t,y_t)$ 红点为待测点，$(x_0,y_0)$ 到$(x_3,y_3)$多边形角点，蓝点$(x_{t1},y_t)$、绿点$(x_{t2},y_t)$假想分别是 向右水平射线 与多边形相交两个被判断点。
蓝点$(x_{t1},y_t)$在以$(x_2,y_2)$ 和$(x_3,y_3)$为端点的线段上，则
$$\frac{x_{t1}-x_3}{y_t-y_3}=\frac{x_2-x_3}{y_2-y_3}$$
整理得
$$x_{t1}=\frac{x_2-x_3}{y_2-y_3}\times (y_t-y_3)+x_3$$
此时，因为蓝点本身在以待测点$(x_t,y_t)$引出的向右水平射线上，所以，只需要验证 $x_t<x_{t1}$ ，即 以待测点$(x_t,y_t)$引出的向右水平射线与以$(x_2,y_2)$ 和$(x_3,y_3)$为端点的线段存在交点。此处，便是代码中的 testx < (xj - xi) * (testy - yi) / (yj - yi) + xi。存在交点，则状态位c取反一次，通过循环检索待测点$(x_t,y_t)$ 与多条边的是否存在交点，转换成状态位c最终状态，来确定待测点与多边形交点的奇偶数。进而判断待测点与多边形的相对位置关系。

三、ptInPolygon

1. 原理思路

待测点总是在 顺次连接 的多边形线段一侧，则该待测点在多边形的外部，否则在多边形的内部。

2. 代码分析

代码位置：navigation/base_local_planner/src/point_grid.cpp

/*** @brief 用于判断一个点是否在给定的多边形内部。*        利用点和多边形的边的方向关系来判断点是否在多边形内部。如果点与多边形的每条边的方向关系都相同（即在多边形的左侧或右侧），则点被认为在多边形内部* @date 2025/01/14
**/bool PointGrid::ptInPolygon(const geometry_msgs::Point32& pt, const std::vector<geometry_msgs::Point>& poly){// 代码检查给定的多边形是否至少有3个顶点。如果顶点数小于3，则返回false，表示无法构成多边形，因此点不可能在多边形内部if(poly.size() < 3)return false;//a point is in a polygon iff the orientation of the point//with respect to sides of the polygon is the same for every//side of the polygonbool all_left = false;bool all_right = false;// 迭代多边形的每一条边来判断点的位置关系for(unsigned int i = 0; i < poly.size() - 1; ++i){//if pt left of a->b// 点在某条边的左侧，则标记为在边的左侧if(orient(poly[i], poly[i + 1], pt) > 0){if(all_right)return false;all_left = true;}//if pt right of a->b// 点在右侧，则标记为在边的右侧else{if(all_left)return false;all_right = true;}}//also need to check the last point with the first point// 检查最后一个点与第一个点之间的边的方向关系if(orient(poly[poly.size() - 1], poly[0], pt) > 0){if(all_right)return false;}else{if(all_left)return false;}return true;}

其中，orient(poly[i], poly[i + 1], pt) > 0 的orient函数解析如下

      /*** @brief  Check the orientation of a pt c with respect to the vector a->b. 即，判断点c 相对于线段ab 的位置（即点 c 位于线段ab 的左侧、右侧还是在线段上）* @param a The start point of the vector         表示线段的一端点* @param b The end point of the vector           表示线段的另一端点* @param c The point to compute orientation for  表示待判断的点* @return orient(a, b, c) < 0 ----> Right, orient(a, b, c) > 0 ----> Left */inline double orient(const geometry_msgs::Point& a, const geometry_msgs::Point& b, const geometry_msgs::Point32& c){double acx = a.x - c.x;         // acx 是从点c 到点a 的 x 方向分量double bcx = b.x - c.x;         // bcx 是从点c 到点b 的 x 方向分量double acy = a.y - c.y;         // acy 是从点c 到点a 的 y 方向分量double bcy = b.y - c.y;         // bcy 是从点c 到点b 的 y 方向分量/*acx * bcy - acy * bcx 计算的结果是两个向量的叉积（Cross Product）叉积的几何意义：两个向量的叉积的绝对值等于这两个向量所构成的平行四边形的面积。叉积的符号指示了这两个向量的旋转方向（顺时针或逆时针）> 0，点 c 位于线段 ab 的左侧。< 0，点 c 位于线段 ab 的右侧。= 0，点 c 在线段 ab 上。*/return acx * bcy - acy * bcx;}```