自動駕駛的關鍵挑戰之一是準確感知和解釋車輛周圍環境的能力。這需要使用各種傳感器，如相機、激光雷達和雷達，來捕捉周圍環境的數據。

然而，這些傳感器捕獲的數據通常位于與車輛自身坐標系不同的坐標系中。這意味著必須將數據轉換到車輛的坐標系中，以便對導航和控制有用。

上面提到的BEV感知，通過視覺算法我們可以在每一幀都提取出特征點。如果在時間序列上，我們想利用這些特征點，我們就無法避免的要使用坐標變換了。

自動駕駛中常用的坐標變換有幾種類型，包括：

1. 平移

平移包括在同一坐標系中將對象從一個位置移動到另一個位置。在自動駕駛中，平移通常用于對齊激光雷達和相機等不同傳感器捕獲的數據，使它們處于同一坐標系中。

2. 旋轉

旋轉包括圍繞同一坐標系中的固定點旋轉對象。在自動駕駛中，旋轉通常用于對齊安裝在車輛上不同角度的傳感器捕獲的數據。

3. 齊次變換

齊次變換包括將平移和旋轉組合成單個變換矩陣。在自動駕駛中我們會用到齊次變換，將數據從傳感器坐標系轉換到車輛坐標系。

為了執行這些坐標變換，使用了各種數學技術，如矩陣乘法和四元數旋轉。這些技術是在軟件庫中實現的，例如C++的Egengen庫和Python的NumPy庫。

二維數據坐標變換的常見場景

情況1

情況2

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()