吉林市教做网站,网站建设有哪些常用行为,阿里邮箱,wordpress 无法显示图片文章目录 0. 目标及思路1. 非线性优化求解器2. 基于VINS-MONO的Marginalization框架构建Hessian矩阵2.1 estimator.cpp移植2.2 solve.cpp/preMakeHessian()2.3 solve.cpp/makeHessian() 3. solve.cpp/solveLinearSystem()求解正规方程4. 更新状态5. 迭代求解6. EVO评估结果7. 待… 文章目录 0. 目标及思路1. 非线性优化求解器2. 基于VINS-MONO的Marginalization框架构建Hessian矩阵2.1 estimator.cpp移植2.2 solve.cpp/preMakeHessian()2.3 solve.cpp/makeHessian() 3. solve.cpp/solveLinearSystem()求解正规方程4. 更新状态5. 迭代求解6. EVO评估结果7. 待填的坑8. Reference9. Appendix9.1 estimator.cpp9.2 solve.cpp9.3 solve.h9.4 系统整体待优化参数维度debug9.5 LM的 λ \lambda λ初始化中的 τ \tau τ取值是否合适9.6 Schur消元求解之后更新先验的residual9.7 计算 χ e T W e \chie^TWe χeTWe时需要考虑loss_function9.8 先验的参数实际上就是V0,P0~P10,Tbc,td而不是一个单独的特殊量9.9 Hessian可视化9.10 load pose_graph 0. 目标及思路
完成VIO课程大作业T1 VINS-MONO使用Ceres的求解器在factor中实现了Jacobian block的构建为了探究非线性优化求解过程我们不使用Ceres手动完成求解整体思路如下
非线性优化求解器Hessian矩阵构建求解正规方程更新状态迭代求解EVO评估结果
以下章节将分别对各个部分进行详细介绍并在最后给出完整代码。
1. 非线性优化求解器
主要包括LM和DogLeg(DL)本文以LM为主进行讲解在LM实现的基础上DL方法按照论文实现即可较容易完成求解。
关于LM的介绍可以参考之前课程Ch3的博客论文可以参考[1]此处不再赘述。
这里强调一下在实现过程中遇到的最难解的问题关于 b b b的符号问题。 在一次迭代求解中我们的目标是求解正规方程 H Δ x − b \begin{align} H\Delta x-b \end{align} HΔx−b 然后更新 x x Δ x \begin{align} xx\Delta x \end{align} xxΔx 关于(1)中右边的 − b -b −b不同地方对于符号的定义不统一导致理解有偏差 b J T e bJ^Te bJTe是在marginalization_factor.cpp的MarginalizationInfo::marginalize()最后从Hessian中反解出来的但是在正规方程中右边是 − b -b −b所以我们后面再求解(1)之前构造b之后需要取一下负否则解出来的 Δ x \Delta x Δx要么非常大要么非常小(1e-30量级的更新不动 x x x)因为之前在这里卡了很久所以在这里先强调一下在第2部分中会结合代码讲解具体在哪里操作。
linearized_jacobians S_sqrt.asDiagonal() * saes2.eigenvectors().transpose();
linearized_residuals S_inv_sqrt.asDiagonal() * saes2.eigenvectors().transpose() * b;记录一下之前阅读Ceres LM源码debug的记录。
在ceres源码中可以找到答案在LevenbergMarquardtStrategy::ComputeStep()函数中有注释是这样的 ceres里面只要求传入Jacobian和residual内部求解的方程 ( A D ′ D ) d y b (ADD)dyb (AD′D)dyb而不是LM正规方程的形式 ( A λ I ) d x b (A\lambda I)dxb (AλI)dxbceres中的 D D D是根据Jacobian构建的一个与 λ \lambda λ有关的系数阵叠加到 A A A上这里不做详细介绍有兴趣的可以看看ceres的源码而我们自己构建的 b b b是 J T e J^Te JTe 所以之前求解的一直是 − Δ x -\Delta x −Δx按照 Δ x \Delta x Δx更新给 x x x属于是错误的方向那么 χ 2 \chi^2 χ2不下降也是正常的进一步地 ρ \rho ρ也就很那下降因为 x x x更新方向不对。至此找到了最根本的问题解决办法是在makeHessian()最后将b取负那么就是手动求解的正确的正规方程了。 Hessian_ A;b_ -b;接下来就是LM的一系列调参
LM初始化时的 τ \tau τ设为 1 e − 15 1e-15 1e−15优化退出条件delta_x_.squaredNorm() 1e-10 || false_cnt 6优化PCG(预处理共轭梯度法 preconditioned conjugate gradient method)求解速度 迭代次数设为Hessian_.rows()1迭代停止阈值设为double threshold 1e-5 * r0.norm()优化PCG对角线预处理和不完备的Cholesky预处理还未做参考博客 2. 基于VINS-MONO的Marginalization框架构建Hessian矩阵
2.1 estimator.cpp移植
手动构建Hessian的步骤其实在marg时已经有过所以我们直接借鉴marg部分的代码将其移植成整个系统的Hessian构建并加上我们的LM和DL整个代码结构如下添加了solver文件夹 marg部分第5篇博客讲过marg掉的变量实际上就是WINDOW[0]帧相关的待优化变量包括 P 0 , V 0 P_0,V_0 P0,V0和strat from [0]的landmark的观测marg的大致流程如下
以factor方式将各个参数块添加到problem中包括MarginalizationFactorIMUFactorProjectionTdFactor(ProjectionFactor)构建residual_block_info来待优化参数同时marg的变量。指定方式是通过drop_set调用addResidualBlockInfo函数将每一个ResidualBlockInfo添加到problem中调用preMarginalize()函数计算各个factor的Jacobian和residual调用marginalize()函数对待优化变量排序marg放前面remain放后面多线程构建Hessian矩阵运用Schur compliment执行marg得到marg后留下的先验从先验A中反解出该线性化点处的linearized_jacobians和linearized_residuals。addr_shift地址映射。
我们需要构建整个系统的Hessian与VINS-MONO的marg不同的是我们可以选择marg也可以选择不marg重点是需要明白我们这里求取Hessian的目的与VINS-MONO的marg不同
VINS-MONO的marg目的是为了求取marg之后留下的先验并不需要求解式(1)所以Schur compliment反解出linearized_jacobians和linearized_residuals之后marg的任务就完成了至于这个线性化点值为多少并不关心(当然也可以(1)求解(2)更新求出这个线性化点)。而我们现在的目的是为了求解出本次迭代优化之后的线性化点也就是我们的待优化变量所以式(1)(2)是我们需要在marg的基础上进一步往下走的。
理清了二者的区别我们的目标就具体很多了基于VINS-MONO的margHessian构建框架我们可以顺利地构建出整个系统的Hessian矩阵和b也就完成了式(1)的构建然后求解式(1)得 Δ x \Delta x Δx带入式(2)即可完成本次迭代。
接下来的核心任务
完成VINS-MONO的marg框架移植构建出Hessian矩阵求解式(1)
下面详细讲解marg的移植以下内容可根据Appendix中的相关代码来理解
新建solve.cppsolve.h照搬marginalization_factor.cpp/h的所有内容并封上自己的namespacesolve为了便于对比调试在estimator.cpp中加上宏隔离CERES_SOLVE用于区分使用Ceres求解和我们自己的手动求解。需要注意我们虽然是照搬marg部分但是我们修改的是后端求解不分所以是在求解部分而不是marg部分添加我们的代码如ceres部分添加prior residualbolck是
MarginalizationFactor *marginalization_factor new MarginalizationFactor(last_marginalization_info);
problem.AddResidualBlock(marginalization_factor, NULL,last_marginalization_parameter_blocks);我们则是
solve::ResidualBlockInfo *residual_block_info new solve::ResidualBlockInfo(marginalization_factor, NULL, last_marginalization_parameter_blocks, vectorint{});
solver.addResidualBlockInfo(residual_block_info);其他factor如法炮制。
需要指出我们在求解式(1)时有好几种实现方法其中一种是使用Schur消元利用Hessian的稀疏性加快式(1)的求解速度这就意味着我们需要指定需要作为 x m m x_{mm} xmm的部分通过drop_set来指定。由于landmark的Jacobian较为稀疏所以我们这里指定了WINDOW内的landmark为 x m m x_{mm} xmm如下所示
solve::ResidualBlockInfo *residual_block_info new solve::ResidualBlockInfo(f, loss_function, vectordouble*{para_Pose[imu_i], para_Pose[imu_j], para_Ex_Pose[0], para_Feature[feature_index]}, vectorint{3});
solver.addResidualBlockInfo(residual_block_info);为了表意更强我们将preMarginalize()和marginalize()改名为preMakeHessian()和makeHessian()功能大体不变分别是求Je和构造Hb。
estimator.cpp总体讲解完毕下面讲解Hessian的构建。 2.2 solve.cpp/preMakeHessian()
solve::ResidualBlockInfo组织了各个factor的信息其中最重要的是Evaluate()函数在Solver::preMakeHessian()会调用主要通过多态求解各个factor的Je每次更新完x之后就需要调用preMakeHessian()并重新makeHessian()。除此之外由于LM和DLza求解过程中如果 ρ 0 \rho0 ρ0会涉及到参数的回滚但是频繁地加减会造成精度下降所以对之前的参数备份进行备份在preMakeHessian()中还开了新的数据parameter_block_data_backup(实际上parameter_block_data也是够用的只是backup表意更强)。
2.3 solve.cpp/makeHessian()
整体部分和marg中一样只是我们这里仅仅只构建Hessian至于原来marg后面的Schur compliment我们放到求解中去做。这里需要注意我们最终构建出来了Hessian_和b_这里的b_即 J T e J^Te JTe跟(1)中差了个负号所以最后需要取个负在前面已经强调过了 Hessian_ A;b_ -b;LM到这里就可以结束了但是DOGLEG由于在迭代时需要Jacobian和residual所以我们需要在这里反解出Je反解出Je在我的机器上大约需要24ms左右耗时较长对于DL方法的迭代影响较大。这里应该有办法构建出J和e但是在VINS-MONO的marg的框架下我目前没想到太好的办法 //DOGLEG需反解出J和eif(method_solve::Solver::kDOGLEG) {TicToc t_solve_J;TicToc t_SelfAdjoint;Eigen::SelfAdjointEigenSolverEigen::MatrixXd saes2(A);//这一句24.3msROS_DEBUG(\nt_SelfAdjoint cost: %f ms, t_SelfAdjoint.toc());Eigen::VectorXd S Eigen::VectorXd((saes2.eigenvalues().array() eps).select(saes2.eigenvalues().array(), 0));Eigen::VectorXd S_sqrt S.cwiseSqrt();//开根号linearized_jacobians S_sqrt.asDiagonal() * saes2.eigenvectors().transpose();Eigen::VectorXd S_inv Eigen::VectorXd((saes2.eigenvalues().array() eps).select(saes2.eigenvalues().array().inverse(), 0));Eigen::VectorXd S_inv_sqrt S_inv.cwiseSqrt();linearized_residuals S_inv_sqrt.asDiagonal() * saes2.eigenvectors().real().transpose() * b;ROS_DEBUG(\nt_solve_J cost: %f ms, t_solve_J.toc());//25ms}3. solve.cpp/solveLinearSystem()求解正规方程
构建完Hessian和b之后就需要对式(1)进行求解此部分主要函数solveLinearSystem()。
3种求解思路
直接Hessian.inverse()使用PCG(预处理共轭梯度法 preconditioned conjugate gradient method)求解式(1)Schur消元PCG求解(采用)
第1种就不讲了直接调函数即可。 第2种使用PCG()迭代求解这里给出PCG的实现PCG拓展可以看这里
Eigen::MatrixXd Solver::pcgSolver(const MatXX A, const VecX b, int maxIter -1) {assert(A.rows() A.cols() PCG solver ERROR: A is not a square matrix);int rows b.rows();int n maxIter 0 ? rows : maxIter;VecX x(VecX::Zero(rows));MatXX M_inv A.diagonal().asDiagonal().inverse();//取对角线阵的逆矩阵VecX r0(b); // initial r b - A*0 bVecX z0 M_inv * r0;VecX p(z0);VecX w A * p;double r0z0 r0.dot(z0);double alpha r0z0 / p.dot(w);VecX r1 r0 - alpha * w;int i 0;double threshold 1e-5 * r0.norm(); //比例调大可以提高阈值放宽停止条件while (r1.norm() threshold i n) {i;VecX z1 M_inv * r1;double r1z1 r1.dot(z1);double belta r1z1 / r0z0;z0 z1;r0z0 r1z1;r0 r1;p belta * p z1;w A * p;alpha r1z1 / p.dot(w);x alpha * p;r1 - alpha * w;}ROS_DEBUG(\nPCG iter times: %d, n: %d, r1.norm(): %f, threshold: %f, i, n, r1.norm(), threshold);return x;
}第3种结合之前ResidualBlockInfo时指定的drop_set在求解时使用Schur消元求出舒尔补然后使用PCG求出delta_x_rr最后求出delta_x_mm组合即得整体delta_x_完成式(1)的求解经试验方法3的速度最快。
注意这里采用Schur compliment的方法要和VINS-MONO的marginalize()中的Schur compliment目的区分开VINS-MONO那里是为了求出prior information matrixSelfAdjointEigenSolver部分是为了反解出Je而这里是为了在Schur compliment实现消元的基础上进一步求解出整个delta_x_整体求解代码如下
/*Solve Hx b, we can use PCG iterative method or use sparse Cholesky*/
//TODO:使用PCG迭代而非SVD分解求解
void Solver::solveLinearSystem() {
//method1直接求逆求解
// delta_x_ Hessian_.inverse() * b_;
// delta_x_ H.ldlt().solve(b_);//method2Schur消元求解marg掉drop_set中的parameter
#ifdef USE_SCHUR//step1:Schur消元求//求解Hxb之前marg时不用求出△x只需要H所以不用对方程组求解但现在优化时需要求解出整个△xTicToc t_Schur_PCG;Eigen::MatrixXd Amm_solver 0.5 * (Hessian_.block(0, 0, m, m) Hessian_.block(0, 0, m, m).transpose());Eigen::VectorXd bmm_solver b_.segment(0, m);Eigen::MatrixXd Amr_solver Hessian_.block(0, m, m, n);Eigen::MatrixXd Arm_solver Hessian_.block(m, 0, n, m);Eigen::MatrixXd Arr_solver Hessian_.block(m, m, n, n);Eigen::VectorXd brr_solver b_.segment(m, n);//求Amm_solver^(-1)TicToc t_Amm_inv;Eigen::MatrixXd Amm_inv_solver Amm_solver.inverse();//SelfAdjointEigenSolver和直接求逆速度差不多ROS_DEBUG(\nt_Amm_inv cost: %f ms, t_Amm_inv.toc());Eigen::MatrixXd tmpA_solver Arm_solver * Amm_inv_solver;//step1: Schur补Eigen::MatrixXd Arr_schur Arr_solver - tmpA_solver * Amr_solver;Eigen::VectorXd brr_schur brr_solver - tmpA_solver * bmm_solver;ROS_DEBUG(here1);// step2: solve Arr_schur * delta_x_rr brr_schur
// method1:直接求逆
// Eigen::MatrixXd Arr_schur_inv Arr_schur.inverse();
// Eigen::VectorXd delta_x_rr Arr_schur_inv * brr_schur;// method2:使用PCG求解TicToc t_PCG_xrr;Eigen::VectorXd delta_x_rr pcgSolver(Arr_schur, brr_schur, Arr_schur.rows()1); //0.3msROS_DEBUG(\n t_PCG_xrr cost %f ms, t_PCG_xrr.toc());Eigen::VectorXd delta_x_mm Amm_inv_solver * (bmm_solver - Amr_solver * delta_x_rr);delta_x_.tail(n) delta_x_rr;delta_x_.head(m) delta_x_mm;memcpy(delta_x_array_, delta_x_.data(), sizeof(double) * (int)delta_x_.size());//转为数组ROS_DEBUG(\nin solveLinearSystem solve equation cost %f ms, t_Schur_PCG.toc());
#elseTicToc t_solve_equation;
// delta_x_ Hessian_.ldlt().solve(b_);int pcg_iter_num Hessian_.rows()1; // PCG迭代次数,原来给的是rows()*2delta_x_ pcgSolver(Hessian_, b_, pcg_iter_num); //0.3msROS_DEBUG(\nin solveLinearSystem solve equation cost %f ms, pcg_iter_num: %d, t_solve_equation.toc(), pcg_iter_num);//15msmemcpy(delta_x_array_, delta_x_.data(), sizeof(double) * (int)delta_x_.size());//转为数组供状态更新使用ROS_DEBUG_STREAM(\nhere3 solve complete, delta_x_.size() delta_x_.size() , delta_x_.norm() delta_x_.norm() , delta_x_.squaredNorm() delta_x_.squaredNorm() );ROS_DEBUG_STREAM(\ndelta_x_: delta_x_.transpose());
#endif
}4. 更新状态
完成式(1)的求解之后需要带入式(2)更新状态这里难点有2
按照VINS-MONO marg的数据管理方法来更新参数是pose部分由于有四元数需要特殊处理。LM和DL涉及到状态的回滚和备份。
相关函数
bool Solver::updateStates(double weight);//weight是LM strategy2时的权重默认传-1.0不加权
bool Solver::backupStates();//备份状态便于后面回滚
bool Solver::rollbackStates();//回滚状态变量
bool Solver::updatePose(const double *x, const double *delta, double *x_plus_delta);主要是一些地址的操作仔细一些就好看代码很好理解这里讲两点
在rollbackStates()中将状态变量备份到parameter_block_data_backup中便于后面回滚。注意memcpy()第3个参数len最好结合数据类型(这里是double)给定sizeof()地址或者直接给int数值都是不对的。
具体代码
bool Solver::updatePose(const double *x, const double *delta, double *x_plus_delta)
{Eigen::Mapconst Eigen::Vector3d _p(x);Eigen::Mapconst Eigen::Quaterniond _q(x 3);Eigen::Mapconst Eigen::Vector3d dp(delta);//数组转四元数Eigen::Quaterniond dq Utility::deltaQ(Eigen::Mapconst Eigen::Vector3d(delta 3));Eigen::MapEigen::Vector3d p(x_plus_delta);Eigen::MapEigen::Quaterniond q(x_plus_delta 3);//Jacobian和residual都是按照6维来处理的但是更新rotation时需要按照四元数来更新p _p dp;q (_q * dq).normalized();//四元数乘法并归一化return true;
}//只更新状态量pqvbabgλ注意prior不是状态量虽然在待优化变量中但是其residual是跟状态量有关Jacobian在一轮优化中不变
//这里更新状态的目的是因为计算chi时会用到residual而residual和状态量有关而先验的residual更新f f J*δxp其中δxpx-x0,也跟状态量x有关
//但是因为在先验factor在Evaluate时会计算residual所以不用手动更新只需要更新最核心的x即可。其他的factor相同。
bool Solver::updateStates(double weight) {int array_size 1000 (WINDOW_SIZE 1) * (SIZE_POSE SIZE_SPEEDBIAS) SIZE_POSE 1 100;double used_delta_x[array_size];if(weight ! -1.0)std::transform(delta_x_array_, delta_x_array_ array_size, used_delta_x, [weight](double x) { return x * weight; });//对delta_x加权elsememcpy(used_delta_x, delta_x_array_, sizeof(double) * array_size);//使用idx来找对应的paramdouble cur_x_array[1000 (WINDOW_SIZE 1) * (SIZE_POSE SIZE_SPEEDBIAS) SIZE_POSE 1 100];for (auto it : parameter_block_idx){const long addr it.first;const int idx it.second;const int tmp_param_block_size parameter_block_size[addr];
/* ROS_DEBUG_STREAM(\nidx: idx , tmp_param_block_size: tmp_param_block_size);*///保存一份待优化变量和delta_x进行数量级对比memcpy( cur_x_array[idx], reinterpret_castdouble *(addr), sizeof(double) *(int)SIZE_POSE);if(tmp_param_block_size SIZE_POSE) {updatePose(reinterpret_castdouble *(addr), delta_x_array_[idx], reinterpret_castdouble *(addr));//TODO:这个backup应该可以用parameter_block_data替代/* ROS_DEBUG_STREAM(\npose after update: tmp_pose.transpose());*/} else {Eigen::Mapconst Eigen::VectorXd x{parameter_block_data_backup[addr], tmp_param_block_size};Eigen::Mapconst Eigen::VectorXd delta_x{delta_x_array_[idx], tmp_param_block_size};Eigen::MapEigen::VectorXd x_plus_delta{reinterpret_castdouble *(addr), tmp_param_block_size};/*ROS_DEBUG_STREAM(\nother parameters before update: x_plus_delta.transpose() \ndelta_x: delta_x.transpose());*/x_plus_delta x delta_x;/*ROS_DEBUG_STREAM(\nother parameters after update: x_plus_delta.transpose());*/}}// 初始化Eigen向量Eigen::MapEigen::VectorXd cur_x(cur_x_array, mn);ROS_DEBUG_STREAM(\ncur_x: cur_x.transpose());preMakeHessian();//计算更新后的Jacobian和residualreturn true;
}//备份状态量
bool Solver::backupStates() {for (auto it : parameter_block_idx){const long addr it.first;const int tmp_param_block_size parameter_block_size[addr];memcpy(parameter_block_data_backup[addr], reinterpret_castdouble *(addr), sizeof(double) * (int)tmp_param_block_size);}return true;
}//回滚状态量
bool Solver::rollbackStates() {for (auto it : parameter_block_idx){const long addr it.first;const int tmp_param_block_size parameter_block_size[addr];memcpy(reinterpret_castdouble *(addr), parameter_block_data_backup[addr], sizeof(double) * (int)tmp_param_block_size);}preMakeHessian();//计算更新后的Jacobian和residualreturn true;
}5. 迭代求解
此部分就不赘述由于前面使用updateStates()已经对状态进行了更新所以真正的状态更新就更新之后是不回滚且备份当前状态简言之在updateStates(weight)之后调用backupStates()即实现真正的状态更新循环更新直至达到迭代停止条件 Δ x \Delta x Δx过小或者cost下降了很多或者其他。
6. EVO评估结果
完成所有iteration轮的迭代之后就完成了本次后端求解部分按照optimization()的整理流程接下来就是marginalizationaddr_shift这些我们就不讲了。
在estimator线程求解完成参数更新之后会发送topic给pose_graph线程在所有数据跑完之后pose_graph线程会存下待估计参数的值存为.csv文件我们使用evo工具、此文件、ground truth文件来对我们的系统进行评估在评估之前我们需要调整VINS-MONO的输出格式使其适配EVO参考以下博客
参考博客1 参考博客2 虽然更改了VINS的输出格式但是pose_graph保存的实际上是描述子和特征点这方面没改所以仍然可以load pose_graph
VINS输出数据类型转换 t n s , t x , t y , t z , R w , R x , R y , R z t_{ns},t_x,t_y, t_z,R_w,R_x,R_y,R_z tns,tx,ty,tz,Rw,Rx,Ry,Rz要转换为 t s , t x , t y , t z , R x , R y , R z , R w t_{s},t_x,t_y, t_z,R_x,R_y,R_z,R_w ts,tx,ty,tz,Rx,Ry,Rz,Rw 时间戳由 n s ns ns转为 s s s旋转四元数由 w , x , y , z w,x,y,z w,x,y,z顺序转为 x , y , z , w x,y,z,w x,y,z,w顺序。ground truth需要使用以下命令转为tum格式evo只支持tum格式的绘制
evo_traj euroc data.csv --save_as_tumevo评估命令
evo_ape tum /你的GroundTruth路径/data.tum /你的手写VINS输出路径/vins_result_loop.txt -vap --plot_modexyz --align --correct_scale最终evo的评估如下图所示 本文实验使用的是MH_01数据集evo时都有-a对比结果如下LM另外两种strategy还没仔细调参所先挖个坑 MH_05助教的RMSE精度比我高很多但是我用Ceres的LM和DL都跑不出来这么高的精度LM甚至不收敛(也可能是我没调好)。
简单对比
Ceres DL综合精度和计算实时性性能最优。就使用Ceres的体验来看DL无论是在速度还是精度方面应该都优于LM。基于我上面的实现的LM和DL通过调参都能收敛综合考虑精度和计算实时性LM的组3较好DL的组12较好DL主要是makeHessian中的反解太耗时不然可以优化更多轮数这也是需要解决的问题。
至于DOGLEG算法的实现可以完全按照[1]的3.3节来实现在LM的基础上实现DL很容易这里就不过多赘述。
至此T1整体工作已完成部分细节后面再细化。
7. 待填的坑
LM strategy12调参没调完。DL反解时间过长没有想到好/的构建Jacobian的方法。PCG的改进EVO多个结果比较如何进行
8. Reference
[1] Madsen, K., Nielsen, H. B., Tingleff, O. (2004). Methods for Non-Linear Least Squares Problems (2nd ed.). [2] Lourakis, M.I., Argyros, A.A. (2005). Is Levenberg-Marquardt the most efficient optimization algorithm for implementing bundle adjustment? Tenth IEEE International Conference on Computer Vision (ICCV’05) Volume 1, 2, 1526-1531 Vol. 2.
9. Appendix
整体代码
9.1 estimator.cpp
#include estimator.h
#include solver/solve.h//#define CERES_SOLVE
uint8_t strategy 3;//先定义为全局变量后面再优化Estimator::Estimator(): f_manager{Rs}
{ROS_INFO(init begins);clearState();
}//视觉测量残差的协方差矩阵
void Estimator::setParameter()
{for (int i 0; i NUM_OF_CAM; i){tic[i] TIC[i];ric[i] RIC[i];}f_manager.setRic(ric);//这里假设标定相机内参时的重投影误差△u1.5 pixel(Sigma)^(-1)(1.5/f * I(2x2))^(-1) (f/1.5 * I(2x2))ProjectionFactor::sqrt_info FOCAL_LENGTH / 1.5 * Matrix2d::Identity();ProjectionTdFactor::sqrt_info FOCAL_LENGTH / 1.5 * Matrix2d::Identity();td TD;
}void Estimator::clearState()
{for (int i 0; i WINDOW_SIZE 1; i){Rs[i].setIdentity();Ps[i].setZero();Vs[i].setZero();Bas[i].setZero();Bgs[i].setZero();dt_buf[i].clear();linear_acceleration_buf[i].clear();angular_velocity_buf[i].clear();if (pre_integrations[i] ! nullptr)delete pre_integrations[i];pre_integrations[i] nullptr;}for (int i 0; i NUM_OF_CAM; i){tic[i] Vector3d::Zero();ric[i] Matrix3d::Identity();}for (auto it : all_image_frame){if (it.second.pre_integration ! nullptr){delete it.second.pre_integration;it.second.pre_integration nullptr;}}solver_flag INITIAL;first_imu false,sum_of_back 0;sum_of_front 0;frame_count 0;solver_flag INITIAL;initial_timestamp 0;all_image_frame.clear();td TD;if (tmp_pre_integration ! nullptr)delete tmp_pre_integration;if (last_marginalization_info ! nullptr)delete last_marginalization_info;tmp_pre_integration nullptr;last_marginalization_info nullptr;last_marginalization_parameter_blocks.clear();f_manager.clearState();failure_occur 0;relocalization_info 0;drift_correct_r Matrix3d::Identity();drift_correct_t Vector3d::Zero();
}//IMU预积分IntegrationBase类IMU预积分具体细节
void Estimator::processIMU(double dt, const Vector3d linear_acceleration, const Vector3d angular_velocity)
{if (!first_imu){first_imu true;acc_0 linear_acceleration;//保存本次measurement中的第一帧IMU数据有啥用gyr_0 angular_velocity;}if (!pre_integrations[frame_count])//如果frame_count的积分为空则new一个预积分对象{pre_integrations[frame_count] new IntegrationBase{acc_0, gyr_0, Bas[frame_count], Bgs[frame_count]};}if (frame_count ! 0)//第0帧[0]没有预积分第[0]与第[1]帧之间才有预积分{pre_integrations[frame_count]-push_back(dt, linear_acceleration, angular_velocity);//调用IntegrationBase中定义的成员函数push_back保存变量并propagate预积分//if(solver_flag ! NON_LINEAR)tmp_pre_integration-push_back(dt, linear_acceleration, angular_velocity);dt_buf[frame_count].push_back(dt);//保存这两帧IMU之间的时间间隔用于预积分linear_acceleration_buf[frame_count].push_back(linear_acceleration);angular_velocity_buf[frame_count].push_back(angular_velocity);//IMU预积分(为什么这里要重新再算一遍push_back里面不是重新算过了吗为什么不直接把delta_p等结果拿出直接用)// 用IMU数据进行积分当积完一个measurement中所有IMU数据后就得到了对应图像帧在世界坐标系中的Ps、Vs、Rs这里为什么是相对于世界坐标系呢为什么不把关于world系的抽出来呢// 下面这一部分的积分在没有成功完成初始化时似乎是没有意义的因为在没有成功初始化时对IMU数据来说是没有世界坐标系的// 当成功完成了初始化后下面这一部分积分才有用它可以通过IMU积分得到滑动窗口中最新帧在世界坐标系中的P V Rint j frame_count;//到后面frame_count一直为window_size即10Vector3d un_acc_0 Rs[j] * (acc_0 - Bas[j]) - g;//为什么要有重力gVector3d un_gyr 0.5 * (gyr_0 angular_velocity) - Bgs[j];Rs[j] * Utility::deltaQ(un_gyr * dt).toRotationMatrix();Vector3d un_acc_1 Rs[j] * (linear_acceleration - Bas[j]) - g;Vector3d un_acc 0.5 * (un_acc_0 un_acc_1);//mid-point中值法计算aw在k~k1时刻内的测量值Ps[j] dt * Vs[j] 0.5 * dt * dt * un_acc;Vs[j] dt * un_acc;}acc_0 linear_acceleration;//更新本次预积分的初始值gyr_0 angular_velocity;
}//实现了视觉与IMU的初始化以及非线性优化的紧耦合
void Estimator::processImage(const mapint, vectorpairint, Eigen::Matrixdouble, 7, 1 image, const std_msgs::Header header)
{ROS_DEBUG(new image coming ------------------------------------------);ROS_DEBUG(Adding feature points %lu, image.size());// 把当前帧图像frame_count的特征点添加到f_manager.feature容器中// 计算第2最新帧与第3最新帧之间的平均视差当前帧是第1最新帧判断第2最新帧是否为KF// 在未完成初始化时如果窗口没有塞满那么是否添加关键帧的判定结果不起作用滑动窗口要塞满// 只有在滑动窗口塞满后或者初始化完成之后才需要滑动窗口此时才需要做关键帧判别根据第2最新关键帧是否为关键帧选择相应的边缘化策略if (f_manager.addFeatureCheckParallax(frame_count, image, td))marginalization_flag MARGIN_OLD;//如果第2新帧是KF则marg掉最老的一帧elsemarginalization_flag MARGIN_SECOND_NEW;//如果第二新帧不是KF则直接丢掉最新帧的视觉measurement并对IMU积分propogateROS_DEBUG(this frame is--------------------%s, marginalization_flag ? reject : accept);ROS_DEBUG(%s, marginalization_flag ? Non-keyframe : Keyframe);ROS_DEBUG(Solving %d, frame_count);ROS_DEBUG(number of feature: %d, f_manager.getFeatureCount());Headers[frame_count] header;ImageFrame imageframe(image, header.stamp.toSec());imageframe.pre_integration tmp_pre_integration;all_image_frame.insert(make_pair(header.stamp.toSec(), imageframe));//用于下一个measurement进行积分tmp_pre_integration new IntegrationBase{acc_0, gyr_0, Bas[frame_count], Bgs[frame_count]};//不知道关于外参的任何info需要标定if(ESTIMATE_EXTRINSIC 2){ROS_INFO(calibrating extrinsic param, rotation movement is needed);if (frame_count ! 0){// 找相邻两帧(bk, bk1)之间的tracking上的点构建一个pair所有pair是一个vector即corres(pondents),first前一帧的去畸变的归一化平面上的点second后一帧的// 要求it.start_frame frame_count_l it.endFrame() frame_count_rvectorpairVector3d, Vector3d corres f_manager.getCorresponding(frame_count - 1, frame_count);Matrix3d calib_ric;//旋转约束SVD分解求取Ric旋转外参//delta_q即qbk_bk1,是从k时刻积分到k1所以是qbk_bk1(从左往右读)if (initial_ex_rotation.CalibrationExRotation(corres, pre_integrations[frame_count]-delta_q, calib_ric)){ROS_WARN(initial extrinsic rotation calib success);ROS_WARN_STREAM(initial extrinsic rotation: endl calib_ric);ric[0] calib_ric;RIC[0] calib_ric;ESTIMATE_EXTRINSIC 1;}}}if (solver_flag INITIAL)// 需要初始化{if (frame_count WINDOW_SIZE)// 滑动窗口中塞满了才进行初始化(初始化并不影响KF的筛选KF筛选仍然使用视差10和tracked_num20来判断满足其一则是KF{bool result false;if( ESTIMATE_EXTRINSIC ! 2 (header.stamp.toSec() - initial_timestamp) 0.1) //确保有足够的frame参与初始化有外参且当前帧时间戳大于初始化时间戳0.1秒{result initialStructure();//执行视觉惯性联合初始化initial_timestamp header.stamp.toSec();}//初始化成功则进行一次非线性优化不成功则进行滑窗操作if(result){solver_flag NON_LINEAR;//求解solveOdometry();//重新三角化并后端求解slideWindow();ROS_DEBUG(Ps[0] addr: %ld, reinterpret_castlong(Ps[0]));f_manager.removeFailures();ROS_INFO(Initialization finish!);last_R Rs[WINDOW_SIZE];last_P Ps[WINDOW_SIZE];last_R0 Rs[0];last_P0 Ps[0];}elseslideWindow();}elseframe_count;//只在这里自增自增到WINDOW_SIZE(10)之后就不再自增了后面都是WINDOW_SIZE(10)即后面的优化都是需要进行marg的}else//flagNON_LINEAR,初始化完成需要求解后端{TicToc t_solve;solveOdometry();ROS_DEBUG(solver costs: %fms, t_solve.toc());// 以下5种情况会判定为fail// 1,2ba或bg过大// 3,4,5本次WINDOW内和上次WINDOW内的最后一帧pose(Tw_b[k])之间的relative pose的t或z或角度变化过大// fail之后会clear state并重启系统重新初始化if (failureDetection()){ROS_WARN(failure detection!);failure_occur 1;clearState();//所有buff预积分等都clearerasedeletesetParameter();//清零外参time offsetROS_WARN(system reboot!);return;}TicToc t_margin;slideWindow();//根据marg flag marg掉old或者2nd管理优化变量数据深度等ROS_DEBUG(Ps[0] addr: %ld, reinterpret_castlong(Ps[0]));f_manager.removeFailures();//去掉未三角化出正深度的landmarkROS_DEBUG(marginalization costs: %fms, t_margin.toc());// prepare output of VINS(本次优化且划窗之后保存WINDOW内的所有KF的translation)key_poses.clear();//slideWindow后最后两个Ps相同所以用11个数据无所谓for (int i 0; i WINDOW_SIZE; i)key_poses.push_back(Ps[i]);last_R Rs[WINDOW_SIZE];//保留这一WINDOW内的最新一帧的信息供下次failureDetection()使用last_P Ps[WINDOW_SIZE];last_R0 Rs[0];last_P0 Ps[0];}
}//执行视觉惯性联合初始化,包含两部分1. visual SfM2.visual和IMU的align(估计gyro biasscale重力细化RefineGravity)
bool Estimator::initialStructure()
{TicToc t_sfm;//check imu observibility{mapdouble, ImageFrame::iterator frame_it;Vector3d sum_g;//遍历window内所有的ImageFramefor (frame_it all_image_frame.begin(), frame_it; frame_it ! all_image_frame.end(); frame_it){double dt frame_it-second.pre_integration-sum_dt;//该帧总时间Vector3d tmp_g frame_it-second.pre_integration-delta_v / dt;//速度/时间加速度sum_g tmp_g;}Vector3d aver_g;aver_g sum_g * 1.0 / ((int)all_image_frame.size() - 1);//线加速度均值因为第一帧没有所以-1double var 0;for (frame_it all_image_frame.begin(), frame_it; frame_it ! all_image_frame.end(); frame_it){double dt frame_it-second.pre_integration-sum_dt;Vector3d tmp_g frame_it-second.pre_integration-delta_v / dt;var (tmp_g - aver_g).transpose() * (tmp_g - aver_g);//cout frame g tmp_g.transpose() endl;}var sqrt(var / ((int)all_image_frame.size() - 1));//求线加速度的标准差//ROS_WARN(IMU variation %f!, var);if(var 0.25)//如果加速度方差小于0.25则证明加速度波动较小证明IMU激励不够TODO这个0.25跟标定qcb旋转外参SVD的特征值的那个0.25有关系吗{ROS_INFO(IMU excitation not enouth!);//return false;}}// global sfmQuaterniond Q[frame_count 1];//存放window内所有帧相对____的pose T___iVector3d T[frame_count 1];//把window内所有id对应的所有feature都存到一个vectorSFMFeature中mapint, Vector3d sfm_tracked_points;vectorSFMFeature sfm_f;for (auto it_per_id : f_manager.feature)//feature是list元素是装了window内的所有该id的feature的vector即一个feature_id对应一个vector{int imu_j it_per_id.start_frame - 1;SFMFeature tmp_feature;tmp_feature.state false;//未被三角化tmp_feature.id it_per_id.feature_id;for (auto it_per_frame : it_per_id.feature_per_frame)//window内该id对应的所有的Matrixdouble, 7, 1{imu_j;Vector3d pts_j it_per_frame.point;tmp_feature.observation.push_back(make_pair(imu_j, Eigen::Vector2d{pts_j.x(), pts_j.y()}));//observation: 所有观测到该特征点的图像帧ID和图像坐标}sfm_f.push_back(tmp_feature);} Matrix3d relative_R;Vector3d relative_T;int l;//选择window内第一个满足“tracking数量20,平均视差30”的帧(l)与最新帧之间的relative pose是从最新帧到第l帧Tl_cur就是下面的Tw_curif (!relativePose(relative_R, relative_T, l)){ROS_INFO(Not enough features or parallax; Move device around);return false;}l_ l;//将l赋给成员便于外面查看l的帧数//求解SfM问题对窗口中每个图像帧求解sfm问题得到所有图像帧相对于参考帧l的旋转四元数Q、平移向量T和特征点坐标sfm_tracked_points。GlobalSFM sfm;if(!sfm.construct(frame_count 1, Q, T, l,relative_R, relative_T,sfm_f, sfm_tracked_points)){ROS_DEBUG(global SFM failed!);//如果初始化不成功就marg掉最老的帧(在all_image_frame中把最老的帧也删掉但是在MARGIN_SECOND_NEW时就不会删掉all_image_frame中的帧)marginalization_flag MARGIN_OLD;return false;}//solve pnp for all frame(直接用cv的库函数没有再使用ceres构建problem)// 由于并不是第一次视觉初始化就能成功此时图像帧数目有可能会超过滑动窗口的大小// 所以再视觉初始化的最后要求出滑动窗口外的帧的位姿// 最后把世界坐标系从帧l的相机坐标系转到帧l的IMU坐标系// 4.对于非滑动窗口的所有帧提供一个初始的R,T然后solve pnp求解posemapdouble, ImageFrame::iterator frame_it;mapint, Vector3d::iterator it;frame_it all_image_frame.begin( );//时间戳map映射ImgFrameImageFrame是里面有的所有id-features的map,features是paircamera_id, Mat7,1for (int i 0; frame_it ! all_image_frame.end( ); frame_it){// provide initial guesscv::Mat r, rvec, t, D, tmp_r;if((frame_it-first) Headers[i].stamp.toSec()) // all_image_frame与滑动窗口中对应的帧SfM阶段已经计算过无需再次计算{frame_it-second.is_key_frame true;// 滑动窗口中所有帧都是关键帧frame_it-second.R Q[i].toRotationMatrix() * RIC[0].transpose();// 根据各帧相机坐标系的姿态和外参得到用各帧IMU坐标系的姿态对应VINS Mono论文(2018年的期刊版论文)中的公式6。frame_it-second.T T[i];i;continue;}if((frame_it-first) Headers[i].stamp.toSec()){i;}// 为滑动窗口外的帧提供一个初始位姿Matrix3d R_inital (Q[i].inverse()).toRotationMatrix();//qwc^(-1)qcwVector3d P_inital - R_inital * T[i];cv::eigen2cv(R_inital, tmp_r);cv::Rodrigues(tmp_r, rvec);cv::eigen2cv(P_inital, t);frame_it-second.is_key_frame false;// 初始化时位于滑动窗口外的帧是非关键帧vectorcv::Point3f pts_3_vector;// 用于pnp解算的3D点vectorcv::Point2f pts_2_vector;// 用于pnp解算的2D点for (auto id_pts : frame_it-second.points) // 对于该帧中的特征点{int feature_id id_pts.first;// 特征点idfor (auto i_p : id_pts.second)// 由于可能有多个相机所以需要遍历。i_p对应着一个相机所拍图像帧的特征点信息{it sfm_tracked_points.find(feature_id);if(it ! sfm_tracked_points.end())//如果找到了已经Triangulation的,说明在sfm_tracked_points中找到了相应的3D点{// 记录该已被Triangulated的id特征点的3D位置Vector3d world_pts it-second;cv::Point3f pts_3(world_pts(0), world_pts(1), world_pts(2));pts_3_vector.push_back(pts_3);// 记录该id的特征点在该帧图像中的2D位置Vector2d img_pts i_p.second.head2();cv::Point2f pts_2(img_pts(0), img_pts(1));pts_2_vector.push_back(pts_2);}}}cv::Mat K (cv::Mat_double(3, 3) 1, 0, 0, 0, 1, 0, 0, 0, 1); if(pts_3_vector.size() 6){cout pts_3_vector size pts_3_vector.size() endl;ROS_DEBUG(Not enough points for solve pnp !);return false;}if (! cv::solvePnP(pts_3_vector, pts_2_vector, K, D, rvec, t, 1)) // pnp求解失败{ROS_DEBUG(solve pnp fail!);return false;}cv::Rodrigues(rvec, r);MatrixXd R_pnp,tmp_R_pnp;cv::cv2eigen(r, tmp_R_pnp);R_pnp tmp_R_pnp.transpose();//qwc qcw^(-1)MatrixXd T_pnp;cv::cv2eigen(t, T_pnp);T_pnp R_pnp * (-T_pnp);frame_it-second.R R_pnp * RIC[0].transpose(); // Tc0_ck * Tbc^(-1) Tc0_bk转到c0系下看bkframe_it-second.T T_pnp;}ROS_DEBUG_STREAM(\nhere l_: l_ \nKF[0] Rs[0]:\n all_image_frame[Headers[0].stamp.toSec()].R);if (visualInitialAlign())//视觉惯性对齐:bggc0sv的估计return true;else{ROS_INFO(misalign visual structure with IMU);return false;}}bool Estimator::visualInitialAlign()
{TicToc t_g;VectorXd x;//待优化变量[vk,vk1,w,s],维度是(all_image_frame.size() * 3 2 1)//估计陀螺仪的偏置速度、重力和尺度初始化重力细化bool result VisualIMUAlignment(all_image_frame, Bgs, g, x);if(!result){ROS_DEBUG(solve g failed!);return false;}//原文we can get the rotation qw c0 between the world frame and the//camera frame c0 by rotating the gravity to the z-axis. We then//rotate all variables from the reference frame (·)c0 to the world//frame (·)w.// change state(以下仅对WINDOW内的frame进行操作)for (int i 0; i frame_count; i){Matrix3d Ri all_image_frame[Headers[i].stamp.toSec()].R;//IMU相对于world(即c0,此时还是l帧)的pose:Tc0_b[k]Vector3d Pi all_image_frame[Headers[i].stamp.toSec()].T;//Rc0_b[k]Ps[i] Pi;Rs[i] Ri;all_image_frame[Headers[i].stamp.toSec()].is_key_frame true;}ROS_DEBUG_STREAM(\nhere l_: l_ \nKF Rs[0]:\n Rs[0]);//1.梳理一下此时all_image_frame[Headers[i].stamp.toSec()].RT都是Tc0_bk//所以Ps,Rs也都是Tc0_bk//将三角化出的深度均设为-1重新三角化VectorXd dep f_manager.getDepthVector();//获取WINDOW内所有成功Triangulated出深度的landmark求其逆深度for (int i 0; i dep.size(); i)dep[i] -1;f_manager.clearDepth(dep);//重新赋深度(都是-1)//triangulat on cam pose , no tic//平移tic未标定设为0Vector3d TIC_TMP[NUM_OF_CAM];for(int i 0; i NUM_OF_CAM; i)TIC_TMP[i].setZero();ric[0] RIC[0];f_manager.setRic(ric);f_manager.triangulate(Ps, (TIC_TMP[0]), (RIC[0]));//Ps是tc0_bk(里面要转为tc_ck使用)double s (x.tail1())(0);//取优化出的scale//gyro bias bg改变了需要重新IMU预积分for (int i 0; i WINDOW_SIZE; i){//对每两帧camera之间的IMU数据重新进行积分(每次积分的观测初值(acc_0,gyro_0)仍然使用之前保存的linearized_acc, linearized_gyro)pre_integrations[i]-repropagate(Vector3d::Zero(), Bgs[i]);}ROS_INFO_STREAM(TIC[0]:\n TIC[0].transpose());//2.这里将Ps转换为(c0)tb0_bkfor (int i frame_count; i 0; i--) {//论文式(6)看起来Rs应该是Rc0_bk(这个时候c0应该还没变为world所以应该是在恢复米制单位)Ps[i] s * Ps[i] - Rs[i] * TIC[0] - (s * Ps[0] - Rs[0] * TIC[0]);//这里输入的Ps还是tc0_bk,输出的Ps是(c0)tb0_bk是在c0系下看的这个translation//TIC[0]为0代表第一项 s * Ps[i] - Rs[i] * TIC[0]s*Ps[i]即s*tc0_b[k]s*tc0_c[k](因为此时Pstc0_b[k])ROS_INFO_STREAM(TIC[0]: TIC[0].transpose() \ns * Ps[i] - Rs[i] * TIC[0]: (s * Ps[i] - Rs[i] * TIC[0]).transpose() \ns * Ps[i]: (s * Ps[i]).transpose() \nl_: l_ \nPs[0]: Ps[0].transpose()//看他是否为0如果不是0则证明我把c0和c[0]弄混了 \ns * Ps[0]: (s * Ps[0]).transpose());}//速度深度处理int kv -1;mapdouble, ImageFrame::iterator frame_i;for (frame_i all_image_frame.begin(); frame_i ! all_image_frame.end(); frame_i){if(frame_i-second.is_key_frame){kv;Vs[kv] frame_i-second.R * x.segment3(kv * 3);//更新bk系下的速度Rc0_bk * (bk)vk (c0)vk}}for (auto it_per_id : f_manager.feature){it_per_id.used_num it_per_id.feature_per_frame.size();if (!(it_per_id.used_num 2 it_per_id.start_frame WINDOW_SIZE - 2))continue;it_per_id.estimated_depth * s;//恢复真实世界下尺度的深度}//g是world系下的重力向量Rs[0]是Rc0_b[0]ROS_DEBUG_STREAM(\nRs[0] is Rc0_b0:\n Rs[0]\nRbc^T:\n RIC[0].transpose());Matrix3d R0 Utility::g2R(g);//求出gc0-gw(0,0,1)的pitch和roll方向的旋转R0ROS_DEBUG_STREAM(\nhere1 R0.yaw \n Utility::R2ypr(R0).x());Eigen::Vector3d here1_Rs0_ypr Utility::R2ypr(Rs[0]);double here1_Rs0_yaw here1_Rs0_ypr.x();//Rs[0].yawdouble yaw Utility::R2ypr(R0 * Rs[0]).x();//和transformed_yaw相等说明不是运算精度的问题可能就是旋转之后yaw会受到影响R0 Utility::ypr2R(Eigen::Vector3d{-yaw, 0, 0}) * R0;ROS_DEBUG_STREAM(\nhere2 yaw :\n yaw \nRs[0].yaw :\n here1_Rs0_yaw \neventually, R0.yaw \n Utility::R2ypr(R0).x());g R0 * g;//将估计的重力g旋转到world系yaw * Rwc0*g^(c0)g^w//Matrix3d rot_diff R0 * Rs[0].transpose();Matrix3d rot_diff R0;//rotdiff最后使得在world系下b[0]真的yaw为0°//(PRV)w_b[k] Rw_b[0] * (PRV)c0_b[k]for (int i 0; i frame_count; i){Ps[i] rot_diff * Ps[i];Rs[i] rot_diff * Rs[i];//(w)vb0_bkVs[i] rot_diff * Vs[i];//(w)vb0_bkROS_DEBUG_STREAM(\ni i Rs[i].yaw \n Utility::R2ypr(Rs[i]).x());}ROS_DEBUG_STREAM(g0 g.transpose());ROS_DEBUG_STREAM(my R0 Utility::R2ypr(Rs[0]).transpose()); return true;
}//选择window内第一个满足tracking数量20,平均视差30的帧(l)与最新帧之间的relative pose是从最新帧到第l帧
bool Estimator::relativePose(Matrix3d relative_R, Vector3d relative_T, int l)
{// find previous frame which contians enough correspondance and parallex with newest frame//对应论文V.A节for (int i 0; i WINDOW_SIZE; i){vectorpairVector3d, Vector3d corres;// 找第i帧和buffer内最后一帧(i, WINDOW_SIZE),之间的tracking上的点构建一个pair// 所有pair是一个vector即corres(pondents),first前一帧的去畸变的归一化平面上的点second后一帧的corres f_manager.getCorresponding(i, WINDOW_SIZE);if (corres.size() 20)//要求两帧的共视点大于20对{double sum_parallax 0;double average_parallax;for (int j 0; j int(corres.size()); j){Vector2d pts_0(corres[j].first(0), corres[j].first(1));Vector2d pts_1(corres[j].second(0), corres[j].second(1));double parallax (pts_0 - pts_1).norm();//计算共视点的视差(欧氏距离)sum_parallax sum_parallax parallax;}average_parallax 1.0 * sum_parallax / int(corres.size());//平均视差//用内参将归一化平面上的点转化到像素平面fx*X/Z cxcx相减抵消z1所以直接就是fx*X//求的Rt是当前帧([WINDOW_SIZE]帧)到第l帧的坐标系变换Rl_[WINDOW_SIZE]if(average_parallax * 460 30 m_estimator.solveRelativeRT(corres, relative_R, relative_T)){l i;
// l l2;
// ROS_DEBUG(change l to l2 %d, l);ROS_DEBUG(average_parallax %f choose l %d and newest frame to triangulate the whole structure, average_parallax * 460, l);return true;}}}return false;
}void Estimator::solveOdometry()
{//需要在WINDOW满之后才进行求解没满之前初始化阶段pose由sfm等求解if (frame_count WINDOW_SIZE)return;//if (solver_flag NON_LINEAR){TicToc t_tri;//在optimize和marg在新的start_frame上重新三角化landmarkf_manager.triangulate(Ps, tic, ric);ROS_DEBUG(triangulation costs %f, t_tri.toc());optimization();}
}//vector转换成double数组因为ceres使用数值数组
//Ps、Rs转变成para_PoseVs、Bas、Bgs转变成para_SpeedBias
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;
}// 优化一次之后求出优化前后的第一帧的T用于后面作用到所有的轨迹上去
// 数据转换vector2double的相反过程
// 同时这里为防止优化结果往零空间变化会根据优化前后第一帧的位姿差进行修正。
void Estimator::double2vector()
{//窗口第一帧优化前的位姿Vector3d origin_R0 Utility::R2ypr(Rs[0]);//R[0]Vector3d origin_P0 Ps[0];if (failure_occur){origin_R0 Utility::R2ypr(last_R0);origin_P0 last_P0;failure_occur 0;}//窗口第一帧优化后的位姿 q(wxyz)Vector3d origin_R00 Utility::R2ypr(Quaterniond(para_Pose[0][6],para_Pose[0][3],para_Pose[0][4],para_Pose[0][5]).toRotationMatrix());//(R_before_after).yaw转到被减变换到before//TODO确定到底是哪个 若是R_after_before.x()则下面使用rot_diff做的矫正就不对了,para_Pose肯定是after的double y_diff origin_R0.x() - origin_R00.x();//TODO了解欧拉角奇异点Matrix3d rot_diff Utility::ypr2R(Vector3d(y_diff, 0, 0));if (abs(abs(origin_R0.y()) - 90) 1.0 || abs(abs(origin_R00.y()) - 90) 1.0){ROS_DEBUG(euler singular point!);rot_diff Rs[0] * Quaterniond(para_Pose[0][6],para_Pose[0][3],para_Pose[0][4],para_Pose[0][5]).toRotationMatrix().transpose();}// 根据位姿差做修正即保证第一帧优化前后位姿不变(似乎只是yaw不变那tilt呢)for (int i 0; i WINDOW_SIZE; i){Rs[i] rot_diff * Quaterniond(para_Pose[i][6], para_Pose[i][3], para_Pose[i][4], para_Pose[i][5]).normalized().toRotationMatrix();Ps[i] rot_diff * Vector3d(para_Pose[i][0] - para_Pose[0][0],para_Pose[i][1] - para_Pose[0][1],para_Pose[i][2] - para_Pose[0][2]) origin_P0;Vs[i] rot_diff * Vector3d(para_SpeedBias[i][0],para_SpeedBias[i][1],para_SpeedBias[i][2]);Bas[i] Vector3d(para_SpeedBias[i][3],para_SpeedBias[i][4],para_SpeedBias[i][5]);Bgs[i] Vector3d(para_SpeedBias[i][6],para_SpeedBias[i][7],para_SpeedBias[i][8]);}//外参for (int i 0; i NUM_OF_CAM; i){tic[i] Vector3d(para_Ex_Pose[i][0],para_Ex_Pose[i][1],para_Ex_Pose[i][2]);ric[i] Quaterniond(para_Ex_Pose[i][6],para_Ex_Pose[i][3],para_Ex_Pose[i][4],para_Ex_Pose[i][5]).toRotationMatrix();}//转为逆深度并置flagVectorXd dep f_manager.getDepthVector();for (int i 0; i f_manager.getFeatureCount(); i)dep(i) para_Feature[i][0];f_manager.setDepth(dep);//time offsetif (ESTIMATE_TD)td para_Td[0][0];// relative info between two loop frameif(relocalization_info){//按照WINDOW内第一帧的yaw角变化对j帧进行矫正Matrix3d relo_r;//j帧矫正之后的TVector3d relo_t;relo_r rot_diff * Quaterniond(relo_Pose[6], relo_Pose[3], relo_Pose[4], relo_Pose[5]).normalized().toRotationMatrix();relo_t rot_diff * Vector3d(relo_Pose[0] - para_Pose[0][0],relo_Pose[1] - para_Pose[0][1],relo_Pose[2] - para_Pose[0][2]) origin_P0;//保证第[0]帧不变之后origin_P0转为世界系下的t//由于pitch和roll是可观的所以我们在BA中一直都在优化pitch和roll但由于yaw不可观//所以即使漂了可能还是满足我们BA的最优解所以需要手动进行矫正//prev_relo_rTw1_bi, relo_PoseTw2_bi这两个pose间的yaw和t的漂移Rw1_w2,tw1_w2double drift_correct_yaw;//yaw drift, Rw1_bi.yaw() - Rw2_bi.yawRw1_w2.yaw()drift_correct_yaw Utility::R2ypr(prev_relo_r).x() - Utility::R2ypr(relo_r).x();drift_correct_r Utility::ypr2R(Vector3d(drift_correct_yaw, 0, 0));//tw1_w2drift_correct_t prev_relo_t - drift_correct_r * relo_t;//Tw2_bi^(-1) * Tw2_bj Tbi_bjrelo_relative_t relo_r.transpose() * (Ps[relo_frame_local_index] - relo_t);relo_relative_q relo_r.transpose() * Rs[relo_frame_local_index];//Rw2_bj.yaw() - Rw2_bi.yaw() Rbi_bj.yaw()relo_relative_yaw Utility::normalizeAngle(Utility::R2ypr(Rs[relo_frame_local_index]).x() - Utility::R2ypr(relo_r).x());/* //验证Tw1w2是否正确。 结果不一样不知道为啥。//1.计算Rw1_w2 Rw1_bi * Rw2_bi^(-1)Matrix3d Rw1_w2 prev_relo_r * relo_r;//2. 计算Tw1_w2中的Rw1_w2 Tw1_bi.R * Tbi_bj.R * Rw2_bj^(-1)Matrix3d Rw1_w2_prime prev_relo_r * relo_relative_q.toRotationMatrix() * Rs[relo_frame_local_index].transpose();ROS_DEBUG_STREAM(\ncheck Rw1_w2:\n Rw1_w2 \nRw1_w2_prime:\n Rw1_w2_prime);*///cout vins relo endl;//cout vins relative_t relo_relative_t.transpose() endl;//cout vins relative_yaw relo_relative_yaw endl;relocalization_info 0;}
}bool Estimator::failureDetection()
{//最后一帧tracking上的点的数量是否足够多if (f_manager.last_track_num 2){ROS_INFO( little feature %d, f_manager.last_track_num);//return true;}//ba和bg都不应过大if (Bas[WINDOW_SIZE].norm() 2.5){ROS_INFO( big IMU acc bias estimation %f, Bas[WINDOW_SIZE].norm());return true;}if (Bgs[WINDOW_SIZE].norm() 1.0){ROS_INFO( big IMU gyr bias estimation %f, Bgs[WINDOW_SIZE].norm());return true;}/*if (tic(0) 1){ROS_INFO( big extri param estimation %d, tic(0) 1);return true;}*///在world系下的pose的t和z变化如果过大则认为failVector3d tmp_P Ps[WINDOW_SIZE];if ((tmp_P - last_P).norm() 5){ROS_INFO( big translation);return true;}if (abs(tmp_P.z() - last_P.z()) 1){ROS_INFO( big z translation);return true;}//relative pose过大则fail//求误差的角度大小对四元数表示的旋转delta q有//delta q [1, 1/2 delta theta]//delta theta [delta q]_xyz * 2弧度制视情况转为degreeMatrix3d tmp_R Rs[WINDOW_SIZE];Matrix3d delta_R tmp_R.transpose() * last_R;Quaterniond delta_Q(delta_R);double delta_angle;delta_angle acos(delta_Q.w()) * 2.0 / 3.14 * 180.0;//转为degreeif (delta_angle 50){ROS_INFO( big delta_angle );//return true;}return false;
}//获得当前优化参数按照自定义solver中的排列方式排列
static void get_cur_parameter(solve::Solver solver, double* cur_x_array) {for (auto it : solver.parameter_block_idx) {const long addr it.first;const int idx it.second;const int tmp_param_block_size solver.parameter_block_size[addr];ROS_ASSERT_MSG(tmp_param_block_size 0, tmp_param_block_size %d, tmp_param_block_size);memcpy( cur_x_array[idx], reinterpret_castdouble *(addr), sizeof(double) *(int)tmp_param_block_size);}
}static bool updatePose(const double *x, const double *delta, double *x_plus_delta)
{Eigen::Mapconst Eigen::Vector3d _p(x);Eigen::Mapconst Eigen::Quaterniond _q(x 3);Eigen::Mapconst Eigen::Vector3d dp(delta);Eigen::Quaterniond dq Utility::deltaQ(Eigen::Mapconst Eigen::Vector3d(delta 3));Eigen::MapEigen::Vector3d p(x_plus_delta);Eigen::MapEigen::Quaterniond q(x_plus_delta 3);p -_p dp ;q (_q.inverse() * dq).normalized();//四元数乘法并归一化return true;
}//计算ceres优化iteration轮之后的delta_x, solver要传引用否则会调用析构函数
static void cal_delta_x(solve::Solver solver, double* x_before, double* x_after, double* delta_x) {for (auto it : solver.parameter_block_idx) {const long addr it.first;const int idx it.second;const int tmp_param_block_size solver.parameter_block_size[addr];double tmp_delta_pose_array[SIZE_POSE];ROS_DEBUG_STREAM(\nidx: idx , tmp_param_block_size: tmp_param_block_size);
// ROS_DEBUG_STREAM(\ndelta_x size: delta_x.size());if (tmp_param_block_size SIZE_POSE) {updatePose(x_after[idx], x_before[idx], delta_x[idx]);} else {Eigen::Mapconst Eigen::VectorXd x_map(x_before[idx], tmp_param_block_size);Eigen::Mapconst Eigen::VectorXd x_plus_delta_map(x_after[idx], tmp_param_block_size);Eigen::MapEigen::VectorXd delta_x_map(delta_x[idx], tmp_param_block_size);delta_x_map x_plus_delta_map - x_map;
// ROS_DEBUG_STREAM(\ndelta_x_map: delta_x_map.transpose());}}
}//后端非线性优化
//大作业T1.a思路 这里要添加自己的makehessian的代码AddResidualBlockSolver()//类似于marg一样管理所有的factor只不过这里的m是WINDOW内所有的landmarkn是所有的PVTbctdrelopose
//管理方式也是地址-idx,地址-size一样在添加的时候指定landmark的drop_set为valid剩下的为非valid
//在最后求解出整个delta x在solve中用LM评估迭代效果并继续迭代
void Estimator::optimization()
{ceres::LossFunction *loss_function;//loss_function new ceres::HuberLoss(1.0);//Huber损失函数loss_function new ceres::CauchyLoss(1.0);//柯西损失函数ceres::Problem problem;//自己写的solversolve::Solver solver(strategy);solver.method_ solve::Solver::kDOGLEG;solver.iterations_ NUM_ITERATIONS;solver.makeHessian_time_sum_ (makeHessian_time_sum_);solver.makeHessian_times_ makeHessian_times_;if(solver.method_solve::Solver::kDOGLEG) {solver.epsilon_1_ 1e-10;solver.epsilon_2_ 1e-6;//h_dl和radius_减小的倍数阈值solver.epsilon_3_ 1e-10;}#ifdef CERES_SOLVE//添加ceres参数块//因为ceres用的是double数组所以在下面用vector2double做类型装换//Ps、Rs转变成para_PoseVs、Bas、Bgs转变成para_SpeedBiasfor (int i 0; i WINDOW_SIZE 1; i){ceres::LocalParameterization *local_parameterization new PoseLocalParameterization();problem.AddParameterBlock(para_Pose[i], SIZE_POSE, local_parameterization);//ceres里叫参数块g2o里是顶点和边problem.AddParameterBlock(para_SpeedBias[i], SIZE_SPEEDBIAS);}//ESTIMATE_EXTRINSIC!0则camera到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]);}elseROS_DEBUG(estimate extinsic param);}//相机和IMU硬件不同步时估计两者的时间偏差if (ESTIMATE_TD){problem.AddParameterBlock(para_Td[0], 1);//problem.SetParameterBlockConstant(para_Td[0]);}#else//自己写的solver如何固定住外参呢不加入ResidualBlockInfo即可
#endifTicToc t_whole, t_prepare;vector2double();//用于check维度std::unordered_maplong, uint8_t param_addr_check;//所有param维度std::unordered_maplong, uint8_t landmark_addr_check;//landmark维度//1.添加边缘化残差先验部分size_t size_10;if (last_marginalization_info){// construct new marginlization_factorMarginalizationFactor *marginalization_factor new MarginalizationFactor(last_marginalization_info);//里面设置了上次先验的什么size现在还不懂#ifdef CERES_SOLVEproblem.AddResidualBlock(marginalization_factor, NULL,last_marginalization_parameter_blocks);// /*用于check维度是否正确*/for(int i0; ilast_marginalization_parameter_blocks.size(); i) {size_t tmp_size last_marginalization_info-parameter_block_size[reinterpret_castlong(last_marginalization_parameter_blocks[i])];tmp_size tmp_size7 ? 6: tmp_size;//这个double*的地址代表的待优化变量的local_size把每个地址都记录在map中分配给待优化变量的地址都是连续的for(int j0; jtmp_size; j) {param_addr_check[reinterpret_castlong(last_marginalization_parameter_blocks[i]) (double)j * (long) sizeof(long)] 1;}}//打印prior的Jacobian维度ROS_DEBUG(\nlinearized_jacobians (rows, cols) (%lu, %lu),last_marginalization_info-linearized_jacobians.rows(), last_marginalization_info-linearized_jacobians.cols());size_1 param_addr_check.size();//76ROS_DEBUG(\nprior size1%lu, param_addr_check.size() %lu, landmark size: %lu, except landmark size %lu,size_1, param_addr_check.size(), landmark_addr_check.size(), param_addr_check.size()-landmark_addr_check.size());//landmark_addr_check中多加了个td#else//dropset用于指定求解时需要Schur消元的变量即landmarksolve::ResidualBlockInfo *residual_block_info new solve::ResidualBlockInfo(marginalization_factor, NULL,last_marginalization_parameter_blocks,vectorint{});solver.addResidualBlockInfo(residual_block_info);
#endif}//2.添加IMU残差for (int i 0; i WINDOW_SIZE; i){int j i 1;//两帧KF之间IMU积分时间过长的不进行优化可能lostif (pre_integrations[j]-sum_dt 10.0)continue;IMUFactor* imu_factor new IMUFactor(pre_integrations[j]);//这里的factor就是残差residualceres里面叫factor#ifdef CERES_SOLVEproblem.AddResidualBlock(imu_factor, NULL, para_Pose[i], para_SpeedBias[i], para_Pose[j], para_SpeedBias[j]);//check维度long addr reinterpret_castlong(para_Pose[i]);if(param_addr_check.find(addr) param_addr_check.end()) {ROS_DEBUG(\nIMU add para_Pose[%d], i);for(int k0; kSIZE_POSE-1; k) {param_addr_check[addr (long)k * (long)sizeof(long)] 1;}}addr reinterpret_castlong(para_SpeedBias[i]);if(param_addr_check.find(addr) param_addr_check.end()) {ROS_DEBUG(\nIMU add para_SpeedBias[%d], i);for(int k0; kSIZE_SPEEDBIAS; k) {param_addr_check[addr (long) k * (long) sizeof(long)] 1;}}addr reinterpret_castlong(para_Pose[j]);if(param_addr_check.find(addr) param_addr_check.end()) {ROS_DEBUG(\n IMU add para_Pose[%d], j);for(int k0; kSIZE_POSE-1; k) {param_addr_check[addr (long) k * (long) sizeof(long)] 1;}}addr reinterpret_castlong(para_SpeedBias[j]);if(param_addr_check.find(addr) param_addr_check.end()) {ROS_DEBUG(\n IMU add para_SpeedBias[%d], j);for (int k 0; k SIZE_SPEEDBIAS; k) {param_addr_check[addr (long) k * (long) sizeof(long)] 1;}}
#elsesolve::ResidualBlockInfo *residual_block_info new solve::ResidualBlockInfo(imu_factor, NULL,vectordouble *{para_Pose[i], para_SpeedBias[i], para_Pose[j], para_SpeedBias[j]},vectorint{});solver.addResidualBlockInfo(residual_block_info);
#endif}
#ifdef CERES_SOLVEsize_t size_2 param_addr_check.size() - size_1;//96ROS_DEBUG(\nIMU size2%lu, param_addr_check.size() %lu, landmark size: %lu, except landmark size %lu,size_2, param_addr_check.size(), landmark_addr_check.size(), param_addr_check.size()-landmark_addr_check.size());//landmark_addr_check中多加了个td
#endif//3.添加视觉残差int f_m_cnt 0;int feature_index -1;for (auto it_per_id : f_manager.feature){it_per_id.used_num it_per_id.feature_per_frame.size();//!(至少两次tracking且最新帧1st的tracking不能算因为1st可能被marg掉start_frame最大是[7])if (!(it_per_id.used_num 2 it_per_id.start_frame WINDOW_SIZE - 2))continue;feature_index;int imu_i it_per_id.start_frame, imu_j imu_i - 1;Vector3d pts_i it_per_id.feature_per_frame[0].point;//这个id的feature的第一帧和后面所有的帧分别构建residual blockfor (auto it_per_frame : it_per_id.feature_per_frame){imu_j;if (imu_i imu_j){continue;}Vector3d pts_j it_per_frame.point;//是否要time offsetif (ESTIMATE_TD){//对于一个feature都跟[it_per_id.start_frame]帧进行优化ProjectionTdFactor *f_td new ProjectionTdFactor(pts_i, pts_j, it_per_id.feature_per_frame[0].velocity, it_per_frame.velocity,it_per_id.feature_per_frame[0].cur_td, it_per_frame.cur_td,it_per_id.feature_per_frame[0].uv.y(), it_per_frame.uv.y());
#ifdef CERES_SOLVEproblem.AddResidualBlock(f_td, loss_function, para_Pose[imu_i], para_Pose[imu_j], para_Ex_Pose[0], para_Feature[feature_index], para_Td[0]);//check维度for(int k0; kSIZE_POSE-1; k) {param_addr_check[reinterpret_castlong(para_Pose[imu_i]) (long)k * (long)sizeof(long)] 1;}for(int k0; kSIZE_POSE-1; k) {param_addr_check[reinterpret_castlong(para_Pose[imu_j]) (long)k * (long)sizeof(long)] 1;}for(int k0; kSIZE_POSE-1; k) {param_addr_check[reinterpret_castlong(para_Ex_Pose[0]) (long)k * (long)sizeof(long)] 1;}param_addr_check[reinterpret_castlong(para_Feature[feature_index])] 1;landmark_addr_check[reinterpret_castlong(para_Feature[feature_index])] 1;param_addr_check[reinterpret_castlong(para_Td[0])] 1;/*double **para new double *[5];para[0] para_Pose[imu_i];para[1] para_Pose[imu_j];para[2] para_Ex_Pose[0];para[3] para_Feature[feature_index];para[4] para_Td[0];f_td-check(para);*/
#elsesolve::ResidualBlockInfo *residual_block_info new solve::ResidualBlockInfo(f_td, loss_function,vectordouble*{para_Pose[imu_i], para_Pose[imu_j], para_Ex_Pose[0], para_Feature[feature_index], para_Td[0]},vectorint{3});solver.addResidualBlockInfo(residual_block_info);
#endif}else{ProjectionFactor *f new ProjectionFactor(pts_i, pts_j);
#ifdef CERES_SOLVEproblem.AddResidualBlock(f, loss_function, para_Pose[imu_i], para_Pose[imu_j], para_Ex_Pose[0], para_Feature[feature_index]);//check维度for(int k0; kSIZE_POSE-1; k) {param_addr_check[reinterpret_castlong(para_Pose[imu_i]) (long)k * (long)sizeof(long)] 1;}for(int k0; kSIZE_POSE-1; k) {param_addr_check[reinterpret_castlong(para_Pose[imu_j]) (long)k * (long)sizeof(long)] 1;}for(int k0; kSIZE_POSE-1; k) {param_addr_check[reinterpret_castlong(para_Ex_Pose[0]) (long)k * (long)sizeof(long)] 1;}param_addr_check[reinterpret_castlong(para_Feature[feature_index])] 1;landmark_addr_check[reinterpret_castlong(para_Feature[feature_index])] 1;
#elsesolve::ResidualBlockInfo *residual_block_info new solve::ResidualBlockInfo(f, loss_function,vectordouble*{para_Pose[imu_i], para_Pose[imu_j], para_Ex_Pose[0], para_Feature[feature_index]},vectorint{3});solver.addResidualBlockInfo(residual_block_info);
#endif}f_m_cnt;}}#ifdef CERES_SOLVEsize_t size_3 param_addr_check.size() - size_1 - size_2;//应该和landmark_addr_check.size一样ROS_DEBUG(\nvisual size3%lu, param_addr_check.size() %lu, landmark size: %lu, except landmark size %lu,size_3, param_addr_check.size(), landmark_addr_check.size(), param_addr_check.size()-landmark_addr_check.size());//landmark_addr_check中多加了个td
#endifROS_DEBUG(visual measurement count: %d, f_m_cnt);//总的视觉观测个数观测可能是在不同帧对同一个landmark进行观测所以可能查过1000注意与landmark个数进行区分ROS_DEBUG(prepare for ceres: %f, t_prepare.toc());//4.添加闭环检测残差计算滑动窗口中与每一个闭环关键帧的相对位姿这个相对位置是为后面的图优化(pose graph)准备 或者是 快速重定位(崔华坤PDF7.2节)//这里注意relo_pose是Tw2_bi Tw2_w1 * Tw1_biif(relocalization_info){ROS_DEBUG(\nhas relocation blocks);//printf(set relocalization factor! \n);
#ifdef CERES_SOLVEceres::LocalParameterization *local_parameterization new PoseLocalParameterization();problem.AddParameterBlock(relo_Pose, SIZE_POSE, local_parameterization);
#endifint retrive_feature_index 0;int feature_index -1;for (auto it_per_id : f_manager.feature){ROS_DEBUG(\nfeature_id: %d, it_per_id.feature_id);it_per_id.used_num it_per_id.feature_per_frame.size();if (!(it_per_id.used_num 2 it_per_id.start_frame WINDOW_SIZE - 2))continue;feature_index;int start it_per_id.start_frame;ROS_DEBUG(\nmatch_points size: %lu, retrive_feature_index: %d, match_points.size(), retrive_feature_index);if(start relo_frame_local_index)//必须之前看到过{//1.先在i中match的点中找到可能是现在这个feature的id的indexwhile((int)match_points[retrive_feature_index].z() it_per_id.feature_id retrive_feature_index match_points.size()-2)//.z()存的是ij两帧match上的feature的id{retrive_feature_index;}ROS_DEBUG(\nrelo here1, retrive_feature_index: %d, retrive_feature_index);//2.如果是则WINDOW内的it_per_id.feature_id这个id的landmark就是被loop上的landmark,取归一化坐标if((int)match_points[retrive_feature_index].z() it_per_id.feature_id){//pts_j是i帧的归一化平面上的点这里理解relo_Pose及其重要relo_Pose实际上是Tw2_bi视觉重投影是从WINDOW内的start帧的camera(在w2系下)投影到i帧(在w1系下)耦合了Tw1_w2Vector3d pts_j Vector3d(match_points[retrive_feature_index].x(), match_points[retrive_feature_index].y(), 1.0);Vector3d pts_i it_per_id.feature_per_frame[0].point;//start中的点ProjectionFactor *f new ProjectionFactor(pts_i, pts_j);//relo_Pose是Tw2_bi
#ifdef CERES_SOLVEproblem.AddResidualBlock(f, loss_function, para_Pose[start], relo_Pose, para_Ex_Pose[0], para_Feature[feature_index]);//check维度for(int k0; kSIZE_POSE-1; k) {param_addr_check[reinterpret_castlong(para_Pose[start]) (long)k * (long)sizeof(long)] 1;}for(int k0; kSIZE_POSE-1; k) {param_addr_check[reinterpret_castlong(relo_Pose) (long)k * (long)sizeof(long)] 1;}for(int k0; kSIZE_POSE-1; k) {param_addr_check[reinterpret_castlong(para_Ex_Pose[0]) (long)k * (long)sizeof(long)] 1;}param_addr_check[reinterpret_castlong(para_Feature[feature_index])] 1;landmark_addr_check[reinterpret_castlong(para_Feature[feature_index])] 1;
#elseROS_DEBUG(\nrelo here2);solve::ResidualBlockInfo *residual_block_info new solve::ResidualBlockInfo(f, loss_function,vectordouble*{para_Pose[start], relo_Pose, para_Ex_Pose[0], para_Feature[feature_index]},vectorint{});solver.addResidualBlockInfo(residual_block_info);
#endifretrive_feature_index;ROS_DEBUG(\nrelo here3);}}}ROS_DEBUG(\nrelo here4);}
#ifdef CERES_SOLVEsize_t size_4 param_addr_check.size() - size_1 - size_2 - size_3;//没有loop时应该为0ROS_DEBUG(\nrelocation size_4%lu, param_addr_check.size() %lu, landmark size: %lu, except landmark size %lu,size_4, param_addr_check.size(), landmark_addr_check.size(), param_addr_check.size()-landmark_addr_check.size());//landmark_addr_check中多加了个tdceres::Solver::Options options;options.linear_solver_type ceres::DENSE_SCHUR;//options.num_threads 2;
// options.trust_region_strategy_type ceres::DOGLEG;//狗腿算法与LM较为接近options.trust_region_strategy_type ceres::LEVENBERG_MARQUARDT;//LMoptions.max_num_iterations NUM_ITERATIONS;//options.use_explicit_schur_complement true;//options.minimizer_progress_to_stdout true;//options.use_nonmonotonic_steps true;if (marginalization_flag MARGIN_OLD)options.max_solver_time_in_seconds SOLVER_TIME * 4.0 / 5.0;elseoptions.max_solver_time_in_seconds SOLVER_TIME;TicToc t_solver;ceres::Solver::Summary summary;/* //获得idx和datasolver.preMakeHessian();solver.makeHessian();ROS_DEBUG(delta1);int cur_x_size 1000 (WINDOW_SIZE 1) * (SIZE_POSE SIZE_SPEEDBIAS) SIZE_POSE 1 100;double cur_x_array[cur_x_size], cur_x_array_before[cur_x_size];get_cur_parameter(solver, cur_x_array);memcpy(cur_x_array_before, cur_x_array, sizeof(double) * cur_x_size);Eigen::MapEigen::VectorXd cur_x(cur_x_array, solver.m solver.n);//cur_x_array变了cur_x才会变const Eigen::VectorXd cur_x_before cur_x;*/ROS_DEBUG(delta2);ceres::Solve(options, problem, summary);ROS_DEBUG(delta3);/* get_cur_parameter(solver, cur_x_array);double delta_x_ceres[cur_x_size];Eigen::MapEigen::VectorXd delta_x_ceres_map(delta_x_ceres, solver.m solver.n);cal_delta_x(solver, cur_x_array_before, cur_x_array, delta_x_ceres);ROS_DEBUG_STREAM(\ncur_x before: cur_x_before.transpose() \ncur_x after: cur_x.transpose() \ndelta_x_ceres: delta_x_ceres_map.transpose() \ndelta_x_ceres.norm(): delta_x_ceres_map.norm() , delta_x_ceres.squaredNorm(): delta_x_ceres_map.squaredNorm());*///cout summary.BriefReport() endl;ROS_DEBUG(\nIterations : %d, static_castint(summary.iterations.size()));#else //手写求解器求解ROS_DEBUG(\ndelta0);solver.preMakeHessian();solver.makeHessian();ROS_DEBUG(\ndelta1);int cur_x_size 1000 (WINDOW_SIZE 1) * (SIZE_POSE SIZE_SPEEDBIAS) SIZE_POSE 1 100;double cur_x_array[cur_x_size], cur_x_array_before[cur_x_size];get_cur_parameter(solver, cur_x_array);memcpy(cur_x_array_before, cur_x_array, sizeof(double) * cur_x_size);Eigen::MapEigen::VectorXd cur_x(cur_x_array, solver.m solver.n);//cur_x_array变了cur_x才会变const Eigen::VectorXd cur_x_before cur_x;ROS_DEBUG(\ndelta2);TicToc t_solver;solver.solve();double vins_finish_time t_solver.toc();solver_time_sum_ vins_finish_time;solve_times_;ROS_DEBUG(\nmy solver costs: %f ms, iter nums: %d, avg_solve_time: %f ms, solver_time_sum_: %f, solve_times_: %f,vins_finish_time, NUM_ITERATIONS, solver_time_sum_/solve_times_, solver_time_sum_, solve_times_);get_cur_parameter(solver, cur_x_array);double delta_x[cur_x_size];Eigen::MapEigen::VectorXd delta_x_map(delta_x, solver.m solver.n);ROS_DEBUG(\ndelta3);cal_delta_x(solver, cur_x_array_before, cur_x_array, delta_x);TicToc t_print;ROS_DEBUG_STREAM(
// \ncur_x before: cur_x_before.transpose()
// \ncur_x after: cur_x.transpose() \ndelta_x: delta_x_map.transpose() \ndelta_x.norm(): delta_x_map.norm() , delta_x.squaredNorm(): delta_x_map.squaredNorm());ROS_DEBUG(\nprint costs: %f ms, t_print.toc());
#endif// 防止优化结果在零空间变化通过固定第一帧的位姿(如何固定freegaugefix)double2vector();//边缘化处理//如果次新帧是关键帧将边缘化最老帧及其看到的路标点和IMU数据将其转化为先验TicToc t_whole_marginalization;//如marg掉xi_2则需要处理跟xi_2相关的先验信息IMU信息视觉信息//1. marg 最老帧[0]if (marginalization_flag MARGIN_OLD){//new_marg_info编译器生成默认构造函数MarginalizationInfo *marginalization_info new MarginalizationInfo();vector2double();//1 把上一次先验项中的残差项尺寸为 n 传递给当前先验项并从中取出需要丢弃的状态量// (这一步不是多此一举第2步中的parameter_block不会保证marg掉para_Pose[0]和para_SpeedBias[0]吗)//并不是因为里面要求Jacobian所以必须按照标准的格式传入才能求出正确的Jacobianif (last_marginalization_info)//如果不是第一帧因为第一帧没有marg掉之后生成的先验matrix{//如果上次的先验中有本次需要marg的变量则添加到drop_set中vectorint drop_set;//本次被marg的参数的idxfor (int i 0; i static_castint(last_marginalization_parameter_blocks.size()); i){if (last_marginalization_parameter_blocks[i] para_Pose[0] ||last_marginalization_parameter_blocks[i] para_SpeedBias[0])drop_set.push_back(i);}// construct new marginlization_factor// 用上次marg的info初始化这次的marg_factor再加到这次的info中info管理marg的操作// ceres只管调用marg_factor不直接管info当然factor需要info来初始化所以是marg_factor管info而不是ceresMarginalizationFactor *marginalization_factor new MarginalizationFactor(last_marginalization_info);ResidualBlockInfo *residual_block_info new ResidualBlockInfo(marginalization_factor, NULL,last_marginalization_parameter_blocks,drop_set);
// ROS_DEBUG_STREAM(\nadd MARGIN_OLD last_marginalization_info\n
// \ncost_function-num_residuals(): marginalization_factor-num_residuals()
// \ncost_function-parameter_block_sizes().size: marginalization_factor-parameter_block_sizes().size());marginalization_info-addResidualBlockInfo(residual_block_info);}//2 将滑窗内第 0 帧和第 1 帧间的 IMU 预积分因子 pre_integrations[1]放到marginalization_info 中// 不理解为什么para_Pose[1], para_SpeedBias[1]也要marg{if (pre_integrations[1]-sum_dt 10.0)//两帧间时间间隔少于10s过长时间间隔的不进行marg{IMUFactor* imu_factor new IMUFactor(pre_integrations[1]);//drop_set表示只marg掉[0][1]即P0,V0虽然只drop[0][1]但是evaluate需要所有的变量来计算Jacobian所以还是全部传进去ResidualBlockInfo *residual_block_info new ResidualBlockInfo(imu_factor, NULL,vectordouble *{para_Pose[0], para_SpeedBias[0], para_Pose[1], para_SpeedBias[1]},vectorint{0, 1});marginalization_info-addResidualBlockInfo(residual_block_info);
// ROS_DEBUG_STREAM(\nadd imu_factor\n
// \ncost_function-num_residuals(): imu_factor-num_residuals()
// \ncost_function-parameter_block_sizes().size: imu_factor-parameter_block_sizes().size());}}//3 挑 选 出 第 一 次 观 测 帧 为 第 0 帧 的 路 标 点 将 对 应 的 多 组 视 觉 观 测 放 到marginalization_info 中{int feature_index -1;for (auto it_per_id : f_manager.feature){it_per_id.used_num it_per_id.feature_per_frame.size();if (!(it_per_id.used_num 2 it_per_id.start_frame WINDOW_SIZE - 2))continue;feature_index;int imu_i it_per_id.start_frame, imu_j imu_i - 1;if (imu_i ! 0)//只选择从[0]开始tracking的点continue;Vector3d pts_i it_per_id.feature_per_frame[0].point;//old中的2d坐标for (auto it_per_frame : it_per_id.feature_per_frame){imu_j;if (imu_i imu_j)continue;Vector3d pts_j it_per_frame.point;if (ESTIMATE_TD){ProjectionTdFactor *f_td new ProjectionTdFactor(pts_i, pts_j, it_per_id.feature_per_frame[0].velocity, it_per_frame.velocity,it_per_id.feature_per_frame[0].cur_td, it_per_frame.cur_td,it_per_id.feature_per_frame[0].uv.y(), it_per_frame.uv.y());ResidualBlockInfo *residual_block_info new ResidualBlockInfo(f_td, loss_function,vectordouble *{para_Pose[imu_i], para_Pose[imu_j], para_Ex_Pose[0], para_Feature[feature_index], para_Td[0]},vectorint{0, 3});//只drop掉[0](P0)和[3](tracking始于old的landmark)marginalization_info-addResidualBlockInfo(residual_block_info);}else{ProjectionFactor *f new ProjectionFactor(pts_i, pts_j);ResidualBlockInfo *residual_block_info new ResidualBlockInfo(f, loss_function,vectordouble *{para_Pose[imu_i], para_Pose[imu_j], para_Ex_Pose[0], para_Feature[feature_index]},vectorint{0, 3});marginalization_info-addResidualBlockInfo(residual_block_info);
// ROS_DEBUG_STREAM(\nadd ProjectionFactor\n
// \ncost_function-num_residuals(): f-num_residuals()
// \ncost_function-parameter_block_sizes().size: f-parameter_block_sizes().size());}}}}//得到 上次的先验、IMU测量、视觉观测(都是factor)对应的参数块(parameter_blocks)、雅可比矩阵(jacobians)、残差值(residuals)//与[0]有关的待优化变量存放于parameter_block_data中TicToc t_pre_margin;marginalization_info-preMarginalize();ROS_DEBUG(\npre marginalization %f ms, t_pre_margin.toc());//多线程计算在X0处的整个先验项的参数块雅可比矩阵和残差值//5、多线程构造先验项舒尔补AXb的结构在X0处线性化计算Jacobian和残差TicToc t_margin;marginalization_info-marginalize();ROS_DEBUG(\nmarginalization %f ms, t_margin.toc());//用marg之后的待优化参数去生成新的last_marg_info和last_marg_parameter_blocks供下一次使用//6.调整参数块在下一次窗口中对应的位置往前移一格注意这里是指针后面slideWindow中会赋新值这里只是提前占座std::unordered_maplong, double * addr_shift;for (int i 1; i WINDOW_SIZE; i){//让指针指向addr_shift[reinterpret_castlong(para_Pose[i])] para_Pose[i - 1];addr_shift[reinterpret_castlong(para_SpeedBias[i])] para_SpeedBias[i - 1];double* tmp_para_ptr para_Pose[i-1];double* tmp_ptr addr_shift[reinterpret_castlong(para_Pose[i])];
// for(int j0; j7; j) {
// ROS_DEBUG(\npara_Pose[%d] data: %f, i, *tmp_para_ptr);
// tmp_para_ptr;
// ROS_DEBUG(\naddr_shift[reinterpret_castlong(para_Pose[%d])] data: %f, i, *tmp_ptr);
// tmp_ptr;
// }}for (int i 0; i NUM_OF_CAM; i)addr_shift[reinterpret_castlong(para_Ex_Pose[i])] para_Ex_Pose[i];if (ESTIMATE_TD){addr_shift[reinterpret_castlong(para_Td[0])] para_Td[0];}vectordouble * parameter_blocks marginalization_info-getParameterBlocks(addr_shift);if (last_marginalization_info)delete last_marginalization_info;last_marginalization_info marginalization_info;//保存此次marg infolast_marginalization_parameter_blocks parameter_blocks;}//2. marg最新帧1st仅marg掉视觉poseelse{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){//只drop掉2nd的视觉poseIMU部分是在slideWindow内继承和delete的vectorint drop_set;for (int i 0; i static_castint(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_factorMarginalizationFactor *marginalization_factor new MarginalizationFactor(last_marginalization_info);ResidualBlockInfo *residual_block_info new ResidualBlockInfo(marginalization_factor, NULL,last_marginalization_parameter_blocks,drop_set);
// ROS_DEBUG_STREAM(\nin MARGIN_SECOND_NEW add last_marginalization_info\n
// \ncost_function-num_residuals(): marginalization_factor-num_residuals()
// \ncost_function-parameter_block_sizes().size: marginalization_factor-parameter_block_sizes().size());marginalization_info-addResidualBlockInfo(residual_block_info);}TicToc t_pre_margin;ROS_DEBUG(begin marginalization);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_maplong, double * addr_shift;for (int i 0; i WINDOW_SIZE; i){if (i WINDOW_SIZE - 1)continue;else if (i WINDOW_SIZE){//看不懂啥意思后面不是还要操作slideWindow吗这里搞地址干什么addr_shift[reinterpret_castlong(para_Pose[i])] para_Pose[i - 1];addr_shift[reinterpret_castlong(para_SpeedBias[i])] para_SpeedBias[i - 1];}else{addr_shift[reinterpret_castlong(para_Pose[i])] para_Pose[i];addr_shift[reinterpret_castlong(para_SpeedBias[i])] para_SpeedBias[i];}}for (int i 0; i NUM_OF_CAM; i)addr_shift[reinterpret_castlong(para_Ex_Pose[i])] para_Ex_Pose[i];if (ESTIMATE_TD){addr_shift[reinterpret_castlong(para_Td[0])] para_Td[0];}vectordouble * 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;}}ROS_DEBUG(whole marginalization costs: %f ms, t_whole_marginalization.toc());ROS_DEBUG(whole time for ceres: %f ms, t_whole.toc());
}//滑窗之后WINDOW的最后两个PsVsRsBasBgs相同无论是old还是new
//因为后面预积分要用最新的预积分初值所以为了保证窗口内有11个观测使最后两个相同
void Estimator::slideWindow()
{TicToc t_margin;//把最老的帧冒泡移到最右边然后delete掉在new一个新的对象出来if (marginalization_flag MARGIN_OLD){double t_0 Headers[0].stamp.toSec();back_R0 Rs[0];//back_P0 Ps[0];if (frame_count WINDOW_SIZE){for (int i 0; i WINDOW_SIZE; i)//循环完成也就冒泡完成到最右侧{Rs[i].swap(Rs[i 1]);//世界系下old冒泡std::swap(pre_integrations[i], pre_integrations[i 1]);//每一帧的预积分old冒泡dt_buf[i].swap(dt_buf[i 1]);//各种buf也冒泡linear_acceleration_buf[i].swap(linear_acceleration_buf[i 1]);angular_velocity_buf[i].swap(angular_velocity_buf[i 1]);Headers[i] Headers[i 1];//最后一个是 Headers[WINDOW_SIZE-1] Headers[WINDOW_SIZE]Ps[i].swap(Ps[i 1]);Vs[i].swap(Vs[i 1]);Bas[i].swap(Bas[i 1]);Bgs[i].swap(Bgs[i 1]);}//这一步是为了 new IntegrationBase时传入最新的预积分的初值acc_0, gyr_0babg所以必须要强制等于最新的Headers[WINDOW_SIZE] Headers[WINDOW_SIZE - 1];Ps[WINDOW_SIZE] Ps[WINDOW_SIZE - 1];Vs[WINDOW_SIZE] Vs[WINDOW_SIZE - 1];Rs[WINDOW_SIZE] Rs[WINDOW_SIZE - 1];Bas[WINDOW_SIZE] Bas[WINDOW_SIZE - 1];Bgs[WINDOW_SIZE] Bgs[WINDOW_SIZE - 1];//冒泡到最右边之后把对应的都deletenew或者clear掉delete pre_integrations[WINDOW_SIZE];//delete掉并new新的预积分对象出来pre_integrations[WINDOW_SIZE] new IntegrationBase{acc_0, gyr_0, Bas[WINDOW_SIZE], Bgs[WINDOW_SIZE]};dt_buf[WINDOW_SIZE].clear();linear_acceleration_buf[WINDOW_SIZE].clear();angular_velocity_buf[WINDOW_SIZE].clear();
// ROS_DEBUG(marg_flag: %d, before marg, all_image_frame.size(): %lu, WINDOW_SIZE: %d,
// marginalization_flag, all_image_frame.size(), WINDOW_SIZE);if (true || solver_flag INITIAL){mapdouble, ImageFrame::iterator it_0;it_0 all_image_frame.find(t_0);//t_0是最老帧的时间戳marg_old时删掉了帧但是marg2nd的时候没有动但是在process时候加进来了说明all_image_frame应该是在增长的delete it_0-second.pre_integration;it_0-second.pre_integration nullptr;for (mapdouble, ImageFrame::iterator it all_image_frame.begin(); it ! it_0; it){if (it-second.pre_integration)delete it-second.pre_integration;it-second.pre_integration NULL;}all_image_frame.erase(all_image_frame.begin(), it_0);//erase掉从开始到被marg掉的old之间所有的帧[begin(), it_0)all_image_frame.erase(t_0);//erase掉old帧}slideWindowOld();//管理feature(landmark)
// ROS_DEBUG(marg_flag: %d, after marg, all_image_frame.size(): %lu, WINDOW_SIZE: %d,
// marginalization_flag, all_image_frame.size(), WINDOW_SIZE);}}//如果2nd不是KF则直接扔掉1st的visual测量并在2nd基础上对1st的IMU进行预积分window前面的都不动else{if (frame_count WINDOW_SIZE){
// ROS_DEBUG(marg_flag: %d, before marg, all_image_frame.size(): %lu, WINDOW_SIZE: %d,
// marginalization_flag, all_image_frame.size(), WINDOW_SIZE);for (unsigned int i 0; i dt_buf[frame_count].size(); i)//对最新帧的img对应的imu数据进行循环{double tmp_dt dt_buf[frame_count][i];Vector3d tmp_linear_acceleration linear_acceleration_buf[frame_count][i];Vector3d tmp_angular_velocity angular_velocity_buf[frame_count][i];pre_integrations[frame_count - 1]-push_back(tmp_dt, tmp_linear_acceleration, tmp_angular_velocity);//2nd对1st进行IMU预积分//imu数据保存相当于一个较长的KFeg// |-|-|-|-|-----|// ↑// 这段img为1st时2nd不是KF扔掉了这个2nd的img但buf了IMU数据所以这段imu数据较长dt_buf[frame_count - 1].push_back(tmp_dt);linear_acceleration_buf[frame_count - 1].push_back(tmp_linear_acceleration);angular_velocity_buf[frame_count - 1].push_back(tmp_angular_velocity);}//相对世界系的预积分需要继承过来Headers[frame_count - 1] Headers[frame_count];Ps[frame_count - 1] Ps[frame_count];Vs[frame_count - 1] Vs[frame_count];Rs[frame_count - 1] Rs[frame_count];Bas[frame_count - 1] Bas[frame_count];Bgs[frame_count - 1] Bgs[frame_count];delete pre_integrations[WINDOW_SIZE];pre_integrations[WINDOW_SIZE] new IntegrationBase{acc_0, gyr_0, Bas[WINDOW_SIZE], Bgs[WINDOW_SIZE]};dt_buf[WINDOW_SIZE].clear();linear_acceleration_buf[WINDOW_SIZE].clear();angular_velocity_buf[WINDOW_SIZE].clear();slideWindowNew();
// ROS_DEBUG(marg_flag: %d, after marg, all_image_frame.size(): %lu, WINDOW_SIZE: %d,
// marginalization_flag, all_image_frame.size(), WINDOW_SIZE);}}
}// real marginalization is removed in solve_ceres()
void Estimator::slideWindowNew()
{sum_of_front;f_manager.removeFront(frame_count);
}
// real marginalization is removed in solve_ceres()
void Estimator::slideWindowOld()
{sum_of_back;bool shift_depth solver_flag NON_LINEAR ? true : false;if (shift_depth){Matrix3d R0, R1;Vector3d P0, P1;//Twb * Tbc Twc//0被marg掉的T_marg,1新的第[0]帧的T_newR0 back_R0 * ric[0];R1 Rs[0] * ric[0];P0 back_P0 back_R0 * tic[0];P1 Ps[0] Rs[0] * tic[0];f_manager.removeBackShiftDepth(R0, P0, R1, P1);//为什么要转移深度landmark不是删除了吗}elsef_manager.removeBack();
}void Estimator::setReloFrame(double _frame_stamp, int _frame_index, vectorVector3d _match_points, Vector3d _relo_t, Matrix3d _relo_r)
{relo_frame_stamp _frame_stamp;//与old frame loop上的WINDOW内的帧(j帧)的时间戳relo_frame_index _frame_index;//j帧的帧号match_points.clear();match_points _match_points;//i帧中与j帧中match上的点在i帧中的归一化(x,y,id)//Tw1_biTw1_b_oldprev_relo_t _relo_t;//i帧poseprev_relo_r _relo_r;for(int i 0; i WINDOW_SIZE; i){if(relo_frame_stamp Headers[i].stamp.toSec()){relo_frame_local_index i;//j帧在WINDOW中的下标relocalization_info 1;for (int j 0; j SIZE_POSE; j)//注意这不是赋地址而是new了一个新的优化变量的内存relo_Pose虽然赋初值时为Tw2_bj但是实际上作用是Tw2_birelo_Pose[j] para_Pose[i][j];}}
}
9.2 solve.cpp
#include iostream
#include fstream#include solve.h
#include ../parameters.h#define USE_SCHURnamespace solve{/*solver_info相关函数*///计算每个残差对应的Jacobian并更新 parameter_block_data
void ResidualBlockInfo::Evaluate()
{//每个factor的残差块总维度 和 残差块具体size//residual总维度先验last n76IMU15Visual2residuals.resize(cost_function-num_residuals());//有td时先验factor为13(9*16*1061)IMU factor为4(7,9,7,9)每个feature factor size5(7,7,7,1)//无td时 12 4 4std::vectorint block_sizes cost_function-parameter_block_sizes();/* ROS_DEBUG_STREAM(\ncost_function-num_residuals(): cost_function-num_residuals() \ncost_function-parameter_block_sizes().size: cost_function-parameter_block_sizes().size());
for(int i0; icost_function-parameter_block_sizes().size(); i) {ROS_DEBUG(\nparameter_block_sizes()[%d]: %d, i, cost_function-parameter_block_sizes()[i]);
}*/raw_jacobians new double *[block_sizes.size()];//二重指针指针数组jacobians.resize(block_sizes.size());for (int i 0; i static_castint(block_sizes.size()); i){jacobians[i].resize(cost_function-num_residuals(), block_sizes[i]);raw_jacobians[i] jacobians[i].data();//二重指针,是为了配合ceres的形参 double** jacobians看不懂给data还能够操作地址//dim block_sizes[i] 7 ? 6 : block_sizes[i];}//虚函数调用的是基类自己实现的Evaluate即分别是MarginalizationFactor、IMUFactor 和 ProjectionTdFactor(或ProjectionFactor)的Evaluate()函数cost_function-Evaluate(parameter_blocks.data(), residuals.data(), raw_jacobians);//std::vectorint tmp_idx(block_sizes.size());//Eigen::MatrixXd tmp(dim, dim);//for (int i 0; i static_castint(parameter_blocks.size()); i)//{// int size_i localSize(block_sizes[i]);// Eigen::MatrixXd jacobian_i jacobians[i].leftCols(size_i);// for (int j 0, sub_idx 0; j static_castint(parameter_blocks.size()); sub_idx block_sizes[j] 7 ? 6 : block_sizes[j], j)// {// int size_j localSize(block_sizes[j]);// Eigen::MatrixXd jacobian_j jacobians[j].leftCols(size_j);// tmp_idx[j] sub_idx;// tmp.block(tmp_idx[i], tmp_idx[j], size_i, size_j) jacobian_i.transpose() * jacobian_j;// }//}//Eigen::SelfAdjointEigenSolverEigen::MatrixXd saes(tmp);//std::cout saes.eigenvalues() std::endl;//ROS_ASSERT(saes.eigenvalues().minCoeff() -1e-6);//这里使用的是CauchyLoss应该是计算一个scale对residuals进行加权先不细看TODO了解CauchyLoss等loss函数的意义if (loss_function){double residual_scaling_, alpha_sq_norm_;double sq_norm, rho[3];sq_norm residuals.squaredNorm();//loss_function 为 robust kernel functioninsq_norm outrho out[0] rho(sq_norm),out[1] rho(sq_norm), out[2] rho(sq_norm),loss_function-Evaluate(sq_norm, rho);//求取鲁棒核函数关于||f(x)||^2的一二阶导数//printf(sq_norm: %f, rho[0]: %f, rho[1]: %f, rho[2]: %f\n, sq_norm, rho[0], rho[1], rho[2]);double sqrt_rho1_ sqrt(rho[1]);if ((sq_norm 0.0) || (rho[2] 0.0)){residual_scaling_ sqrt_rho1_;alpha_sq_norm_ 0.0;}else{const double D 1.0 2.0 * sq_norm * rho[2] / rho[1];const double alpha 1.0 - sqrt(D);//求根公式求方程的根residual_scaling_ sqrt_rho1_ / (1 - alpha);alpha_sq_norm_ alpha / sq_norm;}for (int i 0; i static_castint(parameter_blocks.size()); i){jacobians[i] sqrt_rho1_ * (jacobians[i] - alpha_sq_norm_ * residuals * (residuals.transpose() * jacobians[i]));}residuals * residual_scaling_;}
}Solver::~Solver()
{ROS_DEBUG(destractor here1);//new出来的是在堆上的内存需要手动delete释放malloc的内存使用free来释放if(mem_allocated_) {for (auto it parameter_block_data.begin(); it ! parameter_block_data.end(); it)delete[] it-second;ROS_DEBUG(destractor here2);for (auto it parameter_block_data_backup.begin(); it ! parameter_block_data_backup.end(); it)delete[] it-second;}ROS_DEBUG(destractor here3);//这个不能在这delete放因为ceres要用
// for (int i 0; i (int)factors.size(); i)
// {
//
// delete[] factors[i]-raw_jacobians;
// ROS_DEBUG(destractor here31);
// delete[] factors[i]-cost_function;
// ROS_DEBUG(destractor here32);
// delete[] factors[i];
// ROS_DEBUG(destractor here33);
// }ROS_DEBUG(destractor here4);
}void Solver::addResidualBlockInfo(ResidualBlockInfo *residual_block_info)
{factors.emplace_back(residual_block_info);std::vectordouble * parameter_blocks residual_block_info-parameter_blocks;//每个factor的待优化变量的地址std::vectorint parameter_block_sizes residual_block_info-cost_function-parameter_block_sizes();//待优化变量的维度//parameter_blocks.size//有td时先验factor为13(9*16*1061)IMU factor为4(7,9,7,9)每个feature factor size5(7,7,7,1)//无td时 12 4 4for (int i 0; i static_castint(residual_block_info-parameter_blocks.size()); i){double *addr parameter_blocks[i];int size parameter_block_sizes[i];//待优化变量的维度//map没有key时会新建key-value对parameter_block_size[reinterpret_castlong(addr)] size;//global size 优化变量内存地址,localSize
// ROS_DEBUG(in addResidualBlockInfo size: %d, size);}//需要 marg 掉的变量for (int i 0; i static_castint(residual_block_info-drop_set.size()); i){double *addr parameter_blocks[residual_block_info-drop_set[i]];//获得待marg变量的地址//要marg的变量先占个key的座marg之前将m放在一起n放在一起parameter_block_idx[reinterpret_castlong(addr)] 0;//local size 待边缘化的优化变量内存地址,在parameter_block_size中的id}
}void Solver::preMakeHessian()
{
// ROS_INFO_STREAM(\nfactors.size(): factors.size());int i0;ROS_DEBUG(factors size%lu, landmark size%lu, factors.size(), factors.size()-2); //始于[0]帧的landmarkfor (auto it : factors){
// ROS_INFO_STREAM(\nin preMarginalize i: i); //很大能到900多说明[0]观测到了很多landmarkit-Evaluate();//计算每个factor的residual和Jacobian//如果完成过就只计算Jacobian和residual(里面已经耦合了sqrt_info所以直接HJ^T*J,不用J^T*W*J)不用再new内存,重复调用只是为了计算新的Jacobian和residualif(mem_allocated_) {continue;}std::vectorint block_sizes it-cost_function-parameter_block_sizes(); //residual总维度先验last n76IMU15Visual2
/* 测试地址转换之后还能否转换回来long tmp_addr reinterpret_castlong(it-parameter_blocks[0]);double* tmp_pt reinterpret_castdouble *(tmp_addr);ROS_DEBUG_STREAM(\nraw double* it-parameter_blocks[0] , cast to long tmp_addr , back to double* tmp_pt);*/for (int i 0; i static_castint(block_sizes.size()); i){long addr reinterpret_castlong(it-parameter_blocks[i]);//parameter_blocks是vectordouble *存放的是数据的地址int size block_sizes[i];//如果优化变量中没有这个数据就new一片内存放置if (parameter_block_data.find(addr) parameter_block_data.end()){double *data new double[size];//dst,srcmemcpy(data, it-parameter_blocks[i], sizeof(double) * size);parameter_block_data[addr] data;//数据备份double *data_backup new double[size];memcpy(data_backup, it-parameter_blocks[i], sizeof(double) * size);parameter_block_data_backup[addr] data_backup;}}}mem_allocated_ true;
}int Solver::localSize(int size) const
{return size 7 ? 6 : size;
}int Solver::globalSize(int size) const
{return size 6 ? 7 : size;
}//线程函数
static void* ThreadsConstructA(void* threadsstruct)
{ThreadsStruct* p ((ThreadsStruct*)threadsstruct);//遍历该线程分配的所有factors这factor可能是任何一个factorfor (auto it : p-sub_factors){//遍历该factor中的所有参数块,比如IMU factor传入的是vectordouble *{para_Pose[0], para_SpeedBias[0], para_Pose[1], para_SpeedBias[1]}for (int i 0; i static_castint(it-parameter_blocks.size()); i){int idx_i p-parameter_block_idx[reinterpret_castlong(it-parameter_blocks[i])];int size_i p-parameter_block_size[reinterpret_castlong(it-parameter_blocks[i])];if (size_i 7) //对于pose来说是7维的,最后一维为0这里取左边6size_i 6;//只提取local size部分对于pose来说是7维的,最后一维为0这里取左边6维;对于其他待优化变量size_i不变取全部jacobianEigen::MatrixXd jacobian_i it-jacobians[i].leftCols(size_i);for (int j i; j static_castint(it-parameter_blocks.size()); j){int idx_j p-parameter_block_idx[reinterpret_castlong(it-parameter_blocks[j])];int size_j p-parameter_block_size[reinterpret_castlong(it-parameter_blocks[j])];if (size_j 7)size_j 6;Eigen::MatrixXd jacobian_j it-jacobians[j].leftCols(size_j);//主对角线if (i j)p-A.block(idx_i, idx_j, size_i, size_j) jacobian_i.transpose() * jacobian_j;//非主对角线else{p-A.block(idx_i, idx_j, size_i, size_j) jacobian_i.transpose() * jacobian_j;p-A.block(idx_j, idx_i, size_j, size_i) p-A.block(idx_i, idx_j, size_i, size_j).transpose();}}p-b.segment(idx_i, size_i) jacobian_i.transpose() * it-residuals;}}return threadsstruct;
}bool Solver::updatePose(const double *x, const double *delta, double *x_plus_delta)
{Eigen::Mapconst Eigen::Vector3d _p(x);Eigen::Mapconst Eigen::Quaterniond _q(x 3);Eigen::Mapconst Eigen::Vector3d dp(delta);//数组转四元数Eigen::Quaterniond dq Utility::deltaQ(Eigen::Mapconst Eigen::Vector3d(delta 3));Eigen::MapEigen::Vector3d p(x_plus_delta);Eigen::MapEigen::Quaterniond q(x_plus_delta 3);//Jacobian和residual都是按照6维来处理的但是更新rotation时需要按照四元数来更新p _p dp;q (_q * dq).normalized();//四元数乘法并归一化return true;
}void Solver::makeHessian()
{int pos 0;//Hessian矩阵整体维度//it.first是要被marg掉的变量的地址,将其size累加起来就得到了所有被marg的变量的总localSizem//marg的放一起共m维remain放一起共n维for (auto it : parameter_block_idx){it.second pos;//也算是排序1pos localSize(parameter_block_size[it.first]);//PQ7为改为6维}m pos;//要被marg的变量的总维度int tmp_n 0;//与[0]相关总维度for (const auto it : parameter_block_size){if (parameter_block_idx.find(it.first) parameter_block_idx.end())//将不在drop_set中的剩下的维度加起来这一步实际上算的就是n{parameter_block_idx[it.first] pos;//排序2tmp_n localSize(it.second);pos localSize(it.second);}}n pos - m;//remain变量的总维度这样写建立了n和m间的关系表意更强ROS_DEBUG(\nn: %d, tmp_n: %d, n, tmp_n);ROS_DEBUG(\nSolver, pos: %d, m: %d, n: %d, size: %d, pos, m, n, (int)parameter_block_idx.size());TicToc t_summing;Eigen::MatrixXd A(pos, pos);//总系数矩阵Eigen::VectorXd b(pos);//总误差项A.setZero();b.setZero();Hessian_.resize(pos,pos);b_.resize(pos);delta_x_.resize(pos);//构建信息矩阵可以多线程构建
/* //single thread
for (auto it : factors)
{//J^T*Jfor (int i 0; i static_castint(it-parameter_blocks.size()); i){int idx_i parameter_block_idx[reinterpret_castlong(it-parameter_blocks[i])];//要被marg的second0int size_i localSize(parameter_block_size[reinterpret_castlong(it-parameter_blocks[i])]);Eigen::MatrixXd jacobian_i it-jacobians[i].leftCols(size_i);//remain变量的初始jacobianfor (int j i; j static_castint(it-parameter_blocks.size()); j){int idx_j parameter_block_idx[reinterpret_castlong(it-parameter_blocks[j])];int size_j localSize(parameter_block_size[reinterpret_castlong(it-parameter_blocks[j])]);Eigen::MatrixXd jacobian_j it-jacobians[j].leftCols(size_j);//marg变量的初始jacobian//主对角线注意这里是可能之前别的变量在这个地方已经有过值了所以要if (i j)A.block(idx_i, idx_j, size_i, size_j) jacobian_i.transpose() * jacobian_j;//非主对角线else{A.block(idx_i, idx_j, size_i, size_j) jacobian_i.transpose() * jacobian_j;A.block(idx_j, idx_i, size_j, size_i) A.block(idx_i, idx_j, size_i, size_j).transpose();}}b.segment(idx_i, size_i) jacobian_i.transpose() * it-residuals;//J^T*e}
}
ROS_INFO(summing up costs %f ms, t_summing.toc());*///multi threadTicToc t_thread_summing;pthread_t tids[NUM_THREADS];//4个线程构建//携带每个线程的输入输出信息ThreadsStruct threadsstruct[NUM_THREADS];//将先验约束因子平均分配到4个线程中int i 0;for (auto it : factors){threadsstruct[i].sub_factors.push_back(it);i;i i % NUM_THREADS;}//将每个线程构建的A和b加起来for (int i 0; i NUM_THREADS; i){TicToc zero_matrix;threadsstruct[i].A Eigen::MatrixXd::Zero(pos,pos);threadsstruct[i].b Eigen::VectorXd::Zero(pos);threadsstruct[i].parameter_block_size parameter_block_size;//marg里的block_size4个线程共享threadsstruct[i].parameter_block_idx parameter_block_idx;int ret pthread_create( tids[i], NULL, ThreadsConstructA ,(void*)(threadsstruct[i]));//参数4是argvoid*类型取其地址并强制类型转换if (ret ! 0){ROS_WARN(pthread_create error);ROS_BREAK();}}//将每个线程构建的A和b加起来for( int i NUM_THREADS - 1; i 0; i--){pthread_join( tids[i], NULL );//阻塞等待线程完成这里的A和b的操作在主线程中是阻塞的的顺序是pthread_join的顺序A threadsstruct[i].A;b threadsstruct[i].b;}//ROS_DEBUG(thread summing up costs %f ms, t_thread_summing.toc());//ROS_INFO(A diff %f , b diff %f , (A - tmp_A).sum(), (b - tmp_b).sum());Hessian_ A;b_ -b;//DOGLEG需反解出J和eif(method_solve::Solver::kDOGLEG) {TicToc t_solve_J;TicToc t_SelfAdjoint;Eigen::SelfAdjointEigenSolverEigen::MatrixXd saes2(A);//这一句24.3msROS_DEBUG(\nt_SelfAdjoint cost: %f ms, t_SelfAdjoint.toc());Eigen::VectorXd S Eigen::VectorXd((saes2.eigenvalues().array() eps).select(saes2.eigenvalues().array(), 0));Eigen::VectorXd S_sqrt S.cwiseSqrt();//开根号linearized_jacobians S_sqrt.asDiagonal() * saes2.eigenvectors().transpose();Eigen::VectorXd S_inv Eigen::VectorXd((saes2.eigenvalues().array() eps).select(saes2.eigenvalues().array().inverse(), 0));Eigen::VectorXd S_inv_sqrt S_inv.cwiseSqrt();linearized_residuals S_inv_sqrt.asDiagonal() * saes2.eigenvectors().real().transpose() * b;ROS_DEBUG(\nt_solve_J cost: %f ms, t_solve_J.toc());//25ms}
}std::vectordouble * Solver::getParameterBlocks(std::unordered_maplong, double * addr_shift)
{std::vectordouble * keep_block_addr;//remain的优化变量的地址keep_block_size.clear();keep_block_idx.clear();keep_block_data.clear();for (const auto it : parameter_block_idx){if (it.second m)//如果是remain部分{keep_block_size.push_back(parameter_block_size[it.first]);keep_block_idx.push_back(parameter_block_idx[it.first]);keep_block_data.push_back(parameter_block_data[it.first]);keep_block_addr.push_back(addr_shift[it.first]);//待优化变量的首地址需要不变但是首地址对应的变量是P0需要在slideWindow中被冒泡到后面delete掉}}ROS_DEBUG(keep_block_addr[0] long addr: %ld, [1] long addr: %ld,reinterpret_castlong(keep_block_addr[0]), reinterpret_castlong(keep_block_addr[1]));sum_block_size std::accumulate(std::begin(keep_block_size), std::end(keep_block_size), 0);return keep_block_addr;
}//只更新状态量pqvbabgλ注意prior不是状态量虽然在待优化变量中但是其residual是跟状态量有关Jacobian在一轮优化中不变
//这里更新状态的目的是因为计算chi时会用到residual而residual和状态量有关而先验的residual更新f f J*δxp其中δxpx-x0,也跟状态量x有关
//但是因为在先验factor在Evaluate时会计算residual所以不用手动更新只需要更新最核心的x即可。其他的factor相同。
//weight是LM strategy2时的权重默认传-1.0不加权
bool Solver::updateStates(double weight) {int array_size 1000 (WINDOW_SIZE 1) * (SIZE_POSE SIZE_SPEEDBIAS) SIZE_POSE 1 100;double used_delta_x[array_size];if(weight ! -1.0)std::transform(delta_x_array_, delta_x_array_ array_size, used_delta_x, [weight](double x) { return x * weight; });//对delta_x加权elsememcpy(used_delta_x, delta_x_array_, sizeof(double) * array_size);//使用idx来找对应的paramdouble cur_x_array[1000 (WINDOW_SIZE 1) * (SIZE_POSE SIZE_SPEEDBIAS) SIZE_POSE 1 100];for (auto it : parameter_block_idx){const long addr it.first;const int idx it.second;const int tmp_param_block_size parameter_block_size[addr];
/* ROS_DEBUG_STREAM(\nidx: idx , tmp_param_block_size: tmp_param_block_size);*///保存一份待优化变量和delta_x进行数量级对比memcpy( cur_x_array[idx], reinterpret_castdouble *(addr), sizeof(double) *(int)SIZE_POSE);if(tmp_param_block_size SIZE_POSE) {
/* Eigen::MapEigen::VectorXd delta_pose {delta_x_array_[idx], tmp_param_block_size};Eigen::MapEigen::VectorXd tmp_pose {reinterpret_castdouble *(addr), tmp_param_block_size};for(int i0;itmp_param_block_size;i){tmp_pose[i] *(reinterpret_castdouble *(addr sizeof(double)*i));delta_pose[i] delta_x_array_[idxi];}ROS_DEBUG_STREAM(\npose before update: tmp_pose.transpose() \ndelta_pose: delta_pose.transpose());*/updatePose(reinterpret_castdouble *(addr), delta_x_array_[idx], reinterpret_castdouble *(addr));//TODO:这个backup应该可以用parameter_block_data替代/* ROS_DEBUG_STREAM(\npose after update: tmp_pose.transpose());*/} else {Eigen::Mapconst Eigen::VectorXd x{parameter_block_data_backup[addr], tmp_param_block_size};Eigen::Mapconst Eigen::VectorXd delta_x{delta_x_array_[idx], tmp_param_block_size};Eigen::MapEigen::VectorXd x_plus_delta{reinterpret_castdouble *(addr), tmp_param_block_size};/*ROS_DEBUG_STREAM(\nother parameters before update: x_plus_delta.transpose() \ndelta_x: delta_x.transpose());*/x_plus_delta x delta_x;/*ROS_DEBUG_STREAM(\nother parameters after update: x_plus_delta.transpose());*/}}// 初始化Eigen向量Eigen::MapEigen::VectorXd cur_x(cur_x_array, mn);ROS_DEBUG_STREAM(\ncur_x: cur_x.transpose());preMakeHessian();//计算更新后的Jacobian和residualreturn true;
}//备份状态量
bool Solver::backupStates() {for (auto it : parameter_block_idx){const long addr it.first;const int tmp_param_block_size parameter_block_size[addr];memcpy(parameter_block_data_backup[addr], reinterpret_castdouble *(addr), sizeof(double) * (int)tmp_param_block_size);}return true;
}//回滚状态量
bool Solver::rollbackStates() {for (auto it : parameter_block_idx){const long addr it.first;const int tmp_param_block_size parameter_block_size[addr];memcpy(reinterpret_castdouble *(addr), parameter_block_data_backup[addr], sizeof(double) * (int)tmp_param_block_size);}preMakeHessian();//计算更新后的Jacobian和residualreturn true;
}bool Solver::get_cur_parameter(double* cur_x_array) {for (auto it : parameter_block_idx) {const long addr it.first;const int idx it.second;const int tmp_param_block_size parameter_block_size[addr];ROS_ASSERT_MSG(tmp_param_block_size 0, tmp_param_block_size %d, tmp_param_block_size);memcpy( cur_x_array[idx], reinterpret_castdouble *(addr), sizeof(double) *(int)tmp_param_block_size);}return true;
}//在ResidualBlockInfo::Evaluate()中调用的多态Evaluate()函数后已经考虑了loss_function对Jacobian和residual的加权
//分别计算先验和其他factor的chi
double Solver::computeChi() const{//先验的residual维度size_t prior_dim SIZE_SPEEDBIAS (SIZE_POSE-1) * WINDOW_SIZE (SIZE_POSE-1);if(ESTIMATE_TD){prior_dim1;}double tmpChi 0;for (auto it : factors){//TODO先验的也改成正常更新按照参数也是PQV来理解先验的话就应该是正常更新而不是使用norm(),后面看看对不对double this_Chi it-residuals.transpose() * it-residuals;tmpChi this_Chi;// if(it-residuals.size()prior_dim) {
// double this_Chi it-residuals.norm();
// tmpChi this_Chi;
///* ROS_DEBUG_STREAM(\nprior factor, this_Chi this_Chi
// , residuals size: it-residuals.size()
// , residuals: it-residuals.transpose());*/
// } else {
// double this_Chi it-residuals.transpose() * it-residuals;
// tmpChi this_Chi;
///* ROS_DEBUG_STREAM(\nother factor, this_Chi this_Chi
// , residuals size: it-residuals.size()
// , residuals: it-residuals.transpose());*/
// }}ROS_DEBUG_STREAM(\nhere tmpChi tmpChi);return tmpChi;
}/// LM
void Solver::computeLambdaInitLM() {if(strategy_ 1) {currentChi_ computeChi();double maxDiagonal 0;ulong size Hessian_.cols();assert(Hessian_.rows() Hessian_.cols() Hessian is not square);for (ulong i 0; i size; i) {maxDiagonal std::max(fabs(Hessian_(i, i)), maxDiagonal);//取H矩阵的最大值然后*涛}double tau 1e1;//[1e-8,1] tau越小△x越大currentLambda_ tau * maxDiagonal;} else if(strategy_ 2) {// set a large value, so that first updates are small steps in the steepest-descent directioncurrentChi_ computeChi();currentLambda_ 1e16;
// currentLambda_ 1e-3;} else if(strategy_ 3) {ni_ 2.;currentLambda_ -1.;currentChi_ computeChi();stopThresholdLM_ 1e-6 * currentChi_; // 迭代条件为 误差下降 1e-6 倍double maxDiagonal 0;ulong size Hessian_.cols();assert(Hessian_.rows() Hessian_.cols() Hessian is not square);for (ulong i 0; i size; i) {maxDiagonal std::max(fabs(Hessian_(i, i)), maxDiagonal);//取H矩阵的最大值然后*涛}
// double tau 1e-5;double tau 1e-15;//[1e-8,1] tau越小△x越大//currentLambda_ tau * maxDiagonal;ROS_DEBUG_STREAM(\nin computeLambdaInitLM currentChi_ currentChi_ , init currentLambda_ currentLambda_ , maxDiagonal maxDiagonal);}
}void Solver::addLambdatoHessianLM() {ulong size Hessian_.cols();assert(Hessian_.rows() Hessian_.cols() Hessian is not square);for (ulong i 0; i size; i) {if(strategy_1) {Hessian_(i, i) currentLambda_ * Hessian_(i, i); //理解: H(k1) H(k) λ H(k) (1λ) H(k) 策略1} else if(strategy_2 || strategy_3) {Hessian_(i, i) currentLambda_;}}
}void Solver::removeLambdaHessianLM() {ulong size Hessian_.cols();assert(Hessian_.rows() Hessian_.cols() Hessian is not square);// TODO:: 这里不应该减去一个数值的反复加减容易造成数值精度出问题而应该保存叠加lambda前的值在这里直接赋值for (ulong i 0; i size; i) {if(strategy_1) {Hessian_(i, i) / 1.0 currentLambda_;//H回退: H(k) 1/(1λ) * H(k1)策略1} else if(strategy_2 || strategy_3) {Hessian_(i, i) - currentLambda_;}}
}//Nielsen的方法分母直接为L判断\rho的符号
bool Solver::isGoodStepInLM() {bool ret false;assert(Hessian_.rows() Hessian_.cols() Hessian is not square);if(strategy_1) {double tempChi computeChi();ulong size Hessian_.cols();MatXX diag_hessian(MatXX::Zero(size, size));for(ulong i 0; i size; i) {diag_hessian(i, i) Hessian_(i, i);}double scale delta_x_.transpose() * (currentLambda_ * diag_hessian * delta_x_ b_);//scale就是rho的分母double rho (currentChi_ - tempChi) / scale;//计算rho// update currentLambda_double epsilon 0.75;double L_down 9.0;double L_up 11.0;if(rho epsilon isfinite(tempChi)) {currentLambda_ std::max(currentLambda_ / L_down, 1e-7);currentChi_ tempChi;ret true;ROS_DEBUG(choose L_down);} else {currentLambda_ std::min(currentLambda_ * L_up, 1e7);ret false;ROS_DEBUG(choose L_up);}ROS_DEBUG(\nstrategy1: currentLambda_: %e,rho: %f, currentChi_: %e, tempChi: %e, scale: %e,currentLambda_, rho, currentChi_, tempChi, scale);ROS_DEBUG_STREAM(\nret ret , rho0 (rho0) , rho: rho , delta_x_.squaredNorm(): delta_x_.squaredNorm() , delta_x_: delta_x_.transpose() \nb_.norm(): b_.norm() );
// ROS_DEBUG_STREAM(\nb_: b_.transpose());} else if(strategy_2) {// 统计所有的残差double tempChi_p_h 0.0, tempChi_p_alpha_h 0.0;tempChi_p_h computeChi();double alpha_up b_.transpose() * delta_x_;double alpha_down (tempChi_p_h - currentChi_) / 2. 2. * alpha_up;double alpha_tmp alpha_up / alpha_down;double scale 0;scale fabs((alpha_tmp * delta_x_).transpose() * (currentLambda_ * (alpha_tmp * delta_x_) b_));scale 1e-3; // make sure its non-zero :)//回滚刚才更新的xx△x使用α重新更新xx α*△x并重新计算残差和chirollbackStates();updateStates(alpha_tmp);preMakeHessian();//更新状态之后就要更新reidualtempChi_p_alpha_h computeChi();double rho (currentChi_ - tempChi_p_alpha_h) / scale;if (rho 0 isfinite(tempChi_p_alpha_h)) { // last step was good, 误差在下降currentLambda_ std::max(currentLambda_ / (1 alpha_tmp), 1e-7);lm_alpha_ alpha_tmp;currentChi_ tempChi_p_alpha_h;ret true;} else {currentLambda_ currentLambda_ fabs(tempChi_p_alpha_h - currentChi_) / (2 * alpha_tmp);ret false;}ROS_DEBUG(\nstrategy2: currentLambda_: %e,alpha_tmp: %e, rho: %f, currentChi_: %e, tempChi_p_h: %e, tempChi_p_alpha_h: %e, scale: %e,currentLambda_, alpha_tmp, rho, currentChi_, tempChi_p_h, tempChi_p_alpha_h, scale);ROS_DEBUG_STREAM(\nret ret , rho0 (rho0) , rho: rho , delta_x_.squaredNorm(): delta_x_.squaredNorm() , delta_x_: delta_x_.transpose() \nb_.norm(): b_.norm() , b_: b_.transpose());} else if(strategy_3) {double scale 0;scale fabs(delta_x_.transpose() * (currentLambda_ * delta_x_ b_));scale 1e-3; // make sure its non-zero :)// 统计更新后的所有的chi()double tempChi computeChi();double rho (currentChi_ - tempChi) / scale;if (rho 0 isfinite(tempChi)) // last step was good, 误差在下降{double alpha 1. - pow((2 * rho - 1), 3);//更新策略跟课件里面一样//TODO这个ceres里面没有限制上限为2/3alpha std::min(alpha, 2. / 3.);double scaleFactor (std::max)(1. / 3., alpha);currentLambda_ * scaleFactor;//课程里面应该是μ需要绘制曲线ni_ 2; //vcurrentChi_ tempChi;ret true;} else {//如果\rho0则增大阻尼μ减小步长currentLambda_ * ni_;ni_ * 2;//2这个值越大λ增长越快ret false;}ROS_DEBUG(\nstrategy3: currentLambda_: %e, ni_: %e, rho: %f, currentChi_: %e, tempChi: %e, scale: %e,currentLambda_, ni_, rho, currentChi_, tempChi, scale);ROS_DEBUG_STREAM(\nret ret , rho0 (rho0) , rho: rho , delta_x_.squaredNorm(): delta_x_.squaredNorm() , delta_x_: delta_x_.transpose() \nb_.norm(): b_.norm() , b_: b_.transpose());
/* FILE *fp_lambda fopen(file_name_.data(), a);fprintf(fp_lambda, %d, %f\n, try_iter_, currentLambda_);fflush(fp_lambda);fclose(fp_lambda);*/}ROS_DEBUG(\n%d record lambda finish\n, try_iter_);return ret;
}/*
* brief conjugate gradient with perconditioning
*
* the jacobi PCG method 共轭梯度法
* 知乎有一篇帖子是针对PCG进行改进的能减少迭代次数对角线预处理和不完备的Cholesky预处理* https://zhuanlan.zhihu.com/p/521753443
*/
Eigen::MatrixXd Solver::pcgSolver(const MatXX A, const VecX b, int maxIter -1) {assert(A.rows() A.cols() PCG solver ERROR: A is not a square matrix);int rows b.rows();int n maxIter 0 ? rows : maxIter;VecX x(VecX::Zero(rows));MatXX M_inv A.diagonal().asDiagonal().inverse();//取对角线阵的逆矩阵VecX r0(b); // initial r b - A*0 bVecX z0 M_inv * r0;VecX p(z0);VecX w A * p;double r0z0 r0.dot(z0);double alpha r0z0 / p.dot(w);VecX r1 r0 - alpha * w;int i 0;double threshold 1e-5 * r0.norm(); //比例调大可以提高阈值放宽停止条件while (r1.norm() threshold i n) {i;VecX z1 M_inv * r1;double r1z1 r1.dot(z1);double belta r1z1 / r0z0;z0 z1;r0z0 r1z1;r0 r1;p belta * p z1;w A * p;alpha r1z1 / p.dot(w);x alpha * p;r1 - alpha * w;}ROS_DEBUG(\nPCG iter times: %d, n: %d, r1.norm(): %f, threshold: %f, i, n, r1.norm(), threshold);return x;
}/*Solve Hx b, we can use PCG iterative method or use sparse Cholesky*/
//TODO:使用PCG迭代而非SVD分解求解
void Solver::solveLinearSystem() {
//method1直接求逆求解
// delta_x_ Hessian_.inverse() * b_;
// delta_x_ H.ldlt().solve(b_);//method2Schur消元求解marg掉drop_set中的parameter
#ifdef USE_SCHUR//step1:Schur消元求//求解Hxb之前marg时不用求出△x只需要H所以不用对方程组求解但现在优化时需要求解出整个△xTicToc t_Schur_PCG;Eigen::MatrixXd Amm_solver 0.5 * (Hessian_.block(0, 0, m, m) Hessian_.block(0, 0, m, m).transpose());Eigen::VectorXd bmm_solver b_.segment(0, m);Eigen::MatrixXd Amr_solver Hessian_.block(0, m, m, n);Eigen::MatrixXd Arm_solver Hessian_.block(m, 0, n, m);Eigen::MatrixXd Arr_solver Hessian_.block(m, m, n, n);Eigen::VectorXd brr_solver b_.segment(m, n);//求Amm_solver^(-1)TicToc t_Amm_inv;Eigen::MatrixXd Amm_inv_solver Amm_solver.inverse();//SelfAdjointEigenSolver和直接求逆速度差不多ROS_DEBUG(\nt_Amm_inv cost: %f ms, t_Amm_inv.toc());Eigen::MatrixXd tmpA_solver Arm_solver * Amm_inv_solver;//step1: Schur补Eigen::MatrixXd Arr_schur Arr_solver - tmpA_solver * Amr_solver;Eigen::VectorXd brr_schur brr_solver - tmpA_solver * bmm_solver;ROS_DEBUG(here1);// step2: solve Arr_schur * delta_x_rr brr_schur
// method1:直接求逆
// Eigen::MatrixXd Arr_schur_inv Arr_schur.inverse();
// Eigen::VectorXd delta_x_rr Arr_schur_inv * brr_schur;// method2:使用PCG求解TicToc t_PCG_xrr;Eigen::VectorXd delta_x_rr pcgSolver(Arr_schur, brr_schur, Arr_schur.rows()1); //0.3msROS_DEBUG(\n t_PCG_xrr cost %f ms, t_PCG_xrr.toc());Eigen::VectorXd delta_x_mm Amm_inv_solver * (bmm_solver - Amr_solver * delta_x_rr);delta_x_.tail(n) delta_x_rr;delta_x_.head(m) delta_x_mm;memcpy(delta_x_array_, delta_x_.data(), sizeof(double) * (int)delta_x_.size());//转为数组ROS_DEBUG(\nin solveLinearSystem solve equation cost %f ms, t_Schur_PCG.toc());
#elseTicToc t_solve_equation;
// delta_x_ Hessian_.ldlt().solve(b_);int pcg_iter_num Hessian_.rows()1; // PCG迭代次数,原来给的是rows()*2delta_x_ pcgSolver(Hessian_, b_, pcg_iter_num); //0.3msROS_DEBUG(\nin solveLinearSystem solve equation cost %f ms, pcg_iter_num: %d, t_solve_equation.toc(), pcg_iter_num);//15msmemcpy(delta_x_array_, delta_x_.data(), sizeof(double) * (int)delta_x_.size());//转为数组供状态更新使用ROS_DEBUG_STREAM(\nhere3 solve complete, delta_x_.size() delta_x_.size() , delta_x_.norm() delta_x_.norm() , delta_x_.squaredNorm() delta_x_.squaredNorm() );ROS_DEBUG_STREAM(\ndelta_x_: delta_x_.transpose());
#endif
}double Solver::dlComputeDenom(const Eigen::VectorXd h_dl, const Eigen::VectorXd h_gn,const double dl_alpha, const double dl_beta) const {if(h_dl h_gn)return currentChi_;else if(h_dl b_ / b_.norm() * radius_)return ( radius_ * (2.0 * (dl_alpha * b_).norm() - radius_) ) / (2 * dl_alpha); //由于取norm()所以b_符号不变elsereturn 0.5 * dl_alpha * pow(1 - dl_beta, 2) * b_.squaredNorm() dl_beta * (2 - dl_beta) * currentChi_;
}//求解器相关函数
bool Solver::solve() {if(method_kLM) {// ROS_DEBUG(\nlinearized_jacobians (rows, cols) (%lu, %lu), linearized_jacobians.rows(), linearized_jacobians.cols());TicToc t_LM_init;// LM 初始化computeLambdaInitLM();ROS_DEBUG(\nsolver computeLambdaInitLM %f ms, t_LM_init.toc());// LM 算法迭代求解bool stop false;int iter 0;//尝试的lambda次数try_iter_ 0;if(strategy_1) {false_theshold_ 10;} else if(strategy_2) {false_theshold_ 10;} else if (strategy_3) {false_theshold_ 6;}ROS_DEBUG(\nstrategy: %d, false_theshold_: %d, strategy_, false_theshold_);//保存LM阻尼阻尼系数lambda
/* file_name_ ./lambda.csv;
FILE *tmp_fp fopen(file_name_.data(), a);
fprintf(tmp_fp, iter, lambda\n);
fflush(tmp_fp);
fclose(tmp_fp);*/TicToc t_LM_iter;while (!stop (iter iterations_)) {ROS_DEBUG_STREAM(\niter: iter , chi currentChi_ , Lambda currentLambda_ \n);bool oneStepSuccess false;int false_cnt 0;while (!oneStepSuccess) // 不断尝试 Lambda, 直到成功迭代一步{try_iter_;// setLambdaTicToc t_addLambdatoHessianLM;addLambdatoHessianLM();//0.01msROS_DEBUG(\naddLambdatoHessianLM cost %f ms, t_addLambdatoHessianLM.toc());// 第四步解线性方程 H X BTicToc t_solveLinearSystem;solveLinearSystem();//8msROS_DEBUG(\nsolveLinearSystem cost %f ms, t_solveLinearSystem.toc());TicToc t_removeLambdaHessianLM;removeLambdaHessianLM();//0.005msROS_DEBUG(\nremoveLambdaHessianLM cost %f ms, t_removeLambdaHessianLM.toc());// 优化退出条件1 delta_x_ 很小则退出 原来是1e-6if (delta_x_.squaredNorm() 1e-10 || false_cnt false_theshold_) {stop true;ROS_DEBUG(\ndelta_x too small: %e, or false_cnt%d 10 break, delta_x_.squaredNorm(), false_cnt);//都是在这出去的break;} else {ROS_DEBUG_STREAM(\ndelta_x_ squaredNorm matched: delta_x_.squaredNorm() , delta_x_ size: delta_x_.size() , delta_x: delta_x_.transpose() );}TicToc t_updateStates;updateStates(-1.0);//0.08ms 1.更新状态 2.preMakeHessian()计算新的residualROS_DEBUG(\nupdateStates cost %f ms, t_updateStates.toc());// 判断当前步是否可行以及 LM 的 lambda 怎么更新oneStepSuccess isGoodStepInLM();//误差是否下降// 后续处理if (oneStepSuccess) {TicToc t_backupStates;backupStates();//若求解成功则备份当前更新的状态量 0.03msROS_DEBUG(\nbackupStates cost %f ms, t_backupStates.toc());// 在新线性化点 构建 hessianTicToc t_makeHessian;makeHessian();double makeHessian_finish_time t_makeHessian.toc();*makeHessian_time_sum_ makeHessian_finish_time;(*makeHessian_times_);ROS_DEBUG(\nmakeHessian cost: %f ms, avg_makeHessian_time: %f ms, makeHessian_time_sum_: %f, makeHessian_times_: %f,makeHessian_finish_time, (*makeHessian_time_sum_)/(*makeHessian_times_), *makeHessian_time_sum_, *makeHessian_times_);// TODO:: 这个判断条件可以丢掉条件 b_max 1e-12 很难达到这里的阈值条件不应该用绝对值而是相对值
// double b_max 0.0;
// for (int i 0; i b_.size(); i) {
// b_max max(fabs(b_(i)), b_max);
// }
// // 优化退出条件2 如果残差 b_max 已经很小了那就退出
// stop (b_max 1e-12);false_cnt 0;} else {false_cnt;TicToc t_rollbackStates;rollbackStates(); // 误差没下降回滚 0.05msROS_DEBUG(\nrollbackStates cost %f ms, t_rollbackStates.toc());}ROS_DEBUG(\nfalse_cnt: %d, false_cnt);}iter;// 优化退出条件3 currentChi_ 跟第一次的chi2相比下降了 1e6 倍则退出if (sqrt(currentChi_) stopThresholdLM_) {ROS_DEBUG(\ncurrentChi_ decrease matched break condition);stop true;}}ROS_DEBUG(\nLM iterate %f ms, t_LM_iter.toc());
/* std::cout problem solve cost: t_solve.toc() ms std::endl;
std::cout makeHessian cost: t_hessian_cost_ ms std::endl;*/} else if(method_kDOGLEG) {ROS_DEBUG(\nDL iter num: %d, iterations_);//1.初始化 radiusgbJ^Teradius_ 1;epsilon_1_ 1e-10;//向量无穷范数cwiseAbscoordinate-wise逐元素取绝对值colwise().sum()计算每行的绝对值之和maxCoeff()得最大值bool use_last_hessian true;bool stop (linearized_residuals.lpNormEigen::Infinity() epsilon_3_) ||(b_.lpNormEigen::Infinity() epsilon_1_);int iter 0;double rho, tempChi;double rho_numerator, rho_denominator;bool stop_cond_1;currentChi_ computeChi();//2.循环 stop||e||无穷范数阈值3 或 ||g||无穷范数阈值1while (!stop (iter iterations_)) {ROS_DEBUG_STREAM(\niter: iter , chi currentChi_ , radius_ radius_ \n);
// if(!use_last_hessian)
// makeHessian();double dl_alpha b_.squaredNorm() / (linearized_jacobians * b_).squaredNorm();//都是取二范数平方就不区分b的符号了//3.计算h_sdEigen::VectorXd h_sd dl_alpha * b_;//4.计算g_gnsolveLinearSystem();//比较耗时Eigen::VectorXd h_gn delta_x_;//5.计算h_dlEigen::VectorXd h_dl;Eigen::VectorXd a dl_alpha * h_sd;Eigen::VectorXd b h_gn;double c a.transpose() * (b - a);double dl_beta;if(c0) {double tmp_1 (b-a).squaredNorm();dl_beta (-c sqrt(pow(c,2) tmp_1 * (pow(radius_, 2) - a.squaredNorm()))) / tmp_1;} else {double tmp_1 pow(radius_, 2) - a.squaredNorm();dl_beta tmp_1 / (c sqrt(pow(c, 2) (b-a).squaredNorm() * tmp_1));}if(b.norm() radius_)h_dl h_gn;else if(a.norm() radius_)h_dl h_sd / h_sd.norm() * radius_;else {h_dl a dl_beta * (h_gn - a);}//判断是否需要继续迭代get_cur_parameter(cur_x_array_);Eigen::Mapconst Eigen::VectorXd x{cur_x_array_, x_size_};double tmp_1 h_dl.norm();double tmp_2 epsilon_2_*(x.norm() epsilon_2_);stop_cond_1 tmp_1 tmp_2;ROS_DEBUG(\ntmp_1: %f, tmp_2: %f, stop_cond_1: %d, tmp_1, tmp_2, stop_cond_1);if(stop_cond_1) {//设为1e-12stop true;} else {//更新updateStates(-1.0);tempChi computeChi();rho 0.0;rho_numerator currentChi_ - tempChi;rho_denominator dlComputeDenom(h_dl, h_gn, dl_alpha, dl_beta);rho rho_numerator / rho_denominator;if(rho 0) {//执行更新即保存备份backupStates();preMakeHessian();TicToc t_makeHessian;makeHessian();double makeHessian_finish_time t_makeHessian.toc();*makeHessian_time_sum_ makeHessian_finish_time;(*makeHessian_times_);ROS_DEBUG(\nmakeHessian cost: %f ms, avg_makeHessian_time: %f ms, makeHessian_time_sum_: %f, makeHessian_times_: %f,makeHessian_finish_time, (*makeHessian_time_sum_)/(*makeHessian_times_), *makeHessian_time_sum_, *makeHessian_times_);use_last_hessian false;} else {rollbackStates();use_last_hessian true;}get_cur_parameter(cur_x_array_);if(rho 0.75) {radius_ std::max(radius_, 3*h_dl.norm());} else {radius_ 0.5 * radius_;stop radius_ epsilon_2_*(x.norm() epsilon_2_);}}ROS_DEBUG(\nDL: radius_: %e, rho: %f, rho0 %d, currentChi_: %e, tempChi: %e, rho_numerator: %e, rho_denominator: %e,radius_, rho, rho0, currentChi_, tempChi, rho_numerator, rho_denominator);ROS_DEBUG_STREAM(\ndelta_x_.squaredNorm(): delta_x_.squaredNorm() , delta_x_: delta_x_.transpose() \nb_.norm(): b_.norm() , b_: b_.transpose());iter;}ROS_DEBUG(\n finish DL iter, stop: %d or iter: %d %d, stop, iter, iterations_);//6.根据h_dl.norm()判断是否迭代// 若迭代则更新x(涉及状态更新residual计算)计算rho根据rho来更新x和raidus}return true;
}
}9.3 solve.h
#ifndef CATKIN_WS_SOLVE_H
#define CATKIN_WS_SOLVE_H#include ros/ros.h
#include ros/console.h
#include cstdlib
#include pthread.h
#include ceres/ceres.h
#include unordered_map#include eigen_types.h
#include ../utility/tic_toc.h
#include ../parameters.hnamespace solve {/*定义factor管理类* */
const int NUM_THREADS 4;struct ResidualBlockInfo
{ResidualBlockInfo(ceres::CostFunction *_cost_function, ceres::LossFunction *_loss_function, std::vectordouble * _parameter_blocks, std::vectorint _drop_set): cost_function(_cost_function), loss_function(_loss_function), parameter_blocks(_parameter_blocks), drop_set(_drop_set) {}void Evaluate();ceres::CostFunction *cost_function;ceres::LossFunction *loss_function;std::vectordouble * parameter_blocks;//优化变量数据的地址sizes每个优化变量块的变量大小以IMU残差为例为[7,9,7,9]std::vectorint drop_set;//待边缘化的优化变量iddouble **raw_jacobians;//二重指针,是为了配合ceres的形参 double** jacobiansstd::vectorEigen::Matrixdouble, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor jacobians;//这个数据结构看不懂Eigen::VectorXd residuals;//残差 IMU:15X1 视觉:2X1int localSize(int size){return size 7 ? 6 : size;}
};struct ThreadsStruct
{std::vectorResidualBlockInfo * sub_factors;Eigen::MatrixXd A;Eigen::VectorXd b;std::unordered_maplong, int parameter_block_size; //global sizestd::unordered_maplong, int parameter_block_idx; //local size
};class Solver
{
public:Solver(uint8_t strategy): method_(kLM), iterations_(1), strategy_(strategy), lm_alpha_(1), mem_allocated_(false){};~Solver();int localSize(int size) const;int globalSize(int size) const;void addResidualBlockInfo(ResidualBlockInfo *residual_block_info);//加残差块相关信息(优化变量、待marg的变量)void preMakeHessian();//计算每个残差对应的Jacobian并更新parameter_block_datavoid makeHessian();//pos为所有变量维度m为需要marg掉的变量n为需要保留的变量std::vectordouble * getParameterBlocks(std::unordered_maplong, double * addr_shift);std::vectorResidualBlockInfo * factors;//所有观测项int m, n;//m为要边缘化的变量个数(也就是parameter_block_idx的总localSize以double为单位VBias为9PQ为6)n为要保留下来的变量个数//parameter_block_size 和 parameter_block_data分别对应block的大小和实际数据std::unordered_maplong, int parameter_block_size; //global size 优化变量内存地址,localSizeint sum_block_size;std::unordered_maplong, int parameter_block_idx; //local size 排序前是待边缘化的优化变量内存地址,在parameter_block_size中的id排序后是marg, idm维 remain, idn维std::unordered_maplong, double * parameter_block_data;//优化变量内存地址,数据std::unordered_maplong, double * parameter_block_data_backup;//优化变量内存地址,数据std::vectorint keep_block_size; //global sizestd::vectorint keep_block_idx; //local sizestd::vectordouble * keep_block_data;//之前看到的帖子说这是在marg过程中反解出的线性化点的参数值x0Eigen::MatrixXd linearized_jacobians;//线性化点处的JacobianEigen::VectorXd linearized_residuals;//线性化点处的residualconst double eps 1e-8;bool solve();void solveLinearSystem();/// 解线性方程bool updateStates(double weight) ;/// 更新状态变量bool backupStates();//回滚状态变量bool rollbackStates(); // 有时候 update 后残差会变大需要退回去重来double computeChi() const;void computeLambdaInitLM();/// 计算LM算法的初始Lambdavoid addLambdatoHessianLM();/// Hessian 对角线加上或者减去 Lambdavoid removeLambdaHessianLM();bool isGoodStepInLM();/// LM 算法中用于判断 Lambda 在上次迭代中是否可以以及Lambda怎么缩放Eigen::MatrixXd pcgSolver(const MatXX A, const VecX b, int maxIter);/// PCG 迭代线性求解器enum SolveMethod{kLM 0,kDOGLEG 1};SolveMethod method_;int iterations_;//迭代轮数double currentChi_;//LM参数double currentLambda_;//LM中的阻尼因子DOGLEG中的radiusdouble stopThresholdLM_; // LM 迭代退出阈值条件std::string file_name_;int try_iter_;int false_theshold_;//每轮迭代允许的最大失败次数double ni_; //strategy3控制 Lambda 缩放大小double lm_alpha_; //strategy2更新使用的alpha//求解结果
// VecX delta_x_rr_;
// VecX delta_x_mm_;//DL参数double radius_;double epsilon_1_, epsilon_2_, epsilon_3_;
// double dl_alpha_;/// 整个信息矩阵Eigen::MatrixXd Hessian_;Eigen::VectorXd b_;Eigen::VectorXd delta_x_;//多留100的余量这个是成员变量在程序中是局部变量放在栈区不需要手动释放内存因为它会在其作用域结束时自动被销毁const int x_size_ 1000 (WINDOW_SIZE 1) * (SIZE_POSE SIZE_SPEEDBIAS) SIZE_POSE 1 100;double cur_x_array_[1000 (WINDOW_SIZE 1) * (SIZE_POSE SIZE_SPEEDBIAS) SIZE_POSE 1 100];double delta_x_array_[1000 (WINDOW_SIZE 1) * (SIZE_POSE SIZE_SPEEDBIAS) SIZE_POSE 1 100];//是否已调用preMakeHessian分配过内存bool mem_allocated_;uint8_t strategy_;double *makeHessian_time_sum_;//这个需要手撸才能统计时间ceres无法统计double *makeHessian_times_;private:bool get_cur_parameter(double* cur_x_array);double dlComputeDenom(const Eigen::VectorXd h_dl, const Eigen::VectorXd h_gn,const double dl_alpha, const double dl_beta) const;};
}
#endif //CATKIN_WS_SOLVE_H后面这些是在debug时的一些记录可以略过不看。
9.4 系统整体待优化参数维度debug WINDOW内所有待优化变量维度统计估计 t d t_d td无快速重定位时共 172 L 172L 172L其中 L L L为WINDOW内的landmark数量 debug代码定义两个unordered_map容器param_addr_check,landmark_addr_check注意SIZE_POSE要-1去除冗余来统计 //用于check维度std::unordered_maplong, uint8_t param_addr_check;//所有param维度std::unordered_maplong, uint8_t landmark_addr_check;//landmark维度//1.添加边缘化残差先验部分size_t size_10;if (last_marginalization_info){// construct new marginlization_factorMarginalizationFactor *marginalization_factor new MarginalizationFactor(last_marginalization_info);//里面设置了上次先验的什么size现在还不懂problem.AddResidualBlock(marginalization_factor, NULL,last_marginalization_parameter_blocks);// /*用于check维度是否正确*/
// parameter_block_size[reinterpret_castlong(addr)] size;for(int i0; ilast_marginalization_parameter_blocks.size(); i) {if(last_marginalization_info-parameter_block_size[reinterpret_castlong(last_marginalization_parameter_blocks[i])]1) {ROS_DEBUG(here have 1 dimend);
// landmark_addr_check[reinterpret_castlong(last_marginalization_parameter_blocks[i])] 1;}size_t tmp_size last_marginalization_info-parameter_block_size[reinterpret_castlong(last_marginalization_parameter_blocks[i])];tmp_size tmp_size7 ? 6: tmp_size;//这个double*的地址代表的待优化变量的local_size把每个地址都记录在map中分配给待优化变量的地址都是连续的for(int j0; jtmp_size; j) {param_addr_check[reinterpret_castlong(last_marginalization_parameter_blocks[i]) (double)j * (long) sizeof(long)] 1;}}//打印prior的Jacobian维度ROS_DEBUG(\nlinearized_jacobians (rows, cols) (%lu, %lu),last_marginalization_info-linearized_jacobians.rows(), last_marginalization_info-linearized_jacobians.cols());size_1 param_addr_check.size();//应该是76 实际87ROS_DEBUG(\nprior size1%lu, param_addr_check.size() %lu, landmark size: %lu, except landmark size %lu,size_1, param_addr_check.size(), landmark_addr_check.size(), param_addr_check.size()-landmark_addr_check.size());//landmark_addr_check中多加了个td//2.添加IMU残差for (int i 0; i WINDOW_SIZE; i){int j i 1;//两帧KF之间IMU积分时间过长的不进行优化可能lostif (pre_integrations[j]-sum_dt 10.0)continue;IMUFactor* imu_factor new IMUFactor(pre_integrations[j]);//这里的factor就是残差residualceres里面叫factorproblem.AddResidualBlock(imu_factor, NULL, para_Pose[i], para_SpeedBias[i], para_Pose[j], para_SpeedBias[j]);//check维度long addr reinterpret_castlong(para_Pose[i]);if(param_addr_check.find(addr) param_addr_check.end()) {ROS_DEBUG(\nIMU add para_Pose[%d], i);for(int k0; kSIZE_POSE-1; k) {param_addr_check[addr (long)k * (long)sizeof(long)] 1;}}addr reinterpret_castlong(para_SpeedBias[i]);if(param_addr_check.find(addr) param_addr_check.end()) {ROS_DEBUG(\nIMU add para_SpeedBias[%d], i);for(int k0; kSIZE_SPEEDBIAS; k) {param_addr_check[addr (long) k * (long) sizeof(long)] 1;}}addr reinterpret_castlong(para_Pose[j]);if(param_addr_check.find(addr) param_addr_check.end()) {ROS_DEBUG(\n IMU add para_Pose[%d], j);for(int k0; kSIZE_POSE-1; k) {param_addr_check[addr (long) k * (long) sizeof(long)] 1;}}addr reinterpret_castlong(para_SpeedBias[j]);if(param_addr_check.find(addr) param_addr_check.end()) {ROS_DEBUG(\n IMU add para_SpeedBias[%d], j);for (int k 0; k SIZE_SPEEDBIAS; k) {param_addr_check[addr (long) k * (long) sizeof(long)] 1;}}}size_t size_2 param_addr_check.size() - size_1;//应该是81 V2~V10 实际97为啥ROS_DEBUG(\nIMU size2%lu, param_addr_check.size() %lu, landmark size: %lu, except landmark size %lu,size_2, param_addr_check.size(), landmark_addr_check.size(), param_addr_check.size()-landmark_addr_check.size());//landmark_addr_check中多加了个td//3.添加视觉残差int f_m_cnt 0;int feature_index -1;for (auto it_per_id : f_manager.feature){it_per_id.used_num it_per_id.feature_per_frame.size();//!(至少两次tracking且最新帧1st的tracking不能算因为1st可能被marg掉start_frame最大是[7])if (!(it_per_id.used_num 2 it_per_id.start_frame WINDOW_SIZE - 2))continue;feature_index;int imu_i it_per_id.start_frame, imu_j imu_i - 1;Vector3d pts_i it_per_id.feature_per_frame[0].point;//这个id的feature的第一帧和后面所有的帧分别构建residual blockfor (auto it_per_frame : it_per_id.feature_per_frame){imu_j;if (imu_i imu_j){continue;}Vector3d pts_j it_per_frame.point;//是否要time offsetif (ESTIMATE_TD){//对于一个feature都跟[it_per_id.start_frame]帧进行优化ProjectionTdFactor *f_td new ProjectionTdFactor(pts_i, pts_j, it_per_id.feature_per_frame[0].velocity, it_per_frame.velocity,it_per_id.feature_per_frame[0].cur_td, it_per_frame.cur_td,it_per_id.feature_per_frame[0].uv.y(), it_per_frame.uv.y());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]);//check维度for(int k0; kSIZE_POSE-1; k) {param_addr_check[reinterpret_castlong(para_Pose[imu_i]) (long)k * (long)sizeof(long)] 1;}for(int k0; kSIZE_POSE-1; k) {param_addr_check[reinterpret_castlong(para_Pose[imu_j]) (long)k * (long)sizeof(long)] 1;}for(int k0; kSIZE_POSE-1; k) {param_addr_check[reinterpret_castlong(para_Ex_Pose[0]) (long)k * (long)sizeof(long)] 1;}param_addr_check[reinterpret_castlong(para_Feature[feature_index])] 1;landmark_addr_check[reinterpret_castlong(para_Feature[feature_index])] 1;param_addr_check[reinterpret_castlong(para_Td[0])] 1;/*double **para new double *[5];para[0] para_Pose[imu_i];para[1] para_Pose[imu_j];para[2] para_Ex_Pose[0];para[3] para_Feature[feature_index];para[4] para_Td[0];f_td-check(para);*/}else{ProjectionFactor *f new ProjectionFactor(pts_i, pts_j);problem.AddResidualBlock(f, loss_function, para_Pose[imu_i], para_Pose[imu_j], para_Ex_Pose[0], para_Feature[feature_index]);//check维度for(int k0; kSIZE_POSE-1; k) {param_addr_check[reinterpret_castlong(para_Pose[imu_i]) (long)k * (long)sizeof(long)] 1;}for(int k0; kSIZE_POSE-1; k) {param_addr_check[reinterpret_castlong(para_Pose[imu_j]) (long)k * (long)sizeof(long)] 1;}for(int k0; kSIZE_POSE-1; k) {param_addr_check[reinterpret_castlong(para_Ex_Pose[0]) (long)k * (long)sizeof(long)] 1;}param_addr_check[reinterpret_castlong(para_Feature[feature_index])] 1;landmark_addr_check[reinterpret_castlong(para_Feature[feature_index])] 1;}f_m_cnt;}}size_t size_3 param_addr_check.size() - size_1 - size_2;//应该和landmark_addr_check.size一样ROS_DEBUG(\nvisual size3%lu, param_addr_check.size() %lu, landmark size: %lu, except landmark size %lu,size_3, param_addr_check.size(), landmark_addr_check.size(), param_addr_check.size()-landmark_addr_check.size());//landmark_addr_check中多加了个tdROS_DEBUG(visual measurement count: %d, f_m_cnt);//总的视觉观测个数观测可能是在不同帧对同一个landmark进行观测所以可能查过1000注意与landmark个数进行区分ROS_DEBUG(prepare for ceres: %f, t_prepare.toc());//4.添加闭环检测残差计算滑动窗口中与每一个闭环关键帧的相对位姿这个相对位置是为后面的图优化(pose graph)准备 或者是 快速重定位(崔华坤PDF7.2节)//这里注意relo_pose是Tw2_bi Tw2_w1 * Tw1_biif(relocalization_info){//printf(set relocalization factor! \n);ceres::LocalParameterization *local_parameterization new PoseLocalParameterization();problem.AddParameterBlock(relo_Pose, SIZE_POSE, local_parameterization);int retrive_feature_index 0;int feature_index -1;for (auto it_per_id : f_manager.feature){it_per_id.used_num it_per_id.feature_per_frame.size();if (!(it_per_id.used_num 2 it_per_id.start_frame WINDOW_SIZE - 2))continue;feature_index;int start it_per_id.start_frame;if(start relo_frame_local_index)//必须之前看到过{//1.先在i中match的点中找到可能是现在这个feature的id的indexwhile((int)match_points[retrive_feature_index].z() it_per_id.feature_id)//.z()存的是ij两帧match上的feature的id{retrive_feature_index;}//2.如果是则WINDOW内的it_per_id.feature_id这个id的landmark就是被loop上的landmark,取归一化坐标if((int)match_points[retrive_feature_index].z() it_per_id.feature_id){//pts_j是i帧的归一化平面上的点这里理解relo_Pose及其重要relo_Pose实际上是Tw2_bi视觉重投影是从WINDOW内的start帧的camera(在w2系下)投影到i帧(在w1系下)耦合了Tw1_w2Vector3d pts_j Vector3d(match_points[retrive_feature_index].x(), match_points[retrive_feature_index].y(), 1.0);Vector3d pts_i it_per_id.feature_per_frame[0].point;//start中的点ProjectionFactor *f new ProjectionFactor(pts_i, pts_j);//relo_Pose是Tw2_biproblem.AddResidualBlock(f, loss_function, para_Pose[start], relo_Pose, para_Ex_Pose[0], para_Feature[feature_index]);retrive_feature_index;//check维度for(int k0; kSIZE_POSE-1; k) {param_addr_check[reinterpret_castlong(para_Pose[start]) (long)k * (long)sizeof(long)] 1;}for(int k0; kSIZE_POSE-1; k) {param_addr_check[reinterpret_castlong(relo_Pose) (long)k * (long)sizeof(long)] 1;}for(int k0; kSIZE_POSE-1; k) {param_addr_check[reinterpret_castlong(para_Ex_Pose[0]) (long)k * (long)sizeof(long)] 1;}param_addr_check[reinterpret_castlong(para_Feature[feature_index])] 1;landmark_addr_check[reinterpret_castlong(para_Feature[feature_index])] 1;ROS_DEBUG(\nhas relocation blocks\n);} }}}size_t size_4 param_addr_check.size() - size_1 - size_2 - size_3;//没有loop时应该为0ROS_DEBUG(\nrelocation size_4%lu, param_addr_check.size() %lu, landmark size: %lu, except landmark size %lu,size_4, param_addr_check.size(), landmark_addr_check.size(), param_addr_check.size()-landmark_addr_check.size());//landmark_addr_check中多加了个td
9.5 LM的 λ \lambda λ初始化中的 τ \tau τ取值是否合适 上面给的 τ \tau τ在 [ 1 e − 8 , 1 ] [1e-8,1] [1e−8,1] 这个范围内不是绝对的要根据实际的情况来设定
太大可能导致 λ \lambda λ过大步长过小求出的 Δ x \Delta x Δx过小更新不动 x x x太小可能导致 λ \lambda λ过大步长过大 ρ \rho ρ一直 0 0 0无法找到正确的迭代方向 x x x一直无法更新。
比如VINS-MONO中如果使用strategy3则 τ \tau τ大概为 1 e − 15 1e^{-15} 1e−15数量级左右。
9.6 Schur消元求解之后更新先验的residual χ \chi χ实际上就是 e T W e e^TWe eTWe 线性化点再挖一个坑 恍然大悟的evaluate()函数得Jacobian 在preMarginalize()中调用的因为marg不用ceres不会在内部维护jacobian所以需要手动调用计算Jacobian多态调用evaluate()根据上面二重指针从raw_jacobians传给jacobians所以在ThreadsConstructA中可以拿到Jacobian实在是妙。
根据以上理解在LM中每次更新完 x x Δ x xx\Delta x xxΔx后就需要重新计算一下Jacobian和residual下次evaluate时就会用到新的residual
对于prior的residual和VINS-MONO保持一样不动Jacobian的值。
9.7 计算 χ e T W e \chie^TWe χeTWe时需要考虑loss_function
在ResidualBlockInfo::Evaluate()中调用的多态Evaluate()函数后已经考虑了loss_function对Jacobian和residual的加权
9.8 先验的参数实际上就是V0,P0~P10,Tbc,td而不是一个单独的特殊量
9.9 Hessian可视化
之前Debug b的时候曾经反解出J并打印出来过可以看到 J T J J^TJ JTJ还是非常稀疏的 反解之后再算 J T ∗ J J^T*J JT∗J有些为0的已经不为0了但是数值很小大概是1e-10数量级但主对角线可以看出还是正确的所以反解总体上没问题。实际上J的rows()应该是观测的总维度(包括11个P11个V1个Tbc1个tdL个landmark所有观测注意不是L观测道一次就有二维的reidual参考《14讲》P247理解)但是从Hessian中只能反解出相同维度的 J J J此处为 316 172 L 316172L 316172L所以不知道观测的总维度为多少。
9.10 load pose_graph
load pose_graph test: load pose_graph之后发现就很容易产生loop了因为有了之前的特征和描述子在rviz中可以看到产生了非常多的loop边且从一开始就有loop如果是在同一个地方不同的数据集这样对于重定位就比较友好。
