FA_融合和滤波(FF)-图优化

FA ：formulas and algorithm，FF：fusion and filtting

一、图优化算法核心概念与实现原理

1. 基本定义

图优化（Graph Optimization）是一种将优化问题建模为图结构，通过调整图中节点的状态来最小化整体代价函数的算法框架。

图结构：节点（Vertex）表示待优化的变量（如机器人位姿、地图点坐标），边（Edge）表示变量间的约束关系（如传感器观测、运动模型）。
核心目标：找到一组节点状态，使得所有边的代价之和最小（最小二乘问题）。

2. 数学原理

图优化本质是求解稀疏非线性最小二乘问题 ，形式化表达：
x∗=argminx12∑e∈E∥re(x)∥Ωe2x^∗=arg min_x\frac{1}{2}\sum_{e∈E}∥r_e(x)∥_{Ω_e}^{2}x∗=argminx21∑e∈E∥re(x)∥Ωe2

xxx：所有节点的状态向量（优化变量）；
re(x)r_e(x)re(x)：边 e 的残差函数（观测值与预测值的差值）；
ΩeΩ_eΩe：边 e 的信息矩阵（权重，反映约束的置信度）。

3. 核心求解步骤（高斯 - 牛顿法为例）

图优化的主流求解器基于迭代优化，以最常用的高斯 - 牛顿法（Gauss-Newton）为例：

初始化 ：给所有节点赋初始值 x0x_0x0；
线性化 ：在当前迭代点 xkx_kxk 处将残差函数泰勒展开到一阶：re(xk+Δx)≈re(xk)+JeΔxr_e(x_k+Δx)≈r_e(x_k)+J_eΔxre(xk+Δx)≈re(xk)+JeΔx其中 JeJ_eJe 是残差的雅可比矩阵；
构建正规方程 ：将线性化后的残差代入代价函数，求导并令导数为 0，得到：HΔx=b其中 H=∑JeTΩeJeH=∑J_e^TΩ_eJ_eH=∑JeTΩeJe（海森矩阵，稀疏），b=−∑JeTΩereb=−∑J_e^TΩ_er_eb=−∑JeTΩere；
求解增量：利用稀疏矩阵求解器（如 Cholesky 分解）解 Δx；
更新节点 ：xk+1=xk+Δxx_{k+1}=x_k+Δxxk+1=xk+Δx；
收敛判断：若 ∥Δx∥ 小于阈值，停止迭代；否则回到步骤 2。

二、应用场景

图优化是机器人、计算机视觉等领域的核心算法，典型场景包括：

SLAM（同步定位与建图）：如视觉 SLAM（ORB-SLAM）、激光 SLAM，将机器人位姿作为节点，传感器观测（视觉特征匹配、激光里程计）作为边，优化位姿和地图；
Bundle Adjustment（光束法平差）：视觉三维重建的核心步骤，优化相机位姿和三维点坐标，最小化重投影误差；
路径规划与优化：如机器人路径平滑，将路径点作为节点，路径长度 / 避障约束作为边；
姿态估计：多传感器融合（IMU + 视觉）的位姿优化；
自然语言处理：如语义图优化、知识图谱补全。

三、实施步骤（通用流程）

以 SLAM 中的图优化为例，通用实施步骤：

问题建模：
- 确定优化变量（如机器人位姿 xi=[x,y,θ]x_i=[x,y,θ]xi=[x,y,θ]）；
- 定义残差函数（如运动模型残差 r=xj−(xi+u)r=x_j−(x_i+u)r=xj−(xi+u)，u 为里程计输入）；
- 确定信息矩阵（根据传感器精度设置，如激光传感器的信息矩阵对角元素更大）。
构建图结构：
- 初始化节点（如初始位姿）；
- 遍历传感器数据，添加边（约束）。
选择求解器：
- 小规模问题：直接用高斯 - 牛顿、列文伯格 - 马夸尔特（LM）法；
- 大规模问题：用稀疏求解器（如 g2o、Ceres Solver）。
迭代优化：执行高斯 - 牛顿 / LM 迭代，求解最优变量。
结果验证：评估优化后的残差、变量合理性（如位姿是否连续）。

四、优缺点

优点

稀疏性高效：图的边通常是局部的（如相邻位姿约束），海森矩阵稀疏，求解效率高；
鲁棒性强：可通过加权、鲁棒核函数（如 Huber 核）处理异常值；
灵活性高：可灵活添加 / 删除约束（如回环检测后添加回环边）；
精度高：相比滤波类方法（如 EKF），无累计误差，优化结果更精准。

缺点

计算复杂度：大规模图优化的迭代求解仍需较高计算资源（需依赖 GPU / 稀疏求解器）；
初始化敏感：若初始值偏差过大，可能收敛到局部最优；
离线为主：实时性要求高的场景（如高速机器人）需简化模型；
实现复杂：手动推导雅可比矩阵易出错，需依赖成熟库。

五、Python 代码示例

以下是一个简化的 2D 位姿图优化示例（基于numpy实现高斯 - 牛顿法），模拟机器人运动约束优化：

前置条件

安装依赖：

bash 复制代码

pip install numpy scipy

Python代码实现

bash 复制代码

import numpy as np
from scipy.linalg import cho_factor, cho_solve

class PoseGraph2D:
    def __init__(self):
        # 节点：2D位姿 [x, y, theta]，key为节点ID，value为位姿
        self.nodes = {}
        # 边：约束 (from_node, to_node, dx, dy, dtheta, info_matrix)
        # dx/dy/dtheta：期望的相对位姿，info_matrix：6x6信息矩阵（简化为对角阵）
        self.edges = []

    def add_node(self, node_id, x, y, theta):
        """添加位姿节点"""
        self.nodes[node_id] = np.array([x, y, theta], dtype=np.float64)

    def add_edge(self, from_id, to_id, dx, dy, dtheta, info):
        """添加约束边"""
        # info：信息矩阵（权重），这里简化为3x3对角阵
        info_matrix = np.diag(info)
        self.edges.append((from_id, to_id, dx, dy, dtheta, info_matrix))

    def _compute_residual_jacobian(self, from_id, to_id, dx_exp, dy_exp, dtheta_exp):
        """计算单条边的残差和雅可比矩阵"""
        # 当前位姿
        x1, y1, th1 = self.nodes[from_id]
        x2, y2, th2 = self.nodes[to_id]

        # 相对位姿的预测值（从from_id到to_id）
        dx_pred = (x2 - x1) * np.cos(th1) + (y2 - y1) * np.sin(th1)
        dy_pred = -(x2 - x1) * np.sin(th1) + (y2 - y1) * np.cos(th1)
        dtheta_pred = th2 - th1

        # 残差 = 期望 - 预测
        residual = np.array([
            dx_exp - dx_pred,
            dy_exp - dy_pred,
            dtheta_exp - dtheta_pred
        ])

        # 雅可比矩阵：残差对from_id和to_id的偏导数
        # J = [d_residual/d_from, d_residual/d_to]，每个子矩阵3x3
        J_from = np.array([
            [-np.cos(th1), -np.sin(th1), -(x2 - x1)*(-np.sin(th1)) - (y2 - y1)*np.cos(th1)],
            [np.sin(th1), -np.cos(th1), -(x2 - x1)*np.cos(th1) - (y2 - y1)*(-np.sin(th1))],
            [0, 0, -1]
        ])
        J_to = np.array([
            [np.cos(th1), np.sin(th1), 0],
            [-np.sin(th1), np.cos(th1), 0],
            [0, 0, 1]
        ])

        return residual, J_from, J_to

    def optimize(self, max_iter=20, tol=1e-6):
        """高斯-牛顿法优化"""
        node_ids = sorted(self.nodes.keys())
        num_nodes = len(node_ids)
        # 变量维度：每个节点3维（x,y,theta），总维度3*num_nodes
        dim = 3 * num_nodes

        for iter in range(max_iter):
            # 初始化海森矩阵H和梯度b
            H = np.zeros((dim, dim))
            b = np.zeros(dim)

            # 遍历所有边，构建H和b
            for from_id, to_id, dx, dy, dtheta, info in self.edges:
                residual, J_from, J_to = self._compute_residual_jacobian(from_id, to_id, dx, dy, dtheta)

                # 节点ID转索引
                idx_from = node_ids.index(from_id) * 3
                idx_to = node_ids.index(to_id) * 3

                # 计算H的块矩阵
                H[idx_from:idx_from+3, idx_from:idx_from+3] += J_from.T @ info @ J_from
                H[idx_from:idx_from+3, idx_to:idx_to+3] += J_from.T @ info @ J_to
                H[idx_to:idx_to+3, idx_from:idx_from+3] += J_to.T @ info @ J_from
                H[idx_to:idx_to+3, idx_to:idx_to+3] += J_to.T @ info @ J_to

                # 计算梯度b
                b[idx_from:idx_from+3] += J_from.T @ info @ residual
                b[idx_to:idx_to+3] += J_to.T @ info @ residual

            # 求解H * delta_x = b（高斯-牛顿法）
            try:
                # Cholesky分解加速求解（H是正定矩阵）
                cho, lower = cho_factor(H)
                delta_x = cho_solve((cho, lower), b)
            except np.linalg.LinAlgError:
                print("海森矩阵奇异，优化失败")
                break

            # 更新节点
            for i, node_id in enumerate(node_ids):
                self.nodes[node_id] += delta_x[i*3 : (i+1)*3]

            # 收敛判断
            delta_norm = np.linalg.norm(delta_x)
            print(f"迭代 {iter+1}: 增量范数 = {delta_norm:.8f}")
            if delta_norm < tol:
                print("优化收敛！")
                break

# ---------------------- 测试示例 ----------------------
if __name__ == "__main__":
    # 1. 初始化图优化器
    pg = PoseGraph2D()

    # 2. 添加节点（初始位姿，带噪声）
    # 真实位姿：节点0=(0,0,0)，节点1=(1,0,0)，节点2=(1,1,np.pi/2)
    # 初始值添加噪声：节点0=(0.1, 0.1, 0.1)，节点1=(1.1, 0.1, 0.1)，节点2=(1.1, 1.1, np.pi/2 + 0.1)
    pg.add_node(0, 0.1, 0.1, 0.1)
    pg.add_node(1, 1.1, 0.1, 0.1)
    pg.add_node(2, 1.1, 1.1, np.pi/2 + 0.1)

    # 3. 添加约束边（期望的相对位姿，信息矩阵权重[100,100,100]）
    # 边0-1：期望相对位姿(1,0,0)
    pg.add_edge(0, 1, 1.0, 0.0, 0.0, [100.0, 100.0, 100.0])
    # 边1-2：期望相对位姿(0,1,np.pi/2)
    pg.add_edge(1, 2, 0.0, 1.0, np.pi/2, [100.0, 100.0, 100.0])
    # 边0-2（回环约束）：期望相对位姿(1,1,np.pi/2)
    pg.add_edge(0, 2, 1.0, 1.0, np.pi/2, [100.0, 100.0, 100.0])

    # 4. 打印优化前的位姿
    print("优化前的位姿：")
    for node_id in sorted(pg.nodes.keys()):
        x, y, th = pg.nodes[node_id]
        print(f"节点{node_id}: x={x:.4f}, y={y:.4f}, theta={th:.4f}")

    # 5. 执行优化
    pg.optimize()

    # 6. 打印优化后的位姿
    print("\n优化后的位姿：")
    for node_id in sorted(pg.nodes.keys()):
        x, y, th = pg.nodes[node_id]
        print(f"节点{node_id}: x={x:.4f}, y={y:.4f}, theta={th:.4f}")

代码解释

核心类PoseGraph2D：
- add_node/add_edge：构建图的节点（位姿）和边（约束）；
- _compute_residual_jacobian：计算单条约束的残差（期望 - 预测）和雅可比矩阵（残差对节点的偏导数）；
- optimize：高斯 - 牛顿迭代求解，核心是构建稀疏海森矩阵H和梯度b，通过 Cholesky 分解求解增量并更新节点。
测试示例：
- 模拟 3 个机器人位姿节点，添加相邻约束和回环约束；
- 初始位姿带噪声，优化后位姿收敛到真实值（节点 0≈(0,0,0)，节点 1≈(1,0,0)，节点 2≈(1,1,π/2)）。

输出结果（示例）

bash 复制代码

优化前的位姿：
节点0: x=0.1000, y=0.1000, theta=0.1000
节点1: x=1.1000, y=0.1000, theta=0.1000
节点2: x=1.1000, y=1.1000, theta=1.6708
迭代 1: 增量范数 = 0.14142136
迭代 2: 增量范数 = 0.00000000
优化收敛！

优化后的位姿：
节点0: x=0.0000, y=0.0000, theta=0.0000
节点1: x=1.0000, y=0.0000, theta=0.0000
节点2: x=1.0000, y=1.0000, theta=1.5708

六、进阶说明

实际工程中，很少手动实现图优化（推导雅可比矩阵易出错），常用成熟库：

C++：g2o（图优化专用）、Ceres Solver（通用最小二乘）；
Python：pyg2o（g2o 的 Python 绑定）、ceres-py（Ceres 的 Python 封装）、gtsam（Georgia Tech Smoothing and Mapping Library）。

示例：使用gtsam简化实现（需安装pip install gtsam）：

bash 复制代码

import gtsam
import numpy as np

# 1. 初始化优化器
graph = gtsam.NonlinearFactorGraph()
initial_estimate = gtsam.Values()

# 2. 添加节点（初始位姿）
prior_noise = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.1, 0.1, 0.1]))
initial_estimate.insert(0, gtsam.Pose2(0.1, 0.1, 0.1))
initial_estimate.insert(1, gtsam.Pose2(1.1, 0.1, 0.1))
initial_estimate.insert(2, gtsam.Pose2(1.1, 1.1, np.pi/2 + 0.1))

# 3. 添加约束
noise = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.01, 0.01, 0.01]))
graph.add(gtsam.BetweenFactorPose2(0, 1, gtsam.Pose2(1, 0, 0), noise))
graph.add(gtsam.BetweenFactorPose2(1, 2, gtsam.Pose2(0, 1, np.pi/2), noise))
graph.add(gtsam.BetweenFactorPose2(0, 2, gtsam.Pose2(1, 1, np.pi/2), noise))

# 4. 优化
optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial_estimate)
result = optimizer.optimize()

# 5. 输出结果
print("优化后的位姿：")
for i in range(3):
    pose = result.atPose2(i)
    print(f"节点{i}: x={pose.x():.4f}, y={pose.y():.4f}, theta={pose.theta():.4f}")

七、结束语

核心本质：图优化是将优化问题建模为图结构，通过迭代求解稀疏非线性最小二乘问题，最小化约束残差；
关键步骤：建模（节点 / 边）→ 线性化（残差 + 雅可比）→ 构建正规方程 → 求解增量 → 更新节点；
核心特点：精度高、稀疏高效，但对初始化敏感，大规模场景需依赖成熟求解库（g2o/Ceres/gtsam）。

简单来说，图优化就是最小二乘法优化数值(构建随时间变化的曲线回滚和估计变量值)，具体为有三个时间点的三个关注变量的值，最小二乘拟合曲线，随后估计变量。