自动驾驶与机器人中的slam技术（高翔）第九章（后端优化）run_optimization.cc代码笔记

优化1：
# 第一个点云的位姿，是IMU从0开始积分得到的，与GNSS不对应
# kabsch算法：https://zhuanlan.zhihu.com/p/535105203
# g2o的里鲁棒核：避免误差值过大，导致步长较大。例如Huber核，当误差绝对值小于等于阈值时，仍使用二范数，但是当大于阈值时，使用线性函数，避免增长过大
# 误差值 = 估计值做一些变换后 - 观测值，
# edge->chi2()  //计算误差的2范数
# g2o的边没有定义linearizeOplus时，g2o会采用数值求导来代替解析求导，但是数值求导很慢，不如解析求导迅速，效果不佳，优点是不需要计算雅可比矩阵
# 如果多条边约束同个顶点，那顶点是怎么更新的
# 可以使用g2o_viewer 查看.g2o的文件
# gnss位姿是gnss传感器在世界坐标系下的位姿？lidar优化1后的位姿是本体在世界坐标系下的位姿？

1、读取run_frontend.cc生成的关键帧信息
2、读取地图参数，rtk_outlier_th为rtk残差有效范围，rtk噪声参数，TBG，baseline->gnss，
3、读取lidar和gnss的位置，使用kabsch算法计算这两个点集的变换矩阵TLG，并将lidar位姿转换成本体坐标的位姿？，参考https://zhuanlan.zhihu.com/p/535105203
4、图优化，添加所有的激光位姿作为估计值的顶点，并记录到map，vertices_
5、图优化，由2读取的噪声参数，设置信息矩阵，添加rtk位姿为顶点观测值的一元边，使用rtk_outlier_th作为鲁棒核参数，记录每一条边到gnss_trans_edge_
6、图优化，定义lidar信息矩阵，分别添加vertices_里相邻1或2或3或4或5的lidar位姿作为二元边，位姿之差为观测值。，记录每一条边到lidar_edge_。
    存疑，顶点是估计值是激光位姿，观测值是激光位姿之差，自己约束自己很奇怪，可能跟我没理解lidar_continuous_num_参数有关
7、保存vertices_、gnss_trans_edge_、lidar_edge_ 的估计值和观测值为.g2o文件
8、依次计算边的误差，这里5对应的边求误差时使用了TBG，但是2那里已经配准过了，个人理解这里的TBG应该是没有必要的，而且也没有定义linearizeOplus
9、依次更新顶点，即修改激光位姿
10、重复迭代8-9共10次，
11、去除gnss边的误差大于鲁棒核阈值的边，已经对lidar位姿优化过了，但是gnss仍与lidar之差较大，仍认为gnss是错的。
12、去除异常值之后，再次优化lidar位姿，运行8-10
13、保存vertices_、gnss_trans_edge_、lidar_edge_ 的估计值和观测值为.g2o文件
14、保存结果，往关键帧里添加优化1位姿，即世界坐标系的lidar优化后的位姿，并保存

在执行优化2之前，得先执行run_loopclosure.cc

优化2：
1、读取run_frontend.cc生成的关键帧信息
2、读取地图参数，rtk_outlier_th为rtk残差有效范围，rtk噪声参数，TBG，baseline->gnss，
3、读取run_loopclosure.cc生成的候选帧信息到loop_candidates_
4、执行优化1的4-6
5、图优化，定义loop信息矩阵，分别添加vertices_里对应候选帧的顶点作为二元边，loop_candidates_的位姿之差为观测值，
    使用loop_rk_th作为鲁棒核参数记录每一条边到loop_edge_
6、执行优化1的7-13
7、保存结果，往关键帧里添加优化2位姿，即世界坐标系的lidar优化后的位姿，并保存