1.要完成的工作 1)需要实现Jacobian矩阵和误差项的计算。 2)需要构建线性系统。 2.要达到的目标 利用高斯牛顿方法进行全局优化。 3.测试结果 上图是对test_quadrat数据集合的测试结果。蓝色为优化前的位姿轨迹,粉色为优化后的位姿轨迹。 4.关键函数及对应理论分析 4.1 主函数 主函数主要分为四个部分: 1).从本地读