第十章 基于优化的定位方法

本章是基于先验地图的图优化方法，先验地图的构建可参考多传感器融合定位 第九章 基于优化的建图方法

代码下载：

1.环境配置：

出现以下问题，是由于 make_unique 是c++ 14的新特性，需要在CMakelists.txt 中添加c++14 的编译指向。参考链接

lidar_localization/src/models/sliding_window/ceres_sliding_window.cpp:25:38: error: ‘make_unique’ is not a member of ‘std’config_.loss_function_ptr = std::make_unique<ceres::CauchyLoss>(new ceres::CauchyLoss(1.0));

解决， 在CMakelists.txt 添加

set(CMAKE_CXX_STANDARD 14)

2.因子图优化代码补充

参考博客：

因子图优化 SLAM 研究方向归纳

多传感器融合定位 第九章 基于优化的建图方法

多传感器融合定位理论基础（十二）：滑动窗口原理及应用

本章因子图优化，涉及的内容较多，建议先修参考一下内容：

VINS系列相关代码：滑动窗口部分

邱笑晨博士的《预积分总结与公式推导》(发表于泡泡机器人公众号)

机器人状态估计：理论推导部分

深蓝学院《从零手写VIO》课程：滑窗和边缘话理论讲解 (重点参考)

多传感器融合定位理论基础（十二）：滑动窗口原理及应用 (重点参考)

以下代码补充的公式摘自GeYao助教的手推公式，图表摘自任乾老师课上的笔记，需要补全的代码主要有两大类：

代码参考：1. GEYAO助教的github 2.张嘉皓助教的github

1.实现预积分、地图匹配、边缘化、帧间匹配四种优化因子。

2.将上述四种约束因子，加入滑窗，进行优化

2.1 实现预积分、地图匹配、边缘化、帧间匹配四种优化因子

2.1.1 激光里程计因子

a.示意图

b.激光里程计因子 残差函数和雅克比矩阵 推导

注意的是，GeYao助教的公式推导和代码少了，位置残差对姿态的雅克比，以下为补充的推导。

c.代码补全

FILE: factor_prvag_relative_pose.hpp

    //// TODO: get square root of information matrix://Eigen::Matrix<double, 6, 6>  sqrt_info =  Eigen::LLT<Eigen::Matrix<double, 6 ,6>>(I_).matrixL().transpose() ;

    //// TODO: compute residual://Eigen::Map<Eigen::Matrix<double, 6 ,1>>  residual(residuals);     //  残差 r_P  r_Rresidual.block(INDEX_P,  0 , 3 , 1)   =  ori_i.inverse() * (pos_j - pos_i) - pos_ij ;residual.block(INDEX_R, 0  , 3 , 1)   =  (ori_i.inverse()*ori_j*ori_ij.inverse()).log( );

    //// TODO: compute jacobians:     因为是二元边，所以需要求解两个jacobian//if ( jacobians ) {// compute shared intermediate results:const Eigen::Matrix3d R_i_inv = ori_i.inverse().matrix();const Eigen::Matrix3d J_r_inv = JacobianRInv(residual.block(INDEX_R, 0 ,3 , 1));    //  右雅克比const Eigen::Vector3d pos_ij = ori_i.inverse() * (pos_j - pos_i) ;if ( jacobians[0] ) {      //   残差rL0(rp  rq )  对 T0(p q)  M0(v ba bg) 的雅克比 // implement computing:Eigen::Map<Eigen::Matrix<double, 6 , 15,  Eigen::RowMajor>>  jacobian_i (jacobians[0] );  //  col : rp_i[3] rq_i[3]  row : p[3] q[3] v[3] ba[3] bg[3]  jacobian_i.setZero();jacobian_i.block<3, 3>(INDEX_P, INDEX_P) =  -R_i_inv;jacobian_i.block<3, 3>(INDEX_R,INDEX_R)  =  -J_r_inv*(ori_ij*ori_j.inverse()*ori_i).matrix();jacobian_i.block<3, 3>(INDEX_P,INDEX_R) =  Sophus::SO3d::hat(pos_ij).matrix();jacobian_i = sqrt_info * jacobian_i ;        //   注意 sqrt_i 为对角的协方差矩阵对角线为观测的方差，可理解为传感器的测量误差，用于调整权重用}if ( jacobians[1] ) {      //  残差rL0(rp  rq )  对 T0(p q)  M0(v ba bg) 的雅克比 // implement computing:Eigen::Map<Eigen::Matrix<double, 6 ,15, Eigen::RowMajor>> jacobian_j (jacobians[1]);jacobian_j.setZero();jacobian_j.block<3, 3>(INDEX_P, INDEX_P) = R_i_inv;jacobian_j.block<3, 3>(INDEX_R,INDEX_R)  = J_r_inv*ori_ij.matrix();jacobian_j = sqrt_info * jacobian_j ;}}

    //// TODO: correct residual by square root of information matrix://residual = sqrt_info * residual;

2.1.2 地图匹配因子

a.示意图

b.地图匹配因子 残差函数和雅克比矩阵 推导

c.代码补全
    //// TODO: get square root of information matrix://// Cholesky 分解 : http://eigen.tuxfamily.org/dox/classEigen_1_1LLT.htmlEigen::Matrix<double, 6, 6> sqrt_info = Eigen::LLT<Eigen::Matrix<double, 6, 6>>(I_).matrixL().transpose();
    //// TODO: compute residual://Eigen::Map<Eigen::Matrix<double, 6 ,1>>  residual(residuals);residual.block(INDEX_P, 0 , 3 , 1 )   =  pos - pos_prior ;residual.block(INDEX_R,0 , 3 , 1 )    = (ori*ori_prior.inverse()).log();

    //// TODO: compute jacobians:   一元边//if ( jacobians ) {if ( jacobians[0] ) {// implement jacobian computing:Eigen::Map<Eigen::Matrix<double, 6, 15,  Eigen::RowMajor>> jacobian_prior(jacobians[0] );jacobian_prior.setZero();jacobian_prior.block<3, 3>(INDEX_P,  INDEX_P) = Eigen::Matrix3d::Identity();jacobian_prior.block<3, 3>(INDEX_R, INDEX_R)  = JacobianRInv(residual.block(INDEX_R, 0, 3, 1)) * ori_prior.matrix();jacobian_prior = sqrt_info * jacobian_prior ;}}

    //// TODO: correct residual by square root of information matrix://residual = sqrt_info * residual;

2.1.3 IMU预积分因子

a.示意图

b.IMU预积分因子 残差函数和雅克比矩阵 推导

GeYao助教的推导为基于SO3形式的推导，代码也是SO3的形式。四元数的推导部分上一章已推导完成，可参考多传感器融合定位 第九章 基于优化的建图方法

c.代码补全
    //// TODO: get square root of information matrix:// Cholesky 分解 : http://eigen.tuxfamily.org/dox/classEigen_1_1LLT.htmlEigen::LLT<Eigen::Matrix<double,15,15>> LowerI(I_);// sqrt_info 为上三角阵Eigen::Matrix<double,15,15> sqrt_info = LowerI.matrixL().transpose();

    //// TODO: compute residual://Eigen::Map<Eigen::Matrix<double, 15, 1>>  residual(residuals);residual.block<3, 1>(INDEX_P, 0) = ori_i.inverse().matrix() * (pos_j - pos_i - (vel_i - 0.5 * g_ * T_) * T_) - alpha_ij ;residual.block<3, 1>(INDEX_R,0) = (Sophus::SO3d::exp(theta_ij).inverse()*ori_i.inverse()*ori_j).log(); residual.block<3, 1>(INDEX_V,0)  = ori_i.inverse()* (vel_j - vel_i + g_ * T_) - beta_ij ;residual.block<3, 1>(INDEX_A,0)  = b_a_j - b_a_i ; residual.block<3, 1>(INDEX_G,0)  = b_g_j - b_g_i;

    //// TODO: compute jacobians:   imu预积分的残差 对状态量的雅克比，第九章已推导//if ( jacobians ) {// compute shared intermediate results:const Eigen::Matrix3d R_i_inv = ori_i.inverse().matrix();const Eigen::Matrix3d J_r_inv = JacobianRInv(residual.block(INDEX_R, 0 ,3 , 1));    //  右雅克比if ( jacobians[0] ) {Eigen::Map<Eigen::Matrix<double, 15 , 15,  Eigen::RowMajor>>  jacobian_i (jacobians[0] );  jacobian_i.setZero();// a. residual, position:jacobian_i.block<3, 3>(INDEX_P, INDEX_P) = -R_i_inv;jacobian_i.block<3, 3>(INDEX_P, INDEX_R) = Sophus::SO3d::hat(ori_i.inverse() * (pos_j - pos_i - (vel_i - 0.50 * g_ * T_) * T_));jacobian_i.block<3, 3>(INDEX_P, INDEX_V) = -T_*R_i_inv;jacobian_i.block<3, 3>(INDEX_P, INDEX_A) = -J_.block<3,3>(INDEX_P, INDEX_A);jacobian_i.block<3, 3>(INDEX_P, INDEX_G) = -J_.block<3,3>(INDEX_P, INDEX_G);// b. residual, orientation:jacobian_i.block<3, 3>(INDEX_R, INDEX_R) = -J_r_inv*(ori_j.inverse()*ori_i).matrix();jacobian_i.block<3, 3>(INDEX_R, INDEX_G) = -J_r_inv*(Sophus::SO3d::exp(residual.block<3, 1>(INDEX_R, 0))).matrix().inverse()*J_.block<3,3>(INDEX_R, INDEX_G);// c. residual, velocity:jacobian_i.block<3, 3>(INDEX_V, INDEX_R) = Sophus::SO3d::hat(ori_i.inverse() * (vel_j - vel_i + g_ * T_));jacobian_i.block<3, 3>(INDEX_V, INDEX_V) = -R_i_inv;jacobian_i.block<3, 3>(INDEX_V, INDEX_A) = -J_.block<3,3>(INDEX_V, INDEX_A);jacobian_i.block<3, 3>(INDEX_V, INDEX_G) = -J_.block<3,3>(INDEX_V, INDEX_G);// d. residual, bias accel:jacobian_i.block<3, 3>(INDEX_A, INDEX_A) = -Eigen::Matrix3d::Identity();// d. residual, bias accel:jacobian_i.block<3, 3>(INDEX_G, INDEX_G) = -Eigen::Matrix3d::Identity();jacobian_i = sqrt_info * jacobian_i;}if ( jacobians[1] ) {Eigen::Map<Eigen::Matrix<double, 15, 15, Eigen::RowMajor>> jacobian_j(jacobians[1]);jacobian_j.setZero();// a. residual, position:jacobian_j.block<3, 3>(INDEX_P, INDEX_P) = R_i_inv;// b. residual, orientation:jacobian_j.block<3, 3>(INDEX_R, INDEX_R) = J_r_inv;// c. residual, velocity:jacobian_j.block<3, 3>(INDEX_V, INDEX_V) = R_i_inv;// d. residual, bias accel:jacobian_j.block<3, 3>(INDEX_A, INDEX_A) = Eigen::Matrix3d::Identity();// d. residual, bias accel:jacobian_j.block<3, 3>(INDEX_G, INDEX_G) = Eigen::Matrix3d::Identity();jacobian_j = sqrt_info * jacobian_j;}}

    //// TODO: correct residual by square root of information matrix://residual = sqrt_info * residual;

2.1.4 边缘化先验因子

边缘化先验因子部分，算是滑窗算法的精华部分，可参考vins的做法

a.示意图

b.边缘先验因子 残差函数和雅克比矩阵 推导

c.代码补全
地图匹配H B阵
    //// TODO: Update H://// a. H_mm:H_.block<15,  15>(INDEX_M, INDEX_M) += J_m.transpose() * J_m ;

    //// TODO: Update b://// a. b_m:b_.block<15, 1>(INDEX_M , 0) +=  J_m.transpose() * residuals ;  //  因子图叠加
点云匹配H B阵
   //// TODO: Update H://// a. H_mm:H_.block<15, 15>(INDEX_M, INDEX_M)  += J_m.transpose() * J_m;// b. H_mr:H_.block<15, 15>(INDEX_M, INDEX_R)   += J_m.transpose()* J_r;// c. H_rm:H_.block<15,  15>(INDEX_R, INDEX_M)  += J_r.transpose() * J_m; // d. H_rr:H_.block<15,  15>(INDEX_R, INDEX_R)   += J_r.transpose() * J_r;

    //// TODO: Update b://// a. b_m:b_.block<15,  1>(INDEX_M, 0) += J_m.transpose() * residuals ;// a. b_r:b_.block<15,   1>(INDEX_R,  0) += J_r.transpose() * residuals ;
IMU预积分H阵 B阵
    //// TODO: Update H://// a. H_mm:H_.block<15, 15>(INDEX_M, INDEX_M)  += J_m.transpose() * J_m;// b. H_mr:H_.block<15, 15>(INDEX_M, INDEX_R)   += J_m.transpose()* J_r;// c. H_rm:H_.block<15,  15>(INDEX_R, INDEX_M)  += J_r.transpose() * J_m; // d. H_rr:H_.block<15,  15>(INDEX_R, INDEX_R)   += J_r.transpose() * J_r;

    //// TODO: Update b://// a. b_m:b_.block<15,  1>(INDEX_M, 0) += J_m.transpose() * residuals ;// a. b_r:b_.block<15,   1>(INDEX_R,  0) += J_r.transpose() * residuals ;
边缘化操作 (核心代码)

此部分代码主要参考自vins部分

  void Marginalize(const double *raw_param_r_0) {// TODO: implement marginalization logic//  save x_m_0 Eigen::Map<const  Eigen::Matrix<double, 15 , 1>>  x_0(raw_param_r_0);x_0_ = x_0 ;//   marginalizeconst Eigen::MatrixXd  &H_mm = H_.block<15,  15>(INDEX_M, INDEX_M);const Eigen::MatrixXd  &H_mr   = H_.block<15,   15>(INDEX_M,INDEX_R);const Eigen::MatrixXd  &H_rm   = H_.block<15,   15>(INDEX_R,INDEX_M);const Eigen::MatrixXd  &H_rr     = H_.block<15,    15>(INDEX_R,INDEX_R);const Eigen::VectorXd  &b_m = b_.block<15,  1>(INDEX_M, 0);const Eigen::VectorXd  &b_r   = b_.block<15,   1>(INDEX_R, 0);Eigen::MatrixXd H_mm_inv = H_mm.inverse();Eigen::MatrixXd H_marginalized = H_rr - H_rm * H_mm_inv * H_mr ;Eigen::MatrixXd b_marginalized = b_r - H_rm * H_mm_inv * b_m ;// 线性化残差 和 雅克比Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> saes(H_marginalized);Eigen::VectorXd S = Eigen::VectorXd((saes.eigenvalues().array() > 1.0e-5).select(saes.eigenvalues().array(), 0));Eigen::VectorXd S_inv = Eigen::VectorXd((saes.eigenvalues().array() > 1.0e-5).select(saes.eigenvalues().array().inverse(), 0));Eigen::VectorXd S_sqrt = S.cwiseSqrt();Eigen::VectorXd S_inv_sqrt = S_inv.cwiseSqrt();// finally:J_ = S_sqrt.asDiagonal() * saes.eigenvectors().transpose();         //  b0e_ = S_inv_sqrt.asDiagonal() * saes.eigenvectors().transpose() * b_marginalized;      //   eo}

关于函数SelfAdjointEigenSolver的解析（1.摘自 张嘉皓助教）(2.摘自游博的知乎)

1.由于每次迭代需要使用奇异值分解,从边缘化后的信息矩阵中恢复出来雅克比矩阵 linearized_jacobians 和残差 linearized_residuals ,这两者会作为先验残差带入到下一轮的先验残差的雅克比和残差的计算当中去。这一部分和vins是差不多的,如果看不懂的可以去 vins 那边参考参考。其中推导部分相对比较简单,如图片所示

2.游博知乎解释-更新先验残差

计算边缘因子传递的残差
    //// TODO: compute residual://Eigen::Map<Eigen::Matrix<double, 15, 1>>  residual(residuals);residual = e_ + J_ * dx ;       //  e_prior
计算边缘因子传递的雅克比
    //// TODO: compute jacobian://if ( jacobians ) {if ( jacobians[0] ) {// implement computing:Eigen::Map<Eigen::Matrix<double, 15, 15, Eigen::RowMajor>>  jacobian_marginalization(jacobians[0]);jacobian_marginalization.setZero();jacobian_marginalization = J_ ;        }}

2.2 将四种约束因子，加入滑窗，进行优化

2.2.1 更新状态量

FILE : param_prvag.hpp

        //// TODO: evaluate performance penalty of applying exp-exp-log transform for each update//ori_plus = (Sophus::SO3d::exp(ori) * Sophus::SO3d::exp(d_ori)).log();vel_plus = vel + d_vel;b_a_plus = b_a + d_b_a;b_g_plus = b_g + b_g_plus;

2.2.2 将因子添加到优化框架中

FILE : sliding_window.cpp

    //// add node for new key frame pose://// fix the pose of the first key frame for lidar only mapping:if ( sliding_window_ptr_->GetNumParamBlocks() == 0 ) {// TODO: add init key framesliding_window_ptr_->AddPRVAGParam(current_key_frame_, true);} else {// TODO: add current key framesliding_window_ptr_->AddPRVAGParam(current_key_frame_, false);}

        // TODO: add constraint, GNSS position:sliding_window_ptr_->AddPRVAGMapMatchingPoseFactor(param_index_j, prior_pose, measurement_config_.noise.map_matching);

        // a. lidar frontend://// get relative pose measurement:Eigen::Matrix4d relative_pose = (last_key_frame_.pose.inverse() * current_key_frame_.pose).cast<double>();// TODO: add constraint, lidar frontend / loop closure detection:sliding_window_ptr_->AddPRVAGRelativePoseFactor(param_index_i, param_index_j, relative_pose, measurement_config_.noise.lidar_odometry);

        //// b. IMU pre-integration://if ( measurement_config_.source.imu_pre_integration ) {// TODO: add constraint, IMU pre-integraion:sliding_window_ptr_->AddPRVAGIMUPreIntegrationFactor(param_index_i, param_index_j,imu_pre_integration_);}

2.2.3 ceres 中添加残差块

FILE : ceres_sliding_window.cpp

创建问题
        // TODO: create new sliding window optimization problem:ceres::Problem problem;
添加待优化参数快
            // TODO: add parameter block:      添加待优化的参数快problem.AddParameterBlock(target_key_frame.prvag, 15, local_parameterization);if( target_key_frame.fixed ) {problem.SetParameterBlockConstant(target_key_frame.prvag);}
添加边缘先验因子残差块
            // add marginalization factor into sliding windowproblem.AddResidualBlock(factor_marginalization,NULL,key_frame_r.prvag    //  一元边);
添加地图匹配因子残差块
                // TODO: add map matching factor into sliding windowproblem.AddResidualBlock(factor_map_matching_pose,NULL,     // loss_function key_frame.prvag     //  一元边);
添加帧间点云匹配匹配因子残差块
                // TODO: add relative pose factor into sliding windowproblem.AddResidualBlock(factor_relative_pose,NULL,   // loss_functionkey_frame_i.prvag, key_frame_j.prvag        //  二元边);
添加IMU预积分因子残差块
                // TODO: add IMU factor into sliding windowproblem.AddResidualBlock(factor_imu_pre_integration,NULL,   // loss_functionkey_frame_i.prvag,  key_frame_j.prvag           // 二元边);

3. evo 评估

evo_ape kitti ground_truth.txt optimized.txt -r full --plot --plot_mode xyz

3.1 EKF 与 因子图优化方法比较

Factor Graph EKF IMU-Lidar Fusion
max 5.546784
mean 1.907476
median 1.682371
min 0.000001
rmse 2.167268
sse 21268.254731
std 1.028876 		max 1.166536
mean 0.244853
median 0.187717
min 0.010400
rmse 0.298805
sse 391.959520
std 0.171264

结果：EKF的效果暂时来看要比图优化的效果要好，但是这不一定说明图优化的效果不好，原因是KITTI数据集自身也存在一定的问题，并且在这次对比实验中，EKF 和 factor graph 所使用的先验地图构建方法不一样，EKF 使用第四章的激光里程计mapping,而factor graph 使用的是第九章imu预积分图优化的方法构建的地图。

3.2不同滑窗长度比较

FILE : lidar_localization/config/matching/sliding_window.yaml

修改不同的滑窗长度

# sliding window size:
#
sliding_window_size: 20                 #滑窗长度为20帧
sliding windows length 2 10 15 20 30 地图
max 5.590900 5.590900 5.546784 5.510841 8.340555 2.325375
mean 2.345147 2.26939 1.907476 1.886341 2.906110 0.801601
median 2.178942 2.111248 1.682371 1.718590 2.988234 0.701903
min 0.000001 0.000001 0.000001 0.000001 0.000001 0.023720
rmse 2.648810 2.531902 2.167268 2.143079 3.207210 0.928723
sse 31769.338233 29026.859453 21268.254731 20796.134607 46452.465579 1650.013734
std 1.231456 1.122670 1.028876 1.017106 1.356733 0.469001

总结：

1.滑动窗口的长度也是一个关键的因素,过高或者过低的窗口长度会造成精度的降低。

2.按照乾哥的说法EKF的滤波方法就相当于滑动窗口为1的情况，可以看出，选取适当滑窗大小是有必要的，能够直接影响最后的性能。

4. 注意

sqrt_info 的理解

在求解图优化因子雅克比、残差的过程中，都会乘以一个sqrt_info 矩阵，代码的写法是参考vins，以下参考博客均有对sqrt_info 矩阵的理解

    Eigen::LLT<Eigen::Matrix<double,15,15>> LowerI(I_);// sqrt_info 为上三角阵Eigen::Matrix<double,15,15> sqrt_info = LowerI.matrixL().transpose();

1.vins 的margin factor

2.后端优化 | VINS-Mono 论文公式推导与代码解析分讲

综上个人理解：在高斯牛顿法优化中, 非线性优化的式子为 J^T (W^-1) J = -J^T (W^-1) r，其中W为观测的协方差矩阵，为对角矩阵，对角线的每个元素为该残差的方差，即可理解为观测误差(传感器的噪声)。将W求逆即为残差的信息矩阵，可理解为权重矩阵，用于调整不同的残差项的权重(具有归一化的意义) 。因为ceres 只接受 min(e^T e ) 形式的最小二乘优化形式，所以可以通过cholesy分解的方式将残差的信息矩阵(W^-1)分解为上下三角形式，然后分别乘到残差和雅克比上，其形式与原高斯牛顿法的等式等价，相当于在把残差的信息矩阵开根号，然后分别乘到雅克比和残差上。

//  雅克比 乘 上 sqrt(残差信息矩阵)
jacobian_i = sqrt_info * jacobian_i;
//  残差 乘 上 sqrt(残差信息矩阵)
residual = sqrt_info * residual;

右乘BCH及其求逆公式

此处部分为课程中较多争议的一个部分，为扰动模型中，右雅克比在代码上的实现，摘抄自陈皓嘉助教的“第十章思路讲解”。

FILE : factor_prvag_imu_pre_integration factor_prvag_map_matching_pose factor_prvag_marginalization

//   右雅克比的逆
static Eigen::Matrix3d JacobianRInv(const Eigen::Vector3d &w) {Eigen::Matrix3d J_r_inv = Eigen::Matrix3d::Identity();double theta = w.norm();if ( theta > 1e-5 ) {Eigen::Vector3d a = w.normalized();Eigen::Matrix3d a_hat = Sophus::SO3d::hat(a);double theta_half = 0.5 * theta ;double cot_theta = 1.0 / tan(theta_half);J_r_inv = theta_half * cot_theta * J_r_inv  +  (1.0 - theta_half * cot_theta) * a * a.transpose() + theta_half * a_hat ;}return J_r_inv;}

关于是否需要添加帧间里程计因子的讨论

摘自学员解答：

​ 由于我们播的是相同的数据集，点云地图匹配和激光里程计的结果差不多，如果地图变化比较大，激光里程计的存在会使系统更加鲁棒。

5. 课程答疑总结

5.1 进行舒尔补构建b矩阵是，正负号问题

答：公式上是没问题的，区别在于构建残差是是 (预测-观测) or (观测-预测)

5.2 因子图优化中Ti Tj 问题

问：我有个疑问，这里添加的因子是，激光里程计因子，是点云的scan2scan， 作残差时，是这两帧通过点云匹配相对位姿 Tij 和 Ti Tj计算的相对位姿比较，这里没有矛盾么，因为这里的因子是点云匹配出来的，而Ti Tj 一开始也是点云匹配给的，在做残差时，不相当于自己减自己？

Ti Tj 一般是预测值，一般是由scan2scan 上一时刻的后验POSE给的后验，作为预测。残差模型一般都是遵循 (观测 - 预测)模式，scan2map 先验地图匹配一般不作为先验因子，一般作为观测量。

5.3 EKF 与 factor graph 对比

问：因子图优化对比EKF在工程上的优势，因为我在第十章EKF 和 因子图对比中，发现EKF的效果要好点。

答：在课程的效果中，EKF 对比 因子图优化的效果要好，鉴于数据集本身存在分米级的误差，所以不用太在意。实际上，因子图优化开始取代EKF。在建图领域，大部分都使用优化的方法，在定位，目前大多数还是kalman的方法，包括apollo也是沿用kalman的定位方案，但是优化方法也在逐步取代。优化方法体现的优势在于：EKF的方法基于一阶马尔科夫性，只是局部最优，而图优化的方法是基于全局最优，即使是滑窗方法上的优化方法，也是选取多帧最优。

5.4 scan2map中的map

问：因子图优化中的激光里程计约束指的scan2scan 还是 scan2map, 如果是scan2map 的话，那它和“先验地图匹配因子”的区别是在于 “激光里程计因子”的map是局部分割的小地图，“先验地图匹配因子”的map是大地图是吗？ 使用全局的大地图匹配，对资源消耗不是很大吗？

答：激光里程计：前端激光里程计主要还是loam的方案，使用的是scan2map 方式构建，这里的map并不是只小的先验地图，只是指loam里的submap小特征地图。“先验地图匹配因子”里所说的先验地图，使用滑窗分割出的小地图，为了节省运算资源。

5.5 如何去评估地图的质量精度

a.使用更高精度的惯性传感器，进行建图时的轨迹评估；

b.标志物评判，实地选取一个标志物，使用RTK去测量当地标志物的经纬高，对应地图上同一个标志物，计算误差，多取几个，可以构建出误差系数。

5.6 slam中GPS的主要作用

gps的主要作用个人认为主要有两个：1.消除行驶过程中累积误差； 2.室外重定位过程中的初始化有很大作用。

5.7 点云匹配过程中，特征退化的现象(长廊、隧道)，有什么方法可以判断出来？（待补充）

1.特征退化的场景(如长廊、隧道)，因为只有两侧的观测，可以对匹配的协方差矩阵进行特征值分解，通过判断特征值的分布状况判断。

2.也可已增加imu uwb等辅助传感器。

5.8 IMU上电的bias需要怎么估计？(待补充)

kitti数据集中貌似已估

5.9 图优化 因子图 位姿图 概念（待补充）

5.10 SLAM 算法岗位应届生招聘要求

a.slam 的基础知识，前端匹配：icp ndt， 后端优化： 高斯牛顿法、LM优化

b.代码能力(leetcode top 100 、 剑指offer)

c.常见的开源框架， 优点(为什么选择他) ，缺点(如何改进)

d.项目经验(重点)： 为什么这样搞，这样搞的好处、优势。

​ edited by kaho 2022.3.19

多传感器融合定位 第十章 基于优化的定位方法相关推荐

  1. NeurIPS2021 MBT：多模态数据怎么融合？谷歌提出基于注意力瓶颈的方法，简单高效还省计算量...

    关注公众号,发现CV技术之美 本文分享 NeurIPS 2021 论文『Attention Bottlenecks for Multimodal Fusion』,思考<MBT>多模态数据怎 ...

  2. matlab 声源定位csdn_[转]基于TDOA声源定位算法仿真--MATLAB仿真

    声源定位算法是利用麦克风阵列进行声音定位,属于宽带信号,传统的MUSIC和DOA算法并不适用该场景,本仿真主要用TDOA算法进行定位. 常用的阵列信号定位算法主要有三大类:基于高分辨率谱估计的定位技术 ...

  3. 【泡泡图灵智库】基于优化的视觉惯性SLAM与GNSS紧耦合

    转载自:https://mp.weixin.qq.com/s/jlkXvygaTsql1nk76HD94g [泡泡图灵智库]基于优化的视觉惯性SLAM与GNSS紧耦合 原创 泡泡机器人 泡泡机器人SL ...

  4. 滤波融合（一）基于C++完成一个简单的 卡尔曼滤波器 线性的系统和测量模型

    在机器人定位中,KF或者EKF是常用的传感器融合算法,之前也总结过很多关于EKF的用法: 如何理解卡尔曼滤波(Kalman Filter)实现数据融合 通俗易懂理解扩展卡尔曼滤波EKF用于多源传感器融 ...

  5. （转载）基于TDOA声源定位算法仿真–MATLAB仿真

    (转载)基于TDOA声源定位算法仿真–MATLAB仿真 转载自:https://blog.xxcxw.cn/archives/28 声源定位算法是利用麦克风阵列进行声音定位,属于宽带信号,传统的MUS ...

  6. 基于TDOA声源定位算法仿真–MATLAB仿真

    评论或私信 5元获取解压码 声源定位算法是利用麦克风阵列进行声音定位,属于宽带信号,传统的MUSIC和DOA算法并不适用该场景,本仿真主要用TDOA算法进行定位. 常用的阵列信号定位算法主要有三大类: ...

  7. 多传感器融合定位十四-基于图优化的定位方法

    多传感器融合定位十四-基于图优化的定位方法 1. 基于图优化的定位简介 1.1 核心思路 1.2 定位流程 2. 边缘化原理及应用 2.1 边缘化原理 2.2 从滤波角度理解边缘化 3. 基于kitt ...

  8. 论文笔记——基于因子图消元优化的多传感器融合定位算法

    创新点: 为了提高抗干扰性和容错能力,在链式因子图模型中加入滑动窗口用于保留窗口内历史状态信息:同时为了避免高维矩阵运算,引入消元算法将因子图转化为贝叶斯网络,依次边缘化历史状态,实现矩阵降维. 提出 ...

  9. 多传感器融合定位 第四章 点云地图构建及基于点云地图定位

    多传感器融合定位 第四章 点云地图构建及基于点云地图定位 代码下载 https://github.com/kahowang/sensor-fusion-for-localization-and-map ...

  10. 【多传感器融合定位SLAM专栏】前端里程计、IMU预积分、滤波、图优化推导与应用（3）

    本专栏基于深蓝学院<多传感器融合定位>课程基础上进行拓展,对多传感器融合SLAM的学习过程进行记录 文章目录 第三章 惯性导航原理及误差分析 惯性器件 1. 机械陀螺 2. 激光陀螺 ME ...

最新文章

  1. PHP Fatal error: Allowed memory size of 134217728 bytes exhausted (tried to allocate 8192 bytes)
  2. php 运行 shell命令行参数,PHP exec()在通过浏览器执行时不会执行shell命令
  3. Boost：bind绑定作为一个组合的测试程序
  4. [课程复习] 数据结构之线性表、树、图、查找、排序经典算法复习
  5. 【Luogu】【关卡2-2】交叉模拟（2017年10月）
  6. ElasticSearch5.3插件开发（一）控制台打印信息
  7. 怎么把线稿提取出来_PS教程：提取线稿如此简单？三种方法总有一种适合你
  8. 终于完成词频统计小程序~
  9. 第六章：thymeleaf页面模版-1. 信息输出
  10. echarts3.0版本断点连线的处理
  11. calendar控件使用 extjs_extjs年月选择日历及通用js同步ajax调用返回json object
  12. ajax异步提交数据库,yii表单ajax异步提交，数据保存到数据库
  13. 两个可用于浏览器兼容性测试的Firefox插件
  14. ARP欺骗的艺术 | 断网与监听
  15. Win7主题制作教程 电脑主题制作图文方法
  16. pythonexcel源码_Python对Excel操作详解_Python教程
  17. Home键监听与电源键
  18. CentOS 开机启动详解
  19. 华为 android 菜单键,没有返回和菜单键！华为P10的Home键简单说明
  20. 实验四 串行通信实验

热门文章

  1. mysql 快速复制数据,MySQL快速复制数据库的方法
  2. tplink703无线打印服务器,tplink703路由器怎么配置client模式
  3. 基于深度学习的推荐系统综述 (arxiv 1707.07435) 译文 3.1 ~ 3.3
  4. 简单网页制作html语言,用HTML语言制作简单的网页.doc
  5. js代码实现点击按钮出现60秒倒计时
  6. java微博自动转发_被「李笑来老师」拉黑之「JavaScript微博自动转发的脚本」
  7. BZOJ3787 gty的文艺妹子序列 【树状数组】【分块】
  8. 【WLAN】【基础知识】WIFI那些事儿之DFS
  9. 小甲鱼老师目前所有视频教程下载地址
  10. Golang go mod 使用