GVINS代码学习4

optimization()

1、构建ceres::Problem problem对象，用于存储优化问题的参数和残差项

2、优化问题的参数：

    1、para_Pose[i]:滑动窗口内第i帧的位姿；

para_SpeedBias[i]:滑动窗口内i帧对应的速度；

para_Ex_Pose[i]:相机到IMU的外参变换矩阵；

para_Td[0]:滑动窗口内第一个时刻的相机到IMU的时钟差；

para_yaw_enu_local:ENU坐标系下的偏航角，1维参数块，初始值为实数 ;

para_anc_ecef: 地球固定坐标系ECEF系下锚点位置的参数块，表示定位过程中的从参考点。3维参数块，初始值为包含三个实数的向量;

para_rcv_dt+i*4+k: 包含接收机钟差的参数块，表示不同接收机在不同;

para_rcv_ddt+i:包含接收机钟漂的参数块，表示不同接收机的钟漂，i是循环变量，用于索引不同的接收机，一维参数块，初始值为一个实数 ;

problem.AddParameterBlock(para_Pose[i], SIZE_POSE, local_parameterization);
problem.AddParameterBlock(para_SpeedBias[i], SIZE_SPEEDBIAS);
 problem.AddParameterBlock(para_Ex_Pose[i], SIZE_POSE, local_parameterization);
problem.AddParameterBlock(para_Td[0], 1);
problem.AddParameterBlock(para_yaw_enu_local, 1);
problem.AddParameterBlock(para_anc_ecef, 3);
problem.AddParameterBlock(para_rcv_dt+i*4+k, 1);
problem.AddParameterBlock(para_rcv_ddt+i, 1);

      2、残差项：problem.AddResidualBlock()用于向优化问题中添加残差块（Residual Block）。残差块表示优化问题中的一个约束条件或观测数据，它衡量了当前参数估计与实际观测之间的差异。

     pose_anchor_factor：位姿锚定因子，表示将一个位姿参数与锚定位置进行约束。通过参数块para_Pose[0]与优化问题相关联；

marginalization_factor：边缘化因子，边缘化一些常量并从优化中剔除，通过参数块last_marginalization_parameter_blocks与优化问题相关联；

imu_factor：IMU因子，用于约束两个时刻的姿态和速度偏置之间的关系，通过参数块para_Pose[i]、para_SpeedBias[i]、para_Pose[j]和para_SpeedBias[j]与优化问题相关联；

gnss_factor:GNSS因子，用于约束两个时刻的位姿、速度偏置、接收机钟差、接收机钟漂和导航辅助信息之间的关系，通过多个参数块与优化问题相关联；

dt_ddt_factor：时钟差和时钟漂因子（Clock Bias and Clock Drift Factor），用于约束相邻时刻的接收机钟差和时钟漂之间的关系。它通过多个参数块与优化问题相关联；

ddt_smooth_factor：时钟漂平滑因子（Clock Drift Smoothness Factor），用于约束相邻时刻的接收机时钟漂之间的平滑性。它通过多个参数块与优化问题相关联；

f_td：时间偏差因子（Time Delay Factor），用于约束两个位姿之间的时间偏差。它通过多个参数块与优化问题相关联，并使用损失函数进行加权；

f：投影因子（Projection Factor），用于约束两个位姿之间的图像特征点投影关系。它通过多个参数块与优化问题相关联，并使用损失函数进行加权；

problem.AddResidualBlock(pose_anchor_factor, NULL, para_Pose[0]);
problem.AddResidualBlock(marginalization_factor, NULL,
                                 last_marginalization_parameter_blocks);
problem.AddResidualBlock(imu_factor, NULL, para_Pose[i], para_SpeedBias[i], para_Pose[j], para_SpeedBias[j]);
problem.AddResidualBlock(gnss_factor, NULL, para_Pose[lower_idx], 
                    para_SpeedBias[lower_idx], para_Pose[lower_idx+1], para_SpeedBias[lower_idx+1],
                    para_rcv_dt+i*4+sys_idx, para_rcv_ddt+i, para_yaw_enu_local, para_anc_ecef);
problem.AddResidualBlock(dt_ddt_factor, NULL, para_rcv_dt+i*4+k, 
                    para_rcv_dt+(i+1)*4+k, para_rcv_ddt+i, para_rcv_ddt+i+1);
problem.AddResidualBlock(ddt_smooth_factor, NULL, para_rcv_ddt+i, para_rcv_ddt+i+1);
problem.AddResidualBlock(f_td, loss_function, para_Pose[imu_i], para_Pose[imu_j], para_Ex_Pose[0], para_Feature[feature_index], para_Td[0]);
problem.AddResidualBlock(f, loss_function, para_Pose[imu_i], para_Pose[imu_j], para_Ex_Pose[0], para_Feature[feature_index]);

添加这些残差块，优化问题会根据观测数据和约束条件对参数进行调整，以最小化残差并找到最佳的参数估计。

3、

vector2double()：把现在滑窗里面的未经过非线性化的原数数据（初始值预设值），转化为非线性优化参数para_**。

double2vector()：把非线性优化参数para_**转化为现在滑窗里面的数据

4、边缘化操作

根据边缘化标志位进行边缘化操作，根据上一次边缘化操作的结果或者锚定值以及预积分值构造相应的边缘化因子，并将其添加到边缘化信息中

5、GNSS数据的处理

1. 首先，判断GNSS数据是否准备好，如果准备好，则进入优化过程。
2. 接下来，通过遍历每个时刻的GNSS观测和星历数据，构造GNSS因子，并将其添加到优化问题中。这些因子用于约束两个时刻的位姿、速度偏置、接收机钟差、接收机钟漂和导航辅助信息之间的关系。
3. 构建接收机时延和时延变化率之间的关系，构造时钟差和时钟漂因子，并将其添加到优化问题中。这些因子用于约束相邻时刻的接收机钟差和时钟漂之间的关系。
4. 对于每个特征点，判断其被使用的帧数是否大于等于2且起始帧小于WINDOW_SIZE-2。如果满足条件，则构造ProjectionFactor或ProjectionTdFactor因子，并将其添加到优化问题中。这些因子用于约束两个位姿之间的图像特征点投影关系。
5. 进行边缘化操作，将部分参数从优化问题中移除，以减少优化问题的规模。
6. 使用ceres Solver对优化问题进行求解。设置优化器的选项，包括线性求解器类型、信赖域策略类型、最大迭代次数等。然后调用ceres::Solve函数开始求解优化问题，得到最优的位姿和特征点参数。
7. 对求解结果进行一些后处理操作，比如将角度值限制在[-π, π]范围内，以确保角度的连续性。
8. 最后，将求解得到的位姿和特征点参数转换为向量形式。

6、边缘化

边缘化，执行marginalize()
a：计算参数块（包括边缘化和保留）的大小维度（不是个数），m为总共待边缘化的参数块总大小，n为保留的参数块的总大小
b：构造Ax=b，将每个残差块的残差和雅可比叠加到一块，多线程执行ThreadsConstructA()
c：利用舒尔补，得到一个可以求解保留的参数块的方程（Ax=b），利用下面的公式求解线性化点x0（当前优化解）处雅克比J、残差e
注意事项 程序中，e（残差）=h（观测方程）-z(测量)，所以没负号
/*
Ax = b => JT*J x= JT * e
对A做特征值分解 A = V * S * VT,其中Ｓ是特征值构成的对角矩阵 V是特征列向量构成的矩阵
V的第k个列向量是对应S中第k个特征值的特征向量（eigenvector）
JT * J = (S^(1/2) * VT)T * S^(1/2) * VT = V * S^(1/2)T * S^(1/2) * VT = V * S * VT(对角矩阵的转置等于其本身)
J = S^(1/2) * VT
e = (JT)-1 * b = (V * S^(1/2))-1 * b = S^-(1/2） * VT * b
*/
d：更新边缘化信息，
last_marginalization_info：本次边缘化所有信息
last_marginalization_parameter_blocks：本次边缘化对保留的参数块形成约束，这些参数块在滑窗之后的新地址

A.优化求解
1、定义待优化参数块
a：显式声明有旋转的参数块，本地化四元数，变为自由度为3的李代数
b：是否优化外参
c：是否在线估计传感器时间延迟
2、更换初始值容器（para_xxxx），eigen->double，执行vector2double()
3、添加残差块，计算对应facter的残差和雅可比
a：上一次的边缘化约束（每次迭代只更新残差，固定雅可比）
b：imu预积分，相邻KF约束
c：视觉重投影约束
d：回环约束（如果有）
4、优化结果输出，执行double2vector()，固定窗口内第一帧的位移和航向角

B.边缘化（以MARG_OLD为例，保留本次优化中边缘化参数对保留参数的约束）
1、将本次优化结果转为double类型，执行vector2double()
2、找出和这次要边缘化的参数块（以最老帧为起始观察帧的路标点逆深度，最老帧位姿，最老帧速度零偏）有关的约束（预积分约束，重投影约束，以及上一次边缘化约束）
a：上一次边缘化：
上一次的边缘化约束对当前待边缘化的参数块（para_Pose[0]、para_SpeedBias[0]）有约束
找到其在上一次边缘化留下的参数块中的索引
构造自定义解析代价函数，添加自定义残差块到边缘化总类marginalization_info中
b：预积分
KF[0]-KF[1] pre_integrations[1]约束了这相邻两帧

c：重投影约束
找能被最老帧看到的特征点，遍历看到这个特征点的所有KF，通过这个特征点，建立和最老帧的约束

收集优化A中对最老帧有约束的所有的残差块，进行预处理，执行preMarginalize()，计算每个残差块的残差和雅可比，备份参数块的数据到parameter_block_data

 

processGNSS()

1、遍历传入的 GNSS测量值gnss_meas；

2、根据卫星系统类型对测量值进行第一次过序，只保留GPS、GLONASS、Galileo和BDS（北斗）系统的测量值；

3、检查是否存在有效的信号测量值（频率）和L1观测项；

4、根据跟踪状态对测量值进行进一步的筛选。

5、如果GNSS系统准备就绪（即已经获取到了本地位置信息），则根据卫星的仰角对测量值进行进一步的筛选；

6、最后，将有效的测量值和星历信息存储到名为gnss_meas_buf和gnss_ephem_buf的缓冲区中。