资讯详情

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

第十章 定位方法基于优化

本章是基于先验地图的图纸优化方法。先验地图的构建可参考多传感器集成定位 第九章 基于优化的建图方法

代码下载：

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 总结研究方向

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

多传感器集成定位理论基础(12):滑动窗口的原理及应用

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

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

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

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

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

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

1.实现预积分、地图匹配、边缘化和帧间匹配。

2.在滑窗中加入上述四个约束因素进行优化

2.1 四个优化因素：预积分、地图匹配、边缘化和帧间匹配

2.1.1 激光里程计因子

a.示意图

image-20220319175059507
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_R

    residual.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.html
    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);

              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.html
    Eigen::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)   + 

        标签： w8486a传感器mr03传感器qae26传感器
 
锐单商城拥有海量元器件数据手册、
          IC替代型号，打造
          电子元器件IC百科大全！

锐单商城 - 一站式电子元器件采购平台