slam 估计的轨迹转为 WGS84?
1. 这句话什么意思? WGS84 和 GPS 什么关系
When GPS information is available, the estimated trajectory is transformed to WGS84 coordinates after time synchronized automatically.
这句话的意思是:
当 GPS 信息可用时,估计的轨迹会在时间同步后自动转换为 WGS84 坐标系中的坐标。
WGS84 和 GPS 的关系
- WGS84(World Geodetic System 1984) 是一种全球通用的地理坐标系,定义了地球的形状、参考椭球、重力场模型等。WGS84 坐标通常由 经度(Longitude)、纬度(Latitude)和海拔(Altitude) 表示。
- GPS(Global Positioning System) 是美国的全球定位系统,依赖于卫星提供的定位数据。GPS 输出的位置数据默认采用 WGS84 坐标系。
这句话具体描述的过程
- 你的系统(比如 VINS-Fusion)估计出的轨迹最开始可能是本地坐标系(如惯性系、相机坐标系、局部 UTM 坐标系等)中的轨迹。
- 当 GPS 可用时,系统会通过时间同步 GPS 数据,然后将估计轨迹转换到 WGS84 坐标系(即全球坐标)。
- 这样可以使轨迹与全球坐标系统对齐,而不是仅在一个局部坐标系中工作。
简单来说,GPS 提供的是 WGS84 坐标,而 VINS-Fusion 这类视觉惯性 SLAM 估计的是局部坐标系中的轨迹。当 GPS 数据可用时,系统会进行时间对齐,然后把轨迹转换成 WGS84 坐标系,使其成为全球可用的坐标数据。
2. 将估计轨迹转换到 WGS84 坐标系的方法?
1. 了解坐标系
- 估计轨迹(SLAM/VO/VINS 轨迹):通常是在某个局部坐标系(惯性坐标系、相机坐标系、ENU坐标系、UTM坐标系等)。
- GPS 提供的坐标:默认是 WGS84(经纬度 + 海拔)。
为了转换,通常需要找到局部坐标系和 WGS84 之间的关系。
2. 具体转换步骤
(1) 选定参考点
在 GPS 可用的情况下,选定一个初始时刻的 GPS 位置 (lat₀, lon₀, alt₀),对应局部坐标系中的原点 (x₀, y₀, z₀)。
- lat₀, lon₀, alt₀:来自 GPS(单位:度、米)。
- x₀, y₀, z₀:估计轨迹的局部坐标(单位:米)。
(2) 将局部坐标转换为地球坐标(ECEF)
- GPS 的 WGS84 坐标 (lat, lon, alt) 需要转换为地心地固坐标系(ECEF - Earth-Centered, Earth-Fixed):
- (X, Y, Z) 是地心坐标(单位:米)。
- lat, lon, h 是 GPS 经纬度和高度。
- a = 6378137.0(WGS84 长半轴)。
- b = 6356752.3142(WGS84 短半轴)。
- N = a / sqrt(1 - e² sin²(lat))(卯酉圈半径)。
- e² = (a² - b²) / a²(偏心率平方)。
- 同样地,把参考点 (lat₀, lon₀, alt₀) 也转换到 ECEF (X₀, Y₀, Z₀)。
(3) 计算局部坐标系到 ECEF 的变换
在局部坐标系中,通常采用 ENU(East-North-Up) 坐标系,其中:
- X 轴指向东(East)。
- Y 轴指向北(North)。
- Z 轴指向天顶(Up)。
构造ENU 到 ECEF 的旋转矩阵:
然后,把局部轨迹点 (x, y, z) 转换到 ECEF:
(4) 从 ECEF 变换回 WGS84
把新的 (X', Y', Z') 转换回经纬度:
- 迭代计算纬度:\[lat' = \tan^{-1} \left( \frac{Z'}{\sqrt{X'^2 + Y'^2} \cdot (1 - e²)} \right) \]
- 计算经度:\[lon' = \tan^{-1} \left( \frac{Y'}{X'} \right) \]
- 计算高度:\[h' = \frac{\sqrt{X'^2 + Y'^2}}{\cos(lat')} - N \]
3. 代码示例
这里用 Python 代码演示 WGS84 和局部坐标之间的转换:
import numpy as np
from pyproj import Proj, transform
# WGS84 椭球参数
a = 6378137.0 # 长半轴
b = 6356752.3142 # 短半轴
e2 = (a**2 - b**2) / a**2 # 偏心率平方
def wgs84_to_ecef(lat, lon, alt):
lat, lon = np.radians(lat), np.radians(lon)
N = a / np.sqrt(1 - e2 * np.sin(lat)**2)
X = (N + alt) * np.cos(lat) * np.cos(lon)
Y = (N + alt) * np.cos(lat) * np.sin(lon)
Z = (b**2 / a**2 * N + alt) * np.sin(lat)
return np.array([X, Y, Z])
def ecef_to_wgs84(X, Y, Z):
lon = np.arctan2(Y, X)
p = np.sqrt(X**2 + Y**2)
lat = np.arctan2(Z, p * (1 - e2)) # 初始估计
N = a / np.sqrt(1 - e2 * np.sin(lat)**2)
alt = p / np.cos(lat) - N
return np.degrees(lat), np.degrees(lon), alt
def local_to_wgs84(x, y, z, ref_lat, ref_lon, ref_alt):
""" 将局部坐标 (x, y, z) 转换到 WGS84 """
# 计算参考点的 ECEF 坐标
ref_ecef = wgs84_to_ecef(ref_lat, ref_lon, ref_alt)
# 计算 ENU 到 ECEF 旋转矩阵
ref_lat, ref_lon = np.radians(ref_lat), np.radians(ref_lon)
R = np.array([
[-np.sin(ref_lon), np.cos(ref_lon), 0],
[-np.sin(ref_lat) * np.cos(ref_lon), -np.sin(ref_lat) * np.sin(ref_lon), np.cos(ref_lat)],
[np.cos(ref_lat) * np.cos(ref_lon), np.cos(ref_lat) * np.sin(ref_lon), np.sin(ref_lat)]
])
# 局部坐标转换到 ECEF
ecef_coords = R @ np.array([x, y, z]) + ref_ecef
# ECEF 转回 WGS84
return ecef_to_wgs84(*ecef_coords)
# 示例:假设局部坐标 (x, y, z) = (100, 50, 10)
# 参考点 GPS 坐标 (lat, lon, alt) = (30.0, 114.0, 50)
lat, lon, alt = local_to_wgs84(100, 50, 10, 30.0, 114.0, 50)
print(f"WGS84: lat={lat}, lon={lon}, alt={alt}")
4. 总结
- GPS 坐标是 WGS84,而 VINS-Fusion 估计轨迹是局部坐标,必须进行转换。
- 步骤
- 选定参考点(GPS 原点)。
- 局部坐标 → ECEF 坐标。
- ECEF 坐标 → WGS84 坐标(经纬度 + 高度)。
- 代码实现用 ECEF 作为中间转换,可以准确完成变换。
你可以根据 VINS-Fusion 的原点选取策略,调整参考点选择方式。这样就可以让你的估计轨迹在 GPS 可用时,与 WGS84 对齐! 🚀
3. 可是单目 slam 系统,估计的局部坐标存在尺度问题,这样转换可以吗?
你说得对!单目 SLAM 的局部坐标系存在尺度模糊性(scale ambiguity),直接转换到 WGS84 可能会导致误差,因为 SLAM 估计的轨迹坐标单位是相对尺度,而 GPS 提供的是真实世界尺度(米)。
核心问题:尺度不匹配
-
单目 SLAM 轨迹(VO/VINS/OpenVSLAM):
- 轨迹是相对坐标,没有绝对尺度信息,单位是“未知比例的米”。
- 估计的位移和 GPS 真实位移之间可能相差一个尺度因子 s。
-
GPS 坐标(WGS84 / ECEF):
- 真实世界的绝对坐标(单位:米)。
- 没有尺度模糊性。
所以,如果直接把 SLAM 轨迹 x, y, z 转换为 ECEF/WGS84,可能会导致转换后的轨迹缩放错误。
如何解决单目 SLAM 的尺度问题?
方法 1:使用 GPS 进行尺度对齐
如果 GPS 轨迹是可用的(即在一定时间段内有可靠的 GPS 数据),可以计算一个全局尺度因子,使 SLAM 轨迹与 GPS 轨迹匹配:
其中:
- $ d_{\text{GPS}} $ 是 GPS 轨迹的欧几里得距离(从 WGS84 转换到 UTM/ECEF 后计算)。
- $ d_{\text{SLAM}} $ 是单目 SLAM 估计的轨迹间的欧几里得距离。
- $ s $ 是尺度因子。
然后,把 SLAM 轨迹按尺度 $ s $ 缩放:
之后,再按照 ECEF → WGS84 进行转换,就可以得到合理的 WGS84 坐标。
方法 2:利用 IMU 进行尺度恢复
如果你的单目 SLAM 结合了 IMU(如 VINS-Fusion、OKVIS、MSCKF),IMU 可以提供加速度计,从而帮助估计真实世界的尺度。
在 VINS-Fusion 里,IMU 的单位是 m/s²,可以用于恢复轨迹的真实尺度,这样 SLAM 估计的坐标就不再是任意尺度,而是可以直接用于坐标转换。
方法 3:用已知地图点进行尺度校准
如果在环境中有已知的地标点(比如 GPS 已知的固定位置点),可以通过对齐这些点来确定单目 SLAM 轨迹的真实比例尺。
- 例如,选取 SLAM 轨迹中的两个点 A 和 B,已知它们的GPS 位置(经纬度),计算它们之间的真实距离 $ d_{\text{GPS}} $。
- 计算单目 SLAM 估计的 A 和 B 之间的欧几里得距离 $ d_{\text{SLAM}} $。
- 计算尺度因子:\[s = \frac{d_{\text{GPS}}}{d_{\text{SLAM}}} \]
- 重新缩放整个 SLAM 轨迹:\[(x', y', z') = s \cdot (x, y, z) \]
总结
单目 SLAM 不能直接转换为 WGS84,必须先解决尺度问题:
- 如果有 GPS 数据,可以计算全局尺度因子 $ s $ 并缩放 SLAM 轨迹。
- 如果有 IMU,可以用 VINS-Fusion 自动恢复真实尺度。
- 如果有已知地标点,可以基于这些点计算尺度因子。
最终步骤:
- 尺度恢复(GPS、IMU、地标点等方法)。
- 局部坐标 → ECEF(使用 ENU 旋转矩阵)。
- ECEF → WGS84(转换为经纬度)。
如果你有 GPS 轨迹和 SLAM 轨迹的数据,我可以帮你计算合适的尺度因子!🚀
注意:chatgpt 复制的 公式格式是 \(
和 \)
, \[
和 \]
博客园看起来不支持公式格式:
\( e^{\pi i} + 1 = 0\)
\[
E = mc^2
\]
\(
和 \)
替换为 $
, \[
和 \]
替换为 $$