第2章 传感器技术与数据处理

当一套SLAM系统启动时，第一件事是“看见世界”。但原始数据往往充满噪声、畸变与不同时间轴的错位：如果不把它们处理为“可信输入”，后面的特征提取、配准与优化都会在沙滩上建房。本章从一个工程问题出发——数据从哪里来、怎样变得可信——以叙述的方式串起视觉、激光与IMU三类传感器的原理与处理流程，并解释为什么标定与时间对齐是系统能否稳定的分水岭。

—

2.1 视觉：看见世界的尺度与畸变

视觉传感器是许多SLAM的首选，但“看见”并不等于“看准”。单目相机缺失绝对尺度，双目相机需要合适的基线与精确标定，RGB-D在室内表现出色却受量程与材质影响。工程实践的核心，是把相机的几何与成像过程描述清楚，然后用标定与去畸变让图像落在可计算的空间里。

你会反复用到的关键环节包括：针孔相机模型与畸变校正、内外参标定、双目极线校正、曝光与白平衡控制，以及光度一致性处理（为直接法准备）。这些环节决定了后续特征的稳定性与匹配的可重复性。

下面的示例给出“相机标定与去畸变”的最小流程，你可以按此扩展到双目与RGB-D场景：

代码示例：相机标定与去畸变

"""相机标定与去畸变示例（需安装opencv-python）"""
import cv2 as cv
import numpy as np
from glob import glob# 标定参数
grid_size = (9, 6)  # 内角点数量
square_size = 0.024  # 棋盘格物理边长(米)# 世界坐标系下角点三维坐标
def create_object_points(grid_size, square_size):objp = np.zeros((grid_size[0]*grid_size[1], 3), np.float32)objp[:, :2] = np.mgrid[0:grid_size[0], 0:grid_size[1]].T.reshape(-1, 2)return objp * square_sizeobjpoints, imgpoints = [], []
objp = create_object_points(grid_size, square_size)# 读取标定图片
images = sorted(glob('calib/*.jpg'))  # 放置多张棋盘格图片于calib目录
if len(images) < 8:print('警告：标定图片较少，建议>=10张')for f in images:img = cv.imread(f)gray = cv.cvtColor(img, cv.COLOR_BGR2GRAY)ret, corners = cv.findChessboardCorners(gray, grid_size,flags=cv.CALIB_CB_FAST_CHECK)if ret:corners_refined = cv.cornerSubPix(gray, corners, (11, 11), (-1, -1),criteria=(cv.TERM_CRITERIA_EPS+cv.TERM_CRITERIA_MAX_ITER, 30, 1e-3))objpoints.append(objp)imgpoints.append(corners_refined)cv.drawChessboardCorners(img, grid_size, corners_refined, ret)cv.imshow('corners', img); cv.waitKey(50)ret, K, dist, rvecs, tvecs = cv.calibrateCamera(objpoints, imgpoints, gray.shape[::-1], None, None)
print('内参K=\n', K)
print('畸变参数=', dist.ravel())# 去畸变
sample = cv.imread(images[0]) if images else np.zeros((480, 640, 3), dtype=np.uint8)
newK, roi = cv.getOptimalNewCameraMatrix(K, dist, (sample.shape[1], sample.shape[0]), 1, (sample.shape[1], sample.shape[0]))
undistorted = cv.undistort(sample, K, dist, None, newK)
cv.imshow('undistorted', undistorted); cv.waitKey(0); cv.destroyAllWindows()

视觉处理的常见工程流水线如下：

流程图

---

2.2 激光雷达：结构的真实与工程取舍

与视觉不同，激光雷达直接测量几何结构，常在建图一致性与抗光照方面占优。2D雷达在平面场景高效可靠，3D雷达可提供更完整的空间信息，但数据体量大、计算资源与成本更高。让点云“可用”的关键，是时间畸变补偿（运动校正）、下采样与离群点去除，以及与其它传感器的外参标定。

下面用一个例子展示“下采样与去噪”的基本流程，作为接入ICP/NDT前的常见预处理：

代码示例：点云下采样与去噪

"""使用Open3D进行点云下采样与离群点去除（pip install open3d）"""
import open3d as o3dpcd = o3d.io.read_point_cloud('lidar.pcd')  # 替换为你的点云文件
print(pcd)# 体素下采样
down = pcd.voxel_down_sample(voxel_size=0.05)# 统计离群点去除
cl, ind = down.remove_statistical_outlier(nb_neighbors=20, std_ratio=2.0)
filtered = down.select_by_index(ind)print('原始点数:', np.asarray(pcd.points).shape[0])
print('过滤后点数:', np.asarray(filtered.points).shape[0])o3d.visualization.draw_geometries([filtered], window_name='Filtered Point Cloud')

典型的点云预处理流水线如下：

流程图

---

2.3 IMU与融合：把快速直觉变成稳定理解

IMU像是系统的“前庭系统”，以高频率提供短时稳定的姿态变化信息。它的价值在于让前端在快速运动与弱纹理场景下更稳、更灵敏；它的挑战在于偏置、噪声与积分漂移。工程上，常见策略是做去偏与滤波，采用预积分理论与时间同步把IMU与视觉/激光紧密结合。

下面的示例演示一个最简的“IMU去偏置与姿态积分”，帮助你在沙箱里验证基本过程：

代码示例：简单的IMU去偏置与积分

"""IMU去偏置与姿态积分示例"""
import numpy as npclass SimpleIMUIntegrator:def __init__(self, accel_bias=np.zeros(3), gyro_bias=np.zeros(3), dt=0.01):self.accel_bias = accel_biasself.gyro_bias = gyro_biasself.dt = dtself.v = np.zeros(3)self.p = np.zeros(3)self.R = np.eye(3)  # 旋转矩阵@staticmethoddef hat(w):return np.array([[0, -w[2], w[1]],[w[2], 0, -w[0]],[-w[1], w[0], 0]])def step(self, acc_meas, gyro_meas):# 去偏置acc = acc_meas - self.accel_biasgyro = gyro_meas - self.gyro_bias# 姿态更新（一次指数映射近似）dR = np.eye(3) + self.hat(gyro) * self.dtself.R = self.R @ dR# 重力补偿g = np.array([0, 0, -9.81])a_world = self.R @ acc + g# 速度与位置积分self.v += a_world * self.dtself.p += self.v * self.dtreturn self.p, self.v, self.R# 简单测试
if __name__ == '__main__':imu = SimpleIMUIntegrator(accel_bias=np.array([0.01, -0.01, 0.02]), gyro_bias=np.array([0.001, 0.002, -0.001]))for _ in range(100):acc = np.array([0.0, 0.0, 0.0])  # 静止gyro = np.array([0.0, 0.0, 0.0])p, v, R = imu.step(acc, gyro)print('积分结果位置:', p)

---

2.4 标定与时间同步：把不同“眼睛”放到同一世界

当系统有多种传感器时，核心问题变成：“它们各自的坐标系如何对齐？它们的时间轴是否一致？”这就是外参标定与时间同步。相机-IMU常用时空对齐与预积分残差，相机-雷达可以用板法或AprilTag做几何标定，也可以通过ICP与PnP联合优化得到更精确的外参。

下面给出手眼标定AX=XB的旋转部分快速求解，用于理解标定的数学结构：

代码示例：手眼标定求解（最小二乘）

"""手眼标定AX=XB的旋转部分求解（基于四元数平均）"""
import numpy as np
from scipy.spatial.transform import Rotation as Rdef solve_hand_eye_rot(A_list, B_list):# A_list, B_list: 多组相对位姿变换的旋转R_Ai, R_BiQ = np.zeros((4, 4))for RA, RB in zip(A_list, B_list):rA = R.from_matrix(RA).as_quat()  # xyzwrB = R.from_matrix(RB).as_quat()# 约束：q_X * q_B = q_A * q_X# 写成 Aq = 0 的形式，累积到Q中（简化演示）L = np.array([[ rB[3],  rB[2], -rB[1], -rB[0]],[-rB[2],  rB[3],  rB[0], -rB[1]],[ rB[1], -rB[0],  rB[3], -rB[2]],[ rB[0],  rB[1],  rB[2],  rB[3]]])Rm = np.array([[ rA[3], -rA[2],  rA[1],  rA[0]],[ rA[2],  rA[3], -rA[0],  rA[1]],[-rA[1],  rA[0],  rA[3],  rA[2]],[-rA[0], -rA[1], -rA[2],  rA[3]]])Q += (L - Rm).T @ (L - Rm)# 最小特征值对应的特征向量eigvals, eigvecs = np.linalg.eigh(Q)qx = eigvecs[:, np.argmin(eigvals)]qx = qx / np.linalg.norm(qx)return R.from_quat(qx).as_matrix()

—

2.5 小结与下一步

数据可靠，系统才可靠。本章从“数据如何成为可信输入”出发，贯穿了视觉的几何与光度处理、激光的结构化预处理、IMU的高频稳定性，以及把多源数据放到同一坐标与时间轴上的标定与同步。它们共同决定了前端是否稳健、后端是否能收敛。