基于LM算法的光束平差优化系统

资 源 简 介

该项目旨在设计并实现一个基于Levenberg-Marquardt（LM）算法的高性能非线性最小二乘优化框架，专门用于处理计算机视觉与社交摄影测量中的光束平差（Bundle Adjustment）问题。 系统通过建立精确的几何投影模型，以最小化实际观测到的图像特征点与预测投影点之间的重投影误差为目标函数。 在算法实现层面，LM算法巧妙地结合了梯度下降法的全局鲁棒性与高斯-牛顿法的局部快速收敛特性，通过引入并动态调整阻尼因子（Damping Factor），确保即使在初始估计值不够精确的情况下也能稳定收敛到

详 情 说 明

基于LM算法的光束平差非线性最小二乘优化系统

项目介绍

本项目实现了一个高性能的光束平差（Bundle Adjustment, BA）优化系统，专门用于解决计算机视觉中的多视角几何优化问题。系统核心采用 Levenberg-Marquardt (LM) 算法，通过迭代优化相机位姿（位置与姿态）和三维空间点坐标，使得预测的重投影坐标与实际观测到的图像特征点之间的总误差达到最小。该系统模拟了真实的摄影测量场景，包括相机运动轨迹生成、三维点云分布、包含径向畸变的成像过程以及观测噪声。

功能特性

1. LM 算法核心实现：系统实现了完整的 LM 算法逻辑，支持动态调整阻尼因子（Lambda），在保证全局鲁棒性的同时具备快速的局部收敛能力。
2. Rodrigues 旋转表示：采用 Rodrigues 旋转向量参数化相机姿态，有效避免了欧拉角的万向锁问题，并减少了参数冗余（仅需3维向量表示旋转）。
3. 稀疏矩阵优化：在构造雅可比矩阵（Jacobian）时，充分利用了光束平差问题的稀疏结构，采用三元组形式预分配内存并生成稀疏矩阵，显著降低了大内存占用并提升了线性方程求解效率。
4. 径向畸变模型：成像投影模型中集成了二阶径向畸变（k1, k2），能够模拟真实镜头产生的几何形变，使优化更契合实际应用。
5. 数值梯度计算：系统利用数值微分法计算复杂的非线性投影函数对相机参数及空间点坐标的偏导数，增强了系统对不同投影模型的适配性。
6. 可视化评估：内置自动生成三维空间对比图（对比真值、初始扰动状态与优化后状态）以及迭代收敛曲线图，直观展示优化效果。

实现逻辑与流程

1. 仿真环境初始化：

* 预设相机内参（焦距、主点位置）和畸变系数。 * 生成圆周运动的相机轨迹及位于相机前方的随机三维点云。 * 依据投影模型生成观测值，并人为加入高斯噪声。 * 对真实的位姿和点云引入扰动，以此作为 LM 优化的初始估计值。

1. 状态向量构造：

* 将所有相机的 6 自由度参数（3维旋转向量 + 3维平移向量）与所有三维点的 3 维坐标展开并拼接成一个高维状态向量。

1. LM 迭代循环：

* 残差与雅可比计算：遍历所有观测数据，计算当前参数下的重投影误差，并通过数值扰动法构建 $2N times M$ 维的稀疏雅可比矩阵。 * 线性方程组构建：构建近似 Hessian 矩阵 $H = J^T J$ 和梯度向量 $g = J^T text{residuals}$。 * 阻尼因子控制：在内层循环中尝试不同的 Lambda 值。如果更新后的均方误差（MSE）下降，则接受更新并减小 Lambda；否则拒绝更新并增大 Lambda。 * 状态更新：通过求解 $(H + lambda text{diag}(H)) Delta x = -g$ 获取增量步长，更新状态向量。

1. 终止条件：

* 当达到最大迭代次数，或两次迭代间的误差下降值低于预设阈值时，系统自动停止优化。

1. 结果解析与绘图：

* 将优化后的状态向量重新映射回旋转矩阵、平移向量和空间点坐标。 * 绘制三维场景图及误差收敛曲线。

核心算法与函数实现细节

• 重投影残差计算：该功能模块不仅负责计算预测像素值与观测值之差，还承担着雅可比矩阵的构造任务。它通过预分配 row_idx, col_idx 和 val_idx 数组，高效地填充非零偏导数项，最终生成 MATLAB 稀疏矩阵。
• 投影转换逻辑：实现了从世界坐标系到相机坐标系的刚体变换（$X_c = RX_w + t$），紧接着进行归一化坐标计算、径向畸变修正，最后应用内参矩阵 K 映射至像素坐标。
• 旋转相互转换：

* 向量转矩阵：基于罗德里格斯公式（Rodrigues' rotation formula），利用单位向量和旋转角度构造旋转矩阵。 * 矩阵转向量：通过迹运算和反对称矩阵提取计算旋转角及等效旋转轴。

• 线性方程求解：系统利用运算符针对稀疏 Hessian 矩阵进行求解。虽然代码中提及了 Schur 消元法（Schur Complement）的结构，但在实际计算中，依靠 MATLAB 对稀疏矩阵的底层优化直接求解，保证了复杂 BA 问题的计算效率。

系统要求

• 环境需求：MATLAB R2016b 及以上版本（需支持稀疏矩阵运算及基本绘图工具箱）。
• 硬件建议：由于涉及大规模稀疏矩阵运算，建议内存不少于 8GB。

使用方法

1. 打开 MATLAB 软件。
2. 将相关代码脚本放置于当前工作目录下。
3. 运行主程序脚本。
4. 运行结束后，系统将弹出两个窗口：

* 窗口1展示了空间点的优化前、优化后以及真实位置的分布关系，并标出了相机的位置。 * 窗口2展示了重投影均方误差（MSE）随迭代次数的变化情况。

1. 在命令行窗口（Command Window）可以实时查看每一轮迭代的 MSE 变化和 Lambda 值的调整情况。