1.ORB特征点提取和匹配
ORB特征提取和匹配可参考这篇博客。ORB特征提取主要包含FAST角点提取和BRIEF描述子计算两个步骤。匹配则采用简单的暴力匹配即可。Python代码(依赖OpenCV)如下：
import cv2
# 程序参数
num_features = 500 # ORB特征点数量
disTh = 20.0 # 距离阈值
# Step1. 读取影像
img1 = cv2.imread('frame01.jpg')
img2 = cv2.imread('frame02.jpg')
# Step2. 获取ORB特征点
orb = cv2.ORB_create(nfeatures=num_features)
kps1, des1 = orb.detectAndCompute(img1, None)
kps2, des2 = orb.detectAndCompute(img2, None)
# Step3. 匹配特征点
# Step3.1 使用BFMatcher进行暴力匹配
bf = cv2.BFMatcher(cv2.NORM_HAMMING, crossCheck=True)
matches = bf.match(des1, des2)
# Step3.2 获取当前匹配最小距离
min_dis = 10000
for item in matches:
dis = item.distance
if dis < min_dis:
min_dis = dis
# Step3.3 根据距离预置进一步筛选匹配
filtered_matches = []
for match in matches:
if match.distance <= max(1.1 * min_dis, disTh):
filtered_matches.append(match)
# Step3.4 提取匹配点坐标
good_kps1 = []
good_kps2 = []
for i in range(len(filtered_matches)):
good_kps1.append(kps1[filtered_matches[i].queryIdx].pt)
good_kps2.append(kps2[filtered_matches[i].trainIdx].pt)
# 检查并可视化匹配结果
from HaveFun import vision
print("Number of good matches", len(good_kps1))
img_match = vision.drawMatches(img1, good_kps1, img2, good_kps2)
cv2.imwrite('Matches.png', img_match)
2.位姿估计
2.1 帧间运动估计(2D-2D)
基于两帧的2D特征点匹配结果2D(Image Feature)-2D(Image Feature)，求解两帧之间的空间运动(旋转和平移)，相关理论参考这篇博客和这篇博客。
总体而言，在获取到两帧间的同名点(2D-2D)后，可利用对极几何求解两帧之间的运动(旋转和平移)。进一步细分为两步：变换矩阵求解和矩阵分解求位姿。
对于变换矩阵求解，包括本质矩阵(适用非平面场景、解算需相机内参)、基础矩阵(适用非平面场景、解算不需相机内参)和单应矩阵(适用平面场景、解算不需相机内参)，在OpenCV中都有相应函数可以调用:cv2.findEssentialMat()、cv2.findFundamentalMat()、cv2.findHomography()。
对于矩阵分解求运动，三种变换均需要知道相机的内参矩阵。OpenCV提供了针对本质矩阵和单应矩阵的分解函数:cv2.recoverPose()、cv2.decomposeHomographyMat()。对于基础矩阵，可以先通过内参矩阵将其转化为本质矩阵，再调用本质矩阵分解函数。矩阵分解求运动的核心是SVD方法。
总体而言，2D-2D的帧间运动估计多用于SLAM的初始化阶段，从而快速建立三维地图点和后序的运动尺度参考。
2.1.1 基于本质矩阵求解
本质矩阵核心利用的是对极几何约束构造了多个方程。由于匹配得到的点对个数远大于未知数个数，因此，对于本质矩阵分解得到平移和旋转，核心是对其进行SVD分解寻求最小二乘解。具体是，对系数矩阵进行SVD分解，然后找到奇异值矩阵中最小奇异值所对应的奇异向量，即为方程的最小二乘解。最后，基于分解得到的U、Σ、V，按照公式计算旋转和平移。详细理论推导可参考这篇博客和这篇博客。Python代码如下：
import cv2
import numpy as np
# 程序参数
num_features = 500 # ORB特征点数量
disTh = 20.0 # 距离阈值
K = np.array([[100, 0, 960],
[0, 100, 540],
[0, 0, 1]]) # 影像大小为1920x1080，取理论内参
# Step1. 读取影像
img1 = cv2.imread('frame01.jpg')
img2 = cv2.imread('frame02.jpg')
# Step2. 获取ORB特征点
orb = cv2.ORB_create(nfeatures=num_features)
kps1, des1 = orb.detectAndCompute(img1, None)
kps2, des2 = orb.detectAndCompute(img2, None)
# Step3. 匹配特征点
# Step3.1 使用BFMatcher进行暴力匹配
bf = cv2.BFMatcher(cv2.NORM_HAMMING, crossCheck=True)
matches = bf.match(des1, des2)
# Step3.2 获取当前匹配最小距离
min_dis = 10000
for item in matches:
dis = item.distance
if dis < min_dis:
min_dis = dis
# Step3.3 根据距离预置进一步筛选匹配
filtered_matches = []
for match in matches:
if match.distance <= max(1.1 * min_dis, disTh):
filtered_matches.append(match)
# Step3.4 提取匹配点坐标
good_kps1 = []
good_kps2 = []
for i in range(len(filtered_matches)):
good_kps1.append(kps1[filtered_matches[i].queryIdx].pt)
good_kps2.append(kps2[filtered_matches[i].trainIdx].pt)
# 对于本质矩阵，调用函数分解
E, _ = cv2.findEssentialMat(np.float32(good_kps1), np.float32(good_kps2), K, cv2.RANSAC)
_, R_E, t_E, _ = cv2.recoverPose(E, np.float32(good_kps1), np.float32(good_kps2), K)
print("Essential Matrix:\n", E)
print("Rotation from Essential Matrix:\n", R_E)
print("Translation from Essential Matrix:\n", t_E)
2.1.2 基于基础矩阵求解
前面也提到，基础矩阵包含相机内参矩阵和本质矩阵。所以我们可以先将其转化为本质矩阵，再按照2.1.1部分本质矩阵的分解得到旋转和平移，可以参考这篇博客。Python代码如下：
import cv2
import numpy as np
# 程序参数
num_features = 500 # ORB特征点数量
disTh = 20.0 # 距离阈值
K = np.array([[100, 0, 960],
[0, 100, 540],
[0, 0, 1]]) # 影像大小为1920x1080，取理论内参
# Step1. 读取影像
img1 = cv2.imread('frame01.jpg')
img2 = cv2.imread('frame02.jpg')
# Step2. 获取ORB特征点
orb = cv2.ORB_create(nfeatures=num_features)
kps1, des1 = orb.detectAndCompute(img1, None)
kps2, des2 = orb.detectAndCompute(img2, None)
# Step3. 匹配特征点
# Step3.1 使用BFMatcher进行暴力匹配
bf = cv2.BFMatcher(cv2.NORM_HAMMING, crossCheck=True)
matches = bf.match(des1, des2)
# Step3.2 获取当前匹配最小距离
min_dis = 10000
for item in matches:
dis = item.distance
if dis < min_dis:
min_dis = dis
# Step3.3 根据距离预置进一步筛选匹配
filtered_matches = []
for match in matches:
if match.distance <= max(1.1 * min_dis, disTh):
filtered_matches.append(match)
# Step3.4 提取匹配点坐标
good_kps1 = []
good_kps2 = []
for i in range(len(filtered_matches)):
good_kps1.append(kps1[filtered_matches[i].queryIdx].pt)
good_kps2.append(kps2[filtered_matches[i].trainIdx].pt)
# 对于基础矩阵，无直接分解函数，可以先转为本质矩阵再分解
F, _ = cv2.findFundamentalMat(np.float32(good_kps1), np.float32(good_kps2), cv2.RANSAC)
E_F = np.matmul(K.T, np.matmul(F, K))
_, R_F, t_F, _ = cv2.recoverPose(E_F, np.float32(good_kps1), np.float32(good_kps2), K)
print("Fundamental Matrix:\n", F)
print("Rotation from Fundamental Matrix:\n", R_F)
print("Translation from Fundamental Matrix:\n", t_F)
2.1.3 基于单应矩阵求解
对于单应矩阵的分解，可能存在多种情况。需要结合三角化做进一步判断，可以参考这篇博客和这篇博客。Python代码如下：
import cv2
import numpy as np
# 程序参数
num_features = 500 # ORB特征点数量
disTh = 20.0 # 距离阈值
K = np.array([[100, 0, 960],
[0, 100, 540],
[0, 0, 1]]) # 影像大小为1920x1080，取理论内参
# Step1. 读取影像
img1 = cv2.imread('frame01.jpg')
img2 = cv2.imread('frame02.jpg')
# Step2. 获取ORB特征点
orb = cv2.ORB_create(nfeatures=num_features)
kps1, des1 = orb.detectAndCompute(img1, None)
kps2, des2 = orb.detectAndCompute(img2, None)
# Step3. 匹配特征点
# Step3.1 使用BFMatcher进行暴力匹配
bf = cv2.BFMatcher(cv2.NORM_HAMMING, crossCheck=True)
matches = bf.match(des1, des2)
# Step3.2 获取当前匹配最小距离
min_dis = 10000
for item in matches:
dis = item.distance
if dis < min_dis:
min_dis = dis
# Step3.3 根据距离预置进一步筛选匹配
filtered_matches = []
for match in matches:
if match.distance <= max(1.1 * min_dis, disTh):
filtered_matches.append(match)
# Step3.4 提取匹配点坐标
good_kps1 = []
good_kps2 = []
for i in range(len(filtered_matches)):
good_kps1.append(kps1[filtered_matches[i].queryIdx].pt)
good_kps2.append(kps2[filtered_matches[i].trainIdx].pt)
# 对于单应矩阵，调用函数分解
good_kps1_mat = np.float32(good_kps1)
good_kps2_mat = np.float32(good_kps2)
H, _ = cv2.findHomography(good_kps1_mat, good_kps2_mat, cv2.RANSAC)
_, R_H, t_H, normal_H = cv2.decomposeHomographyMat(H, K)
print("Homography Matrix:\n", H)
print("Rotation from Homography Matrix:\n", R_H)
print("Translation from Homography Matrix:\n", t_H)
# 检查分解结果
best_solution = None
max_valid_ratio = -1
for i in range(len(R_H)):
cur_R = R_H[i]
cur_t = t_H[i]
cur_n = normal_H[i]
# 条件1：旋转矩阵有效性
flag_rotation = np.allclose(cur_R.T @ cur_R, np.eye(3), atol=1e-3) and abs(np.linalg.det(cur_R) - 1) < 1e-3
if not flag_rotation:
continue
# 条件2：三角化正深度
# 三角化3D点
P1 = np.matmul(K, np.hstack((np.eye(3), np.zeros((3,1)))))
P2 = np.matmul(K, np.hstack((cur_R, cur_t)))
points_4d = cv2.triangulatePoints(P1, P2, good_kps1_mat.T.astype(np.float32), good_kps2_mat.T.astype(np.float32))
# 转换为非齐次坐标并计算深度
points_3d = cv2.convertPointsFromHomogeneous(points_4d.T)[:,0,:]
depths = points_3d[:,2]
# 计算正深度点比例
valid_mask = depths > 0
valid_ratio = np.mean(valid_mask)
# 计算重投影误差
reproj_pts = cv2.projectPoints(points_3d[valid_mask], cur_R, cur_t, K, None)[0].squeeze()
errors = np.linalg.norm(good_kps2_mat[valid_mask,:2] - reproj_pts, axis=1)
mean_error = np.mean(errors) if len(errors) > 0 else float('inf')
print("Valid Ratio:", valid_ratio, "Mean Error:", mean_error)
if valid_ratio > max_valid_ratio:
max_valid_ratio = valid_ratio
best_solution = (cur_R, cur_t, cur_n)
print("Best Solution:", best_solution)
2.2 帧间运动估计(2D-3D)
和前面2D-2D求解帧间运动类似。如果我们找到了某一帧上的特征点和三维地图点之间的关系2D(Image Feature)-3D(Point Cloud)，也可以求解运动(位姿)，这便是PnP问题(Perspective-n-Point)，可以参考这篇博客。假设二维特征点为x1，三维空间点位X1，将它们各自增加一维写成齐次。它们通过一个3×4的矩阵P(Projection Matrix)变换，λ表示从相机光心到真实三维点的距离。与单应矩阵H类似的，矩阵P中包含了内参K以及外参R、t，只要我们求得了P并且知道K，就可以恢复出R、t。PnP问题常见包括直接线性变换(DLT)解法、P3P解法、EPnP解法等。在OpenCV中，有专门的cv2.solvePnP()函数可以使用(支持多种常用算法)。Python代码如下：
import numpy as np
import cv2
if __name__ == "__main__":
# 1. 生成模拟数据
# 模拟8个立方体角点的3D坐标 (单位：米)
object_points = np.array([
[0, 0, 0], [1, 0, 0], [1, 1, 0], [0, 1, 0],
[0, 0, 1], [1, 0, 1], [1, 1, 1], [0, 1, 1]
], dtype=np.float32)
# 模拟相机内参 (假设640x480分辨率)
camera_matrix = np.array([
[800, 0, 320],
[0, 800, 240],
[0, 0, 1]
], dtype=np.float32)
# 模拟相机位姿 (旋转和平移)
rvec = np.array([0.3, 0.5, 0.2], dtype=np.float32)
tvec = np.array([0.5, -0.3, 2.0], dtype=np.float32)
# 投影生成2D点
image_points, _ = cv2.projectPoints(
object_points, rvec, tvec, camera_matrix, None)
obj_pts = object_points
img_pts = image_points.squeeze()
K = camera_matrix
dist_coeffs = np.zeros(4) # 假设无畸变
# 2. 使用EPnP算法求解
success, rvec, tvec = cv2.solvePnP(
obj_pts, img_pts, K, dist_coeffs,
flags=cv2.SOLVEPNP_EPNP
)
if not success:
raise RuntimeError("PnP求解失败")
# 3. 结果转换与输出
R, _ = cv2.Rodrigues(rvec)
print("=== 真实位姿 ===")
true_rvec, _ = cv2.Rodrigues(np.array([0.3, 0.5, 0.2]))
print("真实旋转矩阵:\n", true_rvec)
print("真实平移向量:\n", np.array([0.5, -0.3, 2.0]))
print("\n=== 估计位姿 ===")
print("旋转矩阵:\n", R)
print("平移向量:\n", tvec)
# 4. 计算重投影误差
reprojected, _ = cv2.projectPoints(obj_pts, rvec, tvec, K, dist_coeffs)
error = np.linalg.norm(reprojected.squeeze() - img_pts, axis=1).mean()
print("\n平均重投影误差:", error, "像素")
2.3 帧间运动估计(3D-3D)
最后，在SLAM位姿估计中还有一种常见的情况，就是已知3D(Point Cloud)-3D(Point Cloud)的关联，求解相机的运动(这里认为不存在尺度上的变化，是三维刚体变换)。这个场景在LiDAR SLAM中很常见(比如在第一帧扫描到50个点，第二帧扫描到80个点，利用描述子匹配等操作，得到30个三维匹配点)。这里需要明确的是，我们的已知量是两帧观测得到的三维点，变换未知。更简洁来说就是，已知状态A和状态B，求解A到B的变换。而在之前这篇博客里提到的三维空间变换则稍有区别。那里我们介绍的是，已知状态A和变换T，求解状态B。
对于3D-3D的运动求解，核心是基于关联关系构建方程。经典解法是SVD方法，在其基础上，针对不同场景，进一步又衍生出ICP方法和RANSAC方法。
2.3.1 经典SVD方法求解
由于方程数远远多于需要的点对(3对)，所以可以用矩阵SVD分解的方法进行求解，然后基于分解的U、Σ、V，按照公式解算旋转和平移。简单来说包含：计算质心、去中心化、构建协方差矩阵、SVD分解、计算旋转矩阵、计算平移向量这几个步骤。进一步可以参考这个博客和这个博客。Python代码如下：
import numpy as np
if __name__ == "__main__":
# 1. 生成测试数据
source = np.array([
[0,0,0], [1,0,0], [1,1,0], [0,1,0],
[0.5, 0.5, 0.5], [0.3, 0.2, 0.5], [0.7, 0.8, 0.5]])
R_true = np.array([[0.866, -0.5, 0], [0.5, 0.866, 0], [0,0,1]]) # 30度旋转
t_true = np.array([0.3, -0.2, 0.5])
target = (R_true @ source.T).T + t_true
# 2. SVD求解变换矩阵
# 计算质心
centroid_src = np.mean(source, axis=0)
centroid_tgt = np.mean(target, axis=0)
# 去中心化
src_centered = source - centroid_src
tgt_centered = target - centroid_tgt
# 构建协方差矩阵
H = src_centered.T @ tgt_centered
# SVD分解
U, _, Vt = np.linalg.svd(H)
# 计算旋转矩阵
R_est = Vt.T @ U.T
# 处理反射情况(当点云存在镜像对称性时，配准结果出现镜像翻转现象)
if np.linalg.det(R_est) < 0:
Vt[-1,:] *= -1
R_est = Vt.T @ U.T
# 计算平移向量
t_est = centroid_tgt - R_est @ centroid_src
# 输出结果
print("Estimated Rotation Matrix:\n", R_est)
print("Estimated Translation Vector:\n", t_est)
# 3. 评估结果
aligned = (R_est @ source.T).T + t_est
distances = np.linalg.norm(aligned - target, axis=1)
mean_err = np.mean(distances)
max_err = np.max(distances)
print("旋转矩阵误差:", np.linalg.norm(R_true - R_est))
print("平移向量误差:", np.linalg.norm(t_true - t_est))
print("平均点距离:", mean_err)
print("最大点距离:", max_err)
2.3.2 ICP方法求解
ICP（Iterative Closest Point）是SLAM中用于点云配准的核心技术，通过迭代计算两组点云之间的最优变换矩阵（旋转和平移）实现环境建模和定位。ICP通过循环执行“匹配→求解→变换”流程逐步逼近最优解，其核心思想是匹配源点云和目标点云的对应点，然后最小化对应点之间的距离残差以求解变换矩阵。ICP算法的优点是简单高效，缺点是对初始值比较敏感，若两个点集存在较大初始变换，容易陷入局部最优。因此，ICP方法适用于两个点云变换不大的情况，一般要求点云存在部分重叠且初始位姿偏差不宜过大（旋转角<30°），否则可以考虑先进行粗配准。
此外，这里需要特别说明ICP方法和SVD方法的异同。前面介绍的SVD方法它的使用前提条件之一是两个点云之间一一对应且关系明确，而ICP算法不要求两个点集之间有一一对应关系(通过迭代搜索最近邻点建立)。在三维点云配准中，尽管SVD可直接求解已知对应点的变换矩阵，但ICP方法的核心价值在于解决对应点未知这一关键问题，并通过迭代优化提升鲁棒性。这种情况在实际场景中更加常见，因此，ICP算法被广泛应用于LiDAR SLAM的前端中，用于进行初始位姿和运动估计。Python代码如下：
import numpy as np
import matplotlib.pyplot as plt
def visualize_3d(src, dst, transformed=None):
fig = plt.figure(figsize=(10, 8))
ax = fig.add_subplot(111, projection='3d')
ax.scatter(dst[:,0], dst[:,1], dst[:,2], c='blue', label='Target', alpha=0.6)
ax.scatter(src[:,0], src[:,1], src[:,2], c='red', label='Source', alpha=0.6)
if transformed is not None:
ax.scatter(transformed[:,0], transformed[:,1], transformed[:,2],
c='green', label='Aligned', alpha=0.6)
ax.set_xlabel('X')
ax.set_ylabel('Y')
ax.set_zlabel('Z')
ax.legend()
plt.show()
def nearest_neighbor_3d(src, dst):
indices = []
for i in range(src.shape[0]):
dist = np.linalg.norm(dst - src[i], axis=1)
indices.append(np.argmin(dist))
return np.array(indices)
def icp_3d(src, dst, max_iter=50, tolerance=1e-5):
T = np.eye(4)
src_h = np.column_stack([src, np.ones(len(src))])
for i in range(max_iter):
# 最近邻搜索
indices = nearest_neighbor_3d(src_h[:,:3], dst)
correspondences = dst[indices]
# 计算变换矩阵
src_centered = src_h[:,:3] - np.mean(src_h[:,:3], axis=0)
dst_centered = correspondences - np.mean(correspondences, axis=0)
H = src_centered.T @ dst_centered
# SVD分解求旋转
U, _, Vt = np.linalg.svd(H)
R = Vt.T @ U.T
if np.linalg.det(R) < 0: # 反射修正
Vt[2,:] *= -1
R = Vt.T @ U.T
# 计算平移
t = np.mean(correspondences, axis=0) - R @ np.mean(src_h[:,:3], axis=0)
# 构造齐次变换矩阵
T_current = np.eye(4)
T_current[:3,:3] = R
T_current[:3,3] = t
# 更新变换
T = T_current @ T
src_h = (T_current @ src_h.T).T
# 收敛判断
error = np.mean(np.linalg.norm(src_h[:,:3] - correspondences, axis=1))
if error < tolerance:
print(f"Converged at iteration {i} with error {error:.6f}")
break
return T[:3], src_h[:,:3]
if __name__ == "__main__":
# 1. 构造测试数据：三维立方体
src_cube = np.array([[0,0,0], [1,0,0], [0,1,0], [0,0,1],
[1,1,0], [1,0,1], [0,1,1], [1,1,1]])
# 定义三维旋转矩阵
theta_z = np.radians(10)
theta_x = np.radians(5)
Rz = np.array([[np.cos(theta_z), -np.sin(theta_z), 0],
[np.sin(theta_z), np.cos(theta_z), 0],
[0, 0, 1]])
Rx = np.array([[1, 0, 0],
[0, np.cos(theta_x), -np.sin(theta_x)],
[0, np.sin(theta_x), np.cos(theta_x)]])
R_true = Rz @ Rx
t_true = np.array([0.5, 0.2, 0.3]) # 三维平移
dst_cube = (R_true @ src_cube.T).T + t_true
# 2. 执行配准
transform, aligned = icp_3d(src_cube, dst_cube)
rotation_matrix = transform[:3, :3]
translation_vector = transform[:3, 3]
print("Estimated transform:\n", transform)
print("Estimated rotation matrix:\n", rotation_matrix)
print("Estimated translation vector:\n", translation_vector)
# 3. 可视化
visualize_3d(src_cube, dst_cube, aligned)
2.3.3 RANSAC方法求解
最后，介绍RANSAC方法(随机采样一致性)。前面说了，SVD方法适合于3D-3D一一对应的场景。但在实际中，可能出现错误的对应关系，进而影响整个SVD方法求解的最后结果。针对这种场景，可以将传统SVD方法与RANSAC方法进行结合，解决离群点问题。Python代码如下：
import numpy as np
import random
def estimate_rigid_transform(src_points, dst_points):
# 计算质心
src_centroid = np.mean(src_points, axis=0)
dst_centroid = np.mean(dst_points, axis=0)
# 中心化点集
src_centered = src_points - src_centroid
dst_centered = dst_points - dst_centroid
# 计算H矩阵
H = np.dot(src_centered.T, dst_centered)
# SVD分解
U, _, Vt = np.linalg.svd(H)
R = np.dot(Vt.T, U.T)
# 处理反射情况
if np.linalg.det(R) < 0:
Vt[-1,:] *= -1
R = np.dot(Vt.T, U.T)
# 计算平移
t = dst_centroid - np.dot(R, src_centroid)
# 组合变换矩阵
transform = np.column_stack((R, t))
return transform
def ransac_rigid_transform(src_points, dst_points, max_iters=1000, inlier_thresh=5.0, min_inlier_ratio=0.5):
best_transform = None
best_inliers = []
best_error = float('inf')
num_points = src_points.shape[0]
min_sample = 3 # 最小样本数
for _ in range(max_iters):
# 随机采样
sample_indices = random.sample(range(num_points), min_sample)
sample_src = src_points[sample_indices]
sample_dst = dst_points[sample_indices]
# 估计变换
try:
transform = estimate_rigid_transform(sample_src, sample_dst)
except:
continue
# 应用变换
ones = np.ones((num_points, 1))
src_homo = np.hstack((src_points, ones))
transformed = np.dot(src_homo, transform.T)
# 计算误差
errors = np.linalg.norm(transformed - dst_points, axis=1)
inliers = np.where(errors < inlier_thresh)[0]
inlier_ratio = len(inliers) / num_points
# 更新最佳结果
if inlier_ratio > min_inlier_ratio:
current_error = np.mean(errors[inliers])
if current_error < best_error:
best_error = current_error
best_transform = transform
best_inliers = inliers
# 使用所有内点重新估计最终变换
if best_transform is not None and len(best_inliers) >= min_sample:
best_transform = estimate_rigid_transform(src_points[best_inliers], dst_points[best_inliers])
return best_transform, best_inliers
if __name__ == "__main__":
# 1. 构造测试数据：三维立方体
src_cube = np.array([[0,0,0], [1,0,0], [0,1,0], [0,0,1],
[1,1,0], [1,0,1], [0,1,1], [1,1,1]])
# 定义三维旋转矩阵
theta_z = np.radians(10)
theta_x = np.radians(5)
Rz = np.array([[np.cos(theta_z), -np.sin(theta_z), 0],
[np.sin(theta_z), np.cos(theta_z), 0],
[0, 0, 1]])
Rx = np.array([[1, 0, 0],
[0, np.cos(theta_x), -np.sin(theta_x)],
[0, np.sin(theta_x), np.cos(theta_x)]])
R_true = Rz @ Rx
t_true = np.array([0.5, 0.2, 0.3]) # 三维平移
dst_cube = (R_true @ src_cube.T).T + t_true
# 添加离群点
dst_cube[0] += 10
dst_cube[5] += 5
# 使用RANSAC估计变换
transform, inliers = ransac_rigid_transform(src_cube, dst_cube)
rotation = transform[:3, :3]
translation = transform[:3, 3]
print("估计的变换矩阵:\n", transform)
print("旋转矩阵:\n", rotation)
print("平移向量:\n", translation)
print("内点数量:", len(inliers))
print("内点索引:", inliers)
当然，理论上也可以将RANSAC方法与ICP方法结合。
3.RANSAC方法
前面我们使用RANSAC方法对离群点进行了简单的去除。事实上，采用Least Square+SVD的方法可以十分有效地解决问题。Least Square用于构造方程组，SVD则用于解方程组。但这里有个问题，最小二乘方法虽然很棒，但是对离群点十分敏感，可能个别离群点会导致解的不稳定。Random Sample Consensus，随机采样一致性，是去除离群点(Outlier)的好办法。核心思想是找到一个适合绝大多数样本的模型，对于离群点则选择忽略，详细可以参考这个博客和这个博客。RANSAC主要流程如下：
- 从数据点中随机选择指定数量的点构建模型
- 将全部数据点带入模型中计算与真实数据差异，根据误差阈值判定内点或外点
- 统计内点个数并记录对应模型参数
- 重复上述步骤指定次数得到多个结果，选取内点数量最多对应的模型参数即认为正确模型
演示Python代码如下，主要参考这篇博客：
import numpy as np
import random
from matplotlib import pyplot as plt
import math
def getPointPairData(x_start=0, y_start=0, x_end=60, y_end=60, point_num=100, offset_x=35, offset_y=40):
x_data1 = []
y_data1 = []
x_data2 = []
y_data2 = []
for i in range(point_num):
x_data1.append(random.randint(x_start, x_end))
y_data1.append(random.randint(y_start, y_end))
x_data2.append(x_data1[i] + offset_x)
y_data2.append(y_data1[i] + offset_y)
return x_data1, y_data1, x_data2, y_data2
def addNoiseToPointPair(x_data1, y_data1, x_data2, y_data2, mu=0, sigma=1):
noise_x_data1 = []
noise_y_data1 = []
noise_x_data2 = []
noise_y_data2 = []
for i in range(len(x_data1)):
noise_x_data1.append(x_data1[i] + random.gauss(mu, sigma))
noise_y_data1.append(y_data1[i] + random.gauss(mu, sigma))
noise_x_data2.append(x_data2[i] + random.gauss(mu, sigma))
noise_y_data2.append(y_data2[i] + random.gauss(mu, sigma))
return noise_x_data1, noise_y_data1, noise_x_data2, noise_y_data2
def addOutliersToPointPair(x_data1, y_data1, x_data2, y_data2, num_outlier, mu=0, sigma=15):
noise_x_data1 = x_data1[:]
noise_y_data1 = y_data1[:]
noise_x_data2 = x_data2[:]
noise_y_data2 = y_data2[:]
indices = []
for i in range(num_outlier):
index = random.randint(0, len(x_data1) - 1)
indices.append(index)
noise_x_data1[index] = x_data1[index] + random.gauss(mu, sigma)
noise_y_data1[index] = y_data1[index] + random.gauss(mu, sigma)
noise_x_data2[index] = x_data2[index] + random.gauss(mu, sigma)
noise_y_data2[index] = x_data2[index] + random.gauss(mu, sigma)
return noise_x_data1, noise_y_data1, noise_x_data2, noise_y_data2, indices
def drawMatchPairsWithOutliers(x_data1, y_data1, x_data2, y_data2, indices, color="lightgreen"):
plt.title("Point pairs")
plt.scatter(x_data1, y_data1, label="original point")
plt.scatter(x_data2, y_data2, label="matched point")
for i in range(len(x_data1)):
plt.plot([x_data1[i], x_data2[i]], [y_data1[i], y_data2[i]], color=color)
for i in range(len(indices)):
plt.plot([x_data1[indices[i]], x_data2[indices[i]]], [y_data1[indices[i]], y_data2[indices[i]]], color="red")
plt.grid()
plt.tight_layout()
plt.legend()
plt.show()
def lsp_svd(x_data1, y_data1, x_data2, y_data2):
# 按照Ax = 0构造方程，拼接参数矩阵A
# 详见https://zhaoxuhui.top/blog/2019/10/28/robotics-perception-assignment-2.html#5assignment-image-projection-using-homographies
A = []
for i in range(len(x_data1)):
ax = [-x_data1[i], -y_data1[i], -1, 0, 0, 0, x_data1[i] * x_data2[i], y_data1[i] * x_data2[i], x_data2[i]]
ay = [0, 0, 0, -x_data1[i], -y_data1[i], -1, x_data1[i] * y_data2[i], y_data1[i] * y_data2[i], y_data2[i]]
A.append(ax)
A.append(ay)
# 使用SVD解方程
A = np.array(A)
u, s, vt = np.linalg.svd(A)
v = np.transpose(vt)
solution = v[:, -1]
h = np.zeros([3, 3], dtype = float)
h[0, 0] = solution[0]
h[0, 1] = solution[1]
h[0, 2] = solution[2]
h[1, 0] = solution[3]
h[1, 1] = solution[4]
h[1, 2] = solution[5]
h[2, 0] = solution[6]
h[2, 1] = solution[7]
h[2, 2] = solution[8]
return h
def checkAccuracy(x_data1, y_data1, x_data2, y_data2, homo):
error_x = []
error_y = []
error = []
for i in range(len(x_data1)):
p1 = np.ones([3], float)
p1[0] = x_data1[i]
p1[1] = y_data1[i]
p2 = np.matmul(homo, p1)
p2_normal = p2 / p2[2]
reprojection_error_x = abs(x_data2[i] - p2_normal[0])
reprojection_error_y = abs(y_data2[i] - p2_normal[1])
reprojection_error = math.sqrt(math.pow(reprojection_error_x, 2) + math.pow(reprojection_error_y, 2))
error_x.append(reprojection_error_x)
error_y.append(reprojection_error_y)
error.append(reprojection_error)
return error_x, error_y, error
def randomSampling(x_data1, y_data1, x_data2, y_data2, n=4):
x_data1_s = []
y_data1_s = []
x_data2_s = []
y_data2_s = []
for i in range(int(n)):
tmp_index = random.randint(0, len(x_data1) - 1)
x_data1_s.append(x_data1[tmp_index])
y_data1_s.append(y_data1[tmp_index])
x_data2_s.append(x_data2[tmp_index])
y_data2_s.append(y_data2[tmp_index])
return x_data1_s, y_data1_s, x_data2_s, y_data2_s
if __name__ == '__main__':
start_x = 0
end_x = 60
start_y = 0
end_y = 60
num_points = 20
offset_x = 35
offset_y = 40
num_outliers = 3
noise_sigma = 0.5
outlier_sigma = 2
ep = 5
w = 1.0 * (num_points - num_outliers) / num_points
n = 4.0
p = 0.995
# 生成点对数据
x_data1, y_data1, x_data2, y_data2 = getPointPairData(start_x, start_y,
end_x, end_y,
num_points,
offset_x, offset_y)
x_data1_n, y_data1_n, x_data2_n, y_data2_n = addNoiseToPointPair(x_data1, y_data1, x_data2, y_data2,
sigma=noise_sigma)
x_data1_o, y_data1_o, x_data2_o, y_data2_o, indices = addOutliersToPointPair(x_data1_n, y_data1_n,
x_data2_n, y_data2_n,
num_outlier=num_outliers,
sigma=outlier_sigma)
drawMatchPairsWithOutliers(x_data1_o, y_data1_o, x_data2_o, y_data2_o, indices)
good_coefs = []
good_nums = []
errors = []
inliers_pts = []
k = int(math.log(1 - p) / math.log(1 - math.pow(w, n))) + 1
print("=>To estimate correct model with " + round(p * 100, 2).__str__() + \
"% certainty,you should iterate for", k, "times at least")
# RANSAC迭代
for it in range(k):
# 随机采样4个点对
sample_x1, sample_y1, sample_x2, sample_y2 = randomSampling(x_data1_o, y_data1_o, x_data2_o, y_data2_o, n=n)
print("\nTime", it + 1, "/", k)
print("selected point pairs:")
for j in range(len(sample_x1)):
print(sample_x1[j], sample_y1[j], "->", sample_x2[j], sample_y2[j])
# 基于采样点对计算单应性矩阵
h = lsp_svd(sample_x1, sample_y1, sample_x2, sample_y2)
print("Homography matrix:\n", h)
# 统计内点个数和误差
tmp_error = 0
counter = 0
good_pts = []
pts_est_x = []
pts_est_y = []
for i in range(len(x_data1_o)):
p1 = np.ones([3], float)
p1[0] = x_data1_o[i]
p1[1] = y_data1_o[i]
p2 = np.matmul(h, p1)
p2_normal = p2 / p2[2]
pts_est_x.append(p2_normal[0])
pts_est_y.append(p2_normal[1])
reprojection_error_x = abs(x_data2_o[i] - p2_normal[0])
reprojection_error_y = abs(y_data2_o[i] - p2_normal[1])
reprojection_error = math.sqrt(math.pow(reprojection_error_x, 2) + math.pow(reprojection_error_y, 2))
tmp_error += reprojection_error
if reprojection_error <= ep:
counter += 1
good_pts.append(i)
mean_error = tmp_error / len(x_data1_o)
errors.append(mean_error)
good_coefs.append(h)
good_nums.append(counter)
print("good point number:\n", counter, "/", len(x_data1_o))
print("mean error:\n", mean_error, "\n")
if counter > len(inliers_pts):
inliers_pts = []
for i in range(len(good_pts)):
inliers_pts.append(good_pts[i])
# 绘制结果
plt.scatter(x_data1_o, y_data1_o, label="original point")
plt.scatter(x_data2_o, y_data2_o, label="matched point")
plt.scatter(pts_est_x, pts_est_y, label='estimate point')
for i in range(len(x_data1_o)):
if i == 0:
plt.plot([x_data2_o[i], pts_est_x[i]], [y_data2_o[i], pts_est_y[i]], color="pink",
label="estimate error")
plt.plot([x_data1_o[i], x_data2_o[i]], [y_data1_o[i], y_data2_o[i]], color="lightgreen",
label="unselected pair")
else:
plt.plot([x_data2_o[i], pts_est_x[i]], [y_data2_o[i], pts_est_y[i]], color="pink")
plt.plot([x_data1_o[i], x_data2_o[i]], [y_data1_o[i], y_data2_o[i]], color="lightgreen")
for i in range(len(good_pts)):
if i == 0:
plt.plot([x_data1_o[good_pts[i]], x_data2_o[good_pts[i]]],
[y_data1_o[good_pts[i]], y_data2_o[good_pts[i]]],
color="red", label="selected pair")
else:
plt.plot([x_data1_o[good_pts[i]], x_data2_o[good_pts[i]]],
[y_data1_o[good_pts[i]], y_data2_o[good_pts[i]]],
color="red")
plt.title("Time " + (it + 1).__str__() + "/" + k.__str__() + " inliers=" + counter.__str__().zfill(2))
plt.grid()
plt.tight_layout()
plt.legend()
plt.show()
tmp_error = 0
counter = 0
good_pts = []
pts_est = []
best = good_nums.index(max(good_nums))
print("======Result======")
print("best consensus point number:\n", max(good_nums), "/", len(x_data1_o))
print("best homography:\n", good_coefs[best])
print("inliers:")
for i in range(len(inliers_pts)):
print("inlier", (i + 1).__str__().zfill(3), \
x_data1_o[inliers_pts[i]], \
y_data1_o[inliers_pts[i]], "->", \
x_data2_o[inliers_pts[i]], \
y_data2_o[inliers_pts[i]])
# 绘制最终结果
plt.scatter(x_data1_o, y_data1_o, label="original data")
plt.scatter(x_data2_o, y_data2_o, label="match data")
for i in range(len(x_data1_o)):
if i == 0:
plt.plot([x_data1_o[i], x_data2_o[i]], [y_data1_o[i], y_data2_o[i]], color="lightgreen", label="outliers")
else:
plt.plot([x_data1_o[i], x_data2_o[i]], [y_data1_o[i], y_data2_o[i]], color="lightgreen")
for i in range(len(inliers_pts)):
if i == 0:
plt.plot([x_data1_o[inliers_pts[i]], x_data2_o[inliers_pts[i]]],
[y_data1_o[inliers_pts[i]], y_data2_o[inliers_pts[i]]], color="red", label="inliers")
else:
plt.plot([x_data1_o[inliers_pts[i]], x_data2_o[inliers_pts[i]]],
[y_data1_o[inliers_pts[i]], y_data2_o[inliers_pts[i]]], color="red")
plt.grid()
plt.tight_layout()
plt.legend()
plt.show()
4.SLAM中的误差计算
其实在SLAM中的误差计算，本质上是已知A、B两个状态的位姿，求解它们之间的变换。这其实就是类似于2.3部分提到的3D-3D帧间运动估计。
4.1 绝对位姿误差
Absolute Pose Error(绝对位姿误差)指代的是在一个坐标系下，估计位姿和真值之间的差异，多个APE求均方根误差(RMSE)即可得到Absolute Trajectory Error(绝对轨迹误差)。APE可以反映SLAM轨迹与真值的整体一致性。
对于A、B两个状态，其对应的位姿(位置和姿态写在一起变成变换的形式)分别为\(T_{A}\)、\(T_{B}\)。现在需要求解AB之间的变换\(T_{x}\)，则有\(T_{B} = T_{x}T_{A}\)。进一步可以得到\(APE = T_{x} = T_{A}^{-1}T_{B}\)。得到总体的变换矩阵后，可以分别取旋转部分和平移部分用于误差评价。更具体而言：
- 平移部分：误差等于求解平移部分的模长，对于三维空间也即三维的欧氏距离。
- 旋转部分：旋转部分有两种不同评价方式，分别是角误差和矩阵误差。
- 总体误差：我们也可以对整个变换矩阵进行评价，评价方式是将变换矩阵减去对应的单位阵，然后结果计算F范数(和角度的矩阵误差方法一样)。
对于一对位姿变换按照上面的方法即可计算误差。而对于一段轨迹，重复上述步骤，即可计算得到多个APE，我们可以对其计算RMSE。这个RMSE结果就是这段轨迹的绝对轨迹误差(ATE)，如下：
\[ATE = \sqrt{\frac{1}{N} \sum_{i=1}^{N} APE_{i}^{2}}\]一个简单的计算ATE的代码如下：
import numpy as np
import math
def composeTransMat(pose):
trans_mat = np.array([[0, 0, 0, 0], [0, 0, 0, 0], [0, 0, 0, 0], [0, 0, 0, 1]], float)
qx = pose[0]
qy = pose[1]
qz = pose[2]
qw = pose[3]
dx = pose[4]
dy = pose[5]
dz = pose[6]
# 求四元数的模长
qmod = math.sqrt(qx * qx + qy * qy + qz * qz + qw * qw)
# 四元数归一化
qx = qx / qmod
qy = qy / qmod
qz = qz / qmod
qw = qw / qmod
# 计算旋转矩阵
rot = np.array([[0, 0, 0], [0, 0, 0], [0, 0, 0]], float)
rot[0, 0] = 1 - 2 * (qy * qy + qz * qz)
rot[0, 1] = 2 * (qx * qy - qw * qz)
rot[0, 2] = 2 * (qx * qz + qw * qy)
rot[1, 0] = 2 * (qx * qy + qw * qz)
rot[1, 1] = 1 - 2 * (qx * qx + qz * qz)
rot[1, 2] = 2 * (qy * qz - qw * qx)
rot[2, 0] = 2 * (qx * qz - qw * qy)
rot[2, 1] = 2 * (qy * qz + qw * qx)
rot[2, 2] = 1 - 2 * (qx * qx + qy * qy)
# 旋转矩阵(与Matlab quat2dcm函数保持一致)
rot = rot.transpose()
# 组装变换矩阵
trans_mat[0, 0] = rot[0, 0]
trans_mat[0, 1] = rot[0, 1]
trans_mat[0, 2] = rot[0, 2]
trans_mat[1, 0] = rot[1, 0]
trans_mat[1, 1] = rot[1, 1]
trans_mat[1, 2] = rot[1, 2]
trans_mat[2, 0] = rot[2, 0]
trans_mat[2, 1] = rot[2, 1]
trans_mat[2, 2] = rot[2, 2]
trans_mat[0, 3] = dx
trans_mat[1, 3] = dy
trans_mat[2, 3] = dz
return trans_mat
if __name__ == '__main__':
# Poses: qx, qy, qz, qw, x, y, z
pose_ref1 = composeTransMat([-0.757610, -0.348629, -0.497711, 0.238261, 4.460675, -1.680515, 0.579614])
pose_ref2 = composeTransMat([-0.518605, -0.636519, -0.358444, 0.444310, 3.704039, 1.424990, 1.403680])
pose_ref3 = composeTransMat([-0.514605, -0.636519, -0.358444, 0.444310, 3.504039, 1.444990, 1.443680])
pose_ref4 = composeTransMat([-0.510605, -0.636519, -0.358444, 0.444310, 3.404039, 1.454990, 1.453680])
pose_ref5 = composeTransMat([-0.507605, -0.636519, -0.358444, 0.444310, 3.204039, 1.464990, 1.463680])
pose_est1 = composeTransMat([-0.757610, -0.348629, -0.497711, 0.238261, 4.460675, -1.680515, 0.579614]) # 旋转平移完全一致
pose_est2 = composeTransMat([-0.518605, -0.636519, -0.358444, 0.444310, 3.704039, 1.434990, 1.413680]) # 旋转一致，位置有误差
pose_est3 = composeTransMat([-0.534605, -0.636519, -0.258444, 0.454310, 3.504039, 1.444990, 1.443680]) # 旋转有误差，位置一致
pose_est4 = composeTransMat([-0.520605, -0.626519, -0.458444, 0.424310, 3.404039, 1.444990, 1.463680]) # 旋转位置都有误差
pose_est5 = composeTransMat([-0.557605, -0.616519, -0.358444, 0.414310, 3.204039, 1.454990, 1.453680]) # 旋转位置都有误差
# 计算绝对位姿误差(APE)
ape1 = np.matmul(np.linalg.inv(pose_ref1), pose_est1)
ape2 = np.matmul(np.linalg.inv(pose_ref2), pose_est2)
ape3 = np.matmul(np.linalg.inv(pose_ref3), pose_est3)
ape4 = np.matmul(np.linalg.inv(pose_ref4), pose_est4)
ape5 = np.matmul(np.linalg.inv(pose_ref5), pose_est5)
# 利用F范数计算总体的APE误差
error1 = np.linalg.norm(ape1 - np.eye(4), ord='fro')
error2 = np.linalg.norm(ape2 - np.eye(4), ord='fro')
error3 = np.linalg.norm(ape3 - np.eye(4), ord='fro')
error4 = np.linalg.norm(ape4 - np.eye(4), ord='fro')
error5 = np.linalg.norm(ape5 - np.eye(4), ord='fro')
# 利用均方根误差RMSE计算ATE
ate = np.sqrt((error1**2 + error2**2 + error3**2 + error4**2 + error5**2) / 5)
print("Errors:")
print("APE Error1:", error1)
print("APE Error2:", error2)
print("APE Error3:", error3)
print("APE Error4:", error4)
print("APE Error5:", error5)
print("ATE Error:", ate)
4.2 相对位姿误差
Relative Pose Error(相对位姿误差)不像APE一样，计算A、B两个绝对位姿之间的差异，它计算的是绝对位姿差异之间的差异。举例来说，有A、B、C、D四个状态，APE分别为\(APE_{AB}\)、\(APE_{BC}\)、\(APE_{CD}\)，RPE计算的是\(APE_{AB}\)和\(APE_{BC}\)间的差异\(RPE_{AC}\)、\(APE_{BC}\)和\(APE_{CD}\)间的差异\(RPE_{BD}\)，\(RPE_{i,j} = APE_{i}^{-1}APE_{j}\)。RPE可以反映SLAM轨迹与真值的局部一致性。
明白RPE的计算规则后，其具体的计算也十分简单了。因为APE依旧是一个4x4的变换矩阵，所以和前面APE的计算公式相同。详细可以参考这篇博客。各部分如下：
- 平移部分：误差等于平移部分模长，三维空间即三维欧氏距离。
- 旋转部分：旋转部分有两种不同评价方式，分别是角误差和矩阵误差。
- 角误差：将旋转矩阵通过罗德里格斯公式转化为对应旋转向量，然后以旋转向量表示的角度代表误差(弧度角度表示都可以)。
- 矩阵误差：计算旋转矩阵减去单位阵结果的F范数(理论上如果没有旋转，旋转矩阵应该为单位阵)。F范数是把矩阵中每个元素的平方求和后开根号。
- 总体误差：对整个变换矩阵进行评价，评价方式是将变换矩阵减去对应的单位阵，然后结果计算F范数(和角度的矩阵误差方法一样)。
对于一段轨迹，重复上述步骤，即可计算得到多个RPE，我们可以对其计算RMSE。这个RMSE结果就是这段轨迹的相对轨迹误差(RTE)，如下：
\[RTE = \sqrt{\frac{1}{N} \sum_{i=1}^{N} RPE_{i}^{2}}\]一个简单的计算RTE的代码如下：
import numpy as np
import math
def composeTransMat(pose):
trans_mat = np.array([[0, 0, 0, 0], [0, 0, 0, 0], [0, 0, 0, 0], [0, 0, 0, 1]], float)
qx = pose[0]
qy = pose[1]
qz = pose[2]
qw = pose[3]
dx = pose[4]
dy = pose[5]
dz = pose[6]
# 求四元数的模长
qmod = math.sqrt(qx * qx + qy * qy + qz * qz + qw * qw)
# 四元数归一化
qx = qx / qmod
qy = qy / qmod
qz = qz / qmod
qw = qw / qmod
# 计算旋转矩阵
rot = np.array([[0, 0, 0], [0, 0, 0], [0, 0, 0]], float)
rot[0, 0] = 1 - 2 * (qy * qy + qz * qz)
rot[0, 1] = 2 * (qx * qy - qw * qz)
rot[0, 2] = 2 * (qx * qz + qw * qy)
rot[1, 0] = 2 * (qx * qy + qw * qz)
rot[1, 1] = 1 - 2 * (qx * qx + qz * qz)
rot[1, 2] = 2 * (qy * qz - qw * qx)
rot[2, 0] = 2 * (qx * qz - qw * qy)
rot[2, 1] = 2 * (qy * qz + qw * qx)
rot[2, 2] = 1 - 2 * (qx * qx + qy * qy)
# 旋转矩阵(与Matlab quat2dcm函数保持一致)
rot = rot.transpose()
# 组装变换矩阵
trans_mat[0, 0] = rot[0, 0]
trans_mat[0, 1] = rot[0, 1]
trans_mat[0, 2] = rot[0, 2]
trans_mat[1, 0] = rot[1, 0]
trans_mat[1, 1] = rot[1, 1]
trans_mat[1, 2] = rot[1, 2]
trans_mat[2, 0] = rot[2, 0]
trans_mat[2, 1] = rot[2, 1]
trans_mat[2, 2] = rot[2, 2]
trans_mat[0, 3] = dx
trans_mat[1, 3] = dy
trans_mat[2, 3] = dz
return trans_mat
if __name__ == '__main__':
# Poses: qx, qy, qz, qw, x, y, z
pose_ref1 = composeTransMat([-0.757610, -0.348629, -0.497711, 0.238261, 4.460675, -1.680515, 0.579614])
pose_ref2 = composeTransMat([-0.518605, -0.636519, -0.358444, 0.444310, 3.704039, 1.424990, 1.403680])
pose_ref3 = composeTransMat([-0.514605, -0.636519, -0.358444, 0.444310, 3.504039, 1.444990, 1.443680])
pose_ref4 = composeTransMat([-0.510605, -0.636519, -0.358444, 0.444310, 3.404039, 1.454990, 1.453680])
pose_ref5 = composeTransMat([-0.507605, -0.636519, -0.358444, 0.444310, 3.204039, 1.464990, 1.463680])
pose_est1 = composeTransMat([-0.757610, -0.348629, -0.497711, 0.238261, 4.460675, -1.680515, 0.579614]) # 旋转平移完全一致
pose_est2 = composeTransMat([-0.518605, -0.636519, -0.358444, 0.444310, 3.704039, 1.434990, 1.413680]) # 旋转一致，位置有误差
pose_est3 = composeTransMat([-0.534605, -0.636519, -0.258444, 0.454310, 3.504039, 1.444990, 1.443680]) # 旋转有误差，位置一致
pose_est4 = composeTransMat([-0.520605, -0.626519, -0.458444, 0.424310, 3.404039, 1.444990, 1.463680]) # 旋转位置都有误差
pose_est5 = composeTransMat([-0.557605, -0.616519, -0.358444, 0.414310, 3.204039, 1.454990, 1.453680]) # 旋转位置都有误差
# 计算绝对位姿误差(APE)
ape1 = np.matmul(np.linalg.inv(pose_ref1), pose_est1)
ape2 = np.matmul(np.linalg.inv(pose_ref2), pose_est2)
ape3 = np.matmul(np.linalg.inv(pose_ref3), pose_est3)
ape4 = np.matmul(np.linalg.inv(pose_ref4), pose_est4)
ape5 = np.matmul(np.linalg.inv(pose_ref5), pose_est5)
# 计算相对位姿误差(RPE)
rpe1 = np.matmul(np.linalg.inv(ape1), ape2)
rpe2 = np.matmul(np.linalg.inv(ape2), ape3)
rpe3 = np.matmul(np.linalg.inv(ape3), ape4)
rpe4 = np.matmul(np.linalg.inv(ape4), ape5)
# 利用F范数计算总体的RPE误差
error1 = np.linalg.norm(rpe1 - np.eye(4), ord='fro')
error2 = np.linalg.norm(rpe2 - np.eye(4), ord='fro')
error3 = np.linalg.norm(rpe3 - np.eye(4), ord='fro')
error4 = np.linalg.norm(rpe4 - np.eye(4), ord='fro')
# 利用均方根误差RMSE计算RTE
rte = np.sqrt((error1**2 + error2**2 + error3**2 + error4**2) / 4)
print("Errors:")
print("RPE Error1:", error1)
print("RPE Error2:", error2)
print("RPE Error3:", error3)
print("RPE Error4:", error4)
print("RTE Error:", rte)
5.卡尔曼滤波器
关于卡尔曼滤波器的详细介绍和使用说明见这篇博客，这里只是简单介绍。
5.1 什么是卡尔曼滤波
卡尔曼滤波是一种实时状态估计算法，用于在噪声环境中通过结合传感器观测值和预测模型，输出最优系统状态（如位置或速度）。其核心思想是动态平衡预测值和观测值，通过权重（卡尔曼增益）调整估计结果，确保方差最小化以提高精度。卡尔曼滤波的核心价值在于其数学最优性（如协方差最小化）和适应性，广泛应用于需要高精度、低延迟状态估计的场景。
5.2 它能用来解决什么问题
详见这篇博客。卡尔曼滤波作为一种最优状态估计算法，主要解决动态系统中的噪声干扰问题，通过融合预测模型与传感器观测值输出高精度状态估计。更具体说包含两类场景：
- 感兴趣的变量无法直接观测，需要通过一个或多个带有噪声的观测量计算得到，通过卡尔曼滤波可给出感兴趣变量的最优估计。
- 有多个传感器同时观测，需要做传感器数据的动态融合，通过卡尔曼滤波可给出最优融合结果。
5.3 卡尔曼滤波步骤
详见这篇博客。Kalman Filter包含两步：状态预测(Predict)与测量更新(Update)，进一步分为5个步骤：
- Step1. 根据上一个状态，结合给定模型，预测当前时刻状态：
- Step2. 根据给定误差和上一状态误差，计算当前时刻预测状态对应的误差：
- Step3. 根据上一状态预测误差、给定的观测误差和观测矩阵，计算卡尔曼增益(这里括号外的-1对于标量表示分数，对于矩阵表示求逆)：
- Step4. 根据当前预测状态、实际观测和观测矩阵，结合卡尔曼增益平衡预测与观测权重，修正状态预测：
- Step5. 根据计算得到的卡尔曼增益、上一个状态预测误差、给定的观测矩阵，计算修正后状态对应的误差：
这里简单展示利用卡尔曼滤波估算温度的例子：需要测量火箭发动机内部的温度，但因为温度太高无法直接测量，只能间接测量外部温度。现在需要基于观测预测对应时刻最优的内部温度。完整示例见这里。代码如下：
import numpy as np
# 给定参数
H = 0.5 # 观测矩阵
F = 1.0 # 状态转移矩阵
B = 100.0 # 控制输入矩阵
Q = 0.1 # 过程噪声协方差矩阵
R = 0.5 # 测量噪声协方差矩阵
# 指定理论值，方便评估精度，实际无法获得
ideal_value = np.zeros(12) + 200
# 系列观测与控制输入
z = [102, 104, 98, 99.7, 102, 101.5, 100.4, 101.6, 103.8, 97, 102.1, 99.3]
u = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
# 给状态赋初值
prev_state = z[0] / H
prev_error = 1.0
# 用于存放估计的状态
predict_states = []
measure_states = []
# 开始卡尔曼滤波
for i in range(len(z)):
# 预测 - 状态
predict_state = F * prev_state + B * u[i]
# 预测 - 误差
predict_error = F * prev_error * F + Q
# 更新 - 卡尔曼增益
kalman_gain = (predict_error * H) / (H * predict_error * H + R)
# 更新 - 状态
update_state = predict_state + kalman_gain * (z[i] - H * predict_state)
# 更新 - 误差
update_error = (1 - kalman_gain * H) * predict_error
# 变量赋值开启下次迭代
prev_state = update_state
prev_error = update_error
# 将预测状态保存起来
predict_states.append(update_state)
measure_states.append(z[i] / H)
# 精度评价
measure_errors = np.abs(measure_states - ideal_value)
predict_errors = np.abs(predict_states - ideal_value)
for i in range(len(predict_errors)):
print(i+1, "/", len(predict_errors), "predict error:", round(predict_errors[i], 5), "measure error:", round(measure_errors[i], 5))
print("mean measure", round(np.mean(measure_errors), 5))
print("mean predict", round(np.mean(predict_errors), 5))
6.IMU惯导
本部分主要参考这篇博客。
6.1 常见IMU噪声
详见这篇博客，这里简单总结。IMU主要由陀螺仪和加速度计组合而成，陀螺仪主要测量角速度，加速度计测量线性加速度。其主要包含以下几种噪声：
-
常量偏置(Constant Bias)：偏置是指传感器没有经历任何运动的情况下，陀螺仪/加速度计的平均输出。因为理论上，传感器没有经历任何旋转，输出的角速度以及一段时间内的角速度均值都为0、加速度也为重力加速度保持不变。消除方式：可以将传感器静止放置在某处，记录其长时间的输出并对其求平均。这个平均值就被作为该IMU的常量偏置。对于每个IMU输出，都减去这个偏置量即可。
-
热机械白噪声/随机游走(Thermo-Mechanical White Noise/Random Walk)：传感器会受到一些热机械噪声的影响，它的频率远比传感器的采样频率要高。这种机械噪声会给计算结果引入一种零均值随机游走误差，角度随机游走噪声的标准差与时间的平方根成比例、加速度随机游走噪声的标准差与时间的3/2次方成比例。随机游走噪声可以看作是热机械噪声的表现，反过来，热机械噪声是随机游走噪声的成因。
-
闪变噪声/偏置稳定性(Flicker Noise/Bias Stability)：由于电子器件的闪变噪声(Flicker Noise)影响，MEMS陀螺仪的偏置也不会一直不变，而是随时间游走。
-
温度影响(Temperature Effects)：由于气温变化导致的误差随时间线性增长。对于MEMS陀螺仪来说，偏置与温度的关系通常是高度非线性的。有些IMU自带了温度传感器，可以用于校正。
-
校正误差(Calibration Errors)：主要指尺度因子、对齐、线性化等误差。这些误差通常只有在IMU启动的时候才会被观测到，并且会导致累积漂移，它们的幅度与运动的速度和时长成比例。
6.2 IMU位姿推理
此部分参考这篇博客内容。
一个惯性系统相对于全局参考系的朝向/姿态可以通过来自系统速率陀螺仪测量的角速度信号\(\omega_{b}(t) = (\omega_{bx}(t),\omega_{by}(t),\omega_{bz}(t))^{T}\)积分获得(原理是角动量守恒)。对于一个IMU而言，其陀螺仪的观测是x、y、z方向上的角速度。因此，利用陀螺仪的测量推算位姿，其实就是在求某一时刻的旋转矩阵。理论公式：
\[C(t) = C(0) \cdot exp(\int_{0}^{t}\Omega(t) dt )\]对于\(t\)时刻的姿态，我们只需要知道起始时刻的姿态，以及当前\(t\)时刻的角速度，并对其从0到\(t\)做积分，并与初始姿态相乘即可得到当前t时刻的姿态\(C(t)\)。
在实际解算中，对上式进行一系列离散化与线性化(泰勒展开)，最终公式如下：
\[C(t+ \delta t) = C(t) (I+\frac{sin\sigma}{\sigma}B+\frac{1-cos\sigma}{\sigma^2}B^2)\]其中，陀螺仪的相邻两次观测的时间间隔为\(\delta t\)，\(\sigma = |w_{b}\delta t|\)，\(\omega_{b} = (\omega_{bx},\omega_{by},\omega_{bz})^{T}\)即为陀螺仪观测。\(B = \begin{bmatrix} 0 & -w_{bz}\delta t & w_{by}\delta t\\ w_{bz}\delta t & 0 & -w_{bx}\delta t\\ -w_{by}\delta t & w_{bx}\delta t & 0 \end{bmatrix}\)。
在这个式子中，我们拿到陀螺仪观测和时间间隔以后，就可以直接计算了。
下面给出按照上述理论进行位姿推理的代码：
import numpy as np
from math import sin, cos
def skew_symmetric(w):
"""构建反对称矩阵"""
return np.array([[0, -w[2], w[1]],
[w[2], 0, -w[0]],
[-w[1], w[0], 0]])
def update_rotation_matrix(C, gyro, dt):
"""更新方向余弦矩阵"""
w = gyro * dt # 角速度积分
sigma = np.linalg.norm(w)
B = skew_symmetric(w)
if sigma < 1e-6:
return C # 无显著旋转
# 泰勒展开近似
C_new = C @ (np.eye(3) +
(sin(sigma)/sigma)*B +
((1-cos(sigma))/sigma**2)*B@B)
return C_new / np.linalg.norm(C_new) # 归一化
def rotation_to_euler(C):
"""转换为欧拉角(弧度)"""
pitch = np.arcsin(-C[2,0])
roll = np.arctan2(C[2,1], C[2,2])
yaw = np.arctan2(C[1,0], C[0,0])
return np.array([roll, pitch, yaw])
# 初始化参数
C = np.eye(3) # 初始姿态矩阵
gyro_bias = np.array([0.01, -0.005, 0.003]) # 陀螺仪零偏
dt = 0.01 # 采样周期(秒)
# 模拟陀螺仪数据流
gyro_readings = [
np.array([0.1, 0.05, 0.02]), # rad/s
np.array([0.1, 0.05, 0.02]),
np.array([0.1, 0.05, 0.02]),
np.array([0.1, 0.05, 0.02]),
np.array([0.12, 0.03, 0.01]),
np.array([0.12, 0.03, 0.01]),
np.array([0.12, 0.03, 0.01]),
np.array([0.12, 0.03, 0.01]),
np.array([-0.05, 0.1, 0.03]),
np.array([-0.05, 0.1, 0.03]),
np.array([-0.05, 0.1, 0.03]),
np.array([-0.05, 0.1, 0.03])
]
for gyro in gyro_readings:
# 1. 去除零偏
gyro_corrected = gyro - gyro_bias
# 2. 更新旋转矩阵
C = update_rotation_matrix(C, gyro_corrected, dt)
# 3. 转换为欧拉角显示
angles = np.degrees(rotation_to_euler(C))
print(f"Roll: {angles[0]:.2f}°, Pitch: {angles[1]:.2f}°, Yaw: {angles[2]:.2f}°")
6.3 IMU位置推理
此部分参考这篇博客内容。
在\(t\)时刻，我们只要知道了此时的姿态\(C(t)\)(由陀螺仪测量、计算)以及加速度计测量的加速度\(a_{b}(t)=(a_{bx}(t),a_{by}(t),a_{bz}(t))^{T}\)就可以一步步求得全局坐标系下的位置：
- 先将加速度转化到全局坐标系，再减去重力，对剩余部分积分得到速度
- 再将初始速度加到积分的速度上得到总速度
- 最后对速度再做一次积分，得到位移
- 再加上初始位置，即为最终在全局坐标系下的位置
计算公式如下：
\[s_{g}(t) = s_{g}(0) + \int_{0}^{t}(v_{g}(0) + \int_{0}^{t}(C(t)a_{b}(t) - g_{g}(t)) dt)dt\]在实际实现时，对积分过程按照矩形法则线性化和离散化，最终公式如下：
\[\left\{\begin{align} & a_{g}(t) = C(t)a_{b}(t) \\ & v_{g}(t+\delta t) = v_{g}(t) + \delta t \cdot (a_{g}(t+\delta t) - g_{g}) \\ & s_{g}(t+\delta t) = s_{g}(t) + \delta t \cdot v_{g}(t+\delta t) \end{align}\right.\]依据上述公式，利用Python实现代码如下：
import numpy as np
from math import sin, cos, sqrt
def initialize_navigation():
# 初始化导航参数
C = np.eye(3) # 方向余弦矩阵(初始为单位矩阵)
v = np.zeros(3) # 速度向量(全局坐标系)
s = np.zeros(3) # 位置向量(全局坐标系)
g = np.array([0, 0, 9.8]) # 重力加速度
gyro_bias = np.array([0.01, -0.02, 0.005]) # 陀螺仪零偏
accel_bias = np.array([0.05, -0.03, 0.1]) # 加速度计零偏
return C, v, s, g, gyro_bias, accel_bias
def update_attitude(C, gyro, dt):
# 罗德里格斯旋转公式更新姿态
w = gyro * dt
sigma = np.linalg.norm(w)
if sigma < 1e-6:
return C
# 构建反对称矩阵
B = np.array([
[0, -w[2], w[1]],
[w[2], 0, -w[0]],
[-w[1], w[0], 0]
])
# 旋转矩阵更新
C_new = C @ (np.eye(3) +
(sin(sigma)/sigma)*B +
((1-cos(sigma))/sigma**2)*B@B)
# 正交化处理
U, _, Vt = np.linalg.svd(C_new)
return U @ Vt
def update_position(C, v, s, accel, g, dt):
# 坐标系转换和位置更新
a_global = C @ accel - g # 转换到全局坐标系并消除重力
v_new = v + a_global * dt # 速度积分
s_new = s + v_new * dt # 位置积分
return v_new, s_new
def inertial_navigation(gyro_data, accel_data, sample_rate=100):
# 主处理流程
C, v, s, g, gyro_bias, accel_bias = initialize_navigation()
dt = 1/sample_rate
positions = []
attitudes = []
for i in range(len(gyro_data)):
# 传感器校准
gyro = gyro_data[i] - gyro_bias
accel = accel_data[i] - accel_bias
# 更新姿态
C = update_attitude(C, gyro, dt)
# 更新位置
v, s = update_position(C, v, s, accel, g, dt)
# 存储结果
positions.append(s.copy())
attitudes.append(C.copy())
return np.array(positions), np.array(attitudes)
if __name__ == "__main__":
# 模拟传感器数据(100Hz采样率)
duration = 5 # 秒
sample_rate = 100 # Hz
t = np.linspace(0, duration, duration*sample_rate)
# 模拟陀螺仪数据(rad/s)
gyro_x = 0.3 * np.sin(2*np.pi*0.5*t)
gyro_y = 0.2 * np.cos(2*np.pi*0.3*t)
gyro_z = 0.4 * np.sin(2*np.pi*0.2*t)
gyro_data = np.vstack((gyro_x, gyro_y, gyro_z)).T
# 模拟加速度计数据(m/s^2)
accel_x = 0.1 * np.sin(2*np.pi*0.4*t)
accel_y = 0.15 * np.cos(2*np.pi*0.6*t)
accel_z = 9.8 + 0.2 * np.sin(2*np.pi*0.1*t)
accel_data = np.vstack((accel_x, accel_y, accel_z)).T
# 主处理流程
C, v, s, g, gyro_bias, accel_bias = initialize_navigation()
dt = 1/sample_rate
positions = []
attitudes = []
for i in range(len(gyro_data)):
# 传感器校准
gyro = gyro_data[i] - gyro_bias
accel = accel_data[i] - accel_bias
# 更新姿态
C = update_attitude(C, gyro, dt)
# 更新位置
v, s = update_position(C, v, s, accel, g, dt)
# 存储结果
positions.append(s.copy())
attitudes.append(C.copy())
# 打印展示
print("position:", s)
print("orientation:", C)
可以看到，只需要知道加速度计的观测以及此时在全局坐标系下的姿态，即可以推导位置。总体而言，位置计算是依赖于陀螺仪的积分结果的。因为设计到某时刻本体坐标系和全局坐标系之间的转换关系，这个转换关系只有通过陀螺仪才能得到。这样做的结果是，如果陀螺仪有了比较大的误差，那么位置的估计也不会好。
本文作者原创,未经许可不得转载,谢谢配合
