描述

如题。一年前我写过另外一篇位姿变换的文章，但最近事情太多，脑子乱的很。而我又面临一些比较重要的开发任务，其中会涉及到一些简单的坐标变换。但坐标系有很多，导致我写代码时总得再花点时间来确定什么基坐标系啊自定义坐标系。

于是，我决定再写一篇更直白的文章，以节省以后的时间。这篇文章会涉及到自动驾驶或移动机器人领域可能遇到的坐标变换场景，对于变换后的数据点或特征点，再介绍一些好用的处理办法。

文章主要的逻辑是这样的：

BEV感知是什么——感知结果用矢量地图会更好——矢量地图需要二维特征点——二维特征点需要变换——特征点处理算法——处理后的点就可以生成矢量了

文章下，使用“车”，来表示平面内有朝向的车或机器人

一、BEV感知的二维特征点

首先来简单介绍一下什么是BEV感知。

BEV感知（Bird’s Eye View Perception）是一种用于自动驾驶汽车的感知系统，用于提供车辆周围自上而下的视图。该系统用于检测和跟踪车辆路径中的行人、车辆和障碍物等物体。

BEV图往往是利用四路环视鱼眼图，经过内外参标定后拼接而成。对于拼接后的BEV视图，可以利用深度学习进行语义分割。分割后的BEV视图，通过计算机视觉算法可以提取出车辆、行人等障碍物的外轮廓。利用这些特征，我们可以生成视觉矢量地图，提供给下游的规划算法使用。

那么，什么是矢量地图？

栅格地图与矢量地图

栅格地图，是一种由单元格网格组成的地图，其中每个单元格代表地图的一小部分。为每个单元指定一个值，该值表示地图的一个特征，例如海拔、位置占用或植被。

矢量地图是由点、线和多边形组成的地图，用于表示地图上的特征。每个特征都由一组坐标表示，这些坐标定义了其形状和位置。矢量地图通常用于表示离散数据，如道路、建筑物和其他基础设施。

就优势而言，矢量地图比栅格地图具有以下优势：

1.可缩放性：矢量地图可以无限缩放，而不会丢失分辨率。这意味着它们可以放大以显示精细的细节，而不会像素化。

2.存储空间较小：矢量地图元素的存储大小通常小于栅格地图，这使得它们在自动驾驶系统运行中更容易存储和传输。

3.灵活性：矢量地图可以很容易地编辑和更新，这使它们比栅格地图更灵活。例如，如果添加或删除道路，可以在矢量地图上轻松更新

4.更好的标记：矢量地图可以更好地标记特征，因为可以精确的标注元素的语义类别，并可以轻松地更新

二、坐标变换意义与类型

自动驾驶的关键挑战之一是准确感知和解释车辆周围环境的能力。这需要使用各种传感器，如相机、激光雷达和雷达，来捕捉周围环境的数据。然而，这些传感器捕获的数据通常位于与车辆自身坐标系不同的坐标系中。这意味着必须将数据转换到车辆的坐标系中，以便对导航和控制有用。

上面提到的BEV感知，通过视觉算法我们可以在每一帧都提取出特征点。如果在时间序列上，我们想利用这些特征点，我们就无法避免的要使用坐标变换了。

自动驾驶中常用的坐标变换有几种类型，包括：

1. 平移

平移包括在同一坐标系中将对象从一个位置移动到另一个位置。在自动驾驶中，平移通常用于对齐激光雷达和相机等不同传感器捕获的数据，使它们处于同一坐标系中。

2. 旋转

旋转包括围绕同一坐标系中的固定点旋转对象。在自动驾驶中，旋转通常用于对齐安装在车辆上不同角度的传感器捕获的数据。

3. 齐次变换

齐次变换包括将平移和旋转组合成单个变换矩阵。在自动驾驶中我们会用到齐次变换，将数据从传感器坐标系转换到车辆坐标系。

为了执行这些坐标变换，使用了各种数学技术，如矩阵乘法和四元数旋转。这些技术是在软件库中实现的，例如C++的Egengen库和Python的NumPy库。

二维数据坐标变换的常见场景

情况1

有一个世界坐标系，它的原点是（0，0）。车在位置（x_{car}, y_{car}）上，朝向为 \theta。以当前车的位姿作为一个坐标系，在车坐标系的（x, y）位置存在一个点。则这个点在世界坐标系的位置（x_{world}, y_{world}）为

情况2

有一个世界坐标系，它的原点是（0，0）。车有一个位置1和位置2，在位置1的位姿是（x_1, y_1, \theta_1），在位置2的位姿是（x_2, y_2, \theta_2）, 如果在位置1的坐标系下有一个点P，它的坐标值为（x, y）

实际上，情况2就是情况1的一种变形

为了计算P点在位置2坐标系下的位置（x’, y’），我们需要首先按照情况1来计算P点在世界坐标系下的位置（x_{world}, y_{world}）

P点在位置2坐标系下的位置（x’, y’），如下：

python代码

写一个python代码，来验证坐标变换

import matplotlib.pyplot as plt
plt.title('Calibration')
plt.xlabel('x (m)')
plt.ylabel('y (m)')
plt.axis('equal')

# 坐标轴长度
coord_length = 0.2

# 两个车辆位姿
x1 = 1
y1 = 4
theta1 = 0.1
print("car pose1: ["+str(x1), ", "+str(y1) + ", "+str(theta1)+"]")
x2 = 3
y2 = 5
theta2 = 1.3
print("car pose2: ["+str(x2), ", "+str(y2) + ", "+str(theta2)+"]")

# 位姿1下有一个点p
x = 2
y = 1
print("["+str(x), ", "+str(y) + "] in pose1")

# 画位姿1的坐标轴
x_cord = math.cos(theta1) * coord_length + x1
y_cord = math.sin(theta1) * coord_length + y1
plt.plot([x1, x_cord], [y1, y_cord], 'r')
x_cord = math.cos(theta1 + math.pi/2) * coord_length + x1
y_cord = math.sin(theta1 + math.pi/2) * coord_length + y1
plt.plot([x1, x_cord], [y1, y_cord], 'b')
# 画位姿2的坐标轴
x_cord = math.cos(theta2) * coord_length + x2
y_cord = math.sin(theta2) * coord_length + y2
plt.plot([x2, x_cord], [y2, y_cord], 'r')
x_cord = math.cos(theta2 + math.pi/2) * coord_length + x2
y_cord = math.sin(theta2 + math.pi/2) * coord_length + y2
plt.plot([x2, x_cord], [y2, y_cord], 'b')

# p点在世界坐标系下的位置
x_world = math.cos(theta1) * x - math.sin(theta1) * y + x1
y_world = math.sin(theta1) * x + math.cos(theta1) * y + y1
print("["+str(x_world), ", "+str(y_world) + "] in world")
plt.plot(x_world, y_world, 'r*')

# p点在位姿2下的位置
x_ = math.cos(theta2) * (x_world-x2) + math.sin(theta2) * (y_world-y2)
y_ = -math.sin(theta2) * (x_world-x2) + math.cos(theta2) * (y_world-y2)
print("["+str(x_), ", "+str(y_) + "] in pose2")

# 用计算出来的p点位姿2下的位置，来再算出在世界坐标系下的位置
x_world_ = math.cos(theta2) * x_ - math.sin(theta2) * y_ + x2
y_world_ = math.sin(theta2) * x_ + math.cos(theta2) * y_ + y2
print("use 2 to check ["+str(x_world_), ", "+str(y_world_) + "]")
plt.plot(x_world_, y_world_, 'b*')

plt.show()

三、特征点处理算法

1. Ramer-Douglas-Peucker

Ramer-Douglas-Peucker，又称拉默-道格拉斯-普克算法
道格拉斯算法是一种直线简化算法，可以在保持曲线形状的同时减少曲线中的点数。

它的工作原理是递归地将曲线划分为更小的线段，并用一条线近似每个线段。然后，该算法检查原始曲线和近似直线之间的距离。如果距离大于指定的公差，则算法会细分线段并重复该过程。如果距离小于公差，则算法会删除中间点，然后移动到下一个线段。

关于道格拉斯算法的具体实现过程，不在此赘述。来一版可以直接使用的C++代码，里面使用了递归。

// Define a function to calculate the distance between two points
float distance(cv::Point2f p1, cv::Point2f p2) {
    return std::sqrt(std::pow(p2.x - p1.x, 2) + std::pow(p2.y - p1.y, 2));
}

// Define a function to find the point with the maximum distance from a line segment
cv::Point2f find_furthest_point(std::vector<cv::Point2f> points, cv::Point2f p1, cv::Point2f p2) {
    float max_distance = 0;
    cv::Point2f furthest_point;
    for (auto point : points) {
        float current_distance = std::fabs((point.y - p1.y) * (p2.x - p1.x) - (point.x - p1.x) * (p2.y - p1.y)) / distance(p1, p2);

        if (current_distance > max_distance) {
            max_distance = current_distance;
            furthest_point = point;
        }
    }
    return furthest_point;
}

// Define the Douglas-Peucker algorithm function
void douglas_peucker(std::vector<cv::Point2f> points, float epsilon, std::vector<cv::Point2f>& simplified_points) {
    // Find the point with the maximum distance from the line segment
    float max_distance = 0;
    int furthest_index;

    cv::Point2f p1 = points[0];
    cv::Point2f p2 = points.back();
    for (int i = 1; i < points.size(); i++) {
        float current_distance = std::fabs((points[i].y - p1.y) * (p2.x - p1.x) - (points[i].x - p1.x) * (p2.y - p1.y)) / distance(p1, p2);

        if (current_distance > max_distance) {
            max_distance = current_distance;
            furthest_index = i;
        }
    }

    // If the maximum distance is greater than epsilon, recursively simplify the two sub-lines
    if (max_distance > epsilon) {
        std::vector<cv::Point2f> left_points(points.begin(), points.begin() + furthest_index + 1);
        std::vector<cv::Point2f> right_points(points.begin() + furthest_index, points.end());
        std::vector<cv::Point2f> left_simplified_points;
        std::vector<cv::Point2f> right_simplified_points;

        douglas_peucker(left_points, epsilon, left_simplified_points);
        // Recursively simplify the right sub-line
        douglas_peucker(right_points, epsilon, right_simplified_points);
        // Combine the simplified sub-lines
        simplified_points.insert(simplified_points.end(), left_simplified_points.begin(), left_simplified_points.end());
        simplified_points.insert(simplified_points.end(), right_simplified_points.begin() + 1, right_simplified_points.end());
    }
    // If the maximum distance is less than or equal to epsilon, add the endpoints to the simplified points
    else {
        simplified_points.push_back(points.front());
        simplified_points.push_back(points.back());
    }
}

2. 道格拉斯算法的特点

道格拉斯算法，存在它的优势与劣势

优势：

• 该算法的实现和理解相对简单。
• 它可以用于简化任何类型的曲线，而不仅仅是直线或多段线。
• 通过调整公差参数，可以使用它来保留曲线的重要特征，例如拐角或拐点。

缺点：

• 对于大型数据集或复杂曲线，该算法耗时久。
• 所得到的简化曲线可能在视觉上不令人满意或不平滑，尤其是在公差参数设置过高的情况下。
• 该算法不适用于具有不同密度或曲率的曲线，因为它假设点的均匀分布。

因此在实际使用中，针对实际出现的问题，我们需要对该算法进行对应的优化。我在工程中已经出现了不平滑的问题，关于优化以后再说。

总结

总之，坐标变换是自动驾驶的一个关键组成部分，使车辆能够准确感知和解释周围的环境。通过了解不同类型的坐标变换和用于执行这些变换的数学技术，开发人员可以创建更强大、更准确的自动驾驶系统。

写的不容易，欢迎各位朋友点赞并加关注，谢谢！