书接上回，初始化完成以后，就是进行非线性优化了，我们来看看这个函数吧。
状态向量包括滑窗内的n+1个所有相机的状态，包括位置，朝向，速度，加速度计bias和陀螺仪bias，相机到IMU的外参，m+1个3D点的逆深度： X = [ x 0 , x 1 , ⋯ , x c b , λ 0 , λ 1 , ⋯ , λ m ] X = [x_0,x_1,\cdots,x_c^b,\lambda_0,\lambda_1,\cdots,\lambda_m] X=[x0,x1,⋯,xcb,λ0,λ1,⋯,λm] x k = [ p b k w , v b k w , q b k w , b a , b g ] x_k=[p_{b_k}^w,v_{b_k}^w,q_{b_k}^w,b_a,b_g] xk=[pbkw,vbkw,qbkw,ba,bg] x c b = [ p c b , q c b ] x_c^b=[p_c^b,q_c^b] xcb=[pcb,qcb]
min X = { ∥ r p − J p X ∥ 2 + ∑ k ∈ B ∥ r B ( z ^ b k + 1 b k , X ) ∥ P b k + 1 b k 2 + ∑ ( i , j ) ∈ C ∥ r C ( z ^ l C j , X ) ∥ p l C j 2 } \min_X=\lbrace \Vert r_p-JpX\Vert^2+\sum_{k\in B}\Vert r_B(\hat z_{b_{k+1}}^{b_k},X)\Vert _{P_{b_{k+1}}^{b_k}}^2 +\sum _{(i,j)\in C}\Vert r_C(\hat z_l^{C_j},X) \Vert_{p_l^{C_j}}^2\rbrace Xmin={∥rp−JpX∥2+k∈B∑∥rB(z^bk+1bk,X)∥Pbk+1bk2+(i,j)∈C∑∥rC(z^lCj,X)∥plCj2} 三个残差项分别为：边缘化的先验信息，IMU的测量残差，视觉的重投影残差，三种残差都是用马氏距离表示的。
残差：两帧之间PVQ和bias的变化量 优化变量：
残差： 优化变量：
根据次新帧是否是关键帧，分为两种边缘化策略：通过对比次新帧和次次新帧的视差，来决定marg掉次新帧或者最老帧，
当次新帧为关键帧时，MARGIN_OLD，将marg掉最老帧，及其看到的路标点和相关联的IMU数据，将其转换为先验信息加到整体的目标函数。当次新帧不是关键帧的时候，MARGIN_SECOND_NEW, 我们将直接丢掉次新帧及它的视觉观测边，而不对次新帧进行marg，因为我们认为当前帧和次新帧很相似，直接丢弃不会造成整个约束关系丢失过多信息，但是，要保留次新帧的IMU数据，从而保证IMU预积分的连贯性。这里仅仅写出了优化的变量和对应的残差，具体的推导和代码的对应，需要单独的写一章出来。
如果不需要标定，就固定为常量
for (int i = 0; i < NUM_OF_CAM; i++) { ceres::LocalParameterization *local_parameterization = new PoseLocalParameterization(); problem.AddParameterBlock(para_Ex_Pose[i], SIZE_POSE, local_parameterization); if (!ESTIMATE_EXTRINSIC) { ROS_DEBUG("fix extinsic param"); problem.SetParameterBlockConstant(para_Ex_Pose[i]);//这个变量固定为常量 } else ROS_DEBUG("estimate extinsic param"); }给parameterBlock赋值。
void Estimator::vector2double() { for (int i = 0; i <= WINDOW_SIZE; i++) { para_Pose[i][0] = Ps[i].x(); para_Pose[i][1] = Ps[i].y(); para_Pose[i][2] = Ps[i].z(); Quaterniond q{Rs[i]}; para_Pose[i][3] = q.x(); para_Pose[i][4] = q.y(); para_Pose[i][5] = q.z(); para_Pose[i][6] = q.w(); para_SpeedBias[i][0] = Vs[i].x(); para_SpeedBias[i][1] = Vs[i].y(); para_SpeedBias[i][2] = Vs[i].z(); para_SpeedBias[i][3] = Bas[i].x(); para_SpeedBias[i][4] = Bas[i].y(); para_SpeedBias[i][5] = Bas[i].z(); para_SpeedBias[i][6] = Bgs[i].x(); para_SpeedBias[i][7] = Bgs[i].y(); para_SpeedBias[i][8] = Bgs[i].z(); } for (int i = 0; i < NUM_OF_CAM; i++) { para_Ex_Pose[i][0] = tic[i].x(); para_Ex_Pose[i][1] = tic[i].y(); para_Ex_Pose[i][2] = tic[i].z(); Quaterniond q{ric[i]}; para_Ex_Pose[i][3] = q.x(); para_Ex_Pose[i][4] = q.y(); para_Ex_Pose[i][5] = q.z(); para_Ex_Pose[i][6] = q.w(); } VectorXd dep = f_manager.getDepthVector(); for (int i = 0; i < f_manager.getFeatureCount(); i++) para_Feature[i][0] = dep(i); if (ESTIMATE_TD) para_Td[0][0] = td; }第一次执行这段代码的时候，没有先验信息，所以这段是不执行的，第二次执行的时候就有了，last_marginalization_info的赋值出现在后面的代码里。
class MarginalizationInfo { public: ~MarginalizationInfo(); int localSize(int size) const; int globalSize(int size) const; //加残差块相关信息（优化变量，待marg的变量） void addResidualBlockInfo(ResidualBlockInfo *residual_block_info); //计算每个残差对应的jacobian，并更新parameter_block_data void preMarginalize(); //pos为所有变量的维度，m要marg的变量，n为要保存的变量 void marginalize(); std::vector<double *> getParameterBlocks(std::unordered_map<long, double *> &addr_shift); //所有观测项 std::vector<ResidualBlockInfo *> factors; //m为要marg掉的变量个数，也就是parameter_block_idx的总localSize,以double为单位，VBias为9，PQ为6 //n为要保留下的优化变量的变量个数 n=localSize(parameter_block_size) - m int m, n; //<优化变量的内存地址, localSize> 为每个优化变量的变量大小，IMU(7,9,7,9) std::unordered_map<long, int> parameter_block_size; //global size int sum_block_size; //<待marg的优化变量内存地址，在parameter_block_index中的id> std::unordered_map<long, int> parameter_block_idx; //local size //<优化变量内存地址，数据> 每个优化块的数据 std::unordered_map<long, double *> parameter_block_data; std::vector<int> keep_block_size; //global size std::vector<int> keep_block_idx; //local size std::vector<double *> keep_block_data; //边缘化之后从信息矩阵中恢复出来的雅可比矩阵和残差向量 Eigen::MatrixXd linearized_jacobians; Eigen::VectorXd linearized_residuals; const double eps = 1e-8; };当marg掉最老帧的时候，parameter_block_size为所有变量及其对应的localSize，parameter_block_data为对应的数据（double* 类型的）。下面举例说明，假设最老帧即第0帧观测到了k = 68个路标点那么代码中的parameter_block_size和parameter_block_data两个vector的大小均为： 2 ( V 0 : 1 ) + 11 ( P 0 : 10 ) + 1 ( T b c ) + 1 ( t b ) + 68 ( λ 1 : k ) = 83 2(V_{0:1})+11(P_{0:10})+1(T_{bc})+1(t_b)+68(\lambda_{1:k})=83 2(V0:1)+11(P0:10)+1(Tbc)+1(tb)+68(λ1:k)=83 pose=所有变量总localSize为： 2 × 9 + 11 × 6 + 1 × 6 + 1 × 1 + 68 × 1 = 159 2 \times 9+11\times6+1\times 6+1\times 1+68\times 1=159 2×9+11×6+1×6+1×1+68×1=159 parameter_block_idx为需要marg掉的变量，仍然假设第一帧看到了k=68个路标点，则parameter_block_idx数组的大小为： p a r a m e t e r _ b l o c k _ i d x . s i z e ( ) = 1 ( P 0 ) + 1 ( V 0 ) + 68 ( λ 1 : k ) = 70 parameter\_block\_idx.size()=1(P_0)+1(V_0)+68(\lambda _{1:k})=70 parameter_block_idx.size()=1(P0)+1(V0)+68(λ1:k)=70 MarginalizationInfo::m表示需要marg变量总的loaclSize, 即为 1 × 6 + 1 × 9 + 68 × 1 = 83 1\times 6+ 1\times 9+68\times 1=83 1×6+1×9+68×1=83 那么，MarginalizationInfo::n表示需要保留的变量的总localSIze，为 n = p o s − m = 159 − 83 = 76 n=pos-m=159-83=76 n=pos−m=159−83=76
在这里只进行了边缘化，没有求解，求解的过程放在了下一轮的优化中。
直接扔掉次新帧和它的视觉观测边，不对次新帧进行marg。但是保存了次新帧的IMU数据，从而保证预积分的连贯性。
else { /** * 当次新帧不是关键帧时，直接剪切掉次新帧和它的视觉边（该帧和路标点之间的关联），而不对次新帧进行marginalize处理 * 但是要保留次新帧的IMU数据，从而保证IMU预积分的连续性，这样才能积分计算出下一帧的测量值 */ if (last_marginalization_info && std::count(std::begin(last_marginalization_parameter_blocks), std::end(last_marginalization_parameter_blocks), para_Pose[WINDOW_SIZE - 1])) { MarginalizationInfo *marginalization_info = new MarginalizationInfo(); vector2double(); if (last_marginalization_info) { vector<int> drop_set; for (int i = 0; i < static_cast<int>(last_marginalization_parameter_blocks.size()); i++) { ROS_ASSERT(last_marginalization_parameter_blocks[i] != para_SpeedBias[WINDOW_SIZE - 1]); if (last_marginalization_parameter_blocks[i] == para_Pose[WINDOW_SIZE - 1]) drop_set.push_back(i); } // construct new marginlization_factor MarginalizationFactor *marginalization_factor = new MarginalizationFactor(last_marginalization_info); ResidualBlockInfo *residual_block_info = new ResidualBlockInfo(marginalization_factor, NULL, last_marginalization_parameter_blocks, drop_set); marginalization_info->addResidualBlockInfo(residual_block_info); } TicToc t_pre_margin; ROS_DEBUG("begin marginalization"); //计算每次IMU和视觉观测(cost_function)对应参数块(parameters_block),雅可比矩阵(jacobians)，残差值(residuals) marginalization_info->preMarginalize(); ROS_DEBUG("end pre marginalization, %f ms", t_pre_margin.toc()); TicToc t_margin; ROS_DEBUG("begin marginalization"); //多线程计算整个先验项的参数块，雅可比矩阵和残差值 marginalization_info->marginalize(); ROS_DEBUG("end marginalization, %f ms", t_margin.toc()); std::unordered_map<long, double *> addr_shift; for (int i = 0; i <= WINDOW_SIZE; i++) { if (i == WINDOW_SIZE - 1) continue; else if (i == WINDOW_SIZE) { addr_shift[reinterpret_cast<long>(para_Pose[i])] = para_Pose[i - 1]; addr_shift[reinterpret_cast<long>(para_SpeedBias[i])] = para_SpeedBias[i - 1]; } else { addr_shift[reinterpret_cast<long>(para_Pose[i])] = para_Pose[i]; addr_shift[reinterpret_cast<long>(para_SpeedBias[i])] = para_SpeedBias[i]; } } for (int i = 0; i < NUM_OF_CAM; i++) addr_shift[reinterpret_cast<long>(para_Ex_Pose[i])] = para_Ex_Pose[i]; if (ESTIMATE_TD) { addr_shift[reinterpret_cast<long>(para_Td[0])] = para_Td[0]; } vector<double *> parameter_blocks = marginalization_info->getParameterBlocks(addr_shift); if (last_marginalization_info) delete last_marginalization_info; last_marginalization_info = marginalization_info; last_marginalization_parameter_blocks = parameter_blocks; } }优化部分到此结束。
