描述

在自动驾驶与机器人导航中，我们经常会使用到栅格地图：利用SLAM技术将车在一定范围内进行移动，依靠视觉或雷达传感器，对区域内的障碍物进行感知，得到一个指定分辨率指定大小的栅格地图，每个像素点代表了被占据的概率。随后在栅格地图上，我们可以进行例如A*这样的路径规划算法。

而在实际工程中，栅格地图有诸多不变。首先就是构建和维护的困难，本人之前工作中遇到了很多工业的场景，机器人在建图后短时间内栅格地图可用，但由于场景的多变性（例如货物的移动、场景的变化），导致了栅格地图在一段时间后准确率大大下降。对于这种情况，往往需要机器人重新构建地图，或者在机器人运行过程中实时建图才可以解决。显然，这是费时耗力的。

而在解决栅格地图维护困难的方法中，实时构建地图的方法揭示了另外一种解决思路，也是目前自动驾驶领域的一个特点：重感知轻地图。而这篇文章会介绍这一理念的核心概念及相关知识：矢量地图

矢量地图特点

矢量地图和栅格地图的主要区别，在于它们描述地理数据方式的不同。矢量地图使用点、线和多边形来表示特征，而栅格地图使用像素网格来表示相同的特征。在矢量地图中，每个特征都存储为具有自己属性的单独对象，而在栅格地图中，所有特征都由相同类型的数据表示。矢量地图通常更准确、更灵活，而栅格地图则更快、更容易显示。

总结起来，矢量地图相较于栅格具有的优势：

1. 矢量地图能够轻松地表达目标属性，当然栅格地图也可以采用一定机制来表达，不过要复杂得多
2. 矢量地图目标具有空间不变性，不会收到地图分辨率等参数的影响，而栅格地图要随分辨率而变化

而从视觉感知及路径规划的角度来将，矢量地图的优势在于

1. 数据量小，便于存储
2. 矢量可以包含语义信息，利于进一步算法开发
3. 地图生成与维护较为容易
   

常见的矢量地图格式

struct map_vector
{
    float p1_x; // 矢量两端点坐标
    float p1_y;
    float p2_x;
    float p2_y;
    int label; // 类别
    float confidence; // 置信度
}
std::vector<map_vector> vector_map

矢量地图更新

有几种矢量地图更新模式可以用于自动驾驶。

一种常见的方法是在车辆行驶时使用来自车辆的实时传感器数据来更新地图。这可能涉及融合来自激光雷达、雷达和相机等多个传感器的数据，以创建更准确和最新的地图。另一种方法是使用之前驾驶期间收集的数据或从外部来源（如卫星图像或众包数据）定期更新地图。值得注意的是，所使用的具体更新模式将取决于各种因素，如所使用的自动驾驶系统的类型、所处的环境等。而由于本人的知识领域，我们只来讨论通过传感器生成矢量地图的方式。

融合来自多个传感器的数据并生成矢量图的一些常见方法：包括卡尔曼滤波、粒子滤波和贝叶斯网络。这些方法包括组合来自多个传感器的测量值，以估计环境状态并生成地图。而多传感器数据融合，又包括前融合、特征级融合、后融合等。之前写了两篇文章介绍了一种简单的特征融合方法——匈牙利匹配。文章链接如下：

多传感器特征融合——匈牙利匹配算法原理（一）
多传感器特征融合——KM算法原理及C++代码实现（二）

之后还是会继续介绍一些方法，用于多传感器特征融合。下面我们来介绍一些常见的特征处理方法，这些代码可以在任何的融合算法中使用。

常见特征处理方法

针对矢量地图的生成，还是写下一些插件式的代码，毕竟我不能写下太核心的算法实现代码。透过这些插件代码，实际上可以实现一些更深的需求，进而实现在线生成矢量地图的简单诉求。如果对此感兴趣，可以进一步去阅读相关的论文和源码，包括VectorNet、HDMapNet等，之后我也会再写文章分析。

1. 求射线点

沿图像中心点到点A的射线，延长固定距离，得到另外一个点B。如果该点超过图像边缘，会返回图像的边缘。

cv::Point getVectorRectPoint(const cv::Mat& image, const cv::Point& A, double dist) {
    // Create a rectangle that encompasses the image
    cv::Rect rect(0, 0, image.cols, image.rows);

    // Extend a line from the center of the image to point A by a distance of dist
    cv::Point center(image.cols / 2, image.rows / 2);

    cv::Point B = A + (A - center) * (dist / cv::norm(A - center));

    // cv::line(image, center, B, cv::Scalar(255, 255, 255), 1, cv::LINE_AA);

    // Clip the line to the boundaries of the image
    cv::Point pt1(B), pt2(center);
    cv::clipLine(rect, pt1, pt2);

    // Find the intersection point between the clipped line and the image boundary
    double x = pt1.x;
    double y = pt1.y;
    if (pt1.x < 0) {
        x = 0;
        y = pt1.y - pt1.x * (pt1.y - pt2.y) / (pt1.x - pt2.x);
    }
    if (pt1.y < 0) {
        y = 0;
        x = pt1.x - pt1.y * (pt1.x - pt2.x) / (pt1.y - pt2.y);
    }
    if (pt1.x >= image.cols) {
        x = image.cols - 1;
        y = pt1.y + (image.cols - 1 - pt1.x) * (pt1.y - pt2.y) / (pt1.x - pt2.x);
    }
    if (pt1.y >= image.rows) {
        y = image.rows - 1;
        x = pt1.x + (image.rows - 1 - pt1.y) * (pt1.x - pt2.x) / (pt1.y - pt2.y);
    }

    // Return the pixel value at the intersection point
    return cv::Point(x, y);
}

2. 统计区域内像素值的众数

输入一张灰度图，及图像中的任意4个点。这4个点构成一个最大矩形，求出该矩形中像素值的众数。

int find_mode(const cv::Mat& image, const cv::Point& A, const cv::Point& B, const cv::Point& C, const cv::Point& D) {
    // Create a rectangle that encompasses the region defined by the four points
    cv::Rect rect(std::min({A.x, B.x, C.x, D.x}), std::min({A.y, B.y, C.y, D.y}),
                  std::max({A.x, B.x, C.x, D.x}) - std::min({A.x, B.x, C.x, D.x}),
                  std::max({A.y, B.y, C.y, D.y}) - std::min({A.y, B.y, C.y, D.y}));

    // Reshape the image to a 1D array of pixel values within the rectangle
    cv::Mat roi = image(rect);

    cv::imshow("roi", roi);
    int count = 0;
    for (int i = 0; i < roi.rows; i++) {
        for (int j = 0; j < roi.cols; j++) {
            std::cout<<int(roi.at<uchar>(i, j))<<" ";
            if (int(roi.at<uchar>(i, j)) == 95) {
                std::cout<<i<<" "<<j<<std::endl;
                count++;
            }
        }
        std::cout<<std::endl;
    }
    std::cout<<count<<std::endl;

    // Calculate the histogram of the pixel values
    int histSize = 256;
    float range[] = { 0, 256 };
    const float* histRange = { range };
    cv::Mat hist;
    cv::calcHist(&roi, 1, 0, cv::Mat(), hist, 1, &histSize, &histRange);

    // Find the mode of the histogram
    double minVal, maxVal;
    cv::Point minLoc, maxLoc;
    cv::minMaxLoc(hist, &minVal, &maxVal, &minLoc, &maxLoc);

    std::cout<<"minVal: "<<minVal<<" "<<minLoc.x<<" "<<minLoc.y<<std::endl;
    std::cout<<"maxVal: "<<maxVal<<" "<<maxLoc.x<<" "<<maxLoc.y<<std::endl;

    return maxLoc.y;
}

3. 计算两矢量的IOU

#include <cmath>
#include <opencv2/core.hpp>

cv::Vec3f circle_from_diameter(const cv::Point2f& p1, const cv::Point2f& p2) {
    cv::Point2f center = cv::Point2f((p1.x + p2.x) / 2.0, (p1.y + p2.y) / 2.0);
    double radius = cv::norm(p1 - p2) / 2.0;
    return cv::Vec3f(center.x, center.y, radius);
}

double calculate_intersection_area(const cv::Vec3f& c1, const cv::Vec3f& c2) {
    double d = cv::norm(cv::Vec2f(c1[0], c1[1]) - cv::Vec2f(c2[0], c2[1]));
    if (d > c1[2] + c2[2]) { // 两圆相离
        return 0.0;
    } else if (d <= std::abs(c1[2] - c2[2])) { // 大圆包小圆
        double r = std::min(c1[2], c2[2]);
        return M_PI * r * r;
    } else { // 两圆相交
        double a1 = std::acos((c1[2]*c1[2] + d*d - c2[2]*c2[2]) / (2.0*c1[2]*d));
        double a2 = std::acos((c2[2]*c2[2] + d*d - c1[2]*c1[2]) / (2.0*c2[2]*d));
        double area1 = c1[2]*c1[2]*a1 - c1[2]*c1[2]*std::sin(2.0*a1)/2.0;
        double area2 = c2[2]*c2[2]*a2 - c2[2]*c2[2]*std::sin(2.0*a2)/2.0;
        return area1 + area2;
    }
}

double calculate_union_area(const cv::Vec3f& c1, const cv::Vec3f& c2) {
    double intersection_area = calculate_intersection_area(c1, c2);
    double area1 = M_PI * c1[2] * c1[2];
    double area2 = M_PI * c2[2] * c2[2];
    return area1 + area2 - intersection_area;
}

double calculate_iou(const cv::Vec3f& c1, const cv::Vec3f& c2) {
    double intersection_area = calculate_intersection_area(c1, c2);
    double union_area = calculate_union_area(c1, c2);
    return intersection_area / union_area;
}

4. 非极大值抑制

为了表示空间中两条线段之间的相似性，可以使用并集交集（IoU）概念。IoU是两个区域之间重叠的度量，通常用于对象检测任务。在这种情况下，代码计算两个线段之间的IoU，如果它大于某个阈值，则第二个线段被认为与第一个线段相似，并从线段列表中删除。因此，如果两个线段具有高IoU值，则可以认为它们是相似的。

代码按照置信度的降序对向量进行排序，然后遍历每个元素。对于每个元素，它将其与向量中在其之后的所有元素进行比较，并移除与当前元素的IoU大于阈值的任何元素。

// 首先，将矢量按照其置信度从高到低排序
std::sort(vector.begin(), vector.end(), [](const Detection& a, const Detection& b) {
    return a.confidence > b.confidence;
});

// 然后，遍历每个矢量
for (int i = 0; i < vector.size(); ++i) {
    // 对于当前矢量，遍历其后面的所有矢量
    for (int j = i + 1; j < vector.size(); ++j) {
        // 如果当前矢量与后面的某个矢量的 IoU 大于阈值，则将后面的矢量从矢量列表中删除
        if (calculate_iou(vector[i], vector[j]) > iou_threshold) {
            vector.erase(vector.begin() + j);
            --j;
        }
    }
}

5. 坐标变换

以下代码实现这样的功能：
将pose1坐标系下的点point，转换到pose2坐标系下，返回在pose2坐标系下的位置

cv::Point2f transfer_pointCoordinate(cv::Point2f point, PoseParams pose1, PoseParams pose2)
{
    float x_world = cos(pose1.yaw) * point.x - sin(pose1.yaw) * point.y + pose1.x;
    float y_world = sin(pose1.yaw) * point.x + cos(pose1.yaw) * point.y + pose1.y;

    float x_in2 =  cos(pose2.yaw) * (x_world-pose2.x) + sin(pose2.yaw) * (y_world-pose2.y);
    float y_in2 = -sin(pose2.yaw) * (x_world-pose2.x) + cos(pose2.yaw) * (y_world-pose2.y);

    cv::Point2f output;
    output.x = x_in2;
    output.y = y_in2;

    return output;
}