Secret Land

SLAM基础知识速记

Tue, 09 Dec 2025 00:00:00 +0000

请注意:内容可能包含大量图表、公式、代码等，若RSS无法正确显示可直接访问原始网址，获得最好的阅读体验。

在9月份面试的时候，整理了几张SLAM基础知识的相关笔记，里面有些内容还不错，言简意赅。所以这里简单将其再贴出来，以备日后查阅使用。

1.ORB特征提取与匹配

提取：FAST角点 + BREIF描述子
匹配：暴力匹配

2.帧间位姿估计

2.1 2D-2D

原理：对极几何
步骤：变换矩阵求解 + 矩阵分解求位姿
变换矩阵类型包括：
- 本质矩阵 - 非平面场景、内参已知
- 基础矩阵 - 非平面场景、内参未知
- 单应矩阵 - 平面场景、内参未知
矩阵分解核心：SVD分解，利用SVD分解求最小二乘解，得到\(\mathbf{U}\)、\(\mathbf{\Sigma}\)、\(\mathbf{V}\)。

2.2 2D-3D

本质：PnP问题(Perspective-n-Point)
解法：直接线性变换DLT、P3P、EPnP等
- DLT：简单、速度快、精度低，要求≥6对点
- P3P：最低3对点即可，但易受噪声影响
- EPnP：线性复杂度、精度可迭代，要求≥4对点
- 一般默认都采用EPnP解法

2.3 3D-3D

核心是构建对应方程，常见有SVD法和ICP法
SVD法：基于方程直接SVD分解，然后根据\(\mathbf{U}\)、\(\mathbf{\Sigma}\)、\(\mathbf{V}\)，按公式计算旋转、平移。SVD分解中协方差矩阵的物理意义表示两组XYZ系统互相关系，大小为3×3。SVD法具体步骤如下：
- (1) 计算质心
- (2) 构建协方差矩阵
- (3) 去中心化
- (4) SVD分解
- (5) 计算旋转矩阵
- (6) 计算平移向量
ICP法：迭代最近邻法(Iterative Closest Point)。匹配源点云和目标点云对应点，最小化对应点间的距离残差。优点：简单高效；缺点：对初值敏感。ICP法步骤如下：
- (1) 初始对齐/配准
- (2) 迭代优化
  - 找最近点
  - 剔除异常
  - 求解变换
  - 更新点云
- (3) 迭代终止
SVD方法：需要点云对应关系、不需要预配准
ICP方法：无需点云对应关系、需要预配准

3.RANSAC方法

背景：虽然最小二乘很好，但其对外点敏感
核心：找到一个适合于绝大多数样本的模型参数。其本身并非进化算法，并不会随着迭代次数的增加而精度一定更高，而是随机计算指定次数，从里面找到最好的。只能说随机次数越多，找到精度越高模型的概率越大。
步骤：
- (1) 随机选择指定数量点构建模型
- (2) 利用该模型计算整体内点个数
- (3) 选取内点最多对应的模型

4.卡尔曼滤波

核心为状态预测与测量更新两个步骤。
状态预测：
- (1) 根据上一个状态利用模型，预测当前状态
- (2) 根据给定误差与上一个状态误差，预测当前状态误差
测量更新：
- (3) 根据当前预测状态、实际观测和观测矩阵，计算卡尔曼增益
- (4) 根据当前预测状态、实际观测和观测矩阵，结合卡尔曼增益平衡观测与预测权重，修正当前状态预测
- (5) 根据卡尔曼增益、观测矩阵和上一个状态预测误差，修正更新后状态预测对应的误差

5.IMU

误差：常量偏置、偏置稳定性、随机游走机械白噪声、温度误差
姿态推理：陀螺仪
位置推理：陀螺仪+加速度计
误差消除/校正：
- 常量偏置：离线静置消除
- 偏置稳定性：滤波方法
- 白噪声：随机游走建模

6.高斯-牛顿与列文-马夸法比较

两者都用于非线性最小二乘求解
区别在于增量方程的构建
GN：残差小、初值好、实时高
LM：残差大、初值差、尺度不一

Item	GN法	LM法
收敛性	局部二次	线性-二次之间
远区初值	易发散	全局稳定
近区精度	高	可逼近GN精度
病态	步长畸变	数值稳定
参数敏感	敏感	数值稳定

一般使用LM方法更多。

6.1 LM方法

线性化，构建\(\mathbf{J}^{T}r\)与\(\mathbf{J}^{T}\mathbf{J}\)
算阻尼，解\((\mathbf{J}^{T}\mathbf{J}+\mu l)\Delta x = - \mathbf{J}^{T}r\)
判进退，误差下降接受\(\mu / 3\)，否则\(2\mu\)
循环直到\(\left \| \Delta x \right \| < \varepsilon\)

6.2 GN方法

计算残差\(r\)和雅可比矩阵\(\mathbf{J}\)
解线性方程\((\mathbf{J}^{T}\mathbf{J})\Delta x=-\mathbf{J}^{T}r\)
更新\(x\leftarrow x+\Delta x\)，直到\(\left \| \Delta x \right \| < \varepsilon\)

GN方法中，\(\mathbf{J}^{T}\mathbf{J}\)近似为海塞矩阵。其总体表示误差曲面的形状张量，对角线表示沿i参数的自弯曲，非对角元素表示ij参数的耦合弯曲。

7.双视图BA

核心：同时优化位置与姿态，最小化重投影误差
节点：相机位姿、特征点空间位置
边：投影关系
线性化 - 正规方程 - 迭代更新

8.滤波方法与图优化方法对比

Item	滤波方法	图优化方法
时间复杂度	\(O(n^2)\)(协方差矩阵)	\(O(n)\)稀疏图
内存占用	状态量平方	边稀疏
优点	递归，单步可行，嵌入式友好	精度高，支持回环
缺点	存储\(O(n^2)\)，误差累积	需要良好初值，依赖专用库
代表	EKF-SLAM、MSCKF	ORB-SLAM3、VINS-Fusion
适用	资源受限、小场景、无回环	大场景、回环、高精度

9.信息矩阵

表示对不确定度按方向的打分，等于协方差矩阵的逆(不确定度的倒数)。
信息矩阵特征值越大则方向越确定，信息矩阵决定残差边权重。
非对角块：位姿&路标间的信息耦合程度
对角块：总观测确定度

10.ORB-SLAM中的优化

基于g2o库构建

10.1 只优化位姿(Motion-Only BA)

节点：当前位姿(no fixed)
边：地图点观测(fixed)

10.2 同时优化地图点和位姿(Local BA)

节点：直接相邻KeyFrame、间接相邻KeyFrame、地图点
边：单/双目边观测

10.3 视觉-IMU联合优化

核心为：共视关系、时序关系
节点：时序关键帧位姿、时序关键帧IMU、局部视觉关键帧位姿、固定关键帧位姿、固定关键帧IMU、地图点
边：单目边、双目边、惯性边、陀螺仪偏置边、加速度计偏置边
与纯视觉BA相比，增加了时序相邻关键帧的IMU观测。
共视用于视觉优化、时序用于惯性优化，两者共同作用于当前位姿节点，使其优化。

11.直接法与特征点法对比

Item	特征点法	直接法
数据关联	特征提取&匹配	最小化光度误差
误差函数	重投影误差	光度误差
光照鲁棒性	鲁棒	敏感(恒定假设)
纹理	弱纹理易失败	无纹理也可运行(基于梯度)
代表方法	ORB-SLAM	DSO、LSD-SLAM

12.SLAM中的回环

SLAM中包含回环检测和回环校正两方面。

12.1 回环检测

主要利用BoW词袋法，实现逐步筛选，包含两个步骤：

共视帧BoW相似性计算
共视一致性检查(几何检查)

几何检查包括：共视一致性、尺度一致性、旋转/平移合理性、双向投影误差

12.2 回环校正

一次性消除累积误差，步骤：计算sim3闭环边 - 传播位姿 - 融合地图点 - 本质图对齐 - 后台全局BA

利用RANSAC-PnP计算sim3作为闭环约束边(相似变换)
将sim3传播给回环帧的所有共视帧并更新位置
地图点融合与替换
更新共视图与生成树，只优化闭环相关因子图
启动全局BA

13.张正友标定法

棋盘格准备并拍摄
检测角点
构造世界坐标
对每张图求解单应矩阵(3D-2D)
构造约束，解线性内参，Cholesky分解
非线性Refine，最小化投影误差
LiDAR-FAST-Calib、IMU-Kalibr

14.ORB-SLAM中IMU的初始化

首先基于纯视觉优化位姿
然后进行纯IMU优化
最后进行视觉-IMU联合优化
参数：重力方向、尺度、陀螺仪偏置、加速度计偏置、速度

15.IMU预积分

把高频IMU观测压缩为关键帧间的一个紧凑因子，避免逐帧误差累积、重复利用。

包含：相对旋转、速度、位置、协方差雅可比矩阵

16.回环检测和系统重定位异同

重定位用于在LOST后继续Tracking，而回环检测是为了构成回环约束条件。前者更松、实现不同。前者在Tracking中，后者在LoopClosing中。

17.回环本质图

压缩版共视图，只保留生成树边+回环边+强共视边，节点数不变。边的计算复杂度从\(O(n^2)\)到\(O(n)\)。

生成树边：父子关系，保证联通
回环边：Sim3验证后添加
强共视边：阈值判别

18.数组(Array)

直接访问元素，时间复杂度为O(1)

优点：访问速度快，内存紧凑
缺点：无法动态扩展，不适合动态数据

19.链表(Linked List)

动态调整大小，线性查找，复杂度为O(n)

优点：动态查找，适合频繁的插入/删除
缺点：随机访问效率低，不如数组

20.栈(Stack)

后进先出LIFO，常用于递归，深度优先搜索

优点：操作简单，效率高
缺点：只能在栈顶操作，访问其它元素需弹出(无法随机访问)

21.队列(Queue)

先进先出FIFO，常用于广度优先搜索、任务调度等任务

优点：适合按顺序处理场景
缺点：无法随机访问元素

22.双端队列(Deque)

允许两端进行插入、删除，是栈和队列的结合体

优点：灵活双向操作
缺点：空间占用较大

23.哈希表(Unordered_map)

键值对存储数据，无序，查找特定元素时间复杂度为O(1)

24.映射(Map)

有序键值对，底层是红黑树，查找、插入、删除复杂度为O(logn)

优点：元素有序
缺点：效率比unordered_map低

25.集合(Set)

存储唯一元素的有序集合，底层是红黑树

优点：自动排序且保证唯一
缺点：插入、删除不如无序集合

26.动态数组(Vector)

优点：动态容量，支持随机访问
缺点：插入、删除中间元素效率低

27.C++面向对象开发特性

封装、继承、多态、抽象

封装：将数据和方法组合在一起，对外隐藏细节，对外提供接口
继承：从已有类中派生出新类，可修改扩展属性和方法
多态：同一种操作用于不同对象，可以有不同实现
抽象：从具体实例中提取特征形成抽象接口，以便扩展

28.嵌入式平台SLAM优化方向

算法：轻量化
模型：压缩
硬件：协同设计
系统：部署与调度

29.SLAM线程

包含Tracking(主)、LocalMapping、LoopClosing、Optimizer。

30.代码版本控制与合作

使用Git
分支管理：主分支、开发分支、功能分支、修复分支
代码审查
形成文档和知识共享
统一代码风格、备注
权限管理

31.位姿图优化

位姿图只包含位姿，用相对位姿边表达里程计和闭环，把误差修正问题转成稀疏图上的位姿图优化。

32.SLAM和SfM

SLAM：实时、在线、有序、定位
SfM：非实时、离线、无序、建图

33.ORB-SLAM3的特点

高精度、鲁棒、多传感器、多地图

ORB-SLAM在精度、回环、多地图、兼容性上有优势
VINS在高动态、弱纹理等有优势

长期一致性SLAM更优，短期VIO更优。

34.ROS

ROS中有Topic、Service、Action等类型。

Ubuntu启动自动挂载硬盘分区设置

Wed, 05 Nov 2025 00:00:00 +0000

请注意:内容可能包含大量图表、公式、代码等，若RSS无法正确显示可直接访问原始网址，获得最好的阅读体验。

在我的电脑上有Windows和Ubuntu两个系统，其中有一个共用的500GB的数据盘。Ubuntu系统每次启动都要手动挂载，才能进一步访问内容。这篇博客简单记录让Ubuntu自动挂载数据盘的功能。

1.查看想挂载盘的信息

主要包含两个信息：Device名称和ID。

1.1 Device名称

打开终端，输入如下命令：

sudo fdisk -l

可以在终端中看到硬盘相关信息。这里我需要在Ubuntu下挂载的是大小为551.6GB的数据盘。我们需要记住它的名称:/dev/nvme0n1p6。

1.2 ID

打开终端，根据我们刚刚得到的名称进一步查询对应UUID：

sudo blkid /dev/nvme0n1p6

输出如下所示。我们需要记住UUID属性，这里是D6DC-89B5。

2.创建挂载目录

然后需要明确，我们希望将这个硬盘挂载到Ubuntu系统的哪个路径下。经典路径是放在/mnt/文件夹下。如果你想使用的更加“无感”，也可以挂在你自己的主目录下。比如这里，我希望挂在/home/xuhui/Files下，所以新建对应文件夹，如下。

3.测试挂载效果

在终端中输入如下命令(名称和路径换成你自己的)，可以测试挂载效果：

sudo mount /dev/nvme0n1p6 /home/xuhui/Files

挂载好以后，再次打开文件夹可以看到，Files文件夹右上角多了个硬盘图标，这就表示挂载完成了，如下。我们可以在这个文件夹中新建或删除文件。

4.设置开机自动挂载

最后一步，我们需要将刚刚的过程设置为自动完成，主要通过修改/etc/fstab文件完成。输入如下命令打开文件：

sudo gedit /etc/fstab

然后在末尾添加如下内容(UUID和路径换成你自己的)：

UUID=D6DC-89B5 /home/xuhui/Files exfat defaults,uid=1000,gid=1000,umask=000 0 0

添加好以后如下图所示。简单解释各项。第一项为要挂载的盘的UUID，第二项是要挂载到的目录，第三项是挂载磁盘的文件系统格式，第四项为各种参数：defaults表示使用标准挂载选项、uid=1000,gid=1000表示让当前用户（第一个创建的用户）拥有读写权限、umask=000表示所有人可读写执行，最后两个0表示不进行dump/检查。

重新启动电脑，会发现，系统已经自动挂载硬盘了。至此完成了开头提到的任务。额外说明，强烈建议使用exfat格式的分区作为共享数据盘，不要使用NTFS格式。因为NTFS是Windows专属的文件格式，Ubuntu对其支持不够完全，导致可能出现Ubuntu下对文件的修改在Windows下看不到、数据丢失等严重情况。而exfat格式，不管是Ubuntu还是Windows都是原生支持。

5.参考资料

[1] https://blog.csdn.net/qq_35451572/article/details/79541106

SLAM常见功能代码段整理汇总2

Wed, 10 Sep 2025 00:00:00 +0000

请注意:内容可能包含大量图表、公式、代码等，若RSS无法正确显示可直接访问原始网址，获得最好的阅读体验。

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范数(理论上如果没有旋转，旋转矩阵应该为单位阵)。F范数是把矩阵中每个元素的平方求和后开根号。详细可以参考这篇博客。
总体误差：我们也可以对整个变换矩阵进行评价，评价方式是将变换矩阵减去对应的单位阵，然后结果计算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. 根据上一个状态，结合给定模型，预测当前时刻状态：

\[\mathbf{x}_{t} = \mathbf{F}\cdot \mathbf{x}_{t-1} + \mathbf{B}\cdot \mathbf{u}_{t}\]

Step2. 根据给定误差和上一状态误差，计算当前时刻预测状态对应的误差：

\[\mathbf{P}_{t} = \mathbf{F}\cdot \mathbf{P}_{t-1} \cdot \mathbf{F}^{T}+\mathbf{Q}\]

Step3. 根据上一状态预测误差、给定的观测误差和观测矩阵，计算卡尔曼增益(这里括号外的-1对于标量表示分数，对于矩阵表示求逆)：

\[\mathbf{K}_{t} = \mathbf{P}_{t} \cdot \mathbf{H}^{T} \cdot (\mathbf{H}\cdot \mathbf{P}_{t} \cdot \mathbf{H}^{T}+\mathbf{R})^{-1}\]

Step4. 根据当前预测状态、实际观测和观测矩阵，结合卡尔曼增益平衡预测与观测权重，修正状态预测：

\[\mathbf{x}_{t} = \mathbf{x}_t + \mathbf{K}_{t} \cdot (\mathbf{z}_{t} - \mathbf{H} \cdot \mathbf{x}_{t})\]

Step5. 根据计算得到的卡尔曼增益、上一个状态预测误差、给定的观测矩阵，计算修正后状态对应的误差：

\[\mathbf{P}_{t} = (\mathbf{I} - \mathbf{K}_{t} \cdot \mathbf{H}) \cdot \mathbf{P}_{t}\]

这里简单展示利用卡尔曼滤波估算温度的例子：需要测量火箭发动机内部的温度，但因为温度太高无法直接测量，只能间接测量外部温度。现在需要基于观测预测对应时刻最优的内部温度。完整示例见这里。代码如下：

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)

可以看到，只需要知道加速度计的观测以及此时在全局坐标系下的姿态，即可以推导位置。总体而言，位置计算是依赖于陀螺仪的积分结果的。因为设计到某时刻本体坐标系和全局坐标系之间的转换关系，这个转换关系只有通过陀螺仪才能得到。这样做的结果是，如果陀螺仪有了比较大的误差，那么位置的估计也不会好。

卡尔曼滤波使用手册

Thu, 07 Aug 2025 00:00:00 +0000

请注意:内容可能包含大量图表、公式、代码等，若RSS无法正确显示可直接访问原始网址，获得最好的阅读体验。

1.卡尔曼滤波适合什么任务场景

卡尔曼滤波特别适合两种任务和场景：

感兴趣的变量无法直接观测，需要通过一个或多个带有噪声的观测量计算得到，通过卡尔曼滤波可给出感兴趣变量的最优估计。
有多个传感器同时观测，需要做传感器数据的动态融合，通过卡尔曼滤波可给出最优融合结果。

如果你的任务和需求符合上面的任意一点，那么卡尔曼滤波就适合你。可以继续回答下面问题。

2.使用卡尔曼滤波前需明确的内容

在使用卡尔曼滤波解决问题时，如果能够回答清楚下面的问题，使用卡尔曼滤波就比较清晰了。

2.1 观测量是什么

实际观测内容是什么，有几个？

一般而言，对于\(m\)个观测量，对应观测向量\(\mathbf{z}\)大小为\(m \times 1\)。

例如，用一个传感器观测了火箭发动机外部温度\(T_{e}\)，基于它可构成观测向量\(\mathbf{z} = [T_{e}]\)。

2.2 希望估计的量是什么

你希望用卡尔曼滤波得到关于什么量的最优估计？有几个？

一般而言，对于\(n\)个估计量，对应状态向量\(\mathbf{x}\)大小为\(n \times 1\)。

例如，希望得到关于火箭发动机内部温度\(T_{i}\)的最优估计。那么这个内部温度\(T_{i}\)就是希望估计的状态向量\(\mathbf{x} = [T_{i}]\)。

2.3 状态间存在什么关联

基于你的状态定义，从上一个时刻到下一个时刻，状态是否随时间变化？

注意这里抓住“随时间变化”，“时间”这个自变量。随时间变化那就写出对应关系(线性增加或减少)，不变那么上一个状态就和下一个状态相等。一般而言，对于\(n \times 1\)的状态向量，其对应的状态转移矩阵\(\mathbf{F}\)大小为\(n \times n\)。在标准卡尔曼滤波中，状态转移矩阵\(\mathbf{F}\)作为描述系统动态的‌先验知识，不随滤波过程变化。

例如，火箭平稳飞行时发动机内部温度基本保持不变，所以\(t\)时刻的外部温度\(T_{e}^{t}\)和\(t-1\)时刻的外部温度\(T_{e}^{t-1}\)相等，有\(T_{e}^{t} = 1 \cdot T_{e}^{t-1}\)，这里的系数即为状态转移矩阵\(\mathbf{F}\)。

2.4 系统状态是否有外部因素控制

除系统状态\(\mathbf{x}\)本身随时间的演化，是否有外部因素干预？如果有，有几个，其和系统状态间的数学关系是什么(线性表达)？

一般而言，对于\(k\)个外部控制量，对应控制输入向量大小为\(k \times 1\)。又因为控制输入直接作用于大小为\(n \times 1\)系统状态向量\(\mathbf{x}\)，因此控制输入矩阵\(\mathbf{B}\)大小为\(n \times k\)，其中的元素\(b_{ij}\)表示第j个控制输入对第i个状态变量的直接影响强度。在标准卡尔曼滤波中，控制输入矩阵\(\mathbf{B}\)作为描述系统控制的‌先验知识，不随滤波过程变化。

例如，火箭发动机温度受油门影响，油门大则燃料充足，发动机温度升高，反之亦然。假设油门和温度的关系是每增加1油门，温度升高100度。那么这里，控制输入向量\(\mathbf{u}\)即为油门，其对应的控制输入矩阵\(\mathbf{B} = [100]\)。

2.5 状态和观测之间存在什么关联

基于你给定的观测\(\mathbf{z}\)和状态\(\mathbf{x}\)，两者间是否有数学上的关系(基础卡尔曼滤波认为只存在线性关系)？

一般而言，对于\(m \times 1\)的观测向量和\(n \times 1\)的状态向量，其对应的观测矩阵\(\mathbf{H}\)大小为\(n \times m\)。在标准卡尔曼滤波中，观测矩阵\(\mathbf{H}\)作为描述状态和观测间关联的的‌先验，不随滤波过程变化。

例如，假设地面实验已知，发动机外部温度\(T_{e}\)与内部温度\(T_{i}\)存在\(T_{e} = 0.5 \cdot T_{i}\)关系。这里的系数即为观测矩阵\(\mathbf{H}\)。这里需要特别注意的是，观测矩阵是从状态到观测的系数，不能反了。

2.6 实际观测量的误差怎么样

基于你给定的观测量，传感器的实际误差怎么样(一般用方差表示)？

对于单个观测量用单个方差表示，对于多个变量用协方差矩阵表示即可。协方差矩阵对角线元素为各变量的方差。一般而言，对于\(m \times 1\)的观测向量，其对应的协方差矩阵\(\mathbf{R}\)大小为\(m \times m\)。

例如，假设发动机外部传感器的温度测量方差为0.5，那么测量噪声协方差矩阵为\(\mathbf{R} = [0.5]\)。

2.7 运行过程外部干扰怎么样

在运行过程中，是否存在一些难以建模的外部干扰因素，如果有误差怎么样(一般用方差表示)？

一般而言，对于\(n \times 1\)的状态向量，其对应的过程噪声协方差矩阵\(\mathbf{Q}\)大小为\(n \times n\)。

例如，火箭发动机运行过程中剧烈振动可能导致外部温度传感器松动造成测量结果不准，假设导致一个方差为0.1的误差，那么过程噪声协方差矩阵\(\mathbf{Q} = [0.1]\)。

2.8 初始预测状态怎么样

在系统第一次运行的时候，预测的状态是什么，对应的误差是多少？

对于初始预测状态，一般会基于第一个观测和理论上的观测矩阵计算得到。对于初始预测误差，一般会将其设为一个较大的数值，在后续滤波过程中会自动迭代更新。一般而言，对于\(n \times 1\)的状态向量，其对应的状态协方差矩阵\(\mathbf{P}\)大小为\(n \times n\)。

例如，假设外部温度\(T_{e}\)的第一个观测数值\(\mathbf{z}\)为100度，根据观测矩阵\(\mathbf{H}\)，可以得到第一个初始预测状态\(\mathbf{x}\)为200。对应状态协方差矩阵可设置一个较大的值，例如\(\mathbf{P} = [1]\)。

3.卡尔曼滤波核心步骤

3.1 变量说明

常量参数

以下参数在卡尔曼滤波启动前就可以给定，滤波过程中不会改变：

状态转移矩阵\(\mathbf{F}\)
观测矩阵\(\mathbf{H}\)
控制输入矩阵\(\mathbf{B}\)
过程噪声协方差矩阵\(\mathbf{Q}\)
测量噪声协方差矩阵\(\mathbf{R}\)

初始值

卡尔曼滤波作为迭代算法，预测t时刻状态需要知道t-1时刻的状态和误差。因此，在系统刚启动时，需要手动对迭代变量赋值，以便可以进行后续计算：

初始状态估计向量\(\mathbf{x}\)：一般设为第一个观测值
初始状态协方差矩阵\(\mathbf{P}\)：一般设为较大的数

这两个变量在后续迭代过程中都会自动更新。

持续输入

在运行过程中，需要不断输入当前的观测和控制作为状态预测起算数据，卡尔曼滤波基于当前输入和上一时刻状态进行计算。

控制输入向量\(\mathbf{u}\)
实际观测向量\(\mathbf{z}\)

持续输出

卡尔曼滤波可以给出当前时刻的最优估计，主要包括状态和误差：

当前最优估计的状态向量\(\mathbf{x}\)
当前估计状态对应的状态协方差矩阵\(\mathbf{P}\)

3.2 预测(状态预测)

Step1. 根据上一个状态，结合给定模型，预测当前时刻状态：

\[\mathbf{x}_{t} = \mathbf{F}\cdot \mathbf{x}_{t-1} + \mathbf{B}\cdot \mathbf{u}_{t}\]

Step2. 根据给定误差和上一状态误差，计算当前时刻预测状态对应的误差：

\[\mathbf{P}_{t} = \mathbf{F}\cdot \mathbf{P}_{t-1} \cdot \mathbf{F}^{T}+\mathbf{Q}\]

3.3 更新(测量更新)

Step3. 根据上一状态预测误差、给定的观测误差和观测矩阵，计算卡尔曼增益(这里括号外的-1对于标量表示分数，对于矩阵表示求逆)：

\[\mathbf{K}_{t} = \mathbf{P}_{t} \cdot \mathbf{H}^{T} \cdot (\mathbf{H}\cdot \mathbf{P}_{t} \cdot \mathbf{H}^{T}+\mathbf{R})^{-1}\]

Step4. 根据当前预测状态、实际观测和观测矩阵，结合卡尔曼增益平衡预测与观测权重，修正状态预测：

\[\mathbf{x}_{t} = \mathbf{x}_t + \mathbf{K}_{t} \cdot (\mathbf{z}_{t} - \mathbf{H} \cdot \mathbf{x}_{t})\]

Step5. 根据计算得到的卡尔曼增益、上一个状态预测误差、给定的观测矩阵，计算修正后状态对应的误差：

\[\mathbf{P}_{t} = (\mathbf{I} - \mathbf{K}_{t} \cdot \mathbf{H}) \cdot \mathbf{P}_{t}\]

4.实际示例

4.1 一维位置估计

4.1.1 问题描述

一个人以0.2m/s恒定速度在一维空间中向前移动，观测他的位置\(p\)，利用卡尔曼滤波估计他的最优位置。本示例中，观测量就是状态量。

4.1.2 问题建模

问题建模可以从前面几个问题开始：

观测量是什么：人的位置\(p\)，观测向量\(\mathbf{z} = [p]\)。

希望估计的量是什么：人在某个时刻的最优位置，状态向量\(\mathbf{x} = [p]\)。

状态间存在什么关联：人做匀速运动，而根据状态定义即为位置。假设两个状态间的时间差异为\(\Delta t\)，理论上有\(\mathbf{x}_t = \mathbf{x}_{t-1} + 0.2 \cdot \Delta t\)。这里的系数即为状态转移矩阵\(\mathbf{F} = [1]\)。

系统状态是否有外部因素控制：无。控制输入向量\(\mathbf{u}\)为0，其对应的控制输入矩阵\(\mathbf{B} = [0]\)。

状态和观测之间存在什么关联：因为观测是位置，状态也是位置，所以\(\mathbf{z} = 1 \cdot \mathbf{x}\)，这里的系数即为观测矩阵\(\mathbf{H} = [1]\)。

实际观测量的误差怎么样：假设实际位置测量方差为2.0，那么测量噪声协方差矩阵为\(\mathbf{R} = [2.0]\)。

运行过程外部干扰怎么样：假设存在不可预知的大风、下雨等因素导致速度变化，假设导致一个方差为0.5的误差，那么过程噪声协方差矩阵\(\mathbf{Q} = [0.5]\)。

初始预测状态怎么样：假设第一个观测数值\(\mathbf{z}\)为0、第一个初始预测状态\(\mathbf{x}\)为0。对应状态协方差矩阵可设置一个较大的值，例如\(\mathbf{P} = [1]\)。

基于以上答案，便可构建基本变量和卡尔曼滤波核心步骤：

状态转移矩阵\(\mathbf{F} = [1]\)
观测矩阵\(\mathbf{H} = [1]\)
控制输入矩阵\(\mathbf{B} = [0]\)
过程噪声协方差矩阵\(\mathbf{Q} = [0.5]\)
测量噪声协方差矩阵\(\mathbf{R} = [2.0]\)
初始状态估计向量\(\mathbf{x} = [0]\)
初始状态协方差矩阵\(\mathbf{P} = [1]\)
输入：控制输入向量\(\mathbf{u}\)
输入：实际观测向量\(\mathbf{z}\)
输出：当前最优估计状态向量\(\mathbf{x}\)
输出：当前状态对应的状态协方差矩阵\(\mathbf{P}\)

4.1.3 代码实现

基于上面给出的公式和确定的参数，最终，可以通过以下代码实现卡尔曼滤波。

import numpy as np
import matplotlib.pyplot as plt

# 一维位置估计 - 基础卡尔曼滤波演示
# 一个人以0.2m/s恒定速度在一维空间中向前移动，观测他的位置50秒


# 给定参数
H = 1 # 观测矩阵
F = 1 # 状态转移矩阵
B = 0   # 控制输入矩阵
Q = 0.5 # 过程噪声协方差矩阵
R = 2.0 # 测量噪声协方差矩阵


# 指定理论值，方便评估精度，实际无法获得
true_values = []
for i in range(50):
    true_values.append(i*0.2)  # 真实位置为0到10之间
true_values = np.array(true_values, dtype=float)

# 系列观测与控制输入
np.random.seed(0)
z = true_values + np.random.normal(0, 1, size=len(true_values))
u = np.zeros_like(z)

# 给状态赋初值
prev_state = z[0]
prev_error = 1.0

# 用于存放估计的状态
predict_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)


# 精度评价
error_measure = np.mean(np.abs(z  - true_values))
error_kalman = np.mean(np.abs(predict_states - true_values))

print("Mean Error of Measurements:", error_measure)
print("Mean Error of Kalman Filter:", error_kalman)

# 可视化结果
plt.figure(figsize=(10,6))
plt.plot(true_values, np.zeros_like(true_values), label='True Values', linestyle='--')
plt.plot(true_values, z - true_values, label='Noisy Measurements', marker='o', markersize=4)
plt.plot(true_values, predict_states - true_values, label='Kalman Filter', linewidth=2)
plt.legend()
plt.xlabel('Time Step')
plt.ylabel('Value')
plt.title('Basic Kalman Filter Demo')
plt.show()

运行后效果如下：可以看到，相比于基于原始测量值估计出来的结果，卡尔曼滤波估计的结果误差平均降低了约0.36。

4.2 温度测量

4.2.1 问题描述

这里还以前面的例子进行说明。我们需要测量火箭发动机内部的温度，但因为温度太高无法直接测量，只能间接测量外部温度。现在需要基于观测预测对应时刻最优的内部温度。本示例中，观测量和状态量不同，但数量相等，两者间有对应关系。

4.2.2 问题建模

问题建模可以从前面几个问题开始：

观测量是什么：火箭发动机外部温度\(T_{e}\)，观测向量\(\mathbf{z} = [T_{e}]\)。

希望估计的量是什么：火箭发动机内部温度\(T_{i}\)的最优估计，状态向量\(\mathbf{x} = [T_{i}]\)。

状态间存在什么关联：火箭平稳飞行时发动机内部温度基本保持不变，所以\(t\)时刻的外部温度\(T_{e}^{t}\)和\(t-1\)时刻的外部温度\(T_{e}^{t-1}\)相等，有\(T_{e}^{t} = 1 \cdot T_{e}^{t-1}\)，这里的系数即为状态转移矩阵\(\mathbf{F}\)。

系统状态是否有外部因素控制：假设油门和温度的关系是每增加1油门，温度升高100度。控制输入向量\(\mathbf{u}\)即为油门，其对应的控制输入矩阵\(\mathbf{B} = [100]\)。

状态和观测之间存在什么关联：假设发动机外部温度\(T_{e}\)与内部温度\(T_{i}\)存在\(T_{e} = 0.5 \cdot T_{i}\)关系，这里的系数即为观测矩阵\(\mathbf{H}\)。

实际观测量的误差怎么样：假设发动机外部传感器的温度测量方差为0.5，那么测量噪声协方差矩阵为\(\mathbf{R} = [0.5]\)。

运行过程外部干扰怎么样：火箭发动机运行过程中剧烈振动等因素可能导致外部温度传感器松动造成测量结果不准，假设导致一个方差为0.1的误差，那么过程噪声协方差矩阵\(\mathbf{Q} = [0.1]\)。

初始预测状态怎么样：假设外部温度\(T_{e}\)的第一个观测数值\(\mathbf{z}\)为102度，根据观测矩阵\(\mathbf{H}\)的关系，可以得到第一个初始预测状态\(\mathbf{x}\)为204。对应状态协方差矩阵可设置一个较大的值，例如\(\mathbf{P} = [1]\)。

基于以上答案，便可构建基本变量和卡尔曼滤波核心步骤：

状态转移矩阵\(\mathbf{F} = [1]\)
观测矩阵\(\mathbf{H} = [0.5]\)
控制输入矩阵\(\mathbf{B} = [100]\)
过程噪声协方差矩阵\(\mathbf{Q} = [0.1]\)
测量噪声协方差矩阵\(\mathbf{R} = [0.5]\)
初始状态估计向量\(\mathbf{x} = [204]\)
初始状态协方差矩阵\(\mathbf{P} = [1]\)
输入：控制输入向量\(\mathbf{u}\)
输入：实际观测向量\(\mathbf{z}\)
输出：当前最优估计状态向量\(\mathbf{x}\)
输出：当前状态对应的状态协方差矩阵\(\mathbf{P}\)

4.2.3 代码实现

基于上面给出的公式和确定的参数，最终，可以通过以下代码实现卡尔曼滤波。

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

运行后效果如下：可以看到，相比于基于原始测量值估计出来的结果，卡尔曼滤波估计的结果误差平均降低了约1.22。

4.3 GPS二维位置估计

4.3.1 问题描述

对一个路面上行驶的车辆进行GPS位置观测\((x, y)\)，因为GPS信号不佳原因导致位置观测有较大噪声。现在需要根据观测结果估计最优的位置\((x, y)\)和速度信息\((v_x, v_y)\)。在短时间内，认为车辆为匀速运动。假设车上分别有控制x方向和y方向的两个油门，每增加1油门，速度提升10。在本示例中，观测量和状态量不同，且个数不等。

4.3.2 问题建模

问题建模可以从前面几个问题开始：

观测量是什么：车辆的\(x\)、\(y\)坐标，观测向量\(\mathbf{z} = [x, y]^{T}\)。

希望估计的量是什么：车辆真实的\(x\)、\(y\)坐标，状态向量\(\mathbf{x} = [x, y, v_x, v_y]^{T}\)。

状态间存在什么关联：基于定义的状态向量，由于认为是匀速运动，因此\(t\)时刻和\(t-1\)时刻的\(v_x\)、\(v_y\)相等，而x和y则根据公式计算\(x_t = x_{t-1} + v_x \cdot \Delta t\)、\(y_t = y_{t-1} + v_y \cdot \Delta t\)，其中\(\Delta t = T_{t} - T_{t-1}\)。写成矩阵形式如下：

\[\begin{bmatrix} x_t \\ y_t \\ v_x \\ v_y \end{bmatrix} = \begin{bmatrix} 1 & 0 & \Delta t & 0\\ 0 & 1 & 0 & \Delta t\\ 0 & 0 & 1 & 0\\ 0 & 0 & 0 & 1 \end{bmatrix}\begin{bmatrix} x_{t-1} \\ y_{t-1} \\ v_{x} \\ v_{y} \end{bmatrix}\]

这里的系数矩阵即为状态转移矩阵\(\mathbf{F}\)。

系统状态是否有外部因素控制：有x、y两个油门，所以控制输入向量\(\mathbf{u} = [f_{x}, f_{y}]^{T}\)即为油门。每增加1油门，速度提升10，其对应的控制输入矩阵\(\mathbf{B} = \begin{bmatrix} 0 & 0\\ 0 & 0\\ 10 & 0\\ 0 & 10 \end{bmatrix}\)。

状态和观测之间存在什么关联：根据设定的观测向量和状态向量，很容易得到观测矩阵\(\mathbf{H} = \begin{bmatrix} 1 & 0 & 0 & 0\\ 0 & 1 & 0 & 0 \end{bmatrix}\)。

实际观测量的误差怎么样：假设x、y两个观测量的方差分别为5、7，协方差为0.3，那么测量噪声协方差矩阵为\(\mathbf{R} = \begin{bmatrix} 5 & 0.3\\ 0.3 & 7 \end{bmatrix}\)。

运行过程外部干扰怎么样：由于车辆行驶过程中可能出现剧烈震动等因素，导致GPS传感器测量结果不准，假设分别导致x、y方向分别出现0.05和0.03的方差，协方差为0.02，速度方差分别是0.9、0.8。由于状态向量为\(x, y, v_x, v_y\)，因此过程噪声协方差矩阵大小为4×4，\(\mathbf{Q} = \begin{bmatrix} 0.05 & 0.02 & 0 & 0\\ 0.02 & 0.03 & 0 & 0\\ 0 & 0 & 0.9 & 0\\ 0 & 0 & 0 & 0.8 \end{bmatrix}\)。

初始预测状态怎么样：假设初始GPS位置观测为\(\mathbf{z} = [1, 1]^{T}\)，假设第一个初始预测状态\(\mathbf{x} = [1, 1, 1, 1]^{T}\)。对应状态协方差矩阵可设置一个较大的值，例如\(\mathbf{P} = \begin{bmatrix} 100 & 0 & 0 & 0\\ 0 & 100 & 0 & 0\\ 0 & 0 & 100 & 0\\ 0 & 0 & 0 & 100 \end{bmatrix}\)。

4.3.3 代码实现

基于上面给出的公式和确定的参数，最终，可以通过以下代码实现卡尔曼滤波。

import numpy as np
from matplotlib import pyplot as plt

# 给定参数
# 两个状态间的时间间隔
delta_t = 1

# 观测矩阵
H = np.array([[1, 0, 0, 0],
              [0, 1, 0, 0]])
# 状态转移矩阵
F = np.array([[1, 0, delta_t, 0],
              [0, 1, 0, delta_t],
              [0, 0, 1, 0],
              [0, 0, 0, 1]])
# 控制输入矩阵
B = np.array([[0, 0],
              [0, 0],
              [10, 0],
              [0, 10]])
# 过程噪声协方差矩阵
Q = np.array([[0.05, 0.02, 0, 0],
              [0.02, 0.03, 0, 0], 
              [0, 0, 0.9, 0], 
              [0, 0, 0, 0.8]])
# 测量噪声协方差矩阵
R = np.array([[5.0, 0.3],
              [0.3, 7.0]])


# 构造理论值，方便评估精度，实际无法获得
t = np.linspace(0, 10, 400)
x = t
y = 10 * np.sin(t)
ideal_values = np.column_stack((x, y))

ideal_vxs = []
ideal_vys = []

ideal_vxs.append(0)
ideal_vys.append(0)

for i in range(1, len(ideal_values)):
    vx = np.abs(ideal_values[i][0] - ideal_values[i-1][0])
    vy = np.abs(ideal_values[i][1] - ideal_values[i-1][1])
    ideal_vxs.append(vx)
    ideal_vys.append(vy)


# 系列观测与控制输入
z = ideal_values + np.random.normal(0, 0.1, ideal_values.shape)
u = np.zeros_like(ideal_values)
measure_vxs = np.abs(ideal_vxs + np.random.normal(0, 0.1, len(ideal_vxs)))
measure_vys = np.abs(ideal_vys + np.random.normal(0, 0.1, len(ideal_vys)))

# 给状态赋初值
prev_state = np.array([1, 1, 1, 1])
prev_error = np.array([[100, 0, 0, 0],
                       [0, 100, 0, 0], 
                       [0, 0, 100, 0], 
                       [0, 0, 0, 100]])

# 用于存放估计的状态
predict_states = []
predict_xs = []
predict_ys = []

# 开始卡尔曼滤波
for i in range(len(z)):
    # 预测 - 状态
    predict_state = F @ prev_state + B @ u[i]
    # 预测 - 误差
    predict_error = F @ prev_error @ F.T + Q
    
    # 更新 - 卡尔曼增益
    kalman_gain  = predict_error @ H.T @ np.linalg.inv(H @ predict_error @ H.T + R)
    
    # 更新 - 状态
    update_state = predict_state + kalman_gain @ (z[i] - H @ predict_state)
    # 更新 - 误差
    update_error = (np.eye(4) - kalman_gain @ H) @ predict_error
    
    # 变量赋值开启下次迭代
    prev_state = update_state
    prev_error = update_error
    
    # 将预测状态保存起来
    predict_states.append(update_state)
    predict_xs.append(update_state[0])
    predict_ys.append(update_state[1])


# 精度评价
predict_pos_errors = []
predict_vel_errors = []

measure_pos_errors = []
measure_vel_errors = []

for i in range(len(predict_states)):
    cur_state = predict_states[i]
    
    dx = np.abs(cur_state[0] - ideal_values[i][0])
    dy = np.abs(cur_state[1] - ideal_values[i][1])
    predict_pos_errors.append(np.sqrt(dx**2 + dy**2))
    
    dvx = np.abs(np.abs(cur_state[2]) - ideal_vxs[i])
    dvy = np.abs(np.abs(cur_state[3]) - ideal_vys[i])
    predict_vel_errors.append(np.sqrt(dvx**2 + dvy**2))
    
    dx = np.abs(z[i][0] - ideal_values[i][0])
    dy = np.abs(z[i][1] - ideal_values[i][1])
    measure_pos_errors.append(np.sqrt(dx**2 + dy**2))
    
    dvx = np.abs(measure_vxs[i] - ideal_vxs[i])
    dvy = np.abs(measure_vxs[i] - ideal_vys[i])
    measure_vel_errors.append(np.sqrt(dvx**2 + dvy**2))

print("mean measure pos:", round(np.mean(measure_pos_errors), 5))
print("mean predict pos:", round(np.mean(predict_pos_errors), 5))

print("mean measure vel:", round(np.mean(measure_vel_errors), 5))
print("mean predict vel:", round(np.mean(predict_vel_errors), 5))

# 可视化
plt.figure(figsize=(10, 6))
plt.plot(ideal_values[:,0], ideal_values[:,1], 'g-', label='ground truth')
plt.plot(z[:,0], z[:,1], 'r-', label='GPS Measure')
plt.plot(predict_xs, predict_ys, 'b-', label='Predict')
plt.legend()
plt.title('GPS Traj.')
plt.xlabel('X (m)')
plt.ylabel('Y (m)')
plt.grid(True)
plt.show()

运行后效果如下。相比于原始数据计算出的状态，卡尔曼滤波得到的位置和速度对应的误差均有下降。

4.4 多个温度传感器融合

4.4.1 问题描述

在前面的例子中，我们主要介绍的都是感兴趣的量无法直接观测，通过间接观测其它相关量从而推测它的值(即使观测量和预测量相同也可以将其当作无法直接观测处理)，对应卡尔曼滤波适合的第一种情况。现实中还有另一种情况，就是我们可能使用多个传感器对某个变量同时进行观测，需要基于多个观测量给出感兴趣量的最优估计。

还是从简单到复杂，首先从4.2的温度测量开始。为了进一步提高估计精度，我们在火箭发动机的四周分别安装了4个温度传感器。现在需要基于4个外部观测预测对应时刻最优的内部温度。

4.4.2 问题建模

问题建模可以从前面几个问题开始：

观测量是什么：4个火箭发动机外部温度\(T_{e}\)，观测向量\(\mathbf{z} = [T_{e}^{1}, T_{e}^{2}, T_{e}^{3}, T_{e}^{4}]\)。

希望估计的量是什么：火箭发动机内部温度\(T_{i}\)的最优估计，状态向量\(\mathbf{x} = [T_{i}]\)。

状态和观测之间存在什么关联：假设发动机外部温度\(T_{e}\)与内部温度\(T_{i}\)存在\(T_{e} = 0.5 \cdot T_{i}\)关系，这里的系数即为观测矩阵\(\mathbf{H} = \begin{bmatrix} 0.5 \\ 0.5 \\ 0.5 \\ 0.5 \end{bmatrix}\)。

实际观测量的误差怎么样：假设发动机外部传感器的温度测量精度稍有差异、彼此独立观测，那么测量噪声协方差矩阵为\(\mathbf{R} = \begin{bmatrix} 0.5 & 0 & 0 & 0\\ 0 & 0.6 & 0 & 0\\ 0 & 0 & 0.4 & 0\\ 0 & 0 & 0 & 0.3 \end{bmatrix}\)。

初始预测状态怎么样：假设外部温度\(T_{e}\)的第一个观测\(\mathbf{z}\)分别为102、103、101、100.5度，可以得到第一个初始预测状态\(\mathbf{x}\)为204。对应状态协方差矩阵可设置一个较大的值，例如\(\mathbf{P} = [1]\)。

基于以上答案，便可构建基本变量和卡尔曼滤波核心步骤：

状态转移矩阵\(\mathbf{F} = [1]\)
观测矩阵\(\mathbf{H} = \begin{bmatrix} 0.5 \\ 0.5 \\ 0.5 \\ 0.5 \end{bmatrix}\)
控制输入矩阵\(\mathbf{B} = [100]\)
过程噪声协方差矩阵\(\mathbf{Q} = [0.1]\)
测量噪声协方差矩阵\(\mathbf{R} = \begin{bmatrix} 0.5 & 0 & 0 & 0\\ 0 & 0.6 & 0 & 0\\ 0 & 0 & 0.4 & 0\\ 0 & 0 & 0 & 0.3 \end{bmatrix}\)
初始状态估计向量\(\mathbf{x} = [102, 103, 101, 100.5]\)
初始状态协方差矩阵\(\mathbf{P} = [1]\)
输入：控制输入向量\(\mathbf{u}\)
输入：实际观测向量\(\mathbf{z}\)
输出：当前最优估计状态向量\(\mathbf{x}\)
输出：当前状态对应的状态协方差矩阵\(\mathbf{P}\)

4.4.3 代码实现

基于上面给出的公式和确定的参数，最终，可以通过以下代码实现卡尔曼滤波。

import numpy as np

# 给定参数
# 观测矩阵
H = np.array([[0.5], 
              [0.5], 
              [0.5], 
              [0.5]])
# 状态转移矩阵
F = np.array([[1.0]])
# 控制输入矩阵
B = np.array([[100.0]])
# 过程噪声协方差矩阵
Q = np.array([[0.1]])
# 测量噪声协方差矩阵
R = np.array([[0.5, 0, 0, 0], 
              [0, 0.6, 0, 0],
              [0, 0, 0.4, 0], 
              [0, 0, 0, 0.3]])


# 指定理论值，方便评估精度，实际无法获得
ideal_value = np.zeros(12) + 200

# 系列观测与控制输入
z = np.array([[102, 103, 101, 100.5],
              [104, 110, 103, 102],
              [98, 109, 104, 107],
              [99.7, 95, 99, 104],
              [102, 110, 98, 102],
              [101.5, 103, 105, 96],
              [100.4, 102, 103, 103],
              [101.6, 110, 104, 107],
              [103.8, 94, 98, 99],
              [97, 102, 99, 101],
              [102.1, 102, 99, 104],
              [99.3, 108, 102, 103]])

u = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]

# 给状态赋初值
prev_state = np.array([[204]])
prev_error = np.array([[1.0]])

# 用于存放估计的状态
predict_states = []

measure_states = []

# 开始卡尔曼滤波
for i in range(len(z)):
    # # 预测 - 状态
    predict_state = F @ prev_state + B @ np.array([[u[i]]])
    # # 预测 - 误差
    predict_error = F @ prev_error @ F.T + Q
    
    # 更新 - 卡尔曼增益
    kalman_gain  = predict_error @ H.T @ np.linalg.inv(H @ predict_error @ H.T + R)
    
    # 更新 - 状态
    update_state = predict_state + kalman_gain @ (np.array([z[i]]).T - H @ predict_state)
    
    # 更新 - 误差
    update_error = (np.eye(1) - kalman_gain @ H) @ predict_error
    
    # 变量赋值开启下次迭代
    prev_state = update_state
    prev_error = update_error
    
    # 将预测状态保存起来
    predict_states.append(update_state[0][0])
    measure_states.append(np.mean(z[i] * 2))

# 精度评价
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))

运行后效果如下：可以看到，相比于基于原始测量值估计出来的结果，卡尔曼滤波估计的结果误差平均降低了约0.8。

4.5 LiDAR-GPS传感器融合

4.5.1 问题描述

进一步研究多传感器融合问题。对于多传感器融合，本质上就是将多个观测矩阵和测量协方差矩阵融合到一起，其余步骤与之前相同。

对于一个机器人，携带了LiDAR和GPS两个传感器同时观测机器人的位置。现在需要融合两者的观测，给出最优的位置和速度估计。这里假设观测数据已经经过预处理，包括：（1）LiDAR和GPS观测观测频率相同；（2）LiDAR和GPS的位置观测已经转换到同一坐标系。认为在短时间内，机器人为匀速运动。

4.5.2 问题建模

问题建模可以从前面几个问题开始：

观测量是什么：LiDAR的X、Y观测，GPS的X、Y观测，观测向量\(\mathbf{z} = [x_{L}, y_{L}, x_{G}, y_{G}]\)。

希望估计的量是什么：机器人在某个时刻的最优位置和速度，状态向量\(\mathbf{x} = [x, y, v_{x}, v_{y}]\)。

状态和观测之间存在什么关联：根据设定的观测向量和状态向量，按照z=Hx，很容易得到融合LiDAR和GPS的观测矩阵\(\mathbf{H} = \begin{bmatrix} 1 & 0 & 0 & 0\\ 0 & 1 & 0 & 0\\ 1 & 0 & 0 & 0\\ 0 & 1 & 0 & 0 \end{bmatrix}\)。

实际观测量的误差怎么样：假设LiDAR的X、Y两个观测量的方差分别为5、7，协方差为0.3，GPS的X、Y方差分别为10、9，协方差为1。两个传感器互相独立。按照\(x_{L}, y_{L}, x_{G}, y_{G}\)顺序，那么测量噪声协方差矩阵为\(\mathbf{R} = \begin{bmatrix} 5 & 0.3 & 0 & 0\\ 0.3 & 7 & 0 & 0\\ 0 & 0 & 10 & 1\\ 0 & 0 & 1 & 9 \end{bmatrix}\)。

运行过程外部干扰怎么样：由于车辆行驶过程中可能出现剧烈震动等因素，导致传感器测量结果不准，假设分别导致x、y方向分别出现0.05和0.03的方差，协方差为0.02，速度方差分别是0.9、0.8。由于状态向量为\(x, y, v_x, v_y\)，因此过程噪声协方差矩阵大小为4×4，\(\mathbf{Q} = \begin{bmatrix} 0.05 & 0.02 & 0 & 0\\ 0.02 & 0.03 & 0 & 0\\ 0 & 0 & 0.9 & 0\\ 0 & 0 & 0 & 0.8 \end{bmatrix}\)。

初始预测状态怎么样：假设初始观测为\(\mathbf{z} = [0.1, 0.2, 0.5, 0.4]^{T}\)，假设第一个初始预测状态\(\mathbf{x} = [0.2, 0.3, 1, 1]^{T}\)。对应状态协方差矩阵可设置一个较大的值，例如\(\mathbf{P} = \begin{bmatrix} 100 & 0 & 0 & 0\\ 0 & 100 & 0 & 0\\ 0 & 0 & 100 & 0\\ 0 & 0 & 0 & 100 \end{bmatrix}\)。

4.5.3 代码实现

基于上面给出的公式和确定的参数，最终，可以通过以下代码实现卡尔曼滤波。

import numpy as np
from matplotlib import pyplot as plt

# 给定参数
# 两个状态间的时间间隔
delta_t = 1

# 观测矩阵
H = np.array([[1, 0, 0, 0],
              [0, 1, 0, 0],
              [1, 0, 0, 0],
              [0, 1, 0, 0]])
# 状态转移矩阵
F = np.array([[1, 0, delta_t, 0],
              [0, 1, 0, delta_t],
              [0, 0, 1, 0],
              [0, 0, 0, 1]])
# 控制输入矩阵
B = np.array([[0, 0],
              [0, 0],
              [10, 0],
              [0, 10]])
# 过程噪声协方差矩阵
Q = np.array([[0.05, 0.02, 0, 0],
              [0.02, 0.03, 0, 0], 
              [0, 0, 0.9, 0], 
              [0, 0, 0, 0.8]])
# 测量噪声协方差矩阵
R = np.array([[5, 0.3, 0, 0],
              [0.3, 7, 0, 0], 
              [0, 0, 10, 1], 
              [0, 0, 1, 9]])


# 构造理论值，方便评估精度，实际无法获得
t = np.linspace(0, 2*np.pi, 100)
x = 10 * np.cos(t)
y = 10 * np.sin(t)
ideal_values = np.vstack([x, y])

ideal_vxs = []
ideal_vys = []

ideal_vxs.append(0)
ideal_vys.append(0)

for i in range(1, len(ideal_values[0])):
    vx = np.abs(ideal_values[0][i] - ideal_values[0][i-1])
    vy = np.abs(ideal_values[1][i] - ideal_values[1][i-1])
    ideal_vxs.append(vx)
    ideal_vys.append(vy)

# 系列观测与控制输入
z_lidar = ideal_values + np.random.normal(0, 0.5, ideal_values.shape)
z_lidar = z_lidar.T
z_gps = ideal_values + np.random.normal(0, 1, ideal_values.shape)
z_gps = z_gps.T
u = np.zeros_like(ideal_values)
u = u.T
measure_vxs = np.abs(ideal_vxs + np.random.normal(0, 0.1, len(ideal_vxs)))
measure_vys = np.abs(ideal_vys + np.random.normal(0, 0.1, len(ideal_vys)))

# 给状态赋初值
prev_state = np.array([0.2, 0.3, 1, 1])
prev_error = np.array([[100, 0, 0, 0],
                       [0, 100, 0, 0], 
                       [0, 0, 100, 0], 
                       [0, 0, 0, 100]])

# 用于存放估计的状态
predict_states = []
predict_xs = []
predict_ys = []

# 开始卡尔曼滤波
for i in range(len(z_lidar)):
    # 预测 - 状态
    predict_state = F @ prev_state + B @ u[i].T
    # 预测 - 误差
    predict_error = F @ prev_error @ F.T + Q
    
    # 更新 - 卡尔曼增益
    kalman_gain  = predict_error @ H.T @ np.linalg.inv(H @ predict_error @ H.T + R)
    
    # 更新 - 状态
    update_state = predict_state + kalman_gain @ (np.concatenate([z_lidar[i], z_gps[i]]) - H @ predict_state)
    # 更新 - 误差
    update_error = (np.eye(4) - kalman_gain @ H) @ predict_error
    
    # 变量赋值开启下次迭代
    prev_state = update_state
    prev_error = update_error
    
    # 将预测状态保存起来
    predict_states.append(update_state)
    predict_xs.append(update_state[0])
    predict_ys.append(update_state[1])


# 精度评价
predict_pos_errors = []
predict_vel_errors = []

measure_pos_errors = []
measure_vel_errors = []

for i in range(len(predict_states)):
    cur_state = predict_states[i]
    
    dx = np.abs(cur_state[0] - ideal_values[0][i])
    dy = np.abs(cur_state[1] - ideal_values[1][i])
    predict_pos_errors.append(np.sqrt(dx**2 + dy**2))
    
    dvx = np.abs(np.abs(cur_state[2]) - ideal_vxs[i])
    dvy = np.abs(np.abs(cur_state[3]) - ideal_vys[i])
    predict_vel_errors.append(np.sqrt(dvx**2 + dvy**2))
    
    dx = np.abs((z_lidar[i][0]+z_gps[i][0])/2 - ideal_values[0][i])
    dy = np.abs((z_lidar[i][1]+z_gps[i][1])/2 - ideal_values[1][i])
    measure_pos_errors.append(np.sqrt(dx**2 + dy**2))
    
    dvx = np.abs(measure_vxs[i] - ideal_vxs[i])
    dvy = np.abs(measure_vxs[i] - ideal_vys[i])
    measure_vel_errors.append(np.sqrt(dvx**2 + dvy**2))

print("mean measure pos:", round(np.mean(measure_pos_errors), 5))
print("mean predict pos:", round(np.mean(predict_pos_errors), 5))

print("mean measure vel:", round(np.mean(measure_vel_errors), 5))
print("mean predict vel:", round(np.mean(predict_vel_errors), 5))

# 可视化
plt.figure(figsize=(10, 6))
plt.plot(ideal_values[0,:], ideal_values[1,:], 'g-', label='Ground Truth')
plt.plot(z_gps[:,0], z_gps[:,1], 'r-', label='GPS Measure')
plt.plot(z_lidar[:,0], z_lidar[:,1], 'y-', label='LiDAR Measure')
plt.plot(predict_xs, predict_ys, 'b-', label='Predict')
plt.legend()
plt.title('GPS Traj.')
plt.xlabel('X (m)')
plt.ylabel('Y (m)')
plt.grid(True)
plt.show()

运行后效果如下。相比于原始数据通过简单平均计算出的位置和速度，卡尔曼滤波得到的位置和速度对应的误差均有下降。

5.实际应用中的进一步讨论

5.1 过程噪声协方差矩阵Q

过程噪声协方差矩阵Q表征系统模型的不确定性（如未建模动态、外部干扰）。确定方法可以通过理论分析或实验标定。

Q过大‌：过度信任测量值，响应快速，但输出波动大（高频噪声敏感）
Q过小‌：过度信任预测模型，响应滞后，对突变不敏感（系统发散风险）

5.2 观测噪声协方差矩阵R

观测噪声协方差矩阵R反映传感器测量精度。确定方法可以从设备参数中直接获得，也可以通过传感器标定获得。

R过大‌：更信任模型预测、忽略测量值，结果更平滑但可能偏离真实值
R过小‌：更信任测量值，滤波结果接近原始测量

Q/R决定卡尔曼增益K的大小：

高Q/R比导致K增大，更信任测量值
低Q/R比导致K减小，更信任模型预测

5.3 状态初值

对于系统的状态初值，若有先验知识（如系统启动位置），直接赋值；若无先验信息，可用首次测量值初始化。而初始状态对应的协方差矩阵一般可以设为一个较大的数(100等)，但如果过大可能引起初始震荡，如果过小会导致收敛慢，且可能陷入局部最优。

系统不同问题的潜在改善方向：

系统发散：可能是模型不准或Q过小，可以增大Q或改进模型
系统滞后：可能是Q过小，可以增大Q或减小R
系统震荡：可能是R过小，可以增大R

5.4 不同参数对滤波影响分析

下面的代码展示了不同的参数对滤波的影响。

import numpy as np
import matplotlib.pyplot as plt

class KalmanFilter:
    def __init__(self, q, r):
        self.q = q  # 过程噪声方差
        self.r = r  # 测量噪声方差
        self.x = 0  # 初始状态估计
        self.p = 1  # 初始估计误差协方差

    def update(self, z):
        # 预测步骤
        x_pred = self.x
        p_pred = self.p + self.q

        # 更新步骤
        k = p_pred / (p_pred + self.r)
        self.x = x_pred + k * (z - x_pred)
        self.p = (1 - k) * p_pred
        return self.x

def simulate_data(n=100):
    # 生成真实轨迹（正弦波+线性趋势）
    t = np.linspace(0, 10, n)
    truth = 2 * np.sin(t) + 0.1 * t
    # 添加测量噪声
    noise = np.random.normal(0, 1, n)
    return t, truth, truth + noise

def main():
    t, truth, measurements = simulate_data()
    
    # 测试不同Q/R组合
    params = [
        {'q': 0.01, 'r': 1, 'label': 'Q=0.01, R=1 (Conservative)'},
        {'q': 0.1, 'r': 1, 'label': 'Q=0.1, R=1 (Balenced)'},
        {'q': 1, 'r': 1, 'label': 'Q=1, R=1 (Aggressive)'},
        {'q': 0.1, 'r': 0.01, 'label': 'Q=0.1, R=0.01 (Trust Measurement)'}
    ]

    plt.figure(figsize=(12, 6))
    plt.plot(t, truth, 'k-', linewidth=2, label='GT')
    plt.plot(t, measurements, 'r.', alpha=0.3, label='Measurement')

    for param in params:
        kf = KalmanFilter(param['q'], param['r'])
        estimates = [kf.update(z) for z in measurements]
        plt.plot(t, estimates, '--', linewidth=1.5, label=param['label'])

    plt.title('Q/R Test')
    plt.xlabel('Time Step')
    plt.ylabel('State')
    plt.legend()
    plt.tight_layout()
    plt.grid(True)
    plt.show()

if __name__ == '__main__':
    main()

6.非线性情况下的扩展卡尔曼滤波

前面我们提到，经典卡尔曼滤波的假设之一就是状态以及状态和观测之间的关系都是线性的、适用于线性高斯系统，但现实中情况显然会更加复杂。如何将卡尔曼滤波应用于非线性情况是另一个重要的话题。直接来说，对于非线性系统，可以使用扩展卡尔曼滤波器，Extended Kalman Filter, EKF。

一句话概述：扩展卡尔曼滤波通过局部线性化处理实现对动态系统的高效跟踪，其核心原理是通过一阶泰勒展开近似非线性函数，并沿用卡尔曼滤波的递推框架进行状态预测与更新。这种非线性主要体现在状态转移方程、观测方程上。

6.1 EKF和KF的异同

非线性系统不能像线性系统那样写出一个全局、不变的状态转移矩阵\(F\)和观测矩阵\(H\)，这就导致两个问题：

写不出状态转移矩阵\(\mathbf F\)就无法计算预测状态的误差。尽管可以根据模型计算得到预测结果，却没有办法进行预测误差计算。因为卡尔曼滤波中预测误差计算需要用到状态转移矩阵\(\mathbf F\)，但非线性系统又写不出\(\mathbf F\)。
写不出观测矩阵\(\mathbf H\)就无法计算卡尔曼增益。没有卡尔曼增益进一步就无法对预测值进行修正，以及计算相应误差。

为了解决这个问题，可以考虑进行局部线性化：

对于非线性的状态转移：在\(\mathbf X_{t-1}\)时刻对非线性模型采用一阶泰勒展开进行线性化，这样最终能算得一个雅可比矩阵。用这个雅可比矩阵即可代替状态转移矩阵\(\mathbf F\)，结合误差公式可计算预测误差。
对于非线性的观测方程：在\(\mathbf X_t\)时刻对非线性模型采用一阶泰勒展开进行线性化，这样最终能算得一个雅可比矩阵。用这个雅可比矩阵即可代替观测矩阵\(\mathbf H\)，结合公式即可修正估计和计算误差。

因此，所以对于非线性系统，使用扩展卡尔曼滤波应该包含以下几个步骤：

1. 根据非线性状态转移模型和上一个状态，计算当前状态预测值
1. 对非线性状态转移模型在\(X_{t-1}\)处线性化，求解雅可比矩阵\(\mathbf F\)
1. 基于解算的雅可比矩阵\(\mathbf F\)和给定误差，计算当前预测状态对应的误差
1. 对非线性观测模型在\(X_t\)处线性化，求解雅可比矩阵\(\mathbf H\)
1. 根据上一状态预测误差、给定的观测误差和解算的雅可比矩阵\(\mathbf H\)，计算卡尔曼增益
1. 根据当前预测状态、实际观测和雅可比矩阵\(\mathbf H\)，结合卡尔曼增益平衡预测与观测权重，修正状态预测
1. 根据计算得到的卡尔曼增益、上一个状态预测误差、雅可比矩阵\(\mathbf H\)，计算修正后状态对应的误差

不断重复以上步骤即可实现卡尔曼滤波。可以看到，相比于标准KF，主要是增加了第2步和第4步，用于解算状态转移矩阵\(\mathbf F\)和观测矩阵\(\mathbf H\)。这也说明了为什么EKF相比于KF计算量会大。因为每次计算都要在新的位置重新线性化，计算对应的雅可比矩阵。尽管计算F、H的公式可以事前推导完成，但在运行中仍然需要基于该公式重新计算。而在标准KF中，\(\mathbf F\)、\(\mathbf H\)都是常量固定的。当然，在实际使用中，状态转移和测量并非都是非线性，可能某一个为非线性。

最后回答一个问题：为什么非线性系统写不出全局的状态转移矩阵和观测矩阵？主要有以下两方面原因：

输入与输出不成比例：非线性系统的状态转移/观测函数不满足叠加原理(\(f(ax_{1}+bx_{2})\ne af(x_{1})+bf(x_{2})\))，而全局状态转移矩阵\(\mathbf F\)/观测矩阵\(\mathbf H\)则要求状态变换严格遵循线性叠加。
变量间耦合复杂：变量间存在多项式或超越函数(指数、对数、三角函数、反三角函数等)关系，无法通过常数的状态转移矩阵\(\mathbf F\)/观测矩阵\(\mathbf H\)统一描述所有状态/观测的转移规律。

具体来说，比如观测函数中包含三角函数，无法直接写出常量的观测矩阵\(\mathbf H\)。或者状态由多个变量决定。例如t时刻有状态\([A_t, B_t, C_t]\)、t-1时刻有状态\([A_{t-1}, B_{t-1}, C_{t-1}]\)。但\(A_{t}\)并非只由\(A_{t-1}\)决定，可能由\(A_{t-1}\)、\(B_{t-1}\)、\(C_{t-1}\)共同决定。这种情况同样无法写出常量状态转移矩阵，也属于典型非线性场景。

6.2 基础知识

在正式介绍如何对非线性模型进行线性化前，需要先介绍一些基础知识。

6.2.1 泰勒展开

简单回顾一下泰勒展开。EKF主要用到一阶泰勒展开。一阶泰勒展开是用于在某个点附近用线性函数近似表示原函数的数学方法。对于某个函数\(f(x)\)，在\(x_{0}\)处进行泰勒展开，公式如下：

\[f(x) \approx f(x_{0}) + f'(x_{0})(x-x_{0})\]

其中，\(f'(x_{0})\)表示原函数\(f(x)\)在\(x_{0}\)处的导数。一阶泰勒展开在几何上表示函数在\(x_{0}\)处的‌切线‌，通过切线近似原函数。例如，对\(f(x) = sinx\)在\(x_{0}=0\)处展开，有：

\[sinx \approx sin(0) + cos(0)\cdot (x-0) = x\]

6.2.2 求函数的偏导数

在高数中我们学习过求函数的导数。但如果函数有多个自变量，那么我们就可以对其中某个自变量求偏导数，从而反映函数在特定方向上的变化率。求偏导的一个核心原则是对求导的变量按求导法则求导，非求导变量看做常量。

例如，我们有函数\(f(x,y,z) = 3x^{2}+2xy^{2}+6yz+8z+9\)，分别对x、y、z自变量求导有：

\[\left\{\begin{matrix} \frac{\partial f(x,y,z)}{\partial x} = 6x +2y^{2} \\ \frac{\partial f(x,y,z)}{\partial y} = 4x+6z \\ \frac{\partial f(x,y,z)}{\partial z} = 6y+8 \end{matrix}\right.\]

这样，便完成了对函数各自变量偏导数的求解。

6.2.3 向量/矩阵函数求导

在实际中可能会碰到一种函数，这个函数的自变量或因变量不再是标量，而是向量甚至矩阵，这时如何对这个函数求导呢？给出一个向量函数的例子，例如自变量\(\mathbf{x} = [x, y, z]\)，函数为\(\mathbf f(\mathbf{x})= \begin{bmatrix} x^{2}y +z \\ 3y+x+z \end{bmatrix}\)。这个函数的输入和输出都是一个二维向量。例如，输入\(\mathbf{x}=[1, 1, 1]\)，输出为\([2, 5]\)。

首先介绍输入是向量、输出是标量函数的情况，对于某个函数\(f(\mathbf{x})\)，其自变量\(\mathbf{x} = [x_{1}, x_{2}, x_{3}, \dots, x_{n}]^{T}\)包含\(n\)个分量。我们可以分别对各分量求偏导，从而得到一个偏导数向量\([\frac{\partial f}{\partial x_{0}}, \frac{\partial f}{\partial x_{1}}, \frac{\partial f}{\partial x_{2}},\dots,\frac{\partial f}{\partial x_{n}}]^{T}\)。那么，这个偏导数向量就可以看做是该函数对于自变量的导数。

进一步，如果某个函数\(\mathbf f(\mathbf{x})\)输入是一个\(n\)维向量，输出是一个\(m\)维向量，如下所示：

\[\mathbf{y} = f(\mathbf{x}) = \begin{bmatrix} f_{1}(x_{1}, x_{2}, \dots, x_{n}) \\ f_{2}(x_{1}, x_{2}, \dots, x_{n}) \\ \dots \\ f_{m}(x_{1}, x_{2}, \dots, x_{n}) \end{bmatrix}\]

如何求函数的导数呢？对于单个函数，我们按照上面说的，分别对\(n\)个自变量求偏导，得到该函数关于自变量\(\mathbf{x}\)的导数向量。对于多个函数，重复上述步骤，对\(f_{1},f_{2},\dots,f_{m}\)求导，得到\(f_{1}\)关于\(\mathbf{x}\)的导数向量、\(f_{2}\)关于\(\mathbf{x}\)的导数向量…而这些导数向量合在一起，则是整个\(\mathbf f(\mathbf{x})\)关于\(\mathbf{x}\)的导数。最终，我们可以得到如下矩阵：

\[\mathbf J = \frac{\partial \mathbf f}{\partial \mathbf x} = \begin{bmatrix} \frac{\partial f_{1}}{\partial x_{1}} & \dots & \frac{\partial f_{1}}{\partial x_{n}}\\ \vdots & \ddots & \vdots \\ \frac{\partial f_{m}}{\partial x_{1}} & \dots & \frac{\partial f_{m}}{\partial x_{n}} \end{bmatrix}\]

这个矩阵也就表示了整个\(\mathbf f(\mathbf{x})\)关于\(\mathbf{x}\)的导数，其维度为m×n，我们将其称为雅可比矩阵(Jacobian Matrix)。因此，对向量/矩阵函数求导就等于是求雅可比矩阵。

6.3 非线性模型局部线性化

有了上面的知识就更容易理解EKF中的线性化。核心就是对非线性函数在指定位置进行一阶泰勒展开(6.1部分)，由于泰勒展开中包含函数的导数，所以需要对向量函数求导，也就是求对应的雅可比矩阵(6.3部分)。因此，对于一个\(n\)维的向量\(\mathbf x\)，\(\mathbf f(\mathbf x)\)是\(m\)维向量函数时，对其在\(\mathbf a\)处一阶泰勒展开有：

\[\mathbf f(\mathbf x) \approx \mathbf f(\mathbf a)+\mathbf f'(\mathbf a)(\mathbf x - \mathbf a)\]

根据向量函数求导方法，自然有：

\[\mathbf f(\mathbf x) \approx \mathbf f(\mathbf a)+\mathbf J_{\mathbf f} (\mathbf a)(\mathbf x - \mathbf a)\]

其中最核心的也就是求解\(\mathbf f(\mathbf x)\)在\(\mathbf a\)处对应的雅可比矩阵\(\mathbf J_{\mathbf f} (\mathbf a)\)：

\[\mathbf J_{\mathbf f}(\mathbf a) = \begin{bmatrix} \frac{\partial f_{1}}{\partial x_{1}} & \dots & \frac{\partial f_{1}}{\partial x_{n}}\\ \vdots & \ddots & \vdots \\ \frac{\partial f_{m}}{\partial x_{1}} & \dots & \frac{\partial f_{m}}{\partial x_{n}} \end{bmatrix}_{\mathbf x=\mathbf a}\]

6.3.1 状态转移矩阵

对于形如\(\mathbf x_{t} = \mathbf f(\mathbf x_{t-1})\)的状态转移方程，依据一阶泰勒展开公式，在\(\mathbf x_{t-1}\)处展开，有：

\[\mathbf x_{t} \approx \mathbf f(\mathbf x_{t-1})+\frac{\partial \mathbf f}{\partial \mathbf x}|_{\mathbf x = \mathbf x_{t-1}}(\mathbf x - \mathbf x_{t-1})= \mathbf f(\mathbf x_{t-1}) + \mathbf J_{\mathbf f} (\mathbf x_{t-1})(\mathbf x - \mathbf x_{t-1})\]

此时，状态转移矩阵就是函数\(\mathbf f(\mathbf x)\)在\(\mathbf x_{t-1}\)位置对应的雅可比矩阵。例如，对于某个轮式机器人做匀速运动(速度大小不变，方向可变)，我们可以基于轮速计很容易得到其速度\(v\)，同时其搭载了方位测量设备，也可以很容易获得当前的航向\(\theta\)和角速度\(\omega\)。因此，我们的观测量\(\mathbf z = [v, \theta, \omega]^{T}\)。我们希望获得机器人当前最优的位置、航向角、速度，写成状态向量即为\(\mathbf z = [x, y, \theta, v]^{T}\)。对于t-1时刻状态和t时刻状态，我们可以写出如下关系：

\[\left\{\begin{matrix} x_{t} = x_{t-1} + v \cdot cos(\theta_{t-1})\cdot \Delta t\\ y_{t} = y_{t-1} + v \cdot sin(\theta_{t-1})\cdot \Delta t \\ \theta_{t} = \theta_{t-1} + \omega \cdot \Delta t\\ v_{t} = v_{t-1} \end{matrix}\right.\]

这个式子是比较容易理解的。上一个状态的位置加上速度在x、y方向上的分量再乘以时间间隔，即可预测出当前时刻位置。这里其实和4.3部分的例子是类似的。只是在那个例子中，\(v_x\)、\(v_y\)直接是状态变量之一，直接乘以dt即可得到对应方向的位置变化。这里是\(v\)，所以需要乘以\(cos\)和\(sin\)，将其变为\(v_x\)、\(v_y\)。对于航向角，则等于上一状态的角度加上角速度乘以时间间隔得到的角度变化量。对于速度大小，由于认为是匀速运动，所以上一状态速度和当前状态速度相等。对于上面给出的这一组状态转移方程，因为存在非线性的sin和cos。我们对上式对状态向量进行求导，可以得到雅可比矩阵：

\[\mathbf F = \mathbf J_{\mathbf f} = \frac{\partial \mathbf f}{\partial \mathbf x} = \begin{bmatrix} 1 & 0 & -sin(\theta)v\Delta t & cos(\theta) \Delta t\\ 0 & 1 & cos(\theta) v\Delta t & sin(\theta) \Delta t\\ 0 & 0 & 1 & 0\\ 0 & 0 & 0 & 1 \end{bmatrix}\]

一般情况，\(\Delta t\)可设为1，再将t-1时刻对应的航向角和速度代入，即可得到状态转移矩阵F。基于它便可以计算预测状态误差。

6.3.2 观测矩阵

和状态转移矩阵类似的。对于形如\(\mathbf z = \mathbf h(\mathbf x)\)的状态转移方程，依据一阶泰勒展开公式，在\(\mathbf x_{t}\)处展开，有：

\[\mathbf z \approx \mathbf h(\mathbf x_{t-1})+\frac{\partial \mathbf h}{\partial \mathbf x}|_{\mathbf x = \mathbf x_{t-1}}(\mathbf x - \mathbf x_{t-1})= \mathbf h(\mathbf x_{t-1}) + \mathbf J_{\mathbf h} (\mathbf x_{t-1})(\mathbf x - \mathbf x_{t-1})\]

此时，观测矩阵就是函数\(\mathbf h(\mathbf x)\)在\(\mathbf x_{t-1}\)位置对应的雅可比矩阵。例如，对于某个机器人，其观测是相对于某个已知路标\(L(x_{L}, y_{L})\)的距离\(d\)、路标相对于机器人朝向的夹角\(\alpha\)，测量向量为\(\mathbf z = [d, \alpha]^{T}\)。我们关心的状态包括机器人的位置以及航向角，因此状态向量为\(\mathbf x = [x, y, \theta]^{T}\)。首先，基于已知信息写出观测方程，将状态映射到传感器测量值，如下：

\[\left\{\begin{matrix} d = \sqrt{(x_{L}-x)^2+(y_{L}-y)^2} \\ \alpha = arctan(\frac{\mathrm{d} y_{L}-y}{\mathrm{d} x_{L}-x}) - \theta \end{matrix}\right.\]

这里两个式子都比较好理解，第一个就是根据机器人的位置和路标点的位置，计算距离。第二个是首先基于机器人位置和路标点位置计算了全局的方位角\(arctan(\frac{\mathrm{d} y_{L}-y}{\mathrm{d} x_{L}-x})\)，然后通过减去机器人的航向角\(\theta\)，将全局方位角转化为相对于机器人朝向的角度。然后上式对状态向量进行求导。这里需要注意的是，由于观测方程相对复杂，对其求导也较为复杂，涉及链式法则等，感兴趣可以仔细研究。此处为了便于阅读，略过求导过程，直接展示雅可比矩阵：

\[\mathbf H = \mathbf J_{\mathbf h} = \frac{\partial \mathbf h}{\partial \mathbf x} = \begin{bmatrix} -\frac{x_{L}-x}{\sqrt{(x_{L}-x)^2+(y_{L}-y)^2}} & -\frac{y_{L}-y}{\sqrt{(x_{L}-x)^2+(y_{L}-y)^2}} & 0\\ \frac{y_{L}-y}{\sqrt{(x_{L}-x)^2+(y_{L}-y)^2}} & -\frac{x_{L}-x}{\sqrt{(x_{L}-x)^2+(y_{L}-y)^2}} & -1\\ \end{bmatrix}\]

将t时刻对应的位置代入，即可得到观测矩阵H。基于它便可以计算卡尔曼增益、更新预测及对应的误差。

7.实际示例

7.1 机器人非线性运动

7.1.2 问题描述

对于某个平面运动的机器人，其携带了距离传感器和角度传感器，可以测量相对于路标的距离和夹角。现要求根据测量，给出机器人平面位置、航向角的最优估计。

7.1.3 问题建模

问题建模可以从前面几个问题开始：

观测量是什么：观测是相对于某个已知路标\(L(x_{L}, y_{L})\)的距离\(d\)、路标相对于机器人朝向的夹角\(\alpha\)，测量向量为\(\mathbf z = [d, \alpha]^{T}\)。

希望估计的量是什么：希望估计的状态包括机器人的位置以及航向角，因此状态向量为\(\mathbf x = [x, y, \theta]^{T}\)。

状态间存在什么关联：根据给定的状态和运动模型，可以写出机器人状态之间的转移函数，如下：

利用上式即可进行状态预测。这里需要说明的是，上式中涉及到的速度\(v\)和角速度\(\omega\)既不属于观测向量也不属于状态向量。那么从哪里获得他们的数值呢？答案是他们是作为外部控制因素直接输入的，卡尔曼滤波不对其进行估计或预测。对于\(\Delta t\)一般可设为1。这样，上式中所有变量均已知，可得到t时刻预测状态。

进一步，对上式求导，得到雅可比矩阵F：

\[\mathbf F = \mathbf J_{\mathbf f} = \frac{\partial \mathbf f}{\partial \mathbf x} = \begin{bmatrix} 1 & 0 & -sin(\theta)v\Delta t\\ 0 & 1 & cos(\theta) v\Delta t\\ 0 & 0 & 1 \end{bmatrix}\]

对非线性状态转移函数在t-1时刻线性化，\(\Delta t\)一般可设为1，将对应的航向角和速度代入，即可得到状态转移矩阵F。基于它便可以计算预测状态误差。

系统状态是否有外部因素控制：有，主要是速度\(v\)和角速度\(\omega\)，\(\mathbf u = [v, \omega]^{T}\)其直接参与状态预测计算。

状态和观测之间存在什么关联：测量向量为\(\mathbf z = [d, \alpha]^{T}\)，状态向量为\(\mathbf x = [x, y, \theta]^{T}\)。根据数学模型可以写出两者的观测函数，如下：

\[\left\{\begin{matrix} d = \sqrt{(x_{L}-x)^2+(y_{L}-y)^2} \\ \alpha = arctan(\frac{y_{L}-y}{x_{L}-x}) - \theta \end{matrix}\right.\]

对该函数对状态向量求导，可以得到雅可比矩阵H：

将t时刻对应的位置估计代入，即可得到观测矩阵H。基于它便可以计算卡尔曼增益、更新预测及对应的误差。

实际观测量的误差怎么样：测量向量为\(\mathbf z = [d, \alpha]^{T}\)，假设距离观测方差为0.5，角度观测方差0.17，所以观测噪声矩阵\(\mathbf{R} = \begin{bmatrix} 0.5 & 0\\ 0 & 0.17 \end{bmatrix}\)。

运行过程外部干扰怎么样：假设实际中各种不可预见因素给位置造成方差为0.1的干扰、角度造成0.02干扰，所以\(\mathbf{Q} = \begin{bmatrix} 0.1 & 0 & 0\\ 0 & 0.1 & 0\\ 0 & 0 & 0.02 \end{bmatrix}\)。

初始预测状态怎么样：假设初始观测为\(\mathbf z = [0, 0]^T\)，那么初始预测状态\(\mathbf{x} = [0, 0, 0]^{T}\)。对应状态协方差矩阵\(\mathbf{P} = \begin{bmatrix} 1 & 0 & 0\\ 0 & 1 & 0\\ 0 & 0 & 1 \end{bmatrix}\)。

7.1.4 代码实现

基于上面给出的公式和确定的参数，最终，可以通过以下代码实现卡尔曼滤波。

import numpy as np
from matplotlib import pyplot as plt
import math

# 给定参数
# 两个状态间的时间间隔
delta_t = 0.1

# 观测矩阵
def getJacobianH(state, landmark_pos):
    # 获取从状态向量中获取需要的变量进行计算
    cur_x = state[0]
    cur_y = state[1]
    x_l = landmark_pos[0]
    y_l = landmark_pos[1]
    
    dx = x_l - cur_x
    dy = y_l - cur_y
    d = math.sqrt(dx*dx + dy*dy)
    
    H = np.array([[-dx/d, -dy/d, 0],
                  [dy/d, -dx/d, -1]])
    return H

# 状态转移矩阵
def getJacobianF(state, u, dt):
    # 获取从状态向量和控制向量中获取需要的变量进行计算
    theta_rad = state[2]
    vel = u[0]
    F = np.array([[1, 0, -math.sin(theta_rad)*vel*dt],
                  [0, 1, math.cos(theta_rad)*vel*dt],
                  [0, 0, 1]])
    return F

# 过程噪声协方差矩阵
Q = np.array([[0.1, 0, 0],
              [0, 0.1, 0], 
              [0, 0, 0.02]])

# 测量噪声协方差矩阵
R = np.array([[0.5, 0],
              [0, 0.17]])

# 给定路标位置
landmark_pos = np.array([5, 3])

# 控制输入，给定不同阶段的外部控制变量
controls = []
for i in range(100):
    # 不同阶段速度不同
    if i < 30:
        vel = 1.0
        omega = 0.2
    elif 30 <= i < 70:
        vel = 0.8
        omega = -0.3
    else:
        vel = 1.2
        omega = 0.1
    
    controls.append([vel, omega])


# 构造理论值，方便评估精度，实际无法获得
# 理想状态，包括 位置x、位置y、航向角theta
ideal_states = []
ideal_states.append([0, 0, 0])

for i in range(1, 100):
    x = ideal_states[i-1][0] + controls[i][0] * math.cos(ideal_states[i-1][2]) * delta_t
    y = ideal_states[i-1][1] + controls[i][0] * math.sin(ideal_states[i-1][2]) * delta_t
    theta = ideal_states[i-1][2] + controls[i][1] * delta_t
    ideal_states.append([x, y, theta])

# 根据理论状态值，构造理论观测值和噪声观测值
ideal_measurements = []
noisy_measurements = []
for i in range(len(ideal_states)):
    cur_x = ideal_states[i][0]
    cur_y = ideal_states[i][1]
    cur_theta = ideal_states[i][2]
    
    ideal_d = math.sqrt((landmark_pos[0] - cur_x)**2 + (landmark_pos[1] - cur_y)**2)
    ideal_a = math.atan2(landmark_pos[1] - cur_y, landmark_pos[0] - cur_x) - cur_theta
    
    noisy_d = ideal_d + np.random.normal(0, 0.1)
    noisy_a = ideal_a + np.random.normal(0, 0.1)
    
    ideal_measurements.append([ideal_d, ideal_a])
    noisy_measurements.append([noisy_d, noisy_a])

# 系列观测和外部控制
z = noisy_measurements
u = controls

# 给状态赋初值
prev_state = np.array([0, 0, 0])
prev_error = np.array([[1, 0, 0],
                       [0, 1, 0], 
                       [0, 0, 1]])

# 用于存放估计的状态
predict_states = []
predict_xs = []
predict_ys = []

# 开始卡尔曼滤波
for i in range(len(z)):
    # 预测 - 状态
    # predict_state = F @ prev_state + B @ u[i]
    x_t = prev_state[0] + u[i][0] * math.cos(prev_state[2]) * delta_t
    y_t = prev_state[1] + u[i][0] * math.sin(prev_state[2]) * delta_t
    theta_t = prev_state[2] + u[i][1] * delta_t
    predict_state = np.array([x_t, y_t, theta_t])
    
    # 预测 - 计算雅可比矩阵F
    # 在t-1时刻泰勒展开，所以传入上次的状态
    F = getJacobianF(prev_state, u[i], delta_t)
    
    # 预测 - 误差
    predict_error = F @ prev_error @ F.T + Q
    
    # 更新 - 计算雅可比矩阵H
    # 在t时刻泰勒展开，所以传入当前状态
    H = getJacobianH(predict_state, landmark_pos)
    
    # 更新 - 卡尔曼增益
    kalman_gain  = predict_error @ H.T @ np.linalg.inv(H @ predict_error @ H.T + R)
    
    # 更新 - 状态
    update_state = predict_state + kalman_gain @ (z[i] - H @ predict_state)
    
    # 更新 - 误差
    update_error = (np.eye(3) - kalman_gain @ H) @ predict_error
    
    # 变量赋值开启下次迭代
    prev_state = update_state
    prev_error = update_error
    
    # 将预测状态保存起来
    predict_states.append(update_state)
    predict_xs.append(update_state[0])
    predict_ys.append(update_state[1])

# 可视化
plt.figure(figsize=(10, 6))
plt.plot(predict_xs, predict_ys, 'o-', label='Predict')
plt.scatter(*landmark_pos, c='red', marker='*', s=200, label='Landmark')
plt.legend()
plt.title('Traj.')
plt.xlabel('X (m)')
plt.ylabel('Y (m)')
plt.grid(True)
plt.show()

运行后效果如下：

7.2 多传感器融合

下面代码展示了IMU和GPS传感器融合。

import numpy as np
from scipy.linalg import expm

# 系统参数初始化
dt = 0.01  # 采样时间间隔(秒)
state_dim = 9  # 状态维度 [位置x,y,z, 速度vx,vy,vz, 加速度bias ax_b,ay_b,az_b]
state = np.zeros(state_dim)  # 初始状态
P = np.eye(state_dim) * 10  # 初始协方差矩阵

# IMU噪声参数
gyro_noise = 0.001  # 陀螺仪噪声(rad/s)
accel_noise = 0.01  # 加速度计噪声(m/s²)
bias_noise = 1e-5    # 加速度计偏置噪声

# GPS噪声参数
gps_pos_noise = 0.5  # 位置测量噪声(m)

# 过程噪声矩阵
Q = np.diag([0.1, 0.1, 0.1, 0.05, 0.05, 0.05, 0.01, 0.01, 0.01])

# 观测噪声矩阵
R_gps = np.diag([gps_pos_noise, gps_pos_noise, gps_pos_noise])

def skew(v):
    """计算向量v的斜对称矩阵"""
    return np.array([[0, -v[2], v[1]],
                     [v[2], 0, -v[0]],
                     [-v[1], v[0], 0]])

def rotation_matrix(phi, theta, psi):
    """计算旋转矩阵(滚转phi, 俯仰theta, 偏航psi)"""
    R_x = np.array([[1, 0, 0],
                    [0, np.cos(phi), -np.sin(phi)],
                    [0, np.sin(phi), np.cos(phi)]])
    
    R_y = np.array([[np.cos(theta), 0, np.sin(theta)],
                    [0, 1, 0],
                    [-np.sin(theta), 0, np.cos(theta)]])
    
    R_z = np.array([[np.cos(psi), -np.sin(psi), 0],
                    [np.sin(psi), np.cos(psi), 0],
                    [0, 0, 1]])
    
    return R_z @ R_y @ R_x

def predict(imu_accel, imu_gyro):
    """EKF预测步骤（处理IMU数据）"""
    global state, P
    
    # 解包状态变量
    x, y, z, vx, vy, vz, ax_b, ay_b, az_b = state
    
    # 估计姿态（简化的四元数更新）
    phi, theta, psi = 0, 0, 0  # 实际应用中需通过陀螺仪积分计算
    R = rotation_matrix(phi, theta, psi)
    
    # 校正加速度计偏置
    corrected_accel = imu_accel - [ax_b, ay_b, az_b]
    
    # 将加速度转换到全局坐标系
    global_accel = R @ corrected_accel + np.array([0, 0, -9.81])
    
    # 非线性状态转移函数
    def f(x):
        px, py, pz, vx, vy, vz, ax_b, ay_b, az_b = x
        return np.array([
            px + vx * dt + 0.5 * global_accel[0] * dt**2,
            py + vy * dt + 0.5 * global_accel[1] * dt**2,
            pz + vz * dt + 0.5 * global_accel[2] * dt**2,
            vx + global_accel[0] * dt,
            vy + global_accel[1] * dt,
            vz + global_accel[2] * dt,
            ax_b,
            ay_b,
            az_b
        ])
    
    # 计算雅可比矩阵
    F = np.eye(state_dim)
    F[0, 3] = dt  # ∂x/∂vx
    F[1, 4] = dt  # ∂y/∂vy
    F[2, 5] = dt  # ∂z/∂vz
    F[3, 6:9] = -R[0] * dt  # ∂vx/∂bias
    F[4, 6:9] = -R[1] * dt  # ∂vy/∂bias
    F[5, 6:9] = -R[2] * dt  # ∂vz/∂bias
    
    # 状态预测
    state = f(state)
    
    # 协方差预测
    P = F @ P @ F.T + Q

def update_gps(gps_pos):
    """EKF更新步骤（处理GPS数据）"""
    global state, P
    
    # 观测矩阵（GPS直接观测位置）
    H = np.array([
        [1, 0, 0, 0, 0, 0, 0, 0, 0],
        [0, 1, 0, 0, 0, 0, 0, 0, 0],
        [0, 0, 1, 0, 0, 0, 0, 0, 0]
    ])
    
    # 卡尔曼增益计算
    S = H @ P @ H.T + R_gps
    K = P @ H.T @ np.linalg.inv(S)
    
    # 状态更新
    innovation = gps_pos - state[:3]
    state += K @ innovation
    
    # 协方差更新
    P = (np.eye(state_dim) - K @ H) @ P
    
    # 姿态归一化（实际应用中需处理四元数）

# 主函数
def main():
    # 模拟数据
    np.random.seed(42)
    
    # 真实轨迹（圆周运动）
    t = 0
    radius = 50
    angular_vel = 0.1
    
    for step in range(1000):
        t = step * dt
        
        # 生成真实状态
        true_pos = np.array([
            radius * np.cos(angular_vel * t),
            radius * np.sin(angular_vel * t),
            10.0
        ])
        true_vel = np.array([
            -radius * angular_vel * np.sin(angular_vel * t),
            radius * angular_vel * np.cos(angular_vel * t),
            0
        ])
        
        # 生成IMU测量值（含噪声）
        imu_accel = true_vel * angular_vel + np.random.normal(0, accel_noise, 3)
        imu_gyro = np.array([0, 0, angular_vel]) + np.random.normal(0, gyro_noise, 3)
        
        # 生成GPS测量值（含噪声）
        if step % 10 == 0:  # GPS频率10Hz
            gps_data = true_pos + np.random.normal(0, gps_pos_noise, 3)
        
        # EKF处理
        predict(imu_accel, imu_gyro)
        if step % 10 == 0:
            update_gps(gps_data)
        
        # 每100步输出结果
        if step % 100 == 0:
            print(f"Step {step}: Pos={state[:3]}, Vel={state[3:6]}")

if __name__ == "__main__":
    main()

LeetCode常见题目整理汇总1

Wed, 30 Jul 2025 00:00:00 +0000

请注意:内容可能包含大量图表、公式、代码等，若RSS无法正确显示可直接访问原始网址，获得最好的阅读体验。

删除链表的倒数第N个节点

LeetCode链接：https://leetcode.cn/problems/remove-nth-node-from-end-of-list/description
题目描述：给你一个链表，删除链表的倒数第 n 个结点，并且返回链表的头结点。
关键：先遍历链表获得总长度，再找到需要删除的位置，将节点对应的指针修改或删除。
代码：

#include <iostream>
using namespace std;

struct ListNode {
    int val;
    ListNode *next;

    ListNode() : val(0), next(nullptr) {
    }

    ListNode(int x) : val(x), next(nullptr) {
    }

    ListNode(int x, ListNode *next) : val(x), next(next) {
    }
};

int getLength(ListNode *head) {
    int length = 0;
    while (head) {
        ++length;
        head = head->next;
    }
    return length;
}

ListNode *removeNthFromEnd(ListNode *head, int n) {
    ListNode *dummy = new ListNode(0, head);
    int length = getLength(head);
    ListNode *cur = dummy;
    for (int i = 1; i < length - n + 1; ++i) {
        cur = cur->next;
    }
    cur->next = cur->next->next;
    ListNode *ans = dummy->next;
    delete dummy;
    return ans;
}


int main() {
    ListNode *head = NULL;
    ListNode node1 = {1, head};
    ListNode node2 = {2, head};
    ListNode node3 = {3, head};
    ListNode node4 = {4, head};
    ListNode node5 = {5, head};

    node1.next = &node2;
    node2.next = &node3;
    node3.next = &node4;
    node4.next = &node5;

    head = &node1;
    while (head != nullptr) {
        cout << head->val << endl;
        head = head->next;
    }

    head = &node1;
    head = removeNthFromEnd(head, 2);

    head = &node1;
    while (head != nullptr) {
        cout << head->val << endl;
        head = head->next;
    }

    return 0;
}

两两交换链表中的节点

LeetCode链接：https://leetcode.cn/problems/swap-nodes-in-pairs/description
题目描述：给你一个链表，两两交换其中相邻的节点，并返回交换后链表的头节点。你必须在不修改节点内部的值的情况下完成本题（即，只能进行节点交换）。
关键：可以通过递归的方式实现两两交换链表中的节点。用head表示原始链表的头节点，新的链表的第二个节点，用newHead表示新的链表的头节点，原始链表的第二个节点，则原始链表中的其余节点的头节点是newHead.next。令head.next = swapPairs(newHead.next)，表示将其余节点进行两两交换，交换后的新的头节点为head的下一个节点。然后令newHead.next = head，即完成了所有节点的交换。最后返回新的链表的头节点newHead。
代码：

struct ListNode {
    int val;
    ListNode *next;

    ListNode() : val(0), next(nullptr) {
    }

    ListNode(int x) : val(x), next(nullptr) {
    }

    ListNode(int x, ListNode *next) : val(x), next(next) {
    }
};

ListNode swapPairs(ListNode head) {
    if (head == null || head.next == null) {
        return head;
    }
    ListNode newHead = head.next;
    head.next = swapPairs(newHead.next);
    newHead.next = head;
    return newHead;
}

合并两个有序链表

LeetCode链接：https://leetcode.cn/problems/merge-two-sorted-lists/description
题目描述：将两个升序链表合并为一个新的升序链表并返回。新链表是通过拼接给定的两个链表的所有节点组成的。
关键：可以采用递归方式。两个链表头部值较小的一个节点与剩下元素的merge操作结果合并。
代码：

struct ListNode {
    int val;
    ListNode *next;

    ListNode() : val(0), next(nullptr) {
    }

    ListNode(int x) : val(x), next(nullptr) {
    }

    ListNode(int x, ListNode *next) : val(x), next(next) {
    }
};

ListNode* mergeTwoLists(ListNode* l1, ListNode* l2) {
    if (l1 == nullptr) {
        return l2;
    } else if (l2 == nullptr) {
        return l1;
    } else if (l1->val < l2->val) {
        l1->next = mergeTwoLists(l1->next, l2);
        return l1;
    } else {
        l2->next = mergeTwoLists(l1, l2->next);
        return l2;
    }
}

反转链表

LeetCode链接：https://leetcode.cn/problems/reverse-linked-list/description
题目描述：给你单链表的头节点head，请你反转链表，并返回反转后的链表。
关键：在遍历链表时，将当前节点的next指针改为指向前一个节点。由于节点没有引用其前一个节点，因此必须事先存储其前一个节点。在更改引用之前，还需要存储后一个节点。最后返回新的头引用。
代码：

struct ListNode {
    int val;
    ListNode *next;

    ListNode() : val(0), next(nullptr) {
    }

    ListNode(int x) : val(x), next(nullptr) {
    }

    ListNode(int x, ListNode *next) : val(x), next(next) {
    }
};

ListNode* reverseList(ListNode* head) {
    ListNode* prev = nullptr;
    ListNode* curr = head;
    while (curr) {
        ListNode* next = curr->next;
        curr->next = prev;
        prev = curr;
        curr = next;
    }
    return prev;
}

回文链表

LeetCode链接：https://leetcode.cn/problems/palindrome-linked-list/description
题目描述：给你一个单链表的头节点head，请你判断该链表是否为回文链表。如果是，返回true；否则，返回false。
关键：可以将值复制到数组中后用双指针法。确定数组列表是否回文很简单，我们可以使用双指针法来比较两端的元素，并向中间移动。一个指针从起点向中间移动，另一个指针从终点向中间移动。在起点放置一个指针，在结尾放置一个指针，每一次迭代判断两个指针指向的元素是否相同，若不同，返回false；相同则将两个指针向内移动，并继续判断，直到两个指针相遇。
代码：

struct ListNode {
    int val;
    ListNode *next;

    ListNode() : val(0), next(nullptr) {
    }

    ListNode(int x) : val(x), next(nullptr) {
    }

    ListNode(int x, ListNode *next) : val(x), next(next) {
    }
};

bool isPalindrome(ListNode* head) {
    vector<int> vals;
    while (head != nullptr) {
        // emplace_back是C++11引入的容器成员函数，用于在顺序容器（如std::vector、std::list等）尾部直接构造元素，避免临时对象的拷贝或移动开销。功能上等价于push_back()
        vals.emplace_back(head->val);
        head = head->next;
    }
    for (int i = 0, j = (int)vals.size() - 1; i < j; ++i, --j) {
        if (vals[i] != vals[j]) {
            return false;
        }
    }
    return true;
}

环形链表

LeetCode链接：https://leetcode.cn/problems/linked-list-cycle/description
题目描述：给你一个链表的头节点head，判断链表中是否有环。如果链表中有某个节点，可以通过连续跟踪next指针再次到达，则链表中存在环。如果链表中存在环，则返回true。否则，返回false。
关键：遍历所有节点，每次遍历到一个节点时，判断该节点此前是否被访问过。可以使用哈希表来存储所有已经访问过的节点。每次到达一个节点，如果该节点已经存在于哈希表中，则说明该链表是环形链表，否则就将该节点加入哈希表中。重复这一过程，直到遍历完整个链表即可。
代码：

struct ListNode {
    int val;
    ListNode *next;

    ListNode() : val(0), next(nullptr) {
    }

    ListNode(int x) : val(x), next(nullptr) {
    }

    ListNode(int x, ListNode *next) : val(x), next(next) {
    }
};

bool hasCycle(ListNode *head) {
    // 需要包含unordered_set头文件
    unordered_set<ListNode*> seen;
    while (head != nullptr) {
        if (seen.count(head)) {
            return true;
        }
        seen.insert(head);
        head = head->next;
    }
    return false;
}

排序链表

LeetCode链接：https://leetcode.cn/problems/sort-list/description
题目描述：给你链表的头结点head，请将其按升序排列并返回排序后的链表。
关键：对链表自顶向下归并排序的过程可以通过递归实现。递归的终止条件是链表的节点个数小于或等于1，即当链表为空或者链表只包含1个节点时，不需要对链表进行拆分和排序。1.找到链表的中点，以中点为分界，将链表拆分成两个子链表。寻找链表的中点可以使用快慢指针的做法，快指针每次移动2步，慢指针每次移动1步，当快指针到达链表末尾时，慢指针指向的链表节点即为链表的中点。2.对两个子链表分别排序。3.将两个排序后的子链表合并，得到完整的排序后的链表。
代码：

ListNode* sortList(ListNode* head) {
    return sortList(head, nullptr);
}

ListNode* sortList(ListNode* head, ListNode* tail) {
    if (head == nullptr) {
        return head;
    }
    if (head->next == tail) {
        head->next = nullptr;
        return head;
    }
    ListNode* slow = head, *fast = head;
    while (fast != tail) {
        slow = slow->next;
        fast = fast->next;
        if (fast != tail) {
            fast = fast->next;
        }
    }
    ListNode* mid = slow;
    return merge(sortList(head, mid), sortList(mid, tail));
}

ListNode* merge(ListNode* head1, ListNode* head2) {
    ListNode* dummyHead = new ListNode(0);
    ListNode* temp = dummyHead, *temp1 = head1, *temp2 = head2;
    while (temp1 != nullptr && temp2 != nullptr) {
        if (temp1->val <= temp2->val) {
            temp->next = temp1;
            temp1 = temp1->next;
        } else {
            temp->next = temp2;
            temp2 = temp2->next;
        }
        temp = temp->next;
    }
    if (temp1 != nullptr) {
        temp->next = temp1;
    } else if (temp2 != nullptr) {
        temp->next = temp2;
    }
    return dummyHead->next;
}

二叉树的最大深度

LeetCode链接：https://leetcode.cn/problems/maximum-depth-of-binary-tree/description
题目描述：给定一个二叉树root，返回其最大深度。
关键：二叉树的最大深度是指从根节点到最远叶子节点的最长路径上的节点数。通过递归调用函数实现需求。注意二叉树的节点在C++中的具体实现，其实和链表是类似的。不同的是二叉树的节点有两个指针，分别指向其两个子节点。
代码：

#include <iostream>
using namespace std;

struct TreeNode {
    int val;
    TreeNode *left;
    TreeNode *right;

    TreeNode(): val(0), left(nullptr), right(nullptr) {
    }

    TreeNode(int x) : val(x), left(nullptr), right(nullptr) {
    }

    TreeNode(int x, TreeNode *left, TreeNode *right): val(x), left(left), right(right) {
    }
};

int maxDepth(TreeNode *root) {
    if (root == nullptr) return 0;
    return 1 + max(maxDepth(root->left), maxDepth(root->right));
}


int main() {
    TreeNode node1 = {1, nullptr, nullptr};
    TreeNode node2 = {2, nullptr, nullptr};
    TreeNode node3 = {3, nullptr, nullptr};
    TreeNode node4 = {4, nullptr, nullptr};
    TreeNode node5 = {5, nullptr, nullptr};
    TreeNode node6 = {6, nullptr, nullptr};
    TreeNode node7 = {7, nullptr, nullptr};

    node1.left = &node2;
    node1.right = &node3;
    node2.left = &node4;
    node2.right = &node5;
    node5.left = &node6;
    node5.right = &node7;

    TreeNode *root = &node1;
    cout << maxDepth(root) << endl;

    return 0;
}

二叉树的直径

LeetCode链接：https://leetcode.cn/problems/diameter-of-binary-tree/description
题目描述：给你一棵二叉树的根节点，返回该树的直径。二叉树的直径是指树中任意两个节点之间最长路径的长度。这条路径可能经过也可能不经过根节点root。两节点之间路径的长度由它们之间边数表示。
关键：可以使用深度优先搜索。定义一个递归函数depth(node)计算dnode，函数返回该节点为根的子树的深度。先递归调用左儿子和右儿子求得它们为根的子树的深度L和R。递归搜索每个节点并设一个全局变量ans记录dnode的最大值，最后返回ans-1即为树的直径。
代码：

struct TreeNode {
    int val;
    TreeNode *left;
    TreeNode *right;

    TreeNode(): val(0), left(nullptr), right(nullptr) {
    }

    TreeNode(int x) : val(x), left(nullptr), right(nullptr) {
    }

    TreeNode(int x, TreeNode *left, TreeNode *right): val(x), left(left), right(right) {
    }
};

int ans;

int depth(TreeNode* rt){
    if (rt == NULL) {
        return 0; // 访问到空节点了，返回0
    }
    int L = depth(rt->left); // 左儿子为根的子树的深度
    int R = depth(rt->right); // 右儿子为根的子树的深度
    ans = max(ans, L + R + 1); // 计算d_node即L+R+1 并更新ans
    return max(L, R) + 1; // 返回该节点为根的子树的深度
}

int diameterOfBinaryTree(TreeNode* root) {
    ans = 1;
    depth(root);
    return ans - 1;
}

二叉树的中序遍历

LeetCode链接：https://leetcode.cn/problems/binary-tree-inorder-traversal/description
题目描述：给定一个二叉树的根节点root ，返回它的中序遍历。
关键：二叉树的遍历有多种方法，包括前序遍历、中序遍历和后序遍历。前序遍历访问顺序为根节点→左子树→右子树‌，中序遍历访问顺序为左子树→根节点→右子树‌，后序遍历访问顺序为左子树→右子树→根节点‌。对于中序遍历，整个遍历过程天然具有递归的性质，我们可以直接用递归函数来模拟这一过程。
- inorder(root)表示当前遍历到root节点的答案，那么按照定义，我们只要递归调用
- inorder(root.left)来遍历root节点的左子树，然后将root节点的值加入答案，再递归调用
- inorder(root.right)来遍历root节点的右子树即可，递归终止的条件为碰到空节点。
代码：

struct TreeNode {
    int val;
    TreeNode *left;
    TreeNode *right;

    TreeNode(): val(0), left(nullptr), right(nullptr) {
    }

    TreeNode(int x) : val(x), left(nullptr), right(nullptr) {
    }

    TreeNode(int x, TreeNode *left, TreeNode *right): val(x), left(left), right(right) {
    }
};

void inorder(TreeNode* root, vector<int>& res) {
    if (!root) {
        return;
    }
    inorder(root->left, res);
    res.push_back(root->val);
    inorder(root->right, res);
}

vector<int> inorderTraversal(TreeNode* root) {
    vector<int> res;
    inorder(root, res);
    return res;
}

二叉树的层序遍历

LeetCode链接：https://leetcode.cn/problems/binary-tree-level-order-traversal/description
题目描述：给你二叉树的根节点root，返回其节点值的层序遍历。（即逐层地，从左到右访问所有节点）。
关键：可以使用广度优先搜索。和普通广度优先搜索的区别在于，普通广度优先搜索每次只取一个元素拓展，而这里每次取si个元素。
代码：

struct TreeNode {
    int val;
    TreeNode *left;
    TreeNode *right;

    TreeNode(): val(0), left(nullptr), right(nullptr) {
    }

    TreeNode(int x) : val(x), left(nullptr), right(nullptr) {
    }

    TreeNode(int x, TreeNode *left, TreeNode *right): val(x), left(left), right(right) {
    }
};

vector<vector<int>> levelOrder(TreeNode* root) {
    vector <vector <int>> ret;
    if (!root) {
        return ret;
    }

    queue <TreeNode*> q;
    q.push(root);
    while (!q.empty()) {
        int currentLevelSize = q.size();
        ret.push_back(vector <int> ());
        for (int i = 1; i <= currentLevelSize; ++i) {
            auto node = q.front(); q.pop();
            ret.back().push_back(node->val);
            if (node->left) q.push(node->left);
            if (node->right) q.push(node->right);
        }
    }
    
    return ret;
}

翻转二叉树

LeetCode链接：https://leetcode.cn/problems/invert-binary-tree/description
题目描述：给你一棵二叉树的根节点root，翻转这棵二叉树，并返回其根节点。
关键：从根节点开始，递归地对树进行遍历，并从叶子节点先开始翻转。如果当前遍历到的节点 root的左右两棵子树都已经翻转，那么我们只需要交换两棵子树的位置，即可完成以root为根节点的整棵子树的翻转。
代码：

struct TreeNode {
    int val;
    TreeNode *left;
    TreeNode *right;

    TreeNode(): val(0), left(nullptr), right(nullptr) {
    }

    TreeNode(int x) : val(x), left(nullptr), right(nullptr) {
    }

    TreeNode(int x, TreeNode *left, TreeNode *right): val(x), left(left), right(right) {
    }
};

TreeNode* invertTree(TreeNode* root) {
    if (root == nullptr) {
        return nullptr;
    }
    TreeNode* left = invertTree(root->left);
    TreeNode* right = invertTree(root->right);
    root->left = right;
    root->right = left;
    return root;
}

对称二叉树

LeetCode链接：https://leetcode.cn/problems/symmetric-tree/description
题目描述：给你一个二叉树的根节点root，检查它是否轴对称。
关键：可以递归判断。如果一个树的左子树与右子树镜像对称，那么这个树是对称的。通过「同步移动」两个指针的方法来遍历这棵树，p指针和q指针一开始都指向这棵树的根，随后p右移时，q左移，p左移时，q右移。每次检查当前p和q节点的值是否相等，如果相等再判断左右子树是否对称。
代码：

struct TreeNode {
    int val;
    TreeNode *left;
    TreeNode *right;

    TreeNode(): val(0), left(nullptr), right(nullptr) {
    }

    TreeNode(int x) : val(x), left(nullptr), right(nullptr) {
    }

    TreeNode(int x, TreeNode *left, TreeNode *right): val(x), left(left), right(right) {
    }
};

bool check(TreeNode *p, TreeNode *q) {
    if (!p && !q) return true;
    if (!p || !q) return false;
    return p->val == q->val && check(p->left, q->right) && check(p->right, q->left);
}

bool isSymmetric(TreeNode* root) {
    return check(root->left, root->right);
}

旋转图像

LeetCode链接：https://leetcode.cn/problems/rotate-image/description
题目描述：给定一个n×n的二维矩阵matrix表示一个图像。请你将图像顺时针旋转90度。你必须在原地旋转图像，这意味着你需要直接修改输入的二维矩阵。请不要使用另一个矩阵来旋转图像。
关键：核心是找到旋转前后元素之间的关系。对于矩阵中第i行的第j个元素，在旋转后，它出现在倒数第i列的第j个位置。
代码：

 void rotate(vector<vector<int>>& matrix) {
    int n = matrix.size();
    // C++ 这里的 = 拷贝是值拷贝，会得到一个新的数组
    auto matrix_new = matrix;
    for (int i = 0; i < n; ++i) {
        for (int j = 0; j < n; ++j) {
            matrix_new[j][n - i - 1] = matrix[i][j];
        }
    }
    // 这里也是值拷贝
    matrix = matrix_new;
}

搜索二维矩阵

LeetCode链接：https://leetcode.cn/problems/search-a-2d-matrix/description
题目描述：给你一个满足下述两条属性的mxn整数矩阵：每行中的整数从左到右按非严格递增顺序排列。每行的第一个整数大于前一行的最后一个整数。给你一个整数target，如果target在矩阵中，返回true；否则，返回false。
关键：最简单的方法是遍历整个二维矩阵并逐个比较。更加复杂的一点的算法是利用题目给出的矩阵元素性质采用二分查找。可以对矩阵的第一列的元素二分查找，找到最后一个不大于目标值的元素，然后在该元素所在行中二分查找目标值是否存在。
代码：

bool searchMatrix(vector<vector<int>> matrix, int target) {
    
    // upper_bound是C++标准库中的高效二分查找函数，主要用于在有序序列中定位第一个大于目标值的位置‌，包含在<algorithm>中
    auto row = upper_bound(matrix.begin(), matrix.end(), target, [](const int b, const vector<int> &a) {
        return b < a[0];
    });

    if (row == matrix.begin()) {
        return false;
    }
    --row;
    
    // binary_search是C++标准库中的高效查找函数，用于在有序序列中快速判断目标值是否存在‌，包含在<algorithm>中
    return binary_search(row->begin(), row->end(), target);
}

岛屿数量

LeetCode链接：https://leetcode.cn/problems/number-of-islands/solutions
题目描述：给你一个由’1’（陆地）和’0’（水）组成的的二维网格，请你计算网格中岛屿的数量。岛屿总是被水包围，并且每座岛屿只能由水平方向和/或竖直方向上相邻的陆地连接形成。此外，你可以假设该网格的四条边均被水包围。
关键：可以采用深度优先搜索。可以将二维网格看成一个无向图，竖直或水平相邻的1之间有边相连。为了求出岛屿的数量，我们可以扫描整个二维网格。如果一个位置为1，则以其为起始节点开始进行深度优先搜索。在深度优先搜索的过程中，每个搜索到的1都会被重新标记为0。最终岛屿的数量就是我们进行深度优先搜索的次数。
代码：

void dfs(vector<vector<char>>& grid, int r, int c) {
    int nr = grid.size();
    int nc = grid[0].size();

    grid[r][c] = '0';
    if (r - 1 >= 0 && grid[r-1][c] == '1') dfs(grid, r - 1, c);
    if (r + 1 < nr && grid[r+1][c] == '1') dfs(grid, r + 1, c);
    if (c - 1 >= 0 && grid[r][c-1] == '1') dfs(grid, r, c - 1);
    if (c + 1 < nc && grid[r][c+1] == '1') dfs(grid, r, c + 1);
}


int numIslands(vector<vector<char>>& grid) {
    int nr = grid.size();
    if (!nr) return 0;
    int nc = grid[0].size();

    int num_islands = 0;
    for (int r = 0; r < nr; ++r) {
        for (int c = 0; c < nc; ++c) {
            if (grid[r][c] == '1') {
                ++num_islands;
                dfs(grid, r, c);
            }
        }
    }

    return num_islands;
}

矩形面积

LeetCode链接：https://leetcode.cn/problems/rectangle-area/description
题目描述：给你二维平面上两个由直线构成且边与坐标轴平行/垂直的矩形，请你计算并返回两个矩形覆盖的总面积。每个矩形由其左下顶点和右上顶点坐标表示：第一个矩形由其左下顶点(ax1, ay1)和右上顶点(ax2, ay2)定义。第二个矩形由其左下顶点(bx1, by1)和右上顶点(bx2, by2)定义。
关键：两个矩形覆盖的总面积等于两个矩形的面积之和减去两个矩形的重叠部分的面积。
代码：

int computeArea(int ax1, int ay1, int ax2, int ay2, int bx1, int by1, int bx2, int by2) {
    int area1 = (ax2 - ax1) * (ay2 - ay1), area2 = (bx2 - bx1) * (by2 - by1);
    int overlapWidth = min(ax2, bx2) - max(ax1, bx1), overlapHeight = min(ay2, by2) - max(ay1, by1);
    int overlapArea = max(overlapWidth, 0) * max(overlapHeight, 0);
    return area1 + area2 - overlapArea;
}

圆和矩形是否有重叠

LeetCode链接：https://leetcode.cn/problems/circle-and-rectangle-overlapping/description
题目描述：给你一个以(radius, xCenter, yCenter)表示的圆和一个与坐标轴平行的矩形(x1, y1, x2, y2)，其中 (x1, y1)是矩形左下角的坐标，而(x2, y2)是右上角的坐标。如果圆和矩形有重叠的部分，请你返回true，否则返回false。换句话说，请你检测是否存在点(xi, yi)，它既在圆上也在矩形上（两者都包括点落在边界上的情况）。
关键：圆心临界位置的轨迹是一个圆角矩形——如果尝试把圆心向「圆角矩形」内部移动，就一定会出现公共点；如果向「圆角矩形」外部移动，就不会出现公共点。那么问题就转化成了判断圆心是否在这个圆角矩形内，如果在就表示有公共点，否则没有公共点。
代码：

long long distance(int ux, int uy, int vx, int vy) {
    return (long long)pow(ux - vx, 2) + (long long)pow(uy - vy, 2);
}

bool checkOverlap(int radius, int xCenter, int yCenter, int x1, int y1, int x2, int y2) {
    /* 圆心在矩形内部 */
    if (x1 <= xCenter && xCenter <= x2 && y1 <= yCenter && yCenter <= y2) {
        return true;
    }
    /* 圆心在矩形上部*/
    if (x1 <= xCenter && xCenter <= x2 && y2 <= yCenter && yCenter <= y2 + radius) {
        return true;
    }
    /* 圆心在矩形下部*/
    if (x1 <= xCenter && xCenter <= x2 && y1 - radius <= yCenter && yCenter <= y1) {
        return true;
    }
    /* 圆心在矩形左部*/
    if (x1 - radius <= xCenter && xCenter <= x1 && y1 <= yCenter && yCenter <= y2) {
        return true;
    }
    /* 圆心在矩形右部*/
    if (x2 <= xCenter && xCenter <= x2 + radius && y1 <= yCenter && yCenter <= y2) {
        return true;
    }
    /* 矩形左上角 */
    if (distance(xCenter, yCenter, x1, y2) <= radius * radius)  {
        return true;
    }
    /* 矩形左下角 */
    if (distance(xCenter, yCenter, x1, y1) <= radius * radius) {
        return true;
    }
    /* 矩形右上角 */
    if (distance(xCenter, yCenter, x2, y2) <= radius * radius) {
        return true;
    }
    /* 矩形右下角 */
    if (distance(xCenter, yCenter, x1, y2) <= radius * radius) {
        return true;
    }
    /* 无交点 */
    return false;
}

完美矩形

LeetCode链接：https://leetcode.cn/problems/perfect-rectangle/description
题目描述：给你一个数组rectangles，其中rectangles[i] = [xi, yi, ai, bi]表示一个坐标轴平行的矩形。这个矩形的左下顶点是(xi, yi) ，右上顶点是(ai, bi)。如果所有矩形一起精确覆盖了某个矩形区域，则返回true；否则，返回false。
关键：由于精确覆盖意味着矩形的边和顶点会重合在一起，可统计每个矩形顶点的出现次数。除了要满足矩形区域的面积等于所有矩形的面积之和，还要满足矩形区域四角的顶点只能出现一次，且其余顶点的出现次数只能是两次或四次。在代码实现时，我们可以遍历矩形数组，计算矩形区域四个顶点的位置，以及矩形面积之和，并用哈希表统计每个矩形的顶点的出现次数。遍历完成后，检查矩形区域的面积是否等于所有矩形的面积之和，以及每个顶点的出现次数是否满足上述要求。
代码：

def isRectangleCover(self, rectangles: List[List[int]]) -> bool:
    area, minX, minY, maxX, maxY = 0, rectangles[0][0], rectangles[0][1], rectangles[0][2], rectangles[0][3]
    cnt = defaultdict(int)
    for rect in rectangles:
        x, y, a, b = rect[0], rect[1], rect[2], rect[3]
        area += (a - x) * (b - y)

        minX = min(minX, x)
        minY = min(minY, y)
        maxX = max(maxX, a)
        maxY = max(maxY, b)

        cnt[(x, y)] += 1
        cnt[(x, b)] += 1
        cnt[(a, y)] += 1
        cnt[(a, b)] += 1

    if area != (maxX - minX) * (maxY - minY) or cnt[(minX, minY)] != 1 or cnt[(minX, maxY)] != 1 or cnt[(maxX, minY)] != 1 or cnt[(maxX, maxY)] != 1:
        return False

    del cnt[(minX, minY)], cnt[(minX, maxY)], cnt[(maxX, minY)], cnt[(maxX, maxY)]

    return all(c == 2 or c == 4 for c in cnt.values())

爬楼梯

LeetCode链接：https://leetcode.cn/problems/climbing-stairs/description
题目描述：假设你正在爬楼梯。需要n阶你才能到达楼顶。每次你可以爬1或2个台阶。你有多少种不同的方法可以爬到楼顶呢？
关键：可以使用动态规划。
代码：

int climbStairs(int n) {
    int p = 0, q = 0, r = 1;
    for (int i = 1; i <= n; ++i) {
        p = q; 
        q = r; 
        r = p + q;
    }
    return r;
}

C++基础知识快速回顾

Tue, 29 Jul 2025 00:00:00 +0000

请注意:内容可能包含大量图表、公式、代码等，若RSS无法正确显示可直接访问原始网址，获得最好的阅读体验。

本篇博客对C++相关基础知识做个简单的回顾和整理，方便日后查阅。主要参考这个网页。

一、C++基础知识

1、C++是一种静态类型、编译式、通用、大小写敏感、不规则的编程语言，支持过程化编程、面向对象编程和泛型编程。
2、C++被认为是一种中级语言，它综合了高级语言和低级语言的特点。
3、面向对象开发四大特性：封装(Encapsulation)、继承(Inheritance)、多态(Polymorphism)、抽象(Abstraction)。
- 封装(Encapsulation)是将数据和方法组合在一起，对外部隐藏实现细节，只公开对外提供的接口。这样可以提高安全性、可靠性和灵活性。
- 继承(Inheritance)是从已有类中派生出新类，新类具有已有类的属性和方法，并且可以扩展或修改这些属性和方法。这样可以提高代码的复用性和可扩展性。
- 多态(Polymorphism)是指同一种操作作用于不同的对象，可以有不同的解释和实现。它可以通过接口或继承实现，可以提高代码的灵活性和可读性。
- 抽象(Abstraction)是从具体的实例中提取共同的特征，形成抽象类或接口，以便于代码的复用和扩展。抽象类和接口可以让程序员专注于高层次的设计和业务逻辑，而不必关注底层的实现细节。
4、标准C++由三个重要部分组成：
- 核心语言：提供了所有构件块，包括变量、数据类型和常量等
- C++标准库：提供了大量的函数，用于操作文件、字符串等
- 标准模版库(STL)：提供了大量的方法，用于操作数据结构等

二、C++数学相关函数

在C++标准库中有cmath文件，包含了很多数学有关的内置函数。使用前需要#include <cmath>。常用函数和数字常量简单列举如下：

double cos(double): 返回double类型弧度角度对应的余弦值。
double sin(double): 返回double类型弧度角度对应的正弦值。
double tan(double): 返回double类型弧度角度对应的正切值。
double log(double): 返回double类型参数对应的自然对数。
double pow(double, double): 返回double类型的x(第一个参数)的y(第二个参数)次方值。
double sqrt(double): 返回参数的平方根值。
int abs(int): 返回整数参数的绝对值。
double fabs(double): 返回double型参数的绝对值。
double floor(double): 返回小于double型参数的最大整数。

此外，从C++20开始，引入了预先定义好的数学常数，引入#include <numbers>即可使用，部分如下：

自然底数e: std::numbers::e。
π: std::numbers::pi。

三、C++中的结构体

3.1 结构体定义

struct关键词用于定义结构体（structure），与类（class）类似，结构体允许你定义成员变量和成员函数。定义格式如下：

struct type_name {
    member_type1 member_name1;
    member_type2 member_name2;
};

与class类似，可以在 struct中使用public、private和protected来定义成员访问权限。在struct中，默认所有成员都是 public，而class中默认都是private。因此，可以将 struct当作一种简化形式的 class，适合用于没有太多复杂功能的简单数据封装。

相比于类，结构体具有如下优点：

简单数据封装：适合封装多种类型的简单数据，通常用于数据的存储。
轻量级：相比 class，结构体语法更简洁，适合小型数据对象。
面向对象支持：支持构造函数、成员函数和访问权限控制，可以实现面向对象的设计。

结构体对象的创建和类相同，也非常简单，结构体类型名+对象名即可，如下：

#include <iostream>

using namespace std;


struct myTime {
    int day;
    int hour;
    int minute;
    int second;
};


int main() {
    myTime time1;
    return 0;
}

3.2 访问结构体成员

使用成员访问运算符.即可访问成员(可以修改)，和访问类成员和函数相同。如下：

#include <iostream>

using namespace std;


struct myTime {
    int day;
    int hour;
    int minute;
    int second;
};


int main() {
    myTime time1;
    cout << time1.day << endl;
    time1.day = 100;
    cout << time1.day << endl;

    return 0;
}

四、C++中的Vector

vector是一种序列容器，它允许在运行时动态地插入和删除元素(所以又被称为动态数组)。
vector是基于数组的数据结构，但它可以自动管理内存。与 C++数组相比，vector具有更多的灵活性和功能。
vector是C++标准模板库（STL）的一部分，提供了灵活的接口和高效的操作。需要引入<vector>头文件。

4.1 基本特性

动态大小：vector的大小可以根据需要自动增长和缩小。
连续存储：vector中的元素在内存中是连续存储的，这使得访问元素非常快速。
可迭代：vector可以被迭代，你可以使用循环（如for循环）来访问它的元素。
任意元素类型：vector可以存储任何类型的元素，包括内置类型、对象、指针等。

4.2 适合场景

当需要一个可以动态增长和缩小的数组时。
当需要频繁地在序列的末尾添加或移除元素时。
当需要一个可以高效随机访问元素的容器时。

4.3 Vector操作

简单列举部分常用操作函数：

.push_back(): 将元素添加到vector末尾
.size(): 获取vector的元素数量(大小)
.erase(): 删除vector中指定位置的元素
.clear(): 清空vector中的所有元素

五、C++中的数据结构

前面简单介绍了数组、vector等常用的数据结构，这里对C++中的数据结构做进一步的总结和对比。

5.1 数组(Array)

数组是最基础的数据结构，用于存储一组相同类型的数据。其使用不需要额外包含头文件。

特点：

固定大小，一旦声明，大小不能改变。
直接访问元素，时间复杂度为 O(1)。
适合处理大小已知、元素类型相同的集合。

优点：

访问速度快，内存紧凑。

缺点：

大小固定，无法动态扩展，不适合处理大小不确定的数据集。

代码展示如下：

#include <iostream>

using namespace std;

int main() {
    double myArray[3] = {1, 2, 3};

    return 0;
}

5.2 结构体(Struct)

结构体允许将不同类型的数据组合在一起，形成一种自定义的数据类型。其使用不需要额外包含头文件。

特点：

可以包含不同类型的成员变量。
提供了对数据的基本封装，但功能有限。

代码展示如下：

#include <iostream>

using namespace std;

struct myNode {
    int time;
    double value;
};

int main() {
    myNode node1;
    node1.time = 0;
    node1.value = 0;

    return 0;
}

5.3 类(Class)

类是C++中用于面向对象编程的核心结构，允许定义成员变量和成员函数。与struct类似，但功能更强大，支持继承、封装、多态等特性。其使用不需要额外包含头文件。

特点：

可以包含成员变量、成员函数、构造函数、析构函数。
支持面向对象特性，如封装、继承、多态。

代码示例如下：

#include <iostream>

using namespace std;

class Person {
public:
    string name;
    int age;

    Person(string name, int age) {
        this->name = name;
        this->age = age;
    }
};

int main() {
    Person john = Person("John", 10);

    return 0;
}

5.4 链表(Linked List)

链表是一种动态数据结构，由一系列节点组成，每个节点包含数据和指向下一个节点的指针。使用无需额外包含头文件，但需要自己定义节点结构体，然后组成链表。

特点：

动态调整大小，不需要提前定义容量。
插入和删除操作效率高，时间复杂度为O(1)（在链表头部或尾部操作）。
线性查找，时间复杂度为 O(n)。

优点：

动态大小，适合频繁插入和删除的场景。

缺点：

随机访问效率低，不如数组直接访问快。

代码示例：

#include <iostream>

using namespace std;

struct Node {
    int data;
    Node *next;
};


int main() {
    // 新建head指针用于遍历
    Node *head = NULL;

    // 依次添加节点
    Node node1;
    node1.data = 1;
    node1.next = NULL;

    Node node2;
    node2.data = 2;
    node2.next = NULL;

    Node node3;
    node3.data = 3;
    node3.next = NULL;

    // 将各节点连接起来
    head = &node1;
    node1.next = &node2;
    node2.next = &node3;

    // 利用head指针遍历链表
    while (head != NULL) {
        cout << head->data << endl;
        head = head->next;
    }

    return 0;
}

5.5 栈(Stack)

栈是一种后进先出（LIFO, Last In First Out）的数据结构，常用于递归、深度优先搜索等场景。使用需要包含<stack>头文件。

特点：

只允许在栈顶进行插入和删除操作。
时间复杂度为 O(1)。

优点：

操作简单，效率高。

缺点：

只能在栈顶操作，访问其他元素需要弹出栈顶元素(无法随机访问)。

代码示例如下：

#include <iostream>
#include <stack>

using namespace std;


int main() {
    stack<int> s;
    s.push(2);
    s.push(3);
    s.push(4);

    for (int i = 0; i < 3; ++i) {
        cout << s.top() << endl;
        s.pop();
    }

    return 0;
}

5.6 队列(Queue)

队列是一种先进先出（FIFO, First In First Out）的数据结构，常用于广度优先搜索、任务调度等场景。使用需要包含<queue>头文件。

特点：

插入操作在队尾进行，删除操作在队头进行。
时间复杂度为 O(1)。

优点：

适合按顺序处理数据的场景，如任务调度。

缺点：

无法随机访问元素。

代码示例：

#include <iostream>
#include <queue>

using namespace std;


int main() {
    queue<int> q;
    q.push(1);
    q.push(2);
    q.push(3);

    return 0;
}

5.7 双端队列(Deque)

双端队列允许在两端进行插入和删除操作，是栈和队列的结合体。使用需要包含<deque>头文件。

特点：

允许在两端进行插入和删除。
时间复杂度为 O(1)。

优点：

灵活的双向操作。

缺点：

空间占用较大，适合需要在两端频繁操作的场景。

代码示例：

#include <iostream>
#include <deque>

using namespace std;


int main() {
    deque<int> q;
    q.push_back(1);
    q.push_back(2);
    q.push_front(4);

    return 0;
}

5.8 哈希表(Hash Table)

哈希表是一种通过键值对存储数据的数据结构(就是之前熟悉的Key-Value结构)，支持快速查找、插入和删除操作。C++中的 unordered_map是哈希表的实现。使用需要包含<unordered_map>头文件。

特点：

使用哈希函数快速定位元素，时间复杂度为 O(1)。
不保证元素的顺序。

代码示例：

#include <iostream>
#include <unordered_map>

using namespace std;


int main() {
    unordered_map<string, int> map;
    map["test"] = 1;
    cout << map["test"] << endl;

    return 0;
}

5.9 映射(Map)

map是一种有序的键值对容器，底层实现是红黑树。与unordered_map不同，它保证键的顺序，查找、插入和删除的时间复杂度为O(log n)。使用需要包含<map>头文件。

特点：

保证元素按键的顺序排列。
使用二叉搜索树实现。

优点：

元素有序，适合需要按顺序处理数据的场景。

缺点：

操作效率比unordered_map略低。

代码示例：

#include <iostream>
#include <map>

using namespace std;


int main() {
    map<string, int> map;
    map["test"] = 1;
    cout << map["test"] << endl;

    return 0;
}

5.10 集合(Set)

一种用于存储唯一元素的有序集合，底层同样使用红黑树实现。它保证元素不重复且有序。使用需要包含<set>头文件。

特点：

保证元素的唯一性。
元素自动按升序排列。
时间复杂度为 O(log n)。

优点：

自动排序和唯一性保证。

缺点：

插入和删除的效率不如无序集合。

代码示例：

#include <iostream>
#include <set>

using namespace std;


int main() {
    set<int> s;
    s.insert(6);
    s.insert(7);

    cout << *s.begin() << endl;

    return 0;
}

5.11 动态数组(Vector)

vector是C++标准库提供的动态数组实现，可以动态扩展容量，支持随机访问。使用需要包含<vector>头文件。

特点：

动态调整大小。
支持随机访问，时间复杂度为O(1)。
当容量不足时，动态扩展，时间复杂度为摊销O(1)。

优点：

支持随机访问，动态扩展。

缺点：

插入和删除中间元素的效率较低。

代码示例：

#include <iostream>
#include <vector>

using namespace std;


int main() {
    vector<int> v;
    v.push_back(1);
    v.push_back(2);
    v.push_back(3);

    for (int i = 0; i < v.size(); i++) {
        cout << v[i] << endl;
    }

    return 0;
}

六、C++类相关知识

1、指定新建的类继承了一个已有的类的成员，这个已有的类称为基类，新建的类称为派生类。继承代表了is a关系。
2、当一个类派生自基类，该基类可以被继承为public、protected或private几种类型。几乎不使用protected或private继承，通常使用public继承。
3、继承的基本语法：class Dog : public Animal{}。
4、重载是指，在同一个作用域内，可以声明几个功能类似的同名函数，但是这些同名函数的形式参数（指参数的个数、类型或者顺序）必须不同。
5、多态（Polymorphism）按字面的意思就是多种形态。当类之间存在层次结构，并且类之间是通过继承关联时，就会用到多态。C++多态允许使用基类指针或引用来调用子类的重写方法，从而使得同一接口可以表现不同的行为。
6、虚函数（Virtual Functions）
- 在基类中声明一个函数为虚函数，使用关键字virtual。
- 派生类可以重写（override）这个虚函数。
- 调用虚函数时，会根据对象的实际类型来决定调用哪个版本的函数。
- 给用户提供默认行为，允许子类重写。
7、动态绑定（Dynamic Binding）
- 也称为晚期绑定（Late Binding），在运行时确定函数调用的具体实现。
- 需要使用指向基类的指针或引用来调用虚函数，编译器在运行时根据对象的实际类型来决定调用哪个函数。
8、纯虚函数（Pure Virtual Functions）
- 一个包含纯虚函数的类被称为抽象类（Abstract Class），它不能被直接实例化。
- 纯虚函数没有函数体，声明时使用 = 0。
- 它强制派生类提供具体的实现。
- 定义接口，强制子类实现具体行为。
9、多态的优势：
- 代码复用：通过基类指针或引用，可以操作不同类型的派生类对象，实现代码的复用。
- 扩展性：新增派生类时，不需要修改依赖于基类的代码，只需要确保新类正确重写了虚函数。
- 解耦：多态允许程序设计更加模块化，降低类之间的耦合度。 10、数据抽象是指，只向外界提供关键信息，并隐藏其后台的实现细节，即只表现必要的信息而不呈现细节。数据抽象是一种依赖于接口和实现分离的编程（设计）技术。 11、数据封装（Data Encapsulation）是面向对象编程（OOP）的一个基本概念，它通过将数据和操作数据的函数封装在一个类中来实现。这种封装确保了数据的私有性和完整性，实现数据隐藏，防止了外部代码对其直接访问和修改。 12、数据封装是一种把数据和操作数据的函数捆绑在一起的机制，数据抽象是一种仅向用户暴露接口而把具体的实现细节隐藏起来的机制。

一个继承与多态的示例如下：

#include <iostream>
using namespace std;

// 基类 Animal
class Animal {
public:
    // 虚函数 sound，为不同的动物发声提供接口
    virtual void sound() const {
        cout << "Animal makes a sound" << endl;
    }

    // 虚析构函数确保子类对象被正确析构
    virtual ~Animal() {
        cout << "Animal destroyed" << endl;
    }
};

// 派生类 Dog，继承自 Animal
class Dog : public Animal {
public:
    // 重写 sound 方法
    void sound() const override {
        cout << "Dog barks" << endl;
    }

    ~Dog() {
        cout << "Dog destroyed" << endl;
    }
};

// 派生类 Cat，继承自 Animal
class Cat : public Animal {
public:
    // 重写 sound 方法
    void sound() const override {
        cout << "Cat meows" << endl;
    }

    ~Cat() {
        cout << "Cat destroyed" << endl;
    }
};

// 测试多态
int main() {
    Animal *animalPtr; // 基类指针

    // 创建 Dog 对象，并指向 Animal 指针
    animalPtr = new Dog();
    animalPtr->sound(); // 调用 Dog 的 sound 方法
    delete animalPtr; // 释放内存，调用 Dog 和 Animal 的析构函数

    // 创建 Cat 对象，并指向 Animal 指针
    animalPtr = new Cat();
    animalPtr->sound(); // 调用 Cat 的 sound 方法
    delete animalPtr; // 释放内存，调用 Cat 和 Animal 的析构函数

    return 0;
}

七、参考资料

[1] https://www.runoob.com/cplusplus/cpp-intro.html

SLAM常见功能代码段整理汇总1

Fri, 25 Jul 2025 00:00:00 +0000

请注意:内容可能包含大量图表、公式、代码等，若RSS无法正确显示可直接访问原始网址，获得最好的阅读体验。

整理汇总了部分SLAM中常见功能的代码段实现，以备参考。所有代码均在本机测试通过。

1.位姿转换

主要包含四元数、旋转矩阵、欧拉角和旋转轴不同姿态表示之间的转换。

1.1 四元数归一化

描述：给定一个四元数(qw, qx, qy, qz)，将其进行归一化（模长为1）。

关键：按公式\(\sqrt{ {qx}^2+ {qy}^2+ {qz}^2+ {qw}^2}\)求解模长归一化即可，详见这篇博客。

代码：

import math


def normalizeQuat(qw, qx, qy, qz):
    ori_norm = math.sqrt(qx * qx + qy * qy + qz * qz + qw * qw)

    qx_norm = qx / ori_norm
    qy_norm = qy / ori_norm
    qz_norm = qz / ori_norm
    qw_norm = qw / ori_norm

    return qw_norm, qx_norm, qy_norm, qz_norm


if __name__ == '__main__':
    qw = 0.1
    qx = 0.34
    qy = 0.2
    qz = 0.6
    
    qw_norm, qx_norm, qy_norm, qz_norm = normalizeQuat(qw, qx, qy, qz)

    print(qw_norm, qx_norm, qy_norm, qz_norm)

1.2 四元数转旋转矩阵

描述：给定一个非归一化的四元数(qw, qx, qy, qz)，将其转换为对应的旋转矩阵。

关键：按照转换公式直接转换即可，注意转换前四元数要归一化，详见这篇博客。

代码：

import math
import numpy as np


def cvtQuat2Rot(qw, qx, qy, qz):
    q_mod = math.sqrt(qw * qw + qx * qx + qy * qy + qz * qz)
    qw_norm = qw / q_mod
    qx_norm = qx / q_mod
    qy_norm = qy / q_mod
    qz_norm = qz / q_mod

    r11 = 1 - 2 * (qy_norm * qy_norm + qz_norm * qz_norm)
    r12 = 2 * (qx_norm * qy_norm - qw_norm * qz_norm)
    r13 = 2 * (qx_norm * qz_norm + qw_norm * qy_norm)
    r21 = 2 * (qx_norm * qy_norm + qw_norm * qz_norm)
    r22 = 1 - 2 * (qx_norm * qx_norm + qz_norm * qz_norm)
    r23 = 2 * (qy_norm * qz_norm - qw_norm * qx_norm)
    r31 = 2 * (qx_norm * qz_norm - qw_norm * qy_norm)
    r32 = 2 * (qy_norm * qz_norm + qw_norm * qx_norm)
    r33 = 1 - 2 * (qx_norm * qx_norm + qy_norm * qy_norm)

    rot_mat = np.array([[r11, r12, r13], [r21, r22, r23], [r31, r32, r33]])

    return rot_mat


if __name__ == '__main__':
    qw = 0.1239
    qx = 0.6
    qy = 0.2
    qz = 0.7

    rot_mat = cvtQuat2Rot(qw, qx, qy, qz)

    print(rot_mat)

    # check identity matrix
    print(np.matmul(rot_mat, np.transpose(rot_mat)))

1.3 旋转矩阵转四元数

描述：给定一个旋转矩阵，将其转换为对应的归一化四元数(qw, qx, qy, qz)。

关键：按照转换公式直接转换即可，核心是旋转矩阵的迹是否大于0(矩阵主对角线上各个元素总和)，需要根据不同情况使用不同公式。详见这篇博客。

代码：

import numpy as np

def cvtRot2Quat(R):
    # 计算矩阵迹并选择分支
    trace = np.trace(R)
    
    if trace > 0:
        S = np.sqrt(trace + 1.0) * 2
        w = 0.25 * S
        x = (R[2, 1] - R[1, 2]) / S
        y = (R[0, 2] - R[2, 0]) / S
        z = (R[1, 0] - R[0, 1]) / S
    else:
        if R[0, 0] > R[1, 1] and R[0, 0] > R[2, 2]:
            S = np.sqrt(1.0 + R[0, 0] - R[1, 1] - R[2, 2]) * 2
            w = (R[2, 1] - R[1, 2]) / S
            x = 0.25 * S
            y = (R[0, 1] + R[1, 0]) / S
            z = (R[0, 2] + R[2, 0]) / S
        elif R[1, 1] > R[2, 2]:
            S = np.sqrt(1.0 + R[1, 1] - R[0, 0] - R[2, 2]) * 2
            w = (R[0, 2] - R[2, 0]) / S
            x = (R[0, 1] + R[1, 0]) / S
            y = 0.25 * S
            z = (R[1, 2] + R[2, 1]) / S
        else:
            S = np.sqrt(1.0 + R[2, 2] - R[0, 0] - R[1, 1]) * 2
            w = (R[1, 0] - R[0, 1]) / S
            x = (R[0, 2] + R[2, 0]) / S
            y = (R[1, 2] + R[2, 1]) / S
            z = 0.25 * S

    quat = np.array([w, x, y, z])
    return quat / np.linalg.norm(quat)


if __name__ == '__main__':
    rat = np.identity(3)
    quat = cvtRot2Quat(rat)
    print(quat)

1.4 欧拉角转旋转矩阵

描述：给定三个欧拉角(均以角度表示)：绕Z轴旋转得到的偏航角(Yaw)、绕Y轴旋转得到的俯仰角(Pitch)、绕X轴旋转得到的滚转角(Roll)，旋转顺序为Z-Y-X，将其转换为对应的旋转矩阵R。

关键：分别构建围绕三轴的旋转矩阵，再相乘即可。需要注意的是旋转顺序，详见这篇博客。

代码：

import numpy as np


def euler2Rot(yaw, pitch, roll):
    yaw, pitch, roll = np.radians([yaw, pitch, roll])

    # 计算各轴旋转矩阵
    Rz = np.array([
        [np.cos(yaw), -np.sin(yaw), 0],
        [np.sin(yaw), np.cos(yaw), 0],
        [0, 0, 1]
    ])

    Ry = np.array([
        [np.cos(pitch), 0, np.sin(pitch)],
        [0, 1, 0],
        [-np.sin(pitch), 0, np.cos(pitch)]
    ])

    Rx = np.array([
        [1, 0, 0],
        [0, np.cos(roll), -np.sin(roll)],
        [0, np.sin(roll), np.cos(roll)]
    ])

    # 组合旋转矩阵：R = Rz * Ry * Rx
    return np.matmul(Rz, np.matmul(Ry, Rx))


if __name__ == "__main__":
    # 示例1：90度偏航（绕Z轴）
    print("Yaw 90°:\n", euler2Rot(90, 0, 0))

    # 示例2：航空航天常用角度测试
    aircraft_orientation = euler2Rot(30, 15, -10)
    print("\nAircraft orientation (30°,15°,-10°):\n", aircraft_orientation)

1.5 旋转矩阵转欧拉角

描述：给定旋转矩阵R，将其转换为三个欧拉角(均以角度表示)：绕Z轴旋转得到的偏航角(Yaw)、绕Y轴旋转得到的俯仰角(Pitch)、绕X轴旋转得到的滚转角(Roll)，旋转顺序为Z-Y-X。

关键：按照公式计算，需要注意旋转顺序影响结果，万向锁情况要特殊处理，详见这篇博客。

代码：

import numpy as np
import math

def rot2Euler(R):
    # 处理万向节死锁情况(pitch=±90°)
    if abs(R[2,0]) > 0.9999:
        yaw = math.atan2(-R[1,2], R[1,1])
        pitch = math.copysign(math.pi/2, -R[2,0])
        roll = 0.0
    else:
        yaw = math.atan2(R[1,0], R[0,0])
        pitch = math.asin(-R[2,0])
        roll = math.atan2(R[2,1], R[2,2])
    
    yaw, pitch, roll = np.degrees([yaw, pitch, roll])
    return yaw, pitch, roll

if __name__ == "__main__":
    # 示例1: 90度偏航的旋转矩阵
    R_yaw90 = np.array([[0, -1, 0],
                        [1, 0, 0],
                        [0, 0, 1]])
    yaw, pitch, roll = rot2Euler(R_yaw90)
    print("Yaw 90° conversion:", yaw, pitch, roll)

    # 示例2: 万向节死锁情况测试
    R_gimbal = np.array([[0, 0, 1],
                         [0, 1, 0],
                         [-1, 0, 0]])
    yaw, pitch, roll = rot2Euler(R_gimbal)
    print("Gimbal lock case:", yaw, pitch, roll)

1.6 欧拉角转四元数

描述：将旋转顺序为Z-Y-X的欧拉角(角度表示)：偏航角(Yaw)、俯仰角(Pitch)、滚转角(Roll)，转换为归一化四元数。

关键：按照公式计算，需要注意旋转顺序影响结果，详见这篇博客。

代码：

import numpy as np

def euler2Quat(yaw, pitch, roll):
    yaw = np.radians(yaw)
    pitch = np.radians(pitch)
    roll = np.radians(roll)
    
    cy = np.cos(yaw / 2)
    sy = np.sin(yaw / 2)
    cp = np.cos(pitch / 2)
    sp = np.sin(pitch / 2)
    cr = np.cos(roll / 2)
    sr = np.sin(roll / 2)
    
    w = cr * cp * cy + sr * sp * sy
    x = sr * cp * cy - cr * sp * sy
    y = cr * sp * cy + sr * cp * sy
    z = cr * cp * sy - sr * sp * cy

    return np.array([w, x, y, z]) / np.linalg.norm([w, x, y, z])

if __name__ == "__main__":
    yaw = 30
    pitch = 45
    roll = 60
    quaternion = euler2Quat(yaw, pitch, roll)
    print("Quaternion:", quaternion)

1.7 四元数转欧拉角

描述：将归一化四元数(qw, qx, qy, qz)转换为旋转顺序为Z-Y-X的欧拉角(角度表示)：偏航角(Yaw)、俯仰角(Pitch)、滚转角(Roll)。

关键：按照公式计算，详见这篇博客。

代码：

import numpy as np

def quat2Euler(qw, qx, qy, qz):
    qw_norm = np.sqrt(qw**2 + qx**2 + qy**2 + qz**2)
    w = qw / qw_norm
    x = qx / qw_norm
    y = qy / qw_norm
    z = qz / qw_norm
    
    # 计算yaw (Z轴)
    yaw = np.arctan2(2*(w*z + x*y), 1 - 2*(y**2 + z**2))
    # 计算pitch (Y轴)
    pitch = np.arcsin(2*(w*y - z*x))
    # 计算roll (X轴)
    roll = np.arctan2(2*(w*x + y*z), 1 - 2*(x**2 + y**2))
    
    yaw = np.degrees(yaw)
    pitch = np.degrees(pitch)
    roll = np.degrees(roll)
    
    return yaw, pitch, roll

if __name__ == "__main__":
    qw = 0.371
    qx = 0.1
    qy = 0.271
    qz = 0.5
    
    yaw, pitch, roll = quat2Euler(qw, qx, qy, qz)
    
    print("Yaw:", yaw)
    print("Pitch:", pitch)
    print("Roll:", roll)

1.8 旋转向量转旋转矩阵

描述：给定一个旋转向量，将其转化为对应的旋转矩阵。

关键：使用经典罗德里格斯公式可以实现，罗德里格斯公式描述了旋转向量和旋转矩阵之间的关系，详见这篇博客。这个转换是严密的，不会出现歧义情况。旋转向量的模长表示旋转角度的大小(弧度)，旋转向量的方向表示旋转轴的单位向量，这是角轴表示的核心。所以并不能随便对旋转向量进行归一化，否则会损失旋转角度这一信息。关于角轴表示旋转的详细信息，参考这篇博客。

代码：

import math
import numpy as np

def vector2Rot(rot_vec):
    # 旋转角度(弧度)
    theta = np.linalg.norm(rot_vec)

    # 零旋转情况
    if theta < 1e-10:
        return np.eye(3)
    
    # 单位旋转轴
    k = rot_vec / theta  
    
    # 计算叉乘矩阵K
    K = np.array([[0, -k[2], k[1]],
                  [k[2], 0, -k[0]],
                  [-k[1], k[0], 0]])
    
    # 罗德里格斯公式
    I = np.eye(3)
    R = I + math.sin(theta) * K + (1 - math.cos(theta)) * np.dot(K, K)
    
    return R

if __name__ == "__main__":
    rot_vec = np.array([0.1, 0.2, 0.3])
    
    R = vector2Rot(rot_vec)
    
    print("旋转向量:", rot_vec)
    print("旋转矩阵:\n", R)

1.9 旋转矩阵转旋转向量

描述：给定一个旋转矩阵，将其转化为对应的旋转向量。

关键：使用罗德里格斯公式做逆运算可以实现，详见这篇博客。这个转换是严密的，不会出现歧义情况。关于角轴表示旋转的详细信息，参考这篇博客。

代码：

import numpy as np
import math

def rot2Vec(R):
    # 计算旋转角度(弧度)
    theta = math.acos((np.trace(R) - 1) / 2)
    # 无旋转情况
    if abs(theta) < 1e-10:
        return np.zeros(3)
    
    # 计算旋转轴
    K = (R - R.T) / (2 * math.sin(theta))
    k = np.array([K[2,1], K[0,2], K[1,0]])
    
    # 组合为旋转向量
    return k * theta

if __name__ == "__main__":
    # 示例旋转矩阵（绕Z轴旋转45度）
    R = np.array([
        [0.707, -0.707, 0],
        [0.707,  0.707, 0],
        [0,      0,     1]
    ])
    
    print("旋转向量:", rot2Vec(R))

1.10 四元数转旋转向量

描述：给定一个单位四元数(qw, qx, qy, qz)，将其转化为对应的旋转向量。

关键：旋转角度\(\theta\)按四元数的实部\(qw\)计算\(\theta = 2 \cdot arccos(qw)\)，旋转轴\(\mathbf{K}\)按虚部计算\(\mathbf{K} = \frac{(qx, qy, qz)}{sin(\theta/2)}\)。

代码：

import numpy as np
import math

def quat2Vec(qw, qx, qy, qz):
    # 1. 计算旋转角度
    theta = 2 * math.acos(np.clip(qw, -1.0, 1.0))
    
    # 2. 处理无旋转情况
    if abs(theta) < 1e-10:
        return np.zeros(3)
    
    # 3. 计算虚部模长（避免除零）
    imag_norm = math.sqrt(qx*qx + qy*qy + qz*qz)
    # 虚部接近零
    if imag_norm < 1e-10:
        return np.zeros(3)
    
    # 4. 计算旋转轴
    k = np.array([qx, qy, qz]) / imag_norm
    
    # 5. 角度优化
    if theta > math.pi:
        theta = 2 * math.pi - theta
        k = -k
    
    # 6. 组合旋转向量
    return k * theta

# 测试示例
if __name__ == "__main__":
    # 绕Z轴旋转90度的四元数 (w=cos45°, xyz=k*sin45°)
    qw = 0.7071
    qx = 0
    qy = 0
    qz = 0.7071
    
    # 应接近 [0, 0, 1.57] (π/2≈1.57)
    print("旋转向量:", quat2Vec(qw, qx, qy, qz))  

1.11 旋转向量转四元数

描述：给定一个旋转向量，将其转化为对应的单位四元数(qw, qx, qy, qz)。

关键：根据角轴表示，旋转向量\(\mathbf{V}=\theta \mathbf{K}\)，其中，\(\theta\)为弧度表示的旋转角度(也即模长)，\(\mathbf{K}\)为单位旋转轴向量。在四元数表示中，实部\(qw\)对应旋转角度，按\(qw = cos(\theta/2)\)计算，虚部\((qx, qy, qz)\)对应旋转轴向量，按\((qx, qy, qz) = \mathbf{K}\cdot sin(\theta/2)\)计算。

代码：

import numpy as np

def vec2Quat(v):
    # 根据模长求旋转角度(弧度)
    theta = np.linalg.norm(v)
    # 零旋转
    if theta < 1e-10:
        return np.array([1.0, 0.0, 0.0, 0.0])
    
    k = v / theta
    half_theta = theta / 2
    w = np.cos(half_theta)
    xyz = k * np.sin(half_theta)
    return np.concatenate(([w], xyz))

# 测试示例
if __name__ == "__main__":
    # 绕Z轴旋转90度
    v = np.array([0, 0, np.pi/2])
    
    # 应接近 [0.707, 0, 0, 0.707]
    print("四元数:", vec2Quat(v))

1.12 欧拉角到旋转向量

描述：给定三个欧拉角(均以角度表示)：绕Z轴旋转得到的偏航角(Yaw)、绕Y轴旋转得到的俯仰角(Pitch)、绕X轴旋转得到的滚转角(Roll)，旋转顺序为Z-Y-X，将其转化为对应的旋转向量。

关键：根据已有的代码和转换路径，有欧拉角→旋转矩阵→旋转向量，这两步转换均已实现。

代码：

import numpy as np
import math

def euler2Rot(yaw, pitch, roll):
    yaw, pitch, roll = np.radians([yaw, pitch, roll])

    # 计算各轴旋转矩阵
    Rz = np.array([
        [np.cos(yaw), -np.sin(yaw), 0],
        [np.sin(yaw), np.cos(yaw), 0],
        [0, 0, 1]
    ])

    Ry = np.array([
        [np.cos(pitch), 0, np.sin(pitch)],
        [0, 1, 0],
        [-np.sin(pitch), 0, np.cos(pitch)]
    ])

    Rx = np.array([
        [1, 0, 0],
        [0, np.cos(roll), -np.sin(roll)],
        [0, np.sin(roll), np.cos(roll)]
    ])

    # 组合旋转矩阵：R = Rz * Ry * Rx
    return np.matmul(Rz, np.matmul(Ry, Rx))


def rot2Vec(R):
    # 计算旋转角度(弧度)
    theta = math.acos((np.trace(R) - 1) / 2)
    # 无旋转情况
    if abs(theta) < 1e-10:
        return np.zeros(3)
    
    # 计算旋转轴
    K = (R - R.T) / (2 * math.sin(theta))
    k = np.array([K[2,1], K[0,2], K[1,0]])
    
    # 组合为旋转向量
    return k * theta

if __name__ == "__main__":
    # 示例1：90度偏航（绕Z轴）
    rot1 = euler2Rot(90, 0, 0)
    vec1 = rot2Vec(rot1)
    print("旋转向量:", vec1)
    
    # 示例2：航空航天常用角度测试
    rot2 = euler2Rot(30, 15, -10)
    vec2 = rot2Vec(rot2)
    print("旋转向量:", vec2)

1.13 旋转向量到欧拉角

描述：给定旋转向量，将其转换为三个欧拉角(均以角度表示)：绕Z轴旋转得到的偏航角(Yaw)、绕Y轴旋转得到的俯仰角(Pitch)、绕X轴旋转得到的滚转角(Roll)，旋转顺序为Z-Y-X。

关键：根据已有的代码和转换路径，有旋转向量→旋转矩阵→欧拉角，这两步转换均已实现。

代码：

import math
import numpy as np

def vector2Rot(rot_vec):
    # 旋转角度(弧度)
    theta = np.linalg.norm(rot_vec)

    # 零旋转情况
    if theta < 1e-10:
        return np.eye(3)
    
    # 单位旋转轴
    k = rot_vec / theta  
    
    # 计算叉乘矩阵K
    K = np.array([[0, -k[2], k[1]],
                  [k[2], 0, -k[0]],
                  [-k[1], k[0], 0]])
    
    # 罗德里格斯公式
    I = np.eye(3)
    R = I + math.sin(theta) * K + (1 - math.cos(theta)) * np.dot(K, K)
    
    return R


def rot2Euler(R):
    # 处理万向节死锁情况(pitch=±90°)
    if abs(R[2,0]) > 0.9999:
        yaw = math.atan2(-R[1,2], R[1,1])
        pitch = math.copysign(math.pi/2, -R[2,0])
        roll = 0.0
    else:
        yaw = math.atan2(R[1,0], R[0,0])
        pitch = math.asin(-R[2,0])
        roll = math.atan2(R[2,1], R[2,2])
    
    yaw, pitch, roll = np.degrees([yaw, pitch, roll])
    return yaw, pitch, roll


if __name__ == "__main__":
    rot_vec = np.array([0.1, 0.2, 0.3])
    
    R = vector2Rot(rot_vec)
    yaw, pitch, roll = rot2Euler(R)
    
    print(yaw, pitch, roll)

2.位姿插值

主要包含位置和姿态表示的插值和外推。

2.1 平面二维位置插值

描述：给定t1时刻的位置P1、t2时刻的位置P2，线性内插求解t1到t2时刻内任意时刻t对应的位置P。

关键：按内插的原理和公式求解即可，详见这篇博客。

代码：

class Point2D:
    timestamp = 0
    pos_x = 0
    pos_y = 0

    def __init__(self, t, px, py):
        self.timestamp = t
        self.pos_x = px
        self.pos_y = py
    
    def print(self):
        print("Timestamp:" , self.timestamp)
        print("\t Position (x, y):" , self.pos_x, self.pos_y)
        

def interpolate(p1, p2, t):
    dx = (p2.pos_x - p1.pos_x) * ((t - p1.timestamp) / (p2.timestamp - p1.timestamp))
    dy = (p2.pos_y - p1.pos_y) * ((t - p1.timestamp) / (p2.timestamp - p1.timestamp))

    return Point2D(t, p1.pos_x + dx, p1.pos_y + dy)


if __name__ == '__main__':
    p1 = Point2D(100, 1, 2)
    p2 = Point2D(200, 2, 3)

    p3 = interpolate(p1, p2, 150)

    p1.print()
    p2.print()
    p3.print()

2.2 空间三维位置插值

描述：给定t1时刻的位置P1、t2时刻的位置P2，线性内插求解t1到t2时刻内任意时刻t对应的位置P。

关键：按内插的原理和公式求解即可，详见这篇博客。和二维位置流程一致，只是多了Z轴一个维度。

代码：

class Point3D:
    timestamp = 0
    pos_x = 0
    pos_y = 0
    pos_z = 0
    
    def __init__(self, t, px, py, pz):
        self.timestamp = t
        self.pos_x = px
        self.pos_y = py
        self.pos_z = pz
    
    def print(self):
        print("Timestamp:" , self.timestamp)
        print("\t Position (x, y, z):" , self.pos_x, self.pos_y, self.pos_z)
        

def interpolate(p1, p2, t):
    dx = (p2.pos_x - p1.pos_x) * ((t - p1.timestamp) / (p2.timestamp - p1.timestamp))
    dy = (p2.pos_y - p1.pos_y) * ((t - p1.timestamp) / (p2.timestamp - p1.timestamp))
    dz = (p2.pos_z - p1.pos_z) * ((t - p1.timestamp) / (p2.timestamp - p1.timestamp))

    return Point3D(t, p1.pos_x + dx, p1.pos_y + dy, p1.pos_z + dz)


if __name__ == '__main__':
    p1 = Point3D(100, 1, 2, 3)
    p2 = Point3D(200, 2, 3, 5)

    p3 = interpolate(p1, p2, 150)

    p1.print()
    p2.print()
    p3.print()

2.3 Heading航向角角度插值

描述：给定t1时刻的航向角H1、t2时刻的航向角H2，线性内插求解t1到t2时刻内任意时刻t对应的航向角H。

关键：按内插的原理和公式求解即可，详见这篇博客。Heading角度范围为0-360度，正北为0度，顺时针方向为正，也称为航向角、方位角、偏航角。对应欧拉角姿态表示中的Yaw(绕Z轴旋转)，详见这里。这里面有个问题需要注意。对于从航向角1到航向角2，事实上有两种方式，一种是始终选择小于180度的方向变化过去，另一种是永远以顺时针或逆时针方向旋转，变化过去。两种方式有两种不同计算方法。

代码：

class Heading:
    timestamp = 0
    head_angle = 0
    
    def __init__(self, t, angle):
        self.timestamp = t
        self.head_angle = angle
    
    def print(self):
        print("Timestamp:" , self.timestamp)
        print("\t Heading Angle:" , self.head_angle)


# 无论旋转方向(顺时针或逆时针)，单次角度变化绝对值不大于180，若大于180，按较小角度算
def interpolate1(h1, h2, t):
    ratio = (t - h1.timestamp) / (h2.timestamp - h1.timestamp)
    range_heading = h2.head_angle - h1.head_angle
    
    if range_heading < -180:
        range_heading = range_heading + 360
        new_heading = h1.head_angle + range_heading * ratio
    elif range_heading > 180:
        range_heading = 360 - range_heading
        new_heading = h1.head_angle + 360 - range_heading * ratio
    else:
        new_heading = h1.head_angle + range_heading * ratio
    
    if new_heading >= 360:
        new_heading = new_heading - 360

    return Heading(t, new_heading)

# 所有角度变化都是顺时针，无论角度变化大小
def interpolate2(h1, h2, t):
    ratio = (t - h1.timestamp) / (h2.timestamp - h1.timestamp)
    range_heading = h2.head_angle - h1.head_angle

    if range_heading < 0:
        range_heading = range_heading + 360
    new_heading = h1.head_angle + range_heading * ratio
    
    if new_heading >= 360:
            new_heading = new_heading - 360
    
    return Heading(t, new_heading)


if __name__ == '__main__':
    h1 = Heading(100, 10)
    h2 = Heading(200, 350)

    h3_1 = interpolate1(h1, h2, 150)
    h3_2 = interpolate2(h1, h2, 150)

    h1.print()
    h2.print()
    h3_1.print()
    h3_2.print()

2.4 Pitch俯仰角角度插值

描述：给定t1时刻的俯仰角P1、t2时刻的俯仰角P2，线性内插求解t1到t2时刻内任意时刻t对应的俯仰角P。

关键：按内插的原理和公式求解即可，详见这篇博客。Pitch角度范围为-90到90度，水平为0度，向上为正，向下为负，对应欧拉角姿态表示中的俯仰角，详见这里。因为俯仰角变化范围在180度以内，所以内插相比于Heading航向角容易一些，直接两个状态相减即可。

代码：

class Pitch:
    timestamp = 0
    pitch_angle = 0

    def __init__(self, t, angle):
        self.timestamp = t
        self.pitch_angle = angle
    
    def print(self):
        print("Timestamp:" , self.timestamp)
        print("\t Pitch Angle:" , self.pitch_angle)

def interpolate(p1, p2, t):
    ratio = (t - p1.timestamp) / (p2.timestamp - p1.timestamp)
    range_pitch = p2.pitch_angle - p1.pitch_angle

    new_pitch = p1.pitch_angle + range_pitch * ratio
    
    return Pitch(t, new_pitch)


if __name__ == '__main__':
    p1 = Pitch(100, -60)
    p2 = Pitch(200, 40)

    p3 = interpolate(p1, p2, 150)

    p1.print()
    p2.print()
    p3.print()

2.5 Roll翻滚角角度插值

描述：给定t1时刻的翻滚角R1、t2时刻的翻滚角R2，线性内插求解t1到t2时刻内任意时刻t对应的翻滚角R。

关键：按内插的原理和公式求解即可，详见这篇博客。Roll角度范围为-90到90度，水平为0度，向右为正，向下左为负，对应欧拉角姿态表示中的翻滚角，详见这里。因为翻滚角变化范围在180度以内，所以内插相比于Heading航向角容易一些，直接两个状态相减即可。

代码：

class Roll:
    timestamp = 0
    roll_angle = 0

    def __init__(self, t, angle):
        self.timestamp = t
        self.roll_angle = angle
    
    def print(self):
        print("Timestamp:" , self.timestamp)
        print("\t Roll Angle:" , self.roll_angle)

def interpolate(r1, r2, t):
    ratio = (t - r1.timestamp) / (r2.timestamp - r1.timestamp)
    range_roll = r2.roll_angle - r1.roll_angle

    new_roll = r1.roll_angle + range_roll * ratio
    
    return Roll(t, new_roll)


if __name__ == '__main__':
    r1 = Roll(100, -60)
    r2 = Roll(200, 40)

    r3 = interpolate(r1, r2, 150)

    r1.print()
    r2.print()
    r3.print()

2.6 平面位姿(位置+航向角)插值

描述：给定t1时刻的位置P1和航向角H1、t2时刻的位置P2和航向角H2，线性内插求解t1到t2时刻内任意时刻t对应的位置P和航向角H。

关键：按内插的原理和公式求解即可，本质上是位置插值+Heading航向角插值组合。注意事项详见各部分。

代码：

class Pose2D:
    timestamp = 0
    pos_x = 0
    pos_y = 0
    head_angle = 0
    
    def __init__(self, t, px, py, heading):
        self.timestamp = t
        self.pos_x = px
        self.pos_y = py
        self.head_angle = heading
    
    def print(self):
        print("Timestamp:" , self.timestamp)
        print("\t Pose (x, y, heading):" , self.pos_x, self.pos_y, self.head_angle)


# 无论旋转方向(顺时针或逆时针)，单次角度变化不大于180，若大于180，按较小角度算
def interpolate1(p1, p2, t):
    ratio = (t - p1.timestamp) / (p2.timestamp - p1.timestamp)

    dx = (p2.pos_x - p1.pos_x) * ratio
    dy = (p2.pos_y - p1.pos_y) * ratio

    new_x = p1.pos_x + dx
    new_y = p1.pos_y + dy

    range_heading = p2.head_angle - p1.head_angle
    
    if range_heading < -180:
        range_heading = range_heading + 360
        new_heading = p1.head_angle + range_heading * ratio
    elif range_heading > 180:
        range_heading = 360 - range_heading
        new_heading = p1.head_angle + 360 - range_heading * ratio
    else:
        new_heading = p1.head_angle + range_heading * ratio
    
    if new_heading >= 360:
        new_heading = new_heading - 360

    return Pose2D(t, new_x, new_y, new_heading)


# 所有角度变化都是顺时针，无论角度变化大小
def interpolate2(p1, p2, t):
    ratio = (t - p1.timestamp) / (p2.timestamp - p1.timestamp)
    
    dx = (p2.pos_x - p1.pos_x) * ratio
    dy = (p2.pos_y - p1.pos_y) * ratio

    new_x = p1.pos_x + dx
    new_y = p1.pos_y + dy

    range_heading = p2.head_angle - p1.head_angle

    if range_heading < 0:
        range_heading = range_heading + 360
    new_heading = p1.head_angle + range_heading * ratio
    
    if new_heading >= 360:
            new_heading = new_heading - 360

    return Pose2D(t, new_x, new_y, new_heading)


if __name__ == '__main__':
    p1 = Pose2D(100, 1, 2, 350)
    p2 = Pose2D(200, 2, 3, 10)

    p3 = interpolate1(p1, p2, 160)

    p1.print()
    p2.print()
    p3.print()

2.7 空间位姿(位置+三轴姿态)插值

描述：给定t1时刻的位置P1和航向角H1、俯仰角P1、翻滚角R1，t2时刻的位置P2和航向角H2、俯仰角P2、翻滚角R2，线性内插求解t1到t2时刻内任意时刻t对应的位置P和航向角H、俯仰角P、翻滚角R。

关键：按内插的原理和公式求解即可，本质上是位置插值+Heading航向角插值+Pitch俯仰角插值+Roll翻滚角插值组合。注意事项详见各部分。

代码：

class Pose3D:
    timestamp = 0

    pos_x = 0
    pos_y = 0
    pos_z = 0

    head_angle = 0
    pitch_angle = 0
    roll_angle = 0

    
    def __init__(self, t, px, py, pz, heading, pitch, roll):
        self.timestamp = t
        self.pos_x = px
        self.pos_y = py
        self.pos_z = pz
        self.head_angle = heading
        self.pitch_angle = pitch
        self.roll_angle = roll
        
    
    def print(self):
        print("Timestamp:" , self.timestamp)
        print("\t Pose (x, y, z, heading, pitch, roll):" , self.pos_x, self.pos_y, self.pos_z, self.head_angle, self.pitch_angle, self.roll_angle)


def interpolate(p1, p2, t):
    ratio = (t - p1.timestamp) / (p2.timestamp - p1.timestamp)

    # 位置插值
    dx = (p2.pos_x - p1.pos_x) * ratio
    dy = (p2.pos_y - p1.pos_y) * ratio
    dz = (p2.pos_z - p1.pos_z) * ratio

    new_x = p1.pos_x + dx
    new_y = p1.pos_y + dy
    new_z = p1.pos_z + dz

    # Heading插值(使用小于180角的变化方式)
    range_heading = p2.head_angle - p1.head_angle
    
    if range_heading < -180:
        range_heading = range_heading + 360
        new_heading = p1.head_angle + range_heading * ratio
    elif range_heading > 180:
        range_heading = 360 - range_heading
        new_heading = p1.head_angle + 360 - range_heading * ratio
    else:
        new_heading = p1.head_angle + range_heading * ratio
    
    if new_heading >= 360:
        new_heading = new_heading - 360

    # Pitch插值
    range_pitch = p2.pitch_angle - p1.pitch_angle
    new_pitch = p1.pitch_angle + range_pitch * ratio

    # Roll插值
    range_roll = p2.roll_angle - p1.roll_angle
    new_roll = p1.roll_angle + range_roll * ratio

    return Pose3D(t, new_x, new_y, new_z, new_heading, new_pitch, new_roll)


if __name__ == '__main__':
    p1 = Pose3D(100, 1, 2, 3, 350, 30, 40)
    p2 = Pose3D(200, 2, 3, 10, 10, 20, 60)

    p3 = interpolate(p1, p2, 150)

    p1.print()
    p2.print()
    p3.print()

2.8 三维空间位置+姿态状态外推

描述：给定t1时刻的位置P1和航向角H1、俯仰角P1、翻滚角R1，t2时刻的位置P2和航向角H2、俯仰角P2、翻滚角R2，线性内插求解t2时刻后任意时刻t对应的位置P和航向角H、俯仰角P、翻滚角R。

关键：本质上和内插的原理相同，采用线性方式计算，代码完全一致。和内插不同的是它的时间ratio是大于1的。位置插值+Heading航向角插值+Pitch俯仰角插值+Roll翻滚角插值组合。注意事项详见各部分。

代码：

class Pose3D:
    timestamp = 0

    pos_x = 0
    pos_y = 0
    pos_z = 0

    head_angle = 0
    pitch_angle = 0
    roll_angle = 0

    
    def __init__(self, t, px, py, pz, heading, pitch, roll):
        self.timestamp = t
        self.pos_x = px
        self.pos_y = py
        self.pos_z = pz
        self.head_angle = heading
        self.pitch_angle = pitch
        self.roll_angle = roll
        
    
    def print(self):
        print("Timestamp:" , self.timestamp)
        print("\t Pose (x, y, z, heading, pitch, roll):" , self.pos_x, self.pos_y, self.pos_z, self.head_angle, self.pitch_angle, self.roll_angle)


def interpolate(p1, p2, t):
    ratio = (t - p1.timestamp) / (p2.timestamp - p1.timestamp)

    # 位置插值
    dx = (p2.pos_x - p1.pos_x) * ratio
    dy = (p2.pos_y - p1.pos_y) * ratio
    dz = (p2.pos_z - p1.pos_z) * ratio

    new_x = p1.pos_x + dx
    new_y = p1.pos_y + dy
    new_z = p1.pos_z + dz

    # Heading插值(使用小于180角的变化方式)
    range_heading = p2.head_angle - p1.head_angle
    
    if range_heading < -180:
        range_heading = range_heading + 360
        new_heading = p1.head_angle + range_heading * ratio
    elif range_heading > 180:
        range_heading = 360 - range_heading
        new_heading = p1.head_angle + 360 - range_heading * ratio
    else:
        new_heading = p1.head_angle + range_heading * ratio
    
    if new_heading >= 360:
        new_heading = new_heading - 360

    # Pitch插值
    range_pitch = p2.pitch_angle - p1.pitch_angle
    new_pitch = p1.pitch_angle + range_pitch * ratio

    # Roll插值
    range_roll = p2.roll_angle - p1.roll_angle
    new_roll = p1.roll_angle + range_roll * ratio

    return Pose3D(t, new_x, new_y, new_z, new_heading, new_pitch, new_roll)


if __name__ == '__main__':
    p1 = Pose3D(100, 1, 2, 3, 350, 30, 40)
    p2 = Pose3D(200, 2, 3, 10, 10, 20, 60)

    p3 = interpolate(p1, p2, 300)

    p1.print()
    p2.print()
    p3.print()

3.空间位姿变换

主要包含二维和三维的空间变换。

3.1 二维位姿旋转变换

描述：给定t时刻的位置P(x, y)以及对应的Heading角h，求以C点(cx, cy)为中心，顺时针旋转alpha角度(规定逆时针旋转为正方向)，求旋转后的位姿P’(x’, y’, h’)。

关键：对于位置，按照平面旋转公式求解即可，详见这篇博客。对于角度，类似于前面提到的Heading角度内插，原理相同。这里需要注意的是旋转方向的问题，比较容易弄错。

代码：

import math

class Pose2D:
    timestamp = 0
    pos_x = 0
    pos_y = 0
    head_angle = 0

    def __init__(self, t, px, py, heading):
        self.timestamp = t
        self.pos_x = px
        self.pos_y = py
        self.head_angle = heading

    def print(self):
        print("Timestamp:", self.timestamp)
        print("\t Pose (x, y, heading):", self.pos_x, self.pos_y, self.head_angle)


def rotate(cx, cy, rotate_angle, point1):
    rotate_angle_rad = math.radians(rotate_angle)

    # 坐标旋转
    new_x = cx + math.cos(rotate_angle_rad) * point1.pos_x - math.sin(rotate_angle_rad) * point1.pos_y
    new_y = cy + math.sin(rotate_angle_rad) * point1.pos_x - math.cos(rotate_angle_rad) * point1.pos_y

    # Heading角度旋转
    if rotate_angle < 0:
        new_heading = point1.head_angle - rotate_angle
        if new_heading > 360:
            new_heading = new_heading - 360
    else:
        new_heading = point1.head_angle + rotate_angle
        if new_heading < 0:
            new_heading = new_heading + 360

    return Pose2D(point1.timestamp, new_x, new_y, new_heading)

if __name__ == '__main__':
    center_x = 0
    center_y = 0
    rotate_angle = -90 # 角度表示，逆时针为正，顺时针为负

    p1 = Pose2D(1, 0, 1, 30)
    p2 = rotate(center_x, center_y, rotate_angle, p1)

    p1.print()
    p2.print()

3.2 三维位置旋转变换

描述：给定t时刻的位置P(x, y, z)，分别绕Z轴、Y轴、X轴旋转alpha、beta、gamma，求旋转后的位置P’(x’, y’, z’)。

关键：按照旋转角度构建旋转矩阵并组合，最后应用到三维坐标点上即可，详见这篇博客。在构造旋转矩阵时，需要注意旋转方向问题。为了简单记住到底旋转后把谁当作横轴谁当作纵轴，总结了一个简单小方法。即以哪个轴旋转，就用手握住这个轴，然后逆时针转动，最先碰到的那个轴作为横轴，剩下的那个作为纵轴。

代码：

import math
import numpy as np

if __name__ == '__main__':
    init_x = 1
    init_y = 1
    init_z = 1

    rotate_z = 0
    rotate_y = 0
    rotate_x = 0

    pos1 = np.array([[init_x], [init_y], [init_z]])

    rotate_x_rad = math.radians(rotate_x)
    rotate_y_rad = math.radians(rotate_y)
    rotate_z_rad = math.radians(rotate_z)

    # 构造旋转矩阵
    rot_z = np.array(
        [[math.cos(rotate_z_rad), -math.sin(rotate_z_rad), 0],
         [math.sin(rotate_z_rad), math.cos(rotate_z_rad), 0],
         [0, 0, 1]])
    rot_y = np.array([[math.cos(rotate_y_rad), 0, math.sin(rotate_y_rad)],
                      [0, 1, 0],
                      [-math.sin(rotate_y_rad), 0, math.cos(rotate_y_rad)]])
    rot_x = np.array([[1, 0, 0],
                      [0, math.cos(rotate_x_rad), -math.sin(rotate_x_rad)],
                      [0, math.sin(rotate_x_rad), math.cos(rotate_x_rad)]])

    pos2 = np.matmul(rot_z, np.matmul(rot_y, np.matmul(rot_x, pos1)))

    print(pos1)
    print(pos2)

3.3 三维位姿旋转变换

描述：给定t时刻的位置P(x, y, z)以及对应的Heading、Pitch、Roll角度，将其分别绕Z轴、Y轴、X轴旋转alpha、beta、gamma(均为默认正方向)，求旋转后的位置和姿态。

关键：对于位置，按照旋转角度构建旋转矩阵并组合，最后应用到三维坐标点上即可，详见这篇博客。对于角度，各个角度与对应的旋转角累加即可。

代码：

import math
import numpy as np

if __name__ == '__main__':
    init_x = 1
    init_y = 1
    init_z = 1

    init_heading = 0
    init_pitch = 0
    init_roll = 0

    rotate_z = 10
    rotate_y = 0
    rotate_x = 0

    pos1 = np.array([[init_x], [init_y], [init_z]])

    rotate_x_rad = math.radians(rotate_x)
    rotate_y_rad = math.radians(rotate_y)
    rotate_z_rad = math.radians(rotate_z)

    # 构造旋转矩阵
    rot_z = np.array(
        [[math.cos(rotate_z_rad), -math.sin(rotate_z_rad), 0],
         [math.sin(rotate_z_rad), math.cos(rotate_z_rad), 0],
         [0, 0, 1]])
    rot_y = np.array([[math.cos(rotate_y_rad), 0, math.sin(rotate_y_rad)],
                      [0, 1, 0],
                      [-math.sin(rotate_y_rad), 0, math.cos(rotate_y_rad)]])
    rot_x = np.array([[1, 0, 0],
                      [0, math.cos(rotate_x_rad), -math.sin(rotate_x_rad)],
                      [0, math.sin(rotate_x_rad), math.cos(rotate_x_rad)]])

    pos2 = np.matmul(rot_z, np.matmul(rot_y, np.matmul(rot_x, pos1)))

    # Heading角
    new_heading = init_heading + rotate_z

    # Pitch角
    new_pitch = init_pitch + rotate_y

    # Roll角
    new_roll = init_roll + rotate_x

    print(pos1, init_heading, init_pitch, init_roll)
    print(pos2, new_heading, new_pitch, new_roll)

Word文档导出PDF文件不同方案浅析与对比

Fri, 04 Jul 2025 00:00:00 +0000

请注意:内容可能包含大量图表、公式、代码等，若RSS无法正确显示可直接访问原始网址，获得最好的阅读体验。

1.背景

本篇博客主要探讨在将Word文档导出为PDF文件时的一些问题和注意事项，主要是之前写毕业论文时候的一些经验，记录下来以备日后查阅。

在写论文等文档的时候可能你也遇到过这样的情况：精心设计画了一个很漂亮的图，插入Word以后，再导出为PDF，PDF文件中的图片画质显著下降，比较难看，如下图所示：可以看到，洞穴内壁上的纹理在右边导出的PDF中已经模糊了。而且文字部分出现了压缩的马赛克，看起来十分难看。所以，如何尽可能保证输出PDF文档中图像的质量是至关重要的问题，也是本篇博客的关注点。本篇博客使用的软件如下：

Microsoft Office LTSC 专业增强版 2024 Word
Adobe Acrobat 2025

2.基础知识

在继续之前，先介绍一些Word插入图片的小技巧和基础知识。

2.1 Word插入图片后不压缩质量

一般情况下，当我们插入一张图片到Word后，为了控制文件大小，Word默认会对图片进行压缩(这个过程不可逆)。因此，为了获得尽可能高质量的图片，可以在Word设置中设置不压缩图片。点击Word菜单栏中的“文件”-“选项”-“高级”，找到“图像大小和质量”，如下。可以看到，Word中默认的图片分辨率是220ppi。我们可以把“不压缩文件中的图像”选项勾选上，这样下次再向这个文件里插入图片就是原图了。当然，你也可以在上面的作用域里选择“所有新文档”，这样之后Word新建所有文档图片都不压缩。这里需要特别注意两点，第一是Word图片压缩过程是不可逆的，第二是这个选项只对修改设置后新插入的图片有效。换句话说，你之前文档里插入了100张图片，默认情况下已经被压缩了，即使你现在改了配置，之前插入的100张图片也不会再变成原图质量，只能重新插入才可以。所以这个选项如果要修改，一定要记得在一开始就做，不然很多图片还得重插。

2.2 Word插入和输出矢量图

在某些情况下，你可能用Visio画了一个论文的框架图，这个图本身是矢量的，如何把它插到Word里呢？主要有三种办法。

第一种是插入Visio对象。点击Word菜单栏中的“插入”，找到“对象”，然后选择“由文件创建”，选择画好的Visio文件即可，插入后如下图所示。这种方式的优点是比较直观，而且可以通过双击插入的图片直接编辑Visio对象内容。它的缺点也比较明显。首先这种方式插入的图片并不能像普通图片一样，选中图片以后在Word中调起“图片格式”标签，从而进行旋转、长宽精确修改、裁剪等操作，无法方便快捷的和Word其它内容进行整体排版。而且通过个人测试发现，这种直接插入Visio对象的方法有一定概率在导出PDF时图片内部的文字出现丢失甚至乱码(尤其是图片内容本身比较复杂的时候)。因此正常情况下不推荐这种方法。

第二种是在Visio中将内容导出成JPG或PNG位图，然后插入Word文档中。在导出图片的时候，尽量设置高一些的分辨率，导出的图片放大后会更清晰一些(个人喜欢设为300x300像素/in)，导出后的效果如下：可以看到，总体而言，在相同PDF导出配置下，PNG图片比JPG图片质量更高。特别是在文字边缘等纹理复杂区域，JPG出现了明显的压缩马赛克。所以相比于JPG，推荐使用PNG格式导出。但这种方式依然有问题，PNG依旧是位图，图中的文字无法选择，只是分辨率高一些更细致一些。只要放大足够大，导出后的图像边缘依然可以看到像素，如下所示。总体而言，这种方法的有点就是简单方便，导出的位图可以在Word中方便的编辑、排版。缺点则是本质仍是位图，放大后会有马赛克，同时图片中的文字无法选中。如果只是简单情况，可以使用这种方法，否则不推荐。

第三种是在Visio中导出成Word认可的矢量图文件，再插入Word。更具体来说，在Visio中将绘制内容导出为EMF文件。EMF文件全称Enhanced Metafile，中文叫增强型图元文件，由微软提出。感兴趣可以参考这个和这个网页。导出EMF文件后，就和PNG文件一样插入Word，并且支持“图片格式”工具，可以方便的裁剪、修改大小、排版等操作。导出成PDF后，图片内容依旧是矢量，文字可以选中，放大不会变成马赛克，如下：总体而言，这种方法可以像普通位图一样快捷排版，又可以导出矢量图，放大不失真。几乎没有缺点，是最推荐的Word插入矢量图的方式。唯一可能的缺点是，如果绘制的矢量图相对复杂切包含一些位图内容的话，导出的EMF文件可能比较大。不过并不会直接影响最终导出PDF文件的大小。

3.Word文档导出PDF

为了说明和验证Word导出PDF各种方式的差异和优势，创建了一个包含文本、位图、矢量图的文档用于测试，截图如下：之后会观察导出后各图片的质量。

3.1 导出PDF的可行方式

在Word中，有多种导出PDF的方式，主要包含如下几种：

另存为方法1：菜单栏点击“文件”-“另存为”，文件类型选择PDF
另存为方法2：菜单栏点击“文件”-“另存为Adobe PDF方法”
打印方法1：菜单栏点击“文件”-“打印”，打印机选择“Microsoft Print to PDF”
打印方法2：菜单栏点击“文件”-“打印”，打印机选择“Adobe PDF”
Acrobat方法：菜单栏点击“Acrobat”，点击“创建PDF”

先简单介绍一下各方法。首先是打印类的方法，这种方法会把文档以打印的形式转换为PDF，输出的PDF文件是纯图片，文字无法选择。因此除非有特殊要求的场景，否则不推荐此类方法。另存为方法1是Word利用自带的PDF生成器进行输出，基本无法调整输出参数。输出质量总体还可以，适合于普通没有特殊需求的场景，更复杂场景不推荐。Acrobat方法则是基于安装的Acrobat进行生成，有很多参数和预置方案可供选择，可以生成满足不同需求的PDF文件。另存为方法2本质上是Acrobat方法的“快捷方式”，其会基于现有设定好的Acrobat输出方案进行PDF输出。

因此普通情况使用另存为方法1即可，对输出质量或大小有进一步要求则推荐使用Acrobat方法。而对Acrobat方法中的诸多参数和预置方案下面进行简单分析。

3.2 Acrobat导出方案分析

在Acrobat中，默认有Standard-Classic、标准、超大页面、高质量打印、印刷质量、最小文件大小等方案。我们可以点击“高级设置”从而看到各预置模式的详细参数，如下：下面简单对一些重要参数进行分析。

3.2.1 一般参数

如下图所示。在一般参数中，比较核心的是兼容性、分辨率这两个。兼容性是指该PDF文件的版本，如果版本过高，可能在一些非常老的电脑上无法打开，一般用默认值即可。分辨率则是指整个文档的分辨率，单位是“点/英寸”。理论上分辨率越高文档内容越精细，但与之对应文档大小会越大。一般情况下默认的600是足够的。

3.2.2 图像参数

如下图所示。图像参数在很大程度上决定了输出文件的大小以及输出图片的质量。在图像参数中进一步分为三类：彩色图像、灰度图像和单色图像，分别处理。“缩减像素采样”主要是指缩放图片的算法，一般情况下默认即可。而图像分辨率的阈值，一般默认450即可。图像压缩算法决定了图像细节的质量，可以自动或者选择不压缩。最后图像质量表示整体质量，从最低到最高。

3.2.3 字体参数

如下图所示。字体参数主要是指是否把用到的字体嵌入到PDF文件中。一般情况下如果没用到什么特殊字体或者有特殊需求，默认配置即可。

3.2.4 颜色参数

如下图所示。颜色参数主要是控制输出文档的色彩。主要是色彩管理方案。可以选择“保留颜色不变”、“转为CMYK”、 “转为sRGB”等设置。一般用于屏幕显示情况不变或者转为sRGB即可。即使对于需要打印的场景，也无需单独设置，除非有特殊要求必须是CMYK色彩空间。

3.2.5 高级参数

如下图所示。这里面主要是一些PDF文件本身的相关设置，如果没有特殊需求默认即可。

3.2.6 标准参数

如下图所示。这里主要是和PDF文件标准相关的设置，同样的，如果没有特殊需求，默认即可。

3.2.7 各预置方案参数对比

最后，我们也对Acrobat中预置的一些方案的关键参数进行了整理和对比，如下。总体而言，从纸面参数来看，输出质量最好的是“高质量打印”方案。其整体文档分辨率以及各图像的分辨率均最高。相比于“印刷质量”方案，其差别在于颜色空间，“印刷质量”方案将颜色转换为CMYK了。因此，在不考虑其它因素，只考虑质量的情况下，高质量打印方案是最优选择。“标准”方案则是在保证质量的前提下，适当降低了文档分辨率和影像整体质量，适合于普通场景。而所谓的“超大页面”方案，并不是想当然理解的“生成质量很高，所以可以用于超大页面”，而恰恰相反。因为是超大页面，所以对一些细节的要求反而降低了，比如和“标准”方案相比，影像分辨率和质量均下降了。也可以理解，比如对于广告牌等超大页面，没人会贴着看细节，只是远远看到内容就可以了，对细节确实没必要有很高要求。最后，对于“最小文件大小”方案，可以看到，它主要是通过降低影像的分辨率来降低文件大小。

4.PDF文件质量控制

前面我们分析了各输出方案，这里进一步分析。如果不考虑其它因素，只考虑输出PDF文档的质量，显然“高质量打印”是最优的。但是在实际中发现，如果文档很大、很复杂(有很多位图和矢量图)，例如毕业论文，转换时间会变得非常缓慢(几个小时起步)。但“标准”方案则快很多(两三分钟)，分析参数可知，其关键原因就在于文档分辨率和图像质量这两个参数。特别是文档分辨率影响最大。事实上在实际使用中发现，其实文档分辨率并不需要2400，一般默认的600就已经很高了。但在生成速度上会快很多。同时，为了获得最好的影像质量，图像压缩甚至可以选择“不压缩”。因此，我们可以自定义两套输出方案，如下：自定义高质量方案主要基于预置的“标准”方案，参考“高质量打印”方案，将图像质量设置为了最高，其余没有变化。自定义高质量不压缩方案，则是在自定义高质量方案基础上，进一步关闭了影像压缩，提升质量。

基于第三部分开头建立的Word文档，使用不同的PDF导出方式导出，并对结果进行比较。比较结果简单汇总如下表：可以看到，总体而言，最小文件方案效果最差，出现了大量的马赛克。不仅如此，源文档中采用的特殊字体由于PDF中没有嵌入(根据3.2.7部分的对比表格可以看到)，导致文字无法显示。所以正常情况下，不推荐最小文件方案。而Word另存为方法和Acrobat标准方案相比质量稍差，但也还能看。因此，如果电脑上没有安装Acrobat，一般情况下使用Word直出也可以。如果安装了Acrobat，那么还是建议使用Acrobat导出，即使是标准方案，也比Word直出质量稍高。最后，对于高质量打印方案和自定义的两套方案，总体而言，从视觉效果上都非常好，基本看不出差别。再结合文件大小等因素，综合推荐自定义的高质量方案，可以在保证高质量的前提下有较小的文件大小。比如在这里，Acrobat高质量是1.00MB、自定义高质量是0.99MB、自定义高质量未压缩是2.05MB。Acrobat之所以比自定义稍大，从配置参数就可以知道，是因为文档分辨率更高，为2400，而自定义的分辨率为600。

5.PDF文件大小控制

最后，简单谈谈对于PDF文件大小的控制。在日常生活中你也肯定遇到过这样的场景：需要交个PDF文件到某个系统，系统里要求文件大小要在3MB以下。这时，在不改变文档内容本身的情况下，可以考虑通过其它方式来减少文件大小。

首先需要明确的认知是，对于PDF文件大小，主要是由其包含的图片大小和数量决定的。文档中本身文字其实不会占很多空间。因此，优化PDF文件大小至少有以下几个方向可以尝试：

提前压缩图片，尽可能减少插入图片的大小。比如有一张3MB的JPG格式的照片，可以通过如Tinypng等网站压缩后，再插入Word文档。
灰度图转单通道图像。对于某些灰度图，它可能本身还是存了RGB三通道信息，这时可以考虑将图片转为单通道图像，再插入，也可以降低文件大小。
尽可能使用矢量图而非位图。对于自己绘制的图片等，尽可能按照2.2部分介绍的以矢量图形式插入，这样会显著降低文件大小，同时保证输出图片的高质量。
在不明显损失质量的情况下，降低图片分辨率。降低图片分辨率是减少大小的有效手段之一，不过要注意处理后的图片是否还能看清，特别是文字。
以文字代替位图。某些情况下可能是扫描的文字，这时可以用文字取代扫描的内容，也可以大幅减少大小(等于是用文字替换了位图)。
借助PDF24Tools等工具。借助压缩工具等也可以很方便的进行文件大小的压缩。

总之，控制PDF文件大小是个技术活，也需要在质量和大小之间找到平衡，尽可能在看不到的地方“偷工减料”。最后，为了使用方便，也将本博客自定义的输出方案文件上传到了Github，点击查看。方案文件可以在导出PDF的时候直接加载使用。

6.参考资料

[1] https://learn.microsoft.com/en-us/openspecs/windows_protocols/ms-emf/91c257d7-c39d-4a36-9b1f-63e3f73d30ca
[2] https://docs.fileformat.com/zh/image/emf
[3] https://blog.csdn.net/Jiajikang_jjk/article/details/80840067
[4] https://tinify.com
[5] https://tools.pdf24.org/zh

基于百度地图JaveScript API的旅行足迹展示网页实际开发记录

Thu, 19 Jun 2025 00:00:00 +0000

请注意:内容可能包含大量图表、公式、代码等，若RSS无法正确显示可直接访问原始网址，获得最好的阅读体验。

最近因为一些契机，需要做一个网页版的地图相关的开发，本篇博客对开发过程进行简单记录，以备日后查阅。

1.背景介绍

我们的核心需求是给定一些曾经去过的地方，网页需要在地图上展示对应的标记，类似于在一幅现实地图上“插旗”，如下所示。之所以有这个需求，是因为近些年去了一些地方，有时候自己都想不起来到底去过哪里，所以想做一个备忘录。虽然一开始想在高德地图、Google地图之类的APP中通过收藏地图点的形式实现这个需求，但关键的问题是无法分享给别人或者分享的知识文字列表形式的地图点而非可视化地图。例如高德地图中，即使你创建了个收藏列表，但分享给别人后，必须只能在手机端用高德APP打开，不够便捷。他分享的本质是一个超长的网页链接，无法记忆，也不够优雅。而在Google地图中，创建收藏列表后分享链接，也不能完整以地图方式显示标记点，如下。虽然分享链接短了很多，但还是无法记忆，而且在博主电脑浏览器中无法打开。

因此，（1）完全可控、随意修改；（2）良好图形化展示；（3）能够方便分享给别人，这样的需求就逐渐明确了。

2.需求分析与功能设计

在明确了需求以后，就需要根据需求匹配相应技术。对于需求一，最好的解决办法就是自己开发。对于需求二，则可以通过基于各类在线地图API实现。对于需求三，最好的方案是行程在线网页，并匹配短小、有意义的网址（而不是需要安装的移动端APP等）。因此最终选择采用JavaScript、基于百度地图API进行在线网页开发，并在本博客域名下新增字段构成网址(zhaoxuhui.top/world)。

更具体而言，用户在代码中以结构化形式给出数据，代码根据规则读取解析这些数据，并将其转化为可以在地图上显示的标记。完成显示后，进一步实现交互功能。

基础功能包括：地图显示、用户数据读取与解析、标记显示

高级功能包括：标记交互、地图样式切换、数据统计、数据导出等

需要说明的是，这个需求本身并不难，并且其实在很多年前本科的时候就已经有过相关技术积累，可参见这个和这个博客。但之前都是基于Android的移动端开发，并没有相对深入研究JavaScript API。因此，本篇博客进行相关学习。

3.地图API基础介绍

百度地图开发者账户和API Key的申请此处略过，如有需要可以直接去看官网指引。

3.1 地图创建与显示

根据百度地图API官方文档，基础地图的创建、加载与显示主要通过JS代码完成。创建地图必须的代码如下：

// 创建Map实例
var map = new BMapGL.Map('container');
// 初始化地图并设置中心点坐标和地图级别
map.centerAndZoom(new BMapGL.Point(116.404, 39.915), 12);

需要指定地图的中心点和缩放级数。根据示例中心的实际测试，缩放级数可以为浮点数，范围从3到21，数字越大缩放越大。完整的html文件内容如下：

<!DOCTYPE html>
<html lang="zh-CN">
    <head>
        <meta charset="utf-8">
        <title>地图展示</title>
        <meta name="viewport" content="initial-scale=1.0, user-scalable=no">
        <style>
        body,
        html,
        #container {
            overflow: hidden;
            width: 100%;
            height: 100%;
            margin: 0;
            font-family: "微软雅黑";
        }
        </style>
        <script src="//api.map.baidu.com/api?type=webgl&v=1.0&ak=您的密钥"></script>
    </head>
    <body>
        <div id="container"></div>
    </body>
</html>
<script>
    // 创建Map实例
    var map = new BMapGL.Map('container');
    // 初始化地图并设置中心点坐标和地图级别
    map.centerAndZoom(new BMapGL.Point(116.404, 39.915), 12);
</script>

这里，地图是通过div元素进行显示的，id为container，该元素的样式在前面的style标签中进行了定义。运行后效果如下所示。除了地图中心点和缩放级别，还可以根据需求，通过JS给地图设置不同的属性和样式，常见的代码如下：

// 鼠标滚轮缩放（默认不开启）
map.enableScrollWheelZoom(true);

// 设置地图类型为卫星图
map.setMapType(BMAP_SATELLITE_MAP);
// 设置地图类型为矢量图（默认）
map.setMapType(BMAP_NORMAL_MAP);
// 设置地图类型为三维地球
map.setMapType(BMAP_EARTH_MAP);

// 设置地图3D视角
map.setHeading(64.5);
map.setTilt(73);

// 自定义3D视角地图颜色
map.setDisplayOptions({skyColors: ['rgba(186, 0, 255, 0)','rgba(186, 0, 255, 0.2)']});

// 不显示底图地物POI文字（默认开启）
map.setDisplayOptions({poiText: false});
// 不显示底图地物POI图标（默认开启）
map.setDisplayOptions({poiIcon: false});
// 不显示绘制的地图覆盖物（默认显示）
map.setDisplayOptions({overlay: false});
// 不显示绘制的叠加图层（默认显示）
map.setDisplayOptions({layer: false});
// 不显示绘制的3D建筑物（默认显示）
map.setDisplayOptions({building: false});
// 不显示地图路网（默认显示，只对卫星图和地球模式有效）
map.setDisplayOptions({street: false});

// 添加比例尺控件
var scaleCtrl = new BMapGL.ScaleControl();
map.addControl(scaleCtrl);

// 添加缩放控件
var zoomCtrl = new BMapGL.ZoomControl();
map.addControl(zoomCtrl);

// 添加3D控件
var navi3DCtrl = new BMapGL.NavigationControl3D();  
map.addControl(navi3DCtrl);


// 创建定位控件
var locationControl = new BMapGL.LocationControl({
    // 控件的停靠位置（可选，默认左上角）
    anchor: BMAP_ANCHOR_TOP_RIGHT,
    // 控件基于停靠位置的偏移量（可选）
    offset: new BMapGL.Size(20, 20)
});
// 将控件添加到地图上
map.addControl(locationControl);


// 创建城市选择控件
var cityControl = new BMapGL.CityListControl({
    // 控件的停靠位置（可选，默认左上角）
    anchor: BMAP_ANCHOR_TOP_LEFT,
    // 控件基于停靠位置的偏移量（可选）
    offset: new BMapGL.Size(10, 5)
});
// 将控件添加到地图上
map.addControl(cityControl);

以上便可以实现地图的基本创建与相关自定义。

3.2 地图标记

在百度地图JS API中，满足我们需求的地图覆盖物是点标记（Marker）。

3.2.1 创建与显示

参考官方文档，其创建与显示仍通过JS代码完成，代码如下：

// 构造地图点对象
var point = new BMapGL.Point(116.404, 39.925);
// 创建点标记
var marker = new BMapGL.Marker(point);
// 在地图上添加点标记
map.addOverlay(marker);

首先是构造了一个地图点对象，然后基于该对象创建Marker对象，最后将其添加到地图中。完整代码如下。

<!DOCTYPE html>
<html lang="zh-CN">
    <head>
        <meta charset="utf-8">
        <title>地图展示</title>
        <meta name="viewport" content="initial-scale=1.0, user-scalable=no">
        <style>
        body,
        html,
        #container {
            overflow: hidden;
            width: 100%;
            height: 100%;
            margin: 0;
            font-family: "微软雅黑";
        }
        </style>
        <script src="//api.map.baidu.com/api?type=webgl&v=1.0&ak=您的密钥"></script>
    </head>
    <body>
        <div id="container"></div>
    </body>
</html>
<script>
    // 创建Map实例
    var map = new BMapGL.Map('container');
    // 初始化地图并设置中心点坐标和地图级别
    map.centerAndZoom(new BMapGL.Point(116.404, 39.915), 12);

    // 构造地图点对象
    var point = new BMapGL.Point(116.404, 39.925);
    // 创建点标记
    var marker = new BMapGL.Marker(point);
    // 在地图上添加点标记
    map.addOverlay(marker);
</script>

运行效果如下所示。

3.2.2 点击事件

对于地图上显示的Maker，我们自然不止希望能看，还希望可以点击它，然后给我们一些反馈。这些可以通过给Maker添加监听事件和对应的回调函数实现。一个基础的点击事件如下：

// 构造地图点对象
var point = new BMapGL.Point(116.404, 39.925);

// 信息窗口基本属性
var opts = {
    width: 200,
    height: 100,
    title: '故宫博物院'
};
// 创建信息窗口
var infoWindow = new BMapGL.InfoWindow('地址：北京市东城区王府井大街88号乐天银泰百货八层', opts);

// 点标记添加点击事件
marker.addEventListener('click', function () {
    // 回调函数：在point的位置开启信息窗口
    map.openInfoWindow(infoWindow, point);
});

上述代码中，给Maker添加了信息窗口，完整代码如下。

<!DOCTYPE html>
<html lang="zh-CN">
    <head>
        <meta charset="utf-8">
        <title>地图展示</title>
        <meta name="viewport" content="initial-scale=1.0, user-scalable=no">
        <style>
        body,
        html,
        #container {
            overflow: hidden;
            width: 100%;
            height: 100%;
            margin: 0;
            font-family: "微软雅黑";
        }
        </style>
        <script src="//api.map.baidu.com/api?type=webgl&v=1.0&ak=您的密钥"></script>
    </head>
    <body>
        <div id="container"></div>
    </body>
</html>
<script>
    // 创建Map实例
    var map = new BMapGL.Map('container');
    // 初始化地图并设置中心点坐标和地图级别
    map.centerAndZoom(new BMapGL.Point(116.404, 39.915), 12);

    // 构造地图点对象
    var point = new BMapGL.Point(116.404, 39.925);
    // 创建点标记
    var marker = new BMapGL.Marker(point);
    // 在地图上添加点标记
    map.addOverlay(marker);

    // 信息窗口基本属性
    var opts = {
        width: 200,
        height: 100,
        title: '故宫博物院'
    };
    // 创建信息窗口
    var infoWindow = new BMapGL.InfoWindow('地址：北京市东城区王府井大街88号乐天银泰百货八层', opts);

    // 点标记添加点击事件
    marker.addEventListener('click', function () {
        // 回调函数：在point的位置开启信息窗口
        map.openInfoWindow(infoWindow, point);
    });
</script>

运行结果如下。

3.2.3 自定义图标与拖拽

Marker除了默认的红色图标，也支持自定义图标，核心代码如下：

// 构造地图点对象
var pt = new BMapGL.Point(116.417, 39.909);

// 创建图标对象
var myIcon = new BMapGL.Icon("/jsdemo/img/car.png", new BMapGL.Size(52, 26));

// 使用图标创建Marker
var marker = new BMapGL.Marker(pt, {icon: myIcon});

// 将Marker添加到地图
map.addOverlay(marker);

运行效果如下。此外，Marker也支持实时拖拽，只需要修改它的enableDragging即可，如下。

var marker = new BMapGL.Marker(point, {
    enableDragging: true
});

3.3 地图上下文菜单

向地图添加上下文菜单的方式也比较简单，核心代码如下：

var menu = new BMapGL.ContextMenu();
var txtMenuItem = [
    {
        text: '设置为卫星图',
        callback: function (){map.setMapType(BMAP_SATELLITE_MAP);}
    }, {
        text: '设置为矢量图',
        callback: function (){map.setMapType(BMAP_NORMAL_MAP);}
    }
];

for (var i = 0; i < txtMenuItem.length; i++) {
    menu.addItem(new BMapGL.MenuItem(txtMenuItem[i].text, txtMenuItem[i].callback, 100));
}
map.addContextMenu(menu);

上述代码中首先创建了上下文菜单对象，然后构造了菜单内容的列表，并为每一个项目指定了名称和回调函数。然后通过一个for循环将这些项目赋给菜单对象，最后调用相应函数实现菜单添加。完整的html代码如下：

<!DOCTYPE html>
<html lang="zh-CN">
    <head>
        <meta charset="utf-8">
        <title>地图展示</title>
        <meta name="viewport" content="initial-scale=1.0, user-scalable=no">
        <style>
        body,
        html,
        #container {
            overflow: hidden;
            width: 100%;
            height: 100%;
            margin: 0;
            font-family: "微软雅黑";
        }
        </style>
        <script src="//api.map.baidu.com/api?type=webgl&v=1.0&ak=您的密钥"></script>
    </head>
    <body>
        <div id="container"></div>
    </body>
</html>
<script>
    // 创建Map实例
    var map = new BMapGL.Map('container');
    // 初始化地图并设置中心点坐标和地图级别
    map.centerAndZoom(new BMapGL.Point(116.404, 39.915), 12);

    var menu = new BMapGL.ContextMenu();
    var txtMenuItem = [
        {
            text: '设置为卫星图',
            callback: function (){map.setMapType(BMAP_SATELLITE_MAP);}
        }, {
            text: '设置为矢量图',
            callback: function (){map.setMapType(BMAP_NORMAL_MAP);}
        }
    ];

    for (var i = 0; i < txtMenuItem.length; i++) {
        menu.addItem(new BMapGL.MenuItem(txtMenuItem[i].text, txtMenuItem[i].callback, 100));
    }
    map.addContextMenu(menu);
</script>

运行效果如下。

4.关键代码开发与说明

有了上述代码基础，我们就可以进入正式开发了。

4.1 数据输入与解析

旅行数据可以通过远程方式获取也可以直接写到html文件中。因为本需求中不涉及隐私等相关问题，所以采用比较简单的办法，直接写到代码中。我们规定数据的结构是一条记录由一个list储存，list中分别存放目的地经度（浮点型）、目的地纬度（浮点型）、目的地国家（字符串）、目的地城市（字符串）以及访问时间（多个时间用英文逗号隔开），示例如下：

[100.501751, 13.756322, "泰国", "曼谷", "2017年7月, 2023年8月"]

多个目的地通过list进行存储。添加数据的相关代码写到loadTrajectory()函数中便于管理。

4.2 创建基础地图并显示

对于地图，要求总体简单，相关代码如下：

var map = new BMapGL.Map("container", {
    showVectorStreetLayer: true,
    showVectorLine: false,
});
map.enableScrollWheelZoom(true);
map.setMapType(BMAP_NORMAL_MAP);

默认类型地图、显示道路、支持滚轮缩放即可。

4.3 添加地图Marker

基于给定的数据，向地图添加Marker也比较简单，如下。

var myIcon = new BMapGL.Icon("/world/assets/images/flag.png", new BMapGL.Size(32, 32));

for (var i = 0; i < pls.length; i++) {
    var point = new BMapGL.Point(pls[i][0], pls[i][1]);
    var marker = new BMapGL.Marker(point, {icon: myIcon});
    map.addOverlay(marker);
    
    mean_lat = mean_lat + pls[i][1];
    mean_lon = mean_lon + pls[i][0];
}

mean_lat = mean_lat / pls.length;
mean_lon = mean_lon / pls.length;
map.centerAndZoom(new BMapGL.Point(mean_lon, mean_lat), 3);

首先构造了自定义的图标，然后依次遍历数据，进行Marker添加。同时在这个过程中，会动态计算这些Marker的中心点，之后以此作为整个地图的中心点。

4.4 核心功能实现

为了使用更加舒适，设计了“设为卫星图”、“设为矢量图”、“设为3D图”、“信息统计”、“随机漫游”和“记录导出”这几个功能，下面简单介绍。

4.4.1 设置地图功能

这些功能就是将地图设为对应的类型，比较简单。在代码中通过各函数实现，如下。

function setMapSatellite(){
    map.setMapType(BMAP_SATELLITE_MAP);
}

function setMapNormal(){
    map.setMapType(BMAP_NORMAL_MAP);
}

function setMap3D(){
    map.setMapType(BMAP_EARTH_MAP);
}

4.4.2 信息统计功能

基于给定的数据进行一些基础的信息统计，在代码中通过infoStat()函数实现，如下：

function infoStat(){
    var nn_pos = -90;
    var sn_pos = 90;
    var en_pos = 0;
    var wn_pos = 360;
    
    var nn_idx = 0;
    var sn_idx = 0;
    var en_idx = 0;
    var wn_idx = 0;
    
    var str_names = "";
    var str_cts = "";
    var str_nat = "";

    var t_num = 3;
    var t_list = [];
    var tmp_nums = [];

    var num_cts = [];
    var num_cts2 = [];

    var num_nats = [];
    var num_nats2 = [];
    
    var tmp_countries = [];


    for (var i = 0; i < pls.length; i++) {
        var tmp_lon = pls[i][0] + 180;
        var tmp_lat = pls[i][1];
        
        if(tmp_lon > en_pos){
            en_pos = tmp_lon;
            en_idx = i;
        }
        
        if(tmp_lon < wn_pos){
            wn_pos = tmp_lon;
            wn_idx = i;
        }
        
        if(tmp_lat > nn_pos){
            nn_pos = tmp_lat;
            nn_idx = i;
        }
        
        if(tmp_lat < sn_pos){
            sn_pos = tmp_lat;
            sn_idx = i;
        }
        
        var tmp_visit_city_num = pls[i][4].split(",").length;
        num_cts.push(tmp_visit_city_num);
        num_cts2.push(tmp_visit_city_num);
        
        tmp_countries.push(pls[i][2]);
    }

    
    num_cts.sort(function(a,b){return a-b;});
    tmp_nums = Array.from(new Set(num_cts));
    for(var i = 1; i <= t_num; i++){
        t_list.push(tmp_nums[tmp_nums.length-i]);
    }

    for(var i = 0; i <= t_list.length; i++){
        var cur_num = t_list[i];

        for(var j = 0; j < num_cts2.length; j++){
            if(cur_num == num_cts2[j]){
                str_cts = str_cts + pls[j][2] + pls[j][3] + "(" + cur_num.toString() + "次), ";
            }
        }
    }
    str_cts = str_cts.slice(0, str_cts.length-2);
    
    var unique_countries = Array.from(new Set(tmp_countries));
    for (var i = 0; i < unique_countries.length; i++) {
        var tmp_country = unique_countries[i];
        str_names = str_names + tmp_country + ", ";
        
        var tmp_num = 0;
        for (var j = 0; j < pls.length; j++) {
            if(tmp_country == pls[j][2]){
                tmp_num = tmp_num + 1;
            }
        }
        
        num_nats.push(tmp_num);
        num_nats2.push(tmp_num);
    }
    str_names = str_names.slice(0, str_names.length-2);

    num_nats.sort(function(a,b){return a-b;});
    tmp_nums = Array.from(new Set(num_nats));
    t_list = [];
    for(var i = 1; i <= t_num; i++){
        t_list.push(tmp_nums[tmp_nums.length-i]);
    }
    for(var i = 0; i <= t_list.length; i++){
        var cur_num = t_list[i];

        for(var j = 0; j < num_nats2.length; j++){
            if(cur_num == num_nats2[j]){
                str_nat = str_nat + unique_countries[j] + "(" + cur_num.toString() + "次), ";
            }
        }
    }
    str_nat = str_nat.slice(0, str_nat.length-2);

    var str_line1 = "🔓️已解锁"+unique_countries.length.toString()+"个国家/地区的"+pls.length.toString()+"座城市：\n"+str_names+"\n";

    var str_line2 = "🏝️到访城市最多国家（TOP3）：\n"+str_nat+"\n";

    var str_line3 = "🏡到访最多城市（TOP3）：\n"+str_cts+"\n";
    
    var range_hon = (Math.abs(wn_pos-180)+en_pos-180);
    var range_ver = (Math.abs(sn_pos)+nn_pos);
    var hon_pct = range_hon / 360;
    var ver_pct = range_ver / 180;
    var explore_pct = (range_hon * range_ver) / (360*180);
    var str_line4 = "🌏地球探索度："+explore_pct.toFixed(2)+" / 1\n";
    str_line4 = str_line4+"东西跨度："+range_hon.toFixed(4)+"° / 360° (约"+hon_pct.toFixed(2)+"个地球)\n";
    str_line4 = str_line4+"南北跨度："+range_ver.toFixed(4)+"° / 180° (约"+ver_pct.toFixed(2)+"个地球)\n";
    
    var str_line5 = "🧭️探索范围：\n";
    str_line5 = str_line5 + "️最东："+pls[en_idx][2]+pls[en_idx][3]+" (东经"+(en_pos-180).toFixed(4).toString()+")"+"\n";
    str_line5 = str_line5 + "最西："+pls[wn_idx][2]+pls[wn_idx][3]+" (西经"+Math.abs(wn_pos-180).toFixed(4).toString()+")"+"\n";
    str_line5 = str_line5 + "最南："+pls[sn_idx][2]+pls[sn_idx][3]+" (南纬"+Math.abs(sn_pos).toFixed(4).toString()+")"+"\n";
    str_line5 = str_line5 + "最北："+pls[nn_idx][2]+pls[nn_idx][3]+" (北纬"+(nn_pos).toFixed(4).toString()+")"+"\n";
    
    alert(str_line1+"\n"+str_line2+"\n"+str_line3+"\n"+str_line4+"\n"+str_line5);
}

核心就是通过遍历各记录从而计算最值或均值等操作，统计包括到访城市、国家、次数、范围等信息。稍微需要注意的是包括JS中去除重复元素、按需求排序等。最后，通过浏览器的Alert功能弹出提示框显示内容。

4.4.3 数据导出功能

作为尽可能开放的功能实现，面对未来可能的各种潜在需求，我们支持直接导出原始数据，从而可以方便用户进行分析或二次开发。在代码中通过outputInfo()函数实现，如下：

function outputInfo(){
    var str_info = "序号, 国家/地区, 城市, 经度, 纬度, 说明\n";
    for (var i = 0; i < pls.length; i++) {
        var tmp_parts = pls[i][4].split(",")
        var tmp_desp = "";
        if(tmp_parts.length==1){
            tmp_desp = pls[i][4]
        }else{
            for(var j = 0; j < tmp_parts.length; j++) {
                tmp_desp = tmp_desp + tmp_parts[j]
            }
        }

        str_info = str_info + (i+1).toString() + ", " + pls[i][2]+", "+pls[i][3]+", "+pls[i][0].toFixed(4).toString()+", "+pls[i][1].toFixed(4).toString()+", "+tmp_desp+"\n";
    }

    var textFileAsBlob = new Blob([str_info], {type: 'text/plain'});
    var downloadLink = document.createElement('a');
    downloadLink.download = 'Records.csv';
    downloadLink.href = window.URL.createObjectURL(textFileAsBlob);
    downloadLink.click();
}

简单来说，我们通过通用的csv文件格式对原始数据进行了格式化，然后通过JS构造了相应文件。当运行该函数后，浏览器就会自动弹出下载界面。

4.4.4 随机探索功能

为了增加浏览乐趣，也增加了随机探索功能。所谓随机探索是指，程序随机从给定的访问记录中选择一个，并且将其居中，在地图上展示详细信息。在代码中通过explore()函数实现，如下：

function explore(){
    var tmp_idx = (Math.random()*pls.length).toFixed(0);
    if(tmp_idx == pls.length){
        tmp_idx = pls.length - 1;
    }
    
    var tmp_str_loc ="经度："+pls[tmp_idx][0].toFixed(4).toString()+", 纬度："+pls[tmp_idx][1].toFixed(4).toString();
    map.openInfoWindow(new BMapGL.InfoWindow(tmp_str_loc+"<br>时间："+pls[tmp_idx][4],{title:pls[tmp_idx][2]+pls[tmp_idx][3]}), 
    new BMapGL.Point(pls[tmp_idx][0],pls[tmp_idx][1]));
    
    map.centerAndZoom(new BMapGL.Point(pls[tmp_idx][0],pls[tmp_idx][1]),8);
}

其核心是随机数的构造以及map.centerAndZoom()将选择的地点设置为地图中心点的能力。

4.5 构造控制菜单

为了方便不同用户使用，我们提供两种控制方式，一种是PC端，在地图上点击右键即可进行控制，另一种则是移动端，无法右键点击，则通过点击右下角的常驻按钮弹出菜单进行控制。根据上文，我们目前提供给功能包括：设为卫星图、设为矢量图、设为3D图、信息统计、随机漫游、记录导出，这6个功能，对应菜单也有6项。

4.5.1 右键上下文菜单

在3.3部分已经介绍过百度地图API中上下文菜单的添加，实际代码如下：

var menu = new BMapGL.ContextMenu();
var txtMenuItem = [
    {
        text: '设置为卫星图',
        callback: setMapSatellite
    }, {
        text: '设置为矢量图',
        callback: setMapNormal
    }, {
        text: '设置为3D图',
        callback: setMap3D
    }, {
        text: '信息统计',
        callback: infoStat
    }, {
        text: '随机漫游',
        callback: explore
    }, {
        text: '记录导出',
        callback: outputInfo
    }
];

for (var i = 0; i < txtMenuItem.length; i++) {
    menu.addItem(new BMapGL.MenuItem(txtMenuItem[i].text, txtMenuItem[i].callback, 100));
}
map.addContextMenu(menu);

在每一项中，callback属性都设置成了前面提到过的对应回调函数。最后，通过menu.addItem()将项目添加到菜单中，并通过map.addContextMenu()将菜单添加到地图。

4.5.2 常驻菜单

对于常驻菜单，我们首先需要在HTML中构造出它的样子，如下：

<ul id="menuBackground" class="btn-wrap" style="z-index: 99;visibility: hidden">
    <li id="btn_map_sat" class="btn" onclick="setMapSatellite()">卫星图</li>
    <li id="btn_map_norm" class="btn" onclick="setMapNormal()">矢量图</li>
    <li id="btn_map_3d" class="btn" onclick="setMap3D()">3D图</li>
    <li id="btn_stat" class="btn" onclick="infoStat()">统计</li>
    <li id="btn_explore" class="btn" onclick="explore()">漫游</li>
    <li id="btn_output" class="btn" onclick="outputInfo()">导出</li>
    <li id="btn_root" class="btn-root" onclick="showMenu()" style="visibility:visible">💡</li>
</ul>

这里我们采用ul无序列表实现基础的框架，然后li表示每一项。对于每一项分别设置他们的文字内容和对应的点击函数。对于每一项的按钮，他们的类别都是btn，对于整个菜单，类型是btn-wrap。在<style>标签中，我们采用CSS进行了样式定义，如下：

ul li {
list-style: none;
}
.btn-wrap {
    z-index: 999;
    position: fixed;
    width: 65px;
    bottom: 5px;
    right: 10px;
    padding: 5px;
    border-radius: 5px;
    background-color: rgba(265, 265, 265, 0.5);
    box-shadow: 0 2px 6px 0 rgba(27, 142, 236, 0.5);
}
.btn {
    width: 55px;
    height: 30px;
    float: left;
    background-color: #fff;
    color: rgba(27, 142, 236, 1);
    font-size: 14px;
    border:1px solid rgba(27, 142, 236, 1);
    border-radius: 5px;
    margin: 0 5px 5px;
    text-align: center;
    line-height: 30px;
}
.btn:hover {
    background-color: rgba(27, 142, 236, 0.8);
    color: #fff;
    cursor: pointer;
}
.btn-root {
    width: 55px;
    height: 30px;
    float: left;
    background-color: rgb(255, 246, 215);
    color: rgba(27, 142, 236, 1);
    font-size: 14px;
    border:1px solid rgba(27, 142, 236, 1);
    border-radius: 5px;
    margin: 0 5px 5px;
    text-align: center;
    line-height: 30px;
    box-shadow: 0 2px 6px 0 rgba(27, 142, 236, 0.5);
}
.btn-root:hover {
    background-color: rgba(250, 231, 161);
    color: #fff;
    cursor: pointer;
}

这样便实现了一个和4.5.1部分上下文菜单功能相同、可以点击的控制菜单了。但这里有个美学上和用户交互上的问题。作为一个干净整洁的地图展示应用，如果有一堆按钮一直出现在屏幕右下角在某些场合下也会比较“破坏气氛”。我们希望有一个控制菜单显示的按钮在右下角，默认菜单不显示，点击一下按钮菜单显示，再点击一下菜单隐藏。为了实现这个功能，在代码中以showMenu()函数实现，如下：

function showMenu(){
    if(flag_vis==1){
        var element_bk = document.getElementById("menuBackground");
        element_bk.style.visibility = 'hidden';

        var element_btn_root = document.getElementById("btn_root");
        element_btn_root.style.visibility = 'visible';
        element_btn_root.style.boxShadow = '0 2px 6px 0 rgba(27, 142, 236, 0.5)';

        flag_vis = 0;
    }else{
        var element_bk = document.getElementById("menuBackground");
        element_bk.style.visibility = 'visible';

        var element_btn_root = document.getElementById("btn_root");
        element_btn_root.style.boxShadow = 'none';

        flag_vis = 1;
    }
}

总体而言，我们的思路是通过一个全局变量flag_vis控制菜单，而想要控制的对象又是通过JS的document.getElementById()获得。通过将对应属性设置为visible或none/hidden实现菜单的显示或隐藏。

4.6 地图Marker添加监听

以上，我们完成了地图的基本创建、显示以及控制。最后，我们需要对各个Marker添加点击事件，实现详细信息的展示。有了3.2.2部分的基础，会相对简单一些，代码如下：

map.addEventListener("click", function(e) {
    if (e.overlay && e.overlay instanceof BMapGL.Marker) {
        const clickedMarker = e.overlay;

        var cur_lon = clickedMarker.getPosition().lng;
        var cur_lat = clickedMarker.getPosition().lat;

        var clicked_idx = 0;

        for (var i = 0; i < pls.length; i++) {
            var tmp_lon = pls[i][0];
            var tmp_lat = pls[i][1];

            if(Math.abs(tmp_lon - cur_lon) < 0.015 && Math.abs(tmp_lat - cur_lat) < 0.015){
                clicked_idx = i;
                break;
            }
        }

        var str_loc ="经度：" + cur_lon.toFixed(4).toString() + ", 纬度：" + cur_lat.toFixed(4).toString();

        map.openInfoWindow(new BMapGL.InfoWindow(str_loc + "<br>时间：" + pls[clicked_idx][4], {title: pls[clicked_idx][2]+pls[clicked_idx][3]}), 
        new BMapGL.Point(cur_lon, cur_lat));
    }
});

这里的一个重点是，每个Marker需要显示的内容是不同的，并不能每个Marker都显示相同的内容。所以我们需要动态的获取点击状态，然后根据状态判断显示什么。具体来说我们可以通过点击事件的种类和位置进行判断。这里e就是包含了点击事件的返回值。我们分别获得他的位置和类型，再将其与各个Marker的位置进行比较，即可以判断是哪个Marker被点击了，进而显示相应内容。

4.7 完整代码展示

在完成各个核心功能的介绍后，下面展示了完整的Html网页代码：

<!DOCTYPE html>
<html lang="zh-CN">
    <head>
        <meta charset="utf-8">
        <title>🗺Adventure Map - Where I Have Been To</title>
        <meta http-equiv="Content-Type" content="text/html; charset=utf-8">
        <meta name="viewport" content="initial-scale=1.0, user-scalable=no">
        <meta http-equiv="content-language" content="zh-cn">
        <style type="text/css">
            body,
            html,
            #container {
                overflow: hidden;
                width: 100%;
                height: 100%;
                margin: 0;
                font-family: "微软雅黑";
            }

			ul li {
            list-style: none;
			}
			.btn-wrap {
				z-index: 999;
				position: fixed;
				width: 65px;
				bottom: 5px;
				right: 10px;
				padding: 5px;
				border-radius: 5px;
				background-color: rgba(265, 265, 265, 0.5);
				box-shadow: 0 2px 6px 0 rgba(27, 142, 236, 0.5);
			}
			.btn {
				width: 55px;
				height: 30px;
				float: left;
				background-color: #fff;
				color: rgba(27, 142, 236, 1);
				font-size: 14px;
				border:1px solid rgba(27, 142, 236, 1);
				border-radius: 5px;
				margin: 0 5px 5px;
				text-align: center;
				line-height: 30px;
			}
			.btn:hover {
				background-color: rgba(27, 142, 236, 0.8);
				color: #fff;
				cursor: pointer;
			}
			.btn-root {
				width: 55px;
				height: 30px;
				float: left;
				background-color: rgb(255, 246, 215);
				color: rgba(27, 142, 236, 1);
				font-size: 14px;
				border:1px solid rgba(27, 142, 236, 1);
				border-radius: 5px;
				margin: 0 5px 5px;
				text-align: center;
				line-height: 30px;
				box-shadow: 0 2px 6px 0 rgba(27, 142, 236, 0.5);
			}
			.btn-root:hover {
				background-color: rgba(250, 231, 161);
				color: #fff;
				cursor: pointer;
			}
        </style>
        <script src="//api.map.baidu.com/api?type=webgl&v=1.0&ak=yourkey"></script>
    </head>
    <body>
        <div id="container"></div>
		<ul id="menuBackground" class="btn-wrap" style="z-index: 99;visibility: hidden">
			<li id="btn_map_sat" class="btn" onclick="setMapSatellite()">卫星图</li>
			<li id="btn_map_norm" class="btn" onclick="setMapNormal()">矢量图</li>
			<li id="btn_map_3d" class="btn" onclick="setMap3D()">3D图</li>
			<li id="btn_stat" class="btn" onclick="infoStat()">统计</li>
			<li id="btn_explore" class="btn" onclick="explore()">漫游</li>
			<li id="btn_output" class="btn" onclick="outputInfo()">导出</li>
			<li id="btn_root" class="btn-root" onclick="showMenu()" style="visibility: visible">💡</li>
		</ul>
    </body>
</html>

<script>
	// 全局变量
	var flag_vis = 0;
	// 经度 纬度 地名 描述（时间）
    var pls = [];
	var mean_lat = 0.0;
    var mean_lon = 0.0;
	
	function loadTrajectory(){
	    pls.push([100.501751, 13.756322, "泰国", "曼谷", "2017年7月, 2023年8月"]);
		pls.push([100.882244, 12.923159, "泰国", "芭提雅", "2017年7月"]);

		pls.push([82.895857, 54.983751, "俄罗斯", "新西伯利亚", "2017年9月"]);

		pls.push([135.502158, 34.693599, "日本", "大阪", "2018年2月, 2025年5月"]);
		pls.push([135.768128, 35.011533, "日本", "京都", "2018年2月"]);
		pls.push([139.766934, 35.682413, "日本", "东京", "2018年2月, 2019年6月, 2025年5月"]);
		
		pls.push([113.543563, 22.198081, "中国", "澳门", "2018年5月"]);

		pls.push([103.807156, 1.348317, "新加坡", "新加坡", "2018年8月, 2025年4月"]);
		pls.push([115.185296, -8.418173, "印度尼西亚", "巴厘岛", "2018年8月"]);
		pls.push([101.683895, 3.130960, "马来西亚", "吉隆坡", "2018年8月, 2023年8月"]);
		
		pls.push([114.169369, 22.319221, "中国", "香港", "2018年9月, 2019年6月"]);
		
		pls.push([141.354392, 43.060516, "日本", "札幌", "2019年2月"]);
		pls.push([140.993829, 43.189764, "日本", "小樽", "2019年2月"]);
		pls.push([142.363837, 43.770506, "日本", "旭川", "2019年2月"]);
		pls.push([143.353148, 44.355344, "日本", "纹别", "2019年2月"]);
		pls.push([126.630739, 37.474687, "韩国", "仁川", "2019年2月"]);
		
		pls.push([139.546128, 35.318778, "日本", "镰仓", "2019年6月"]);

		pls.push([79.861011, 6.926778, "斯里兰卡", "科伦坡", "2019年7月"]);
		pls.push([73.5090693, 4.175160, "马尔代夫", "马累", "2019年7月"]);
		
		pls.push([102.632541, 17.975771, "老挝", "万象", "2023年5月"]);
		
		pls.push([116.076644, 5.983179, "马来西亚", "亚庇", "2023年8月, 2025年4月"]);
		
		pls.push([31.2356326, 30.044029, "埃及", "开罗", "2023年8月"]);
		pls.push([33.8112233, 27.255908, "埃及", "赫尔格达", "2023年8月"]);
		pls.push([32.6393817, 25.686891, "埃及", "卢克索", "2023年8月"]);
		
		pls.push([126.518976, 33.503221, "韩国", "济州岛", "2023年10月"]);

		pls.push([2.351362, 48.857539, "法国", "巴黎", "2024年2月, 2024年6月, 2024年7月, 2025年2月"]);
		pls.push([2.129729, 48.802315, "法国", "凡尔赛", "2024年2月"]);
		pls.push([2.701560, 48.404639, "法国", "枫丹白露", "2024年2月"]);
		pls.push([-1.511466, 48.636157, "法国", "勒蒙-圣米歇尔", "2024年2月"]);
		pls.push([8.541666, 47.376872, "瑞士", "苏黎世", "2024年2月"]);
		pls.push([8.309269, 47.050113, "瑞士", "卢塞恩", "2024年2月"]);
		pls.push([7.863192, 46.686322, "瑞士", "因特拉肯", "2024年2月"]);
		pls.push([8.041323, 46.624082, "瑞士", "格林德尔瓦尔德", "2024年2月"]);
		pls.push([8.407103, 46.819451, "瑞士", "恩格尔贝格", "2024年2月"]);
		pls.push([51.529835, 25.283296, "卡塔尔", "多哈", "2024年2月"]);
		
		pls.push([1.528824, 49.076658, "法国", "吉维尼", "2024年6月"]);
		pls.push([-21.940789, 64.146976, "冰岛", "雷克雅未克", "2024年6月"]);
	 
		pls.push([50.587274, 26.223222, "巴林", "麦纳麦", "2024年7月"]);
		pls.push([23.727433, 37.983798, "希腊", "雅典", "2024年7月"]);
		pls.push([-3.703301, 40.416602, "西班牙", "马德里", "2024年7月"]);
		pls.push([5.369004, 43.302522, "法国", "马赛", "2024年7月"]);
		pls.push([5.984179, 43.836076, "法国", "瓦朗索勒", "2024年7月"]);
		pls.push([25.461440, 36.393072, "希腊", "圣托里尼", "2024年7月"]);

		pls.push([-0.127600, 51.507189, "英国", "伦敦", "2025年2月"]);
		pls.push([10.752224, 59.913837, "挪威", "奥斯陆", "2025年2月"]);
		pls.push([18.955307, 69.649200, "挪威", "特罗姆瑟", "2025年2月"]);
		pls.push([15.625646, 78.225284, "斯瓦尔巴群岛", "朗伊尔城", "2025年2月"]);
		pls.push([12.568332, 55.676073, "丹麦", "哥本哈根", "2025年2月"]);
	 
		pls.push([118.611544, 4.479346, "马来西亚", "仙本那", "2025年4月"]);
	 
		pls.push([135.804715, 34.685025, "日本", "奈良", "2025年5月"]);
		pls.push([136.906530, 35.181422, "日本", "名古屋", "2025年5月"]);
		pls.push([138.382650, 34.975517, "日本", "静冈", "2025年5月"]);
		pls.push([138.807215, 35.487832, "日本", "富士吉田市", "2025年5月"]);
		pls.push([139.559610, 35.682977, "日本", "三鹰市", "2025年5月"]);
	}

	function showMenu(){
		if(flag_vis==1){
			var element_bk = document.getElementById("menuBackground");
			element_bk.style.visibility = 'hidden';

			var element_btn_root = document.getElementById("btn_root");
			element_btn_root.style.visibility = 'visible';
			element_btn_root.style.boxShadow = '0 2px 6px 0 rgba(27, 142, 236, 0.5)';

			flag_vis = 0;
		}else{
			var element_bk = document.getElementById("menuBackground");
			element_bk.style.visibility = 'visible';

			var element_btn_root = document.getElementById("btn_root");
			element_btn_root.style.boxShadow = 'none';

			flag_vis = 1;
		}
	}

	function outputInfo(){
		var str_info = "序号, 国家/地区, 城市, 经度, 纬度, 说明\n";
		for (var i = 0; i < pls.length; i++) {
			var tmp_parts = pls[i][4].split(",")
			var tmp_desp = "";
			if(tmp_parts.length==1){
				tmp_desp = pls[i][4]
			}else{
				for(var j = 0; j < tmp_parts.length; j++) {
					tmp_desp = tmp_desp + tmp_parts[j]
				}
			}

			str_info = str_info + (i+1).toString() + ", " + pls[i][2]+", "+pls[i][3]+", "+pls[i][0].toFixed(4).toString()+", "+pls[i][1].toFixed(4).toString()+", "+tmp_desp+"\n";
		}

		var textFileAsBlob = new Blob([str_info], {type: 'text/plain'});
		var downloadLink = document.createElement('a');
		downloadLink.download = 'Records.csv';
		downloadLink.href = window.URL.createObjectURL(textFileAsBlob);
		downloadLink.click();
	}

	function setMapSatellite(){
		map.setMapType(BMAP_SATELLITE_MAP);
	}

	function setMapNormal(){
		map.setMapType(BMAP_NORMAL_MAP);
	}

	function setMap3D(){
		map.setMapType(BMAP_EARTH_MAP);
	}

	function infoStat(){
		var nn_pos = -90;
		var sn_pos = 90;
		var en_pos = 0;
		var wn_pos = 360;
		
		var nn_idx = 0;
		var sn_idx = 0;
		var en_idx = 0;
		var wn_idx = 0;
		
		var str_names = "";
		var str_cts = "";
		var str_nat = "";

		var t_num = 3;
		var t_list = [];
		var tmp_nums = [];

		var num_cts = [];
		var num_cts2 = [];

		var num_nats = [];
		var num_nats2 = [];
		
		var tmp_countries = [];


		for (var i = 0; i < pls.length; i++) {
			var tmp_lon = pls[i][0] + 180;
			var tmp_lat = pls[i][1];
			
			if(tmp_lon > en_pos){
				en_pos = tmp_lon;
				en_idx = i;
			}
			
			if(tmp_lon < wn_pos){
				wn_pos = tmp_lon;
				wn_idx = i;
			}
			
			if(tmp_lat > nn_pos){
				nn_pos = tmp_lat;
				nn_idx = i;
			}
			
			if(tmp_lat < sn_pos){
				sn_pos = tmp_lat;
				sn_idx = i;
			}
			
			var tmp_visit_city_num = pls[i][4].split(",").length;
			num_cts.push(tmp_visit_city_num);
			num_cts2.push(tmp_visit_city_num);
			
			tmp_countries.push(pls[i][2]);
		}

		
		num_cts.sort(function(a,b){return a-b;});
		tmp_nums = Array.from(new Set(num_cts));
		for(var i = 1; i <= t_num; i++){
			t_list.push(tmp_nums[tmp_nums.length-i]);
		}

		for(var i = 0; i <= t_list.length; i++){
			var cur_num = t_list[i];

			for(var j = 0; j < num_cts2.length; j++){
				if(cur_num == num_cts2[j]){
					str_cts = str_cts + pls[j][2] + pls[j][3] + "(" + cur_num.toString() + "次), ";
				}
			}
		}
		str_cts = str_cts.slice(0, str_cts.length-2);
		
		var unique_countries = Array.from(new Set(tmp_countries));
		for (var i = 0; i < unique_countries.length; i++) {
			var tmp_country = unique_countries[i];
			str_names = str_names + tmp_country + ", ";
			
			var tmp_num = 0;
			for (var j = 0; j < pls.length; j++) {
				if(tmp_country == pls[j][2]){
					tmp_num = tmp_num + 1;
				}
			}
			
			num_nats.push(tmp_num);
			num_nats2.push(tmp_num);
		}
		str_names = str_names.slice(0, str_names.length-2);

		num_nats.sort(function(a,b){return a-b;});
		tmp_nums = Array.from(new Set(num_nats));
		t_list = [];
		for(var i = 1; i <= t_num; i++){
			t_list.push(tmp_nums[tmp_nums.length-i]);
		}
		for(var i = 0; i <= t_list.length; i++){
			var cur_num = t_list[i];

			for(var j = 0; j < num_nats2.length; j++){
				if(cur_num == num_nats2[j]){
					str_nat = str_nat + unique_countries[j] + "(" + cur_num.toString() + "次), ";
				}
			}
		}
		str_nat = str_nat.slice(0, str_nat.length-2);

		var str_line1 = "🔓️已解锁"+unique_countries.length.toString()+"个国家/地区的"+pls.length.toString()+"座城市：\n"+str_names+"\n";

		var str_line2 = "🏝️到访城市最多国家（TOP3）：\n"+str_nat+"\n";

		var str_line3 = "🏡到访最多城市（TOP3）：\n"+str_cts+"\n";
		
		var range_hon = (Math.abs(wn_pos-180)+en_pos-180);
		var range_ver = (Math.abs(sn_pos)+nn_pos);
		var hon_pct = range_hon / 360;
		var ver_pct = range_ver / 180;
		var explore_pct = (range_hon * range_ver) / (360*180);
		var str_line4 = "🌏地球探索度："+explore_pct.toFixed(2)+" / 1\n";
		str_line4 = str_line4+"东西跨度："+range_hon.toFixed(4)+"° / 360° (约"+hon_pct.toFixed(2)+"个地球)\n";
		str_line4 = str_line4+"南北跨度："+range_ver.toFixed(4)+"° / 180° (约"+ver_pct.toFixed(2)+"个地球)\n";
		
		var str_line5 = "🧭️探索范围：\n";
		str_line5 = str_line5 + "️最东："+pls[en_idx][2]+pls[en_idx][3]+" (东经"+(en_pos-180).toFixed(4).toString()+")"+"\n";
		str_line5 = str_line5 + "最西："+pls[wn_idx][2]+pls[wn_idx][3]+" (西经"+Math.abs(wn_pos-180).toFixed(4).toString()+")"+"\n";
		str_line5 = str_line5 + "最南："+pls[sn_idx][2]+pls[sn_idx][3]+" (南纬"+Math.abs(sn_pos).toFixed(4).toString()+")"+"\n";
		str_line5 = str_line5 + "最北："+pls[nn_idx][2]+pls[nn_idx][3]+" (北纬"+(nn_pos).toFixed(4).toString()+")"+"\n";
		
		alert(str_line1+"\n"+str_line2+"\n"+str_line3+"\n"+str_line4+"\n"+str_line5);
	}

	function explore(){
		var tmp_idx = (Math.random()*pls.length).toFixed(0);
		if(tmp_idx == pls.length){
			tmp_idx = pls.length - 1;
		}
		
		var tmp_str_loc ="经度："+pls[tmp_idx][0].toFixed(4).toString()+", 纬度："+pls[tmp_idx][1].toFixed(4).toString();
		map.openInfoWindow(new BMapGL.InfoWindow(tmp_str_loc+"<br>时间："+pls[tmp_idx][4],{title:pls[tmp_idx][2]+pls[tmp_idx][3]}), 
		new BMapGL.Point(pls[tmp_idx][0],pls[tmp_idx][1]));
		
		map.centerAndZoom(new BMapGL.Point(pls[tmp_idx][0],pls[tmp_idx][1]),8);
	}


	// step1
	loadTrajectory();
	
	
    // step2
    var map = new BMapGL.Map("container", {
        showVectorStreetLayer: true,
        showVectorLine: false,
    });
    map.enableScrollWheelZoom(true);
	map.setMapType(BMAP_NORMAL_MAP);

    var myIcon = new BMapGL.Icon("/world/assets/images/flag.png", new BMapGL.Size(32, 32));


    // step3
    for (var i = 0; i < pls.length; i++) {
        var point = new BMapGL.Point(pls[i][0], pls[i][1]);
        var marker = new BMapGL.Marker(point, {icon: myIcon});
        map.addOverlay(marker);
        
        mean_lat = mean_lat + pls[i][1];
        mean_lon = mean_lon + pls[i][0];
    }

    mean_lat = mean_lat / pls.length;
    mean_lon = mean_lon / pls.length;
    map.centerAndZoom(new BMapGL.Point(mean_lon, mean_lat), 3);
	
	
	// step4
	var menu = new BMapGL.ContextMenu();
	var txtMenuItem = [
		{
			text: '设置为卫星图',
			callback: setMapSatellite
		}, {
			text: '设置为矢量图',
			callback: setMapNormal
		}, {
			text: '设置为3D图',
			callback: setMap3D
		}, {
			text: '信息统计',
			callback: infoStat
		}, {
			text: '随机漫游',
			callback: explore
		}, {
			text: '记录导出',
			callback: outputInfo
		}
	];

	for (var i = 0; i < txtMenuItem.length; i++) {
		menu.addItem(new BMapGL.MenuItem(txtMenuItem[i].text, txtMenuItem[i].callback, 100));
	}
	map.addContextMenu(menu);
	

    // step5
    map.addEventListener("click", function(e) {
        if (e.overlay && e.overlay instanceof BMapGL.Marker) {
            const clickedMarker = e.overlay;

            var cur_lon = clickedMarker.getPosition().lng;
            var cur_lat = clickedMarker.getPosition().lat;

            var clicked_idx = 0;

            for (var i = 0; i < pls.length; i++) {
                var tmp_lon = pls[i][0];
                var tmp_lat = pls[i][1];

                if(Math.abs(tmp_lon - cur_lon) < 0.015 && Math.abs(tmp_lat - cur_lat) < 0.015){
                    clicked_idx = i;
                    break;
                }
            }

			var str_loc ="经度：" + cur_lon.toFixed(4).toString() + ", 纬度：" + cur_lat.toFixed(4).toString();

            map.openInfoWindow(new BMapGL.InfoWindow(str_loc + "<br>时间：" + pls[clicked_idx][4], {title: pls[clicked_idx][2]+pls[clicked_idx][3]}), 
            new BMapGL.Point(cur_lon, cur_lat));
        }
    });
</script>

5.小结与效果展示

完成的网页动态展示如下：可以看到，常规的切换地图、显示信息等功能都可以正常运行。以上便是本次网页开发的记录，理论上也可以扩展为其它基于地图的应用，比如飞机航线可视化、高铁线路可视化等等。

6.参考资料

[1] https://lbsyun.baidu.com/
[2] https://lbsyun.baidu.com/index.php?title=open/jsdemo
[3] https://lbsyun.baidu.com/jsdemo.htm#aCreateMap
[4] https://lbsyun.baidu.com/jsdemo.htm#bSetGetMapZoom

Word论文写作排版问题汇总与解决方案

Mon, 21 Oct 2024 00:00:00 +0000

请注意:内容可能包含大量图表、公式、代码等，若RSS无法正确显示可直接访问原始网址，获得最好的阅读体验。

这段时间一直在准备论文，在论文写作过程中不可避免遇到很多问题。而且遇到的问题中很多还是“疑难杂症”，很难搜索到解决方式。因此这里简单将写作过程中遇到的问题列举出来，并给出解决办法，作为备忘。本文使用的Word为Microsoft Office LTSC 专业增强版 2021，并非WPS Office，但理论上这些技巧在WPS Office中也适用。

1.快速设置中英文字体

1.1 问题描述

写论文的时候有时候可能会有“数字、字母用Times New Roman，中文用楷体”这种相对奇怪的要求，如下图所示。如果文档中英文穿插较少，可以手动选择中文或英文修改对应字体。可是如果文档较多，像动辄10万字的毕业论文，这种做法显然不现实。

1.2 解决方案

此问题的解决方案也比较简单，原则是先设置中文字体，再设置英文字体。具体而言，选择一段需要设置的文字，首先设置字体为“楷体”，这样所有中文就设置好了，然后再选择字体为“Times New Roman”，这样，文字中所有英文和数字也设置完成了，如下所示。

2.同一行文字竖直方向调整

2.1 问题描述

在某些中英文、数字混排情况下，数字和英文是一种字体，中文是另一种字体。但如果你直接这样设置就会发现不同字体在竖直方向上对不齐，有点丑，特别是带有括号的情况，如下所示。可以明显看到，(a)比后面的中文文字明显低了很多，-也没有在中间，↑↓箭头也显得靠下。

2.2 解决方案

这是由于中英文字体不同基线位置导致的。针对这种情况有两种解决方案。第一种是在“段落-中文版式”里修改“文本对齐方式”，如下图所示。可以看到经过调整，数字、字母和符号的位置有轻微变化，一定程度上缓解了问题。但如果你觉得还是不好看，那就采用第二种方式，手动调整，通过修改“字体-高级-位置”来决定是上移还是下移，通过修改“磅值”控制移动幅度(0.5磅倍数)，如下。

3.中英文标题对不齐

3.1 问题描述

在生成目录时，如果标题中的第一个字为英文或数字，那么你可能会遇到英文打头的标题和中文打头的标题怎么也对不齐的情况，强迫症看了比较难受，如下所示。可以看到，2.1小节的“相机”和2.2小节的“SLAM”没有对齐，“SLAM”比“相机”要靠前。

3.2 解决方案

这个问题的主要原因是中英文混排设置有问题。可以通过“段落-中文版式-允许西文在单词中间换行”解决，如下。

4.中英文的间隔调整

4.1 问题描述

在中英文混排中，Word默认会自动调整中英文和数字之间的间隔，使得文档整体看起来比较好看。但有时候这也会带来问题，就是会让中英文之间的间隔过于大，看起来仿佛多了很多空格一样，不太好看，如下。上图中，2021、DARPA等和旁边的汉字空出来非常多空间，在一定程度上影响美观。

4.2 解决方案

解决方法也比较简单。通过取消“段落-中文版式-自动调整中文与西文的间距、自动调整中文与数字的间距”即可，如下所示。

5.括号里字母不居中

5.1 问题描述

在使用Times New Roman字体时，在写括号里的字母时，有时会出现不居中的情况，如下。比如，a、c、o等在括号里是居中的，但是j和左边括号粘一起了，偏左，f又和右边括号粘一起，靠右，不够美观。

5.2 解决方案

解决方案也比较简单，就是手动调整字符间距，使其看起来在括号里是居中的。比如，对于j而言，将该字符加宽，同时为了保证j字符和左边括号的间隔，也可以适当加宽左边括号，使整体看起来更好看。

6.表格内单元格间距调整

6.1 问题描述

有时候表格一行内容比较多，导致数字放不下，出现换行的情况，比较丑，如下。但是写作模板里又规定了字体和大小，也不能随意修改。这时候就比较麻烦。上图中，本该在一行的数字都被挤成了两行。

6.2 解决方案

解决方案也比较简单，核心是修改单元格的边距。选中表格，找到表格属性，找到单元格，再找到右下角的选项，就可以设置边距，从而实现内容调整，如下。这样就可以做到在不修改任何内容的情况下获得更好看的表格排版。

7.列表项对齐

7.1 问题描述

在写论文时，特别是参考文献部分，会有一两百篇文献，文献的序号对齐就比较重要，如下。可以看到，在序号为两位数时还算可以，Word自动帮我们对齐了格式，但当遇到三位数序号后，就会出现比较丑的空格。

7.2 解决方案

解决这个问题的核心是悬挂缩进。默认情况下，在两位序号时，Word使用了2个字符的悬挂缩进。所以我们对三位数的索引设置3个字符的悬挂缩进即可，如下。

8.页面尾部空行

8.1 问题描述

在论文排版时，有时可能会遇到明明上一页尾部还有很大空间，但Word就不再使用了，把段落放到下一页。尽管大部分情况下都没什么问题，但如果篇幅比较紧张，这就在一定程度上浪费了空间，如下图所示。可以看到，“探索时扬起的。。。”这行后面还有很大的空间，但Word却把后续内容放到了下一页。

8.2 解决方案

这个问题主要是Word自动的“孤行控制”导致的。如果有需要，可以手动控制，关闭Word的自动控制。选中需要处理的段落，选择“段落-换行和分页”，把“孤行控制”取消即可，如下。

9.域的更新与锁定

9.1 问题描述

在写论文的时候，难免会有相对复杂的交叉引用。在Word中，更新引用会导致格式的变化，这是比较麻烦的问题。所以，如果论文内容没有变化，可以固定住一部分内容，避免更新引用带来的格式问题。上图中，文献的引用格式本来是设置好的，但因为更新了引用，导致半个括号的出现，格式变了。

9.2 解决方案

解决方案也非常简单。我们可以把不需要修改的引用锁定，这样下次再更新引用的时候就不会更新它们了。选中需要锁定的域，按Ctrl+F11锁定，Ctrl+Shift+F11解除锁定。锁定后，域是更新不了的，如下所示。这样就可以有效避免更新引用给格式带来的潜在问题，可以放心更新引用了。在实际操作中，可以根据标题，一章一章的检查内容、锁定域。在章节目录标题上右键点击“选择标题和内容”即可选择该部分的所有内容，再按照上述步骤锁定或解锁域即可。

10.图片不压缩插入

10.1 问题描述

在某些情况下，我们希望论文里插入的图片是高清的，而不是被压缩以后模糊的，这一点在输出为pdf后特别明显(当然与输出pdf设置也有关，下文会提到)。比如我们在Word中插入一张图片，可以进行如下验证。首先，修改插入图片后的Word文件后缀名，由.docx改成.zip，然后右键解压即可得到对应文件夹(Word文件本质上就是各种插入的多媒体文件加一堆XML)。然后，依次打开word-media文件夹，会在其中找到刚刚我们插入的图片文件。我们将该图片文件与原始图片文件进行比较，即可发现图片被压缩了。上图中，左边为插入的图片文件信息，右边为原始图片信息。可以看到，图片的分辨率变小了，也就意味着图片被压缩了。

10.2 解决方案

解决方案也比较简单。在Word中有相关设置，决定是否压缩插入的图片。选择“文件-选项-高级”，里面能找到“图像大小和质量”选项，将“不压缩文件中的图像”打勾即可，如下。这个方法有两点需要注意：

（1）Word文件可能会变得异常大，加载、打开可能都会变慢。
（2）这个设置只对更改后再插入的图片有效，对更改之前已经插入的图片没有效果。

11.矢量图形插入

11.1 问题描述

写论文时难免会绘制一些示意图。这些示意图可以保存成位图，如png、jpg等格式插入文档，也可以以矢量图形式插入。矢量图插入的方式有以下好处：

（1）矢量图中的文字转换成pdf后可以复制，而位图中的文字不能复制。
（2）矢量图可以无限放大而不会出现马赛克，位图则不行。
（3）矢量图比位图更加节省空间，这对于导出pdf大小有限制的场景很重要。

可以看到，同样一个图片，右侧是以矢量图插入，而左侧是以位图格式插入，右边图片显然更加清晰。

11.2 解决方案

解决方案是，在Word中，矢量图可以通过EMF文件插入。EMF是Enhanced Meta File的简写，中文式增强型图元文件，由微软提出。严格来说，EMF并非完全意义上的矢量文件。他既可以包含矢量信息也可以包含位图信息。所以，在Word文档中插入图片之前，我们需要将利用Visio、Adobe AI等绘制的矢量图形转换成EMF文件，然后再像普通图片那样插入Word文档即可。如上图所示，在Visio中支持输出EMF文件。在Word中使用EMF文件有以下需要注意的点：

（1）如果你的图片是PowerPoint画的，没关系，将绘制的对象全部复制到Visio中，然后通过Visio导出EMF文件，同样是矢量的。
（2）EMF潜在问题就是，文件大小可能变得很大，文档加载速度变慢。
（3）避免矢量图中不同数据类型的嵌套。比如我在Visio中绘制了一个对象，然后我将其粘贴到了PowerPoint中，其余部分都是用PowerPoint画的。完成之后，我又将PowerPoint绘制的所有内容粘贴回Visio，通过Visio导出EMF文件。将这个EMF文件插入Word文档以后，如果选择用Acrobat的方式生成PDF，那么就可能出现图片内容丢失的情况（Word自带的另存为PDF正常）。因此，这种情况需要注意。

12.高质量导出PDF

12.1 问题描述

在输出PDF的时候，在不超过规定文件大小的情况下，肯定是希望图片质量越高越好。但Word默认导出的PDF文件中位图图片质量压缩严重，一般用途还行，但重要用途就显得有些问题，如下图所示。如上图所示，可以看到，左边是Word默认输出的PDF文件，在蜂群打击图片中，绿色的飞机已经被压缩成一团了，而右边的图片则细节较好的保留下来。两个PDF文件的文件大小也有较大差别，Word输出文件大小为297KB，Acrobat输出的文件大小为622KB。

12.2 解决方案

先说结论：如果你想获得好的图片质量，那么用Acrobat方式生成PDF。简单来说，在安装好Acrobat后，打开Word工具栏中就会出现Acrobat选项卡，选择“创建PDF”即可，如下。如果你想进一步控制输出文件的参数，在“Acrobat”选项卡中找到首选项，然后在这里可以看到各种预置模式，可根据需要自行选择。如果这些还不满足你，你可以点击高级设置。这里你可以完全自定义各种参数，包括文档的全局分辨率、图像的压缩程度等，如下。经过实际的对比实验，一般情况下选择“标准”即可。同一个文档（包含位图与矢量文字），不同模式输出的文件大小和质量列举如下：可以看到，预置-标准、预置-印刷质量、预置-高质量打印、自定义-标准高质量、自定义-标准无压缩模式下的图片均有较好细节。但预置-印刷质量图片颜色空间发生了改变。因此，最终剩下预置-标准、预置-高质量打印、自定义-标准高质量、自定义-无压缩均较好，肉眼几乎看不出差别。而在它们四个中，预置标准具有最小文件大小，其次是自定义-标准高质量。因此，可以得出结论：

重点关注输出质量、不看重文件大小：自定义-标准无压缩、预置-高质量打印、自定义-标准高质量、预置-标准
重点关注文件大小、不看重输出质量：预置-标准、自定义-标准高质量、预置-高质量打印、自定义-标准无压缩
兼顾文件大小与输出质量：预置-高质量打印、自定义-标准高质量、预置-标准、自定义-标准无压缩

最后，导出PDF时需要注意：

（1）不管是以何种方式导出PDF，在导出之前，都会自动更新文档中的域。所以可能会出现输出的PDF中未找到域导致的错误等情况。为了避免这种情况，建议在生成PDF前，按照本文第9点介绍的，先把所有域锁定，从而避免错误。
（2）当Word文件较为复杂的时候，Acrobat方式花费时间非常长。以我的论文为例，Word另存为方式直接生成PDF大约需要3-4分钟，而利用Acrobat方式生成则需要约2个小时，所以要合理规划时间。

13.图表目录插入

13.1 问题描述

在论文中，有时候不仅要插入标题目录，还需要插入图索引、表索引等内容，如下所示。显然，一篇论文中，图可能有几十个，不能一个个手动生成。就算手动生成了，也做不到同步修改。因此，如何插入图表目录是需要解决的问题。

13.2 解决方案

图表目录其实和标题目录插入方式是类似的。在Word的“引用”选项卡中，普通目录就是通过“目录-自动目录”插入的。而图表目录则是在“题注”卡片中的“插入表目录”，选择需要插入的域，即可完成，如下。

14.Word单词自动连字符

14.1 问题描述

在中英文混排或者英文写作时，有时可能会遇到某行结尾一行单词写不下的情况。这种时候有两种解决方式，一种是把这个单词完全放到下一行，把该单词前面的句子间距调宽。另一种则是单词能写多少是多少，由连字符连接被分开的单词。在Word中，默认采用第一种自动调整间距的方式，极少情况会自动拆分单词。但有些时候为了节省排版或者字符间距统一，看起来好看一些，可能需要第二种方式。上图中，第一个文献中，第一行的结尾就写不下“Odometry[C]//European”这个单词(尽管它不是一个单词，但因为字母间没有空格，所以Word认为是一个单词)，Word自动把它放到了下一行，将句子前面的单词间距加宽了。

14.2 解决方案

解决方案非常简单。在Word的“布局”选项卡中的“页面设置”中找到“断字”，选择自动即可，如下。但需要注意的是，这个设置其实要慎用，因为他是对文档全局生效的，并不能选择某一句或某一段单独设置。所以文档中只是比较少的位置需要这种断字，可以考虑手动添加连字符解决问题。

15.Word公式中元素对齐

15.1 问题描述

在写论文的时候难免会需要输入公式，而公式内部需要对齐。这里进一步有两种情况，一种是括号括起来的多个公式的对齐，另一种是矩阵的各元素对齐，如下。如上图所示，这是Word默认的左括号多个公式，默认居中对齐，非常难看。我们想要的效果是以等号为参考，等号上下对齐。同时，在下面的4×4矩阵中，由于数字的位数不同以及负号，导致行错位了，比较难看。

15.2 解决方案

首先解决公式的对齐问题。简单来说是，将公式转换为“线性”，然后在需要对齐的地方插入“&”符号，然后再转换为“专用”即可。而至于矩阵元素对齐问题，需要分开来看。Word默认支持3×3及以下矩阵的构建，对于这些矩阵，默认是列居中对齐的，也还可以。如果需要修改，在每列上右键，将“列对齐方式”改成“左对齐”或“右对齐”即可。而对于大小超过3×3的矩阵，Word中并没有提供模板，只能自己拼接。比如构造一个4×4的矩阵，需要先构造一个2×2的矩阵，然后该矩阵的每个元素中再创建一个2×2的矩阵，最终形成4×4矩阵。在网上查阅了很多资料都没有发现比较好的元素对齐办法。目前办法是，通过手动添加空格实现各元素行列的对齐，如下所示。这种方式比较耗时间，但在视觉效果上可以对齐矩阵各元素。

16.Word去掉页眉横线

16.1 问题描述

在写作中，有些页面需要有页眉的横线，有些页面则不需要。这时候就需要根据要求灵活修改页眉的横线，如下所示。

16.2 解决方案

在Word中，添加页眉文字以后，页眉横线就自然出现了，不需要手动添加。如果想去掉只需要把页眉文字删掉即可。而如果删了文字横线依然存在，有两种方式去掉。一种是双击页眉进入编辑模式后，按Ctrl+Shift+N，即可去掉横线。另一种则是进入编辑模式后，选择“开始-样式-正文”，也可以去掉横线，如下。