• ORB-SLAM3从理论到代码实现（一）：Optimizer单帧优化

    1. 前言

    Optimizer是非常重要的代码文件！!
    这一部分代码量巨大，查阅了很多资料结合来看的代码，将分为以下部分进行分析

    • 单帧优化
    • 局部地图优化
    • 全局优化
    • 尺度与重力优化
    • sim3优化
    • 地图回环优化
    • 地图融合优化

    2. 代码分析

    位姿优化，纯视觉时使用。优化目标：单帧的位姿

    该函数的主要作用是在已知当前帧与地图的匹配关系时，通过优化得到当前帧的位姿

    1. int Optimizer::PoseOptimization(Frame *pFrame)
    2. {
    3.     // 该优化函数主要用于Tracking线程中：运动跟踪、参考帧跟踪、地图跟踪、重定位
    4.  
    5.     // Step 1：构造g2o优化器, BlockSolver_6_3表示：位姿 _PoseDim 为6维，路标点 _LandmarkDim 是3维
    6.     g2o::SparseOptimizer optimizer;
    7.     g2o::BlockSolver_6_3::LinearSolverType *linearSolver;
    8.  
    9.     linearSolver = new g2o::LinearSolverDense();
    10.  
    11.     g2o::BlockSolver_6_3 *solver_ptr = new g2o::BlockSolver_6_3(linearSolver);
    12.  
    13.     g2o::OptimizationAlgorithmLevenberg *solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr);
    14.     optimizer.setAlgorithm(solver);
    15.  
    16.     // 输入的帧中,有效的,参与优化过程的2D-3D点对
    17.     int nInitialCorrespondences = 0;
    18.  
    19.     // Set Frame vertex
    20.     // Step 2：添加顶点：待优化当前帧的Tcw
    21.     g2o::VertexSE3Expmap *vSE3 = new g2o::VertexSE3Expmap();
    22.     vSE3->setEstimate(Converter::toSE3Quat(pFrame->mTcw));
    23.     // 设置id，保证本次优化过程中id独立即可
    24.     vSE3->setId(0);
    25.     // 要优化的变量，所以不能固定
    26.     vSE3->setFixed(false);
    27.     // 添加节点
    28.     optimizer.addVertex(vSE3);
    29.  
    30.     // Set MapPoint vertices
    31.     // 当前帧的特征点数量
    32.     const int N = pFrame->N;
    33.  
    34.     vector vpEdgesMono;           // 存放单目边
    35.     vector vpEdgesMono_FHR; // 存放另一目的边
    36.     vector<size_t> vnIndexEdgeMono, vnIndexEdgeRight;                     // 边对应特征点的id
    37.     vpEdgesMono.reserve(N);
    38.     vpEdgesMono_FHR.reserve(N);
    39.     vnIndexEdgeMono.reserve(N);
    40.     vnIndexEdgeRight.reserve(N);
    41.  
    42.     vector vpEdgesStereo; // 存放双目边
    43.     vector<size_t> vnIndexEdgeStereo;
    44.     vpEdgesStereo.reserve(N);
    45.     vnIndexEdgeStereo.reserve(N);
    46.  
    47.     // 自由度为2的卡方分布，显著性水平为0.05，对应的临界阈值5.991
    48.     // 可以理解为卡方值高于5.991 95%的几率维外点
    49.     const float deltaMono = sqrt(5.991);
    50.     // 自由度为3的卡方分布，显著性水平为0.05，对应的临界阈值7.815
    51.     const float deltaStereo = sqrt(7.815);
    52.  
    53.     // Step 3：添加一元边
    54.     {
    55.         // 锁定地图点。由于需要使用地图点来构造顶点和边,因此不希望在构造的过程中部分地图点被改写造成不一致甚至是段错误
    56.         unique_lock lock(MapPoint::mGlobalMutex);
    57.  
    58.         // 遍历当前帧中的所有地图点
    59.         for (int i = 0; i < N; i++)
    60.         {
    61.             MapPoint *pMP = pFrame->mvpMapPoints[i];
    62.             // 如果这个地图点还存在没有被剔除掉
    63.             if (pMP)
    64.             {
    65.                 // Conventional SLAM
    66.                 //  不存在相机2，则有可能为单目与双目
    67.                 if (!pFrame->mpCamera2)
    68.                 {
    69.                     // Monocular observation
    70.                     // 单目情况，因为双目模式下pFrame->mvuRight[i]会大于0
    71.                     if (pFrame->mvuRight[i] < 0)
    72.                     {
    73.                         nInitialCorrespondences++;
    74.                         pFrame->mvbOutlier[i] = false;
    75.  
    76.                         // 对这个地图点的观测，也就是去畸变像素坐标
    77.                         Eigen::Matrix<double, 2, 1> obs;
    78.                         const cv::KeyPoint &kpUn = pFrame->mvKeysUn[i];
    79.                         obs << kpUn.pt.x, kpUn.pt.y;
    80.                         // 新建节点,注意这个节点的只是优化位姿Pose
    81.                         ORB_SLAM3::EdgeSE3ProjectXYZOnlyPose *e = new ORB_SLAM3::EdgeSE3ProjectXYZOnlyPose();
    82.  
    83.                         e->setVertex(0, dynamic_cast(optimizer.vertex(0)));
    84.                         e->setMeasurement(obs);
    85.                         // 这个点的可信程度和特征点所在的图层有关，层数约高invSigma2越小，信息矩阵越小，表示误差越大，优化时考虑的比较少
    86.                         const float invSigma2 = pFrame->mvInvLevelSigma2[kpUn.octave];
    87.                         e->setInformation(Eigen::Matrix2d::Identity() * invSigma2);
    88.                         // 在这里使用了鲁棒核函数
    89.                         g2o::RobustKernelHuber *rk = new g2o::RobustKernelHuber;
    90.                         e->setRobustKernel(rk);
    91.                         rk->setDelta(deltaMono);
    92.  
    93.                         // 设置相机
    94.                         e->pCamera = pFrame->mpCamera;
    95.                         // 地图点的空间位置,作为迭代的初始值，因为是一元边，所以不以节点的形式出现
    96.                         cv::Mat Xw = pMP->GetWorldPos();
    97.                         e->Xw[0] = Xw.at<float>(0);
    98.                         e->Xw[1] = Xw.at<float>(1);
    99.                         e->Xw[2] = Xw.at<float>(2);
    100.  
    101.                         optimizer.addEdge(e);
    102.  
    103.                         vpEdgesMono.push_back(e);
    104.                         vnIndexEdgeMono.push_back(i);
    105.                     }
    106.                     else // 双目
    107.                     {
    108.                         nInitialCorrespondences++;
    109.                         pFrame->mvbOutlier[i] = false;
    110.  
    111.                         // SET EDGE
    112.                         //  观测多了一项右目的坐标
    113.                         Eigen::Matrix<double, 3, 1> obs;
    114.                         const cv::KeyPoint &kpUn = pFrame->mvKeysUn[i];
    115.                         const float &kp_ur = pFrame->mvuRight[i];
    116.                         obs << kpUn.pt.x, kpUn.pt.y, kp_ur;
    117.                         // 新建节点,注意这里也是只优化位姿
    118.                         g2o::EdgeStereoSE3ProjectXYZOnlyPose *e = new g2o::EdgeStereoSE3ProjectXYZOnlyPose();
    119.  
    120.                         e->setVertex(0, dynamic_cast(optimizer.vertex(0)));
    121.                         e->setMeasurement(obs);
    122.                         // 置信程度主要是看左目特征点所在的图层。图层越高，置信度越低，方差越大
    123.                         const float invSigma2 = pFrame->mvInvLevelSigma2[kpUn.octave];
    124.                         // 根据置信度设置信息矩阵
    125.                         Eigen::Matrix3d Info = Eigen::Matrix3d::Identity() * invSigma2;
    126.                         e->setInformation(Info);
    127.                         // 设置核函数
    128.                         g2o::RobustKernelHuber *rk = new g2o::RobustKernelHuber;
    129.                         e->setRobustKernel(rk);
    130.                         rk->setDelta(deltaStereo);
    131.  
    132.                         // 双目下没有加入相机
    133.                         e->fx = pFrame->fx;
    134.                         e->fy = pFrame->fy;
    135.                         e->cx = pFrame->cx;
    136.                         e->cy = pFrame->cy;
    137.                         e->bf = pFrame->mbf;
    138.                         cv::Mat Xw = pMP->GetWorldPos();
    139.                         e->Xw[0] = Xw.at<float>(0);
    140.                         e->Xw[1] = Xw.at<float>(1);
    141.                         e->Xw[2] = Xw.at<float>(2);
    142.  
    143.                         optimizer.addEdge(e);
    144.  
    145.                         vpEdgesStereo.push_back(e);
    146.                         vnIndexEdgeStereo.push_back(i);
    147.                     }
    148.                 }
    149.                 // SLAM with respect a rigid body
    150.                 else  // 两个相机
    151.                 {
    152.                     nInitialCorrespondences++;
    153.  
    154.                     cv::KeyPoint kpUn;
    155.                     // 总数N = 相机1所有特征点数量+相机2所有特征点数量
    156.                     if (i < pFrame->Nleft)
    157.                     { // Left camera observation
    158.                         kpUn = pFrame->mvKeys[i];
    159.  
    160.                         pFrame->mvbOutlier[i] = false;
    161.  
    162.                         Eigen::Matrix<double, 2, 1> obs;
    163.                         obs << kpUn.pt.x, kpUn.pt.y;
    164.  
    165.                         ORB_SLAM3::EdgeSE3ProjectXYZOnlyPose *e = new ORB_SLAM3::EdgeSE3ProjectXYZOnlyPose();
    166.  
    167.                         e->setVertex(0, dynamic_cast(optimizer.vertex(0)));
    168.                         e->setMeasurement(obs);
    169.                         const float invSigma2 = pFrame->mvInvLevelSigma2[kpUn.octave];
    170.                         e->setInformation(Eigen::Matrix2d::Identity() * invSigma2);
    171.  
    172.                         g2o::RobustKernelHuber *rk = new g2o::RobustKernelHuber;
    173.                         e->setRobustKernel(rk);
    174.                         rk->setDelta(deltaMono);
    175.  
    176.                         e->pCamera = pFrame->mpCamera;
    177.                         cv::Mat Xw = pMP->GetWorldPos();
    178.                         e->Xw[0] = Xw.at<float>(0);
    179.                         e->Xw[1] = Xw.at<float>(1);
    180.                         e->Xw[2] = Xw.at<float>(2);
    181.  
    182.                         optimizer.addEdge(e);
    183.  
    184.                         vpEdgesMono.push_back(e);
    185.                         vnIndexEdgeMono.push_back(i);
    186.                     }
    187.                     else
    188.                     { // Right camera observation
    189.                         kpUn = pFrame->mvKeysRight[i - pFrame->Nleft];
    190.  
    191.                         Eigen::Matrix<double, 2, 1> obs;
    192.                         obs << kpUn.pt.x, kpUn.pt.y;
    193.  
    194.                         pFrame->mvbOutlier[i] = false;
    195.  
    196.                         ORB_SLAM3::EdgeSE3ProjectXYZOnlyPoseToBody *e = new ORB_SLAM3::EdgeSE3ProjectXYZOnlyPoseToBody();
    197.  
    198.                         e->setVertex(0, dynamic_cast(optimizer.vertex(0)));
    199.                         e->setMeasurement(obs);
    200.                         const float invSigma2 = pFrame->mvInvLevelSigma2[kpUn.octave];
    201.                         e->setInformation(Eigen::Matrix2d::Identity() * invSigma2);
    202.  
    203.                         g2o::RobustKernelHuber *rk = new g2o::RobustKernelHuber;
    204.                         e->setRobustKernel(rk);
    205.                         rk->setDelta(deltaMono);
    206.  
    207.                         e->pCamera = pFrame->mpCamera2;
    208.                         cv::Mat Xw = pMP->GetWorldPos();
    209.                         e->Xw[0] = Xw.at<float>(0);
    210.                         e->Xw[1] = Xw.at<float>(1);
    211.                         e->Xw[2] = Xw.at<float>(2);
    212.  
    213.                         e->mTrl = Converter::toSE3Quat(pFrame->mTrl);
    214.  
    215.                         optimizer.addEdge(e);
    216.  
    217.                         vpEdgesMono_FHR.push_back(e);
    218.                         vnIndexEdgeRight.push_back(i);
    219.                     }
    220.                 }
    221.             }
    222.         }
    223.     }
    224.     // 如果没有足够的匹配点,那么就只好放弃了
    225.     // cout << "PO: vnIndexEdgeMono.size() = " << vnIndexEdgeMono.size() << "   vnIndexEdgeRight.size() = " << vnIndexEdgeRight.size() << endl;
    226.     if (nInitialCorrespondences < 3)
    227.         return 0;
    228.  
    229.     // We perform 4 optimizations, after each optimization we classify observation as inlier/outlier
    230.     // At the next optimization, outliers are not included, but at the end they can be classified as inliers again.
    231.     // Step 4：开始优化，总共优化四次，每次优化迭代10次,每次优化后，将观测分为outlier和inlier，outlier不参与下次优化
    232.     // 由于每次优化后是对所有的观测进行outlier和inlier判别，因此之前被判别为outlier有可能变成inlier，反之亦然
    233.     // 基于卡方检验计算出的阈值（假设测量有一个像素的偏差）
    234.     const float chi2Mono[4] = {5.991, 5.991, 5.991, 5.991};   // 单目
    235.     const float chi2Stereo[4] = {7.815, 7.815, 7.815, 7.815}; // 双目
    236.     const int its[4] = {10, 10, 10, 10};                      // 四次迭代，每次迭代的次数
    237.  
    238.     // bad 的地图点个数
    239.     int nBad = 0;
    240.     // 一共进行四次优化，每次会剔除外点
    241.     for (size_t it = 0; it < 4; it++)
    242.     {
    243.         vSE3->setEstimate(Converter::toSE3Quat(pFrame->mTcw));
    244.         // 其实就是初始化优化器,这里的参数0就算是不填写,默认也是0,也就是只对level为0的边进行优化
    245.         optimizer.initializeOptimization(0);
    246.         // 开始优化，优化10次
    247.         optimizer.optimize(its[it]);
    248.  
    249.         nBad = 0;
    250.         // 优化结束,开始遍历参与优化的每一条误差边(单目或双相机的相机1)
    251.         for (size_t i = 0, iend = vpEdgesMono.size(); i < iend; i++)
    252.         {
    253.             ORB_SLAM3::EdgeSE3ProjectXYZOnlyPose *e = vpEdgesMono[i];
    254.  
    255.             const size_t idx = vnIndexEdgeMono[i];
    256.  
    257.             // 如果这条误差边是来自于outlier，也就是上一次优化去掉的
    258.             // 重新计算，因为上一次外点这一次可能成为内点
    259.             if (pFrame->mvbOutlier[idx])
    260.             {
    261.                 e->computeError();
    262.             }
    263.  
    264.             // 就是error*\Omega*error,表征了这个点的误差大小(考虑置信度以后)
    265.             const float chi2 = e->chi2();
    266.  
    267.             if (chi2 > chi2Mono[it])
    268.             {
    269.                 pFrame->mvbOutlier[idx] = true;
    270.                 e->setLevel(1); // 设置为outlier , level 1 对应为外点,上面的过程中我们设置其为不优化
    271.                 nBad++;
    272.             }
    273.             else
    274.             {
    275.                 pFrame->mvbOutlier[idx] = false;
    276.                 e->setLevel(0); // 设置为inlier, level 0 对应为内点,上面的过程中我们就是要优化这些关系
    277.             }
    278.  
    279.             if (it == 2)
    280.                 e->setRobustKernel(0); // 除了前两次优化需要RobustKernel以外, 其余的优化都不需要 -- 因为重投影的误差已经有明显的下降了
    281.         }
    282.         // 对于相机2
    283.         for (size_t i = 0, iend = vpEdgesMono_FHR.size(); i < iend; i++)
    284.         {
    285.             ORB_SLAM3::EdgeSE3ProjectXYZOnlyPoseToBody *e = vpEdgesMono_FHR[i];
    286.  
    287.             const size_t idx = vnIndexEdgeRight[i];
    288.  
    289.             if (pFrame->mvbOutlier[idx])
    290.             {
    291.                 e->computeError();
    292.             }
    293.  
    294.             const float chi2 = e->chi2();
    295.  
    296.             if (chi2 > chi2Mono[it])
    297.             {
    298.                 pFrame->mvbOutlier[idx] = true;
    299.                 e->setLevel(1);
    300.                 nBad++;
    301.             }
    302.             else
    303.             {
    304.                 pFrame->mvbOutlier[idx] = false;
    305.                 e->setLevel(0);
    306.             }
    307.  
    308.             if (it == 2)
    309.                 e->setRobustKernel(0);
    310.         }
    311.         // 对于双目
    312.         for (size_t i = 0, iend = vpEdgesStereo.size(); i < iend; i++)
    313.         {
    314.             g2o::EdgeStereoSE3ProjectXYZOnlyPose *e = vpEdgesStereo[i];
    315.  
    316.             const size_t idx = vnIndexEdgeStereo[i];
    317.  
    318.             if (pFrame->mvbOutlier[idx])
    319.             {
    320.                 e->computeError();
    321.             }
    322.  
    323.             const float chi2 = e->chi2();
    324.  
    325.             if (chi2 > chi2Stereo[it])
    326.             {
    327.                 pFrame->mvbOutlier[idx] = true;
    328.                 e->setLevel(1);
    329.                 nBad++;
    330.             }
    331.             else
    332.             {
    333.                 e->setLevel(0);
    334.                 pFrame->mvbOutlier[idx] = false;
    335.             }
    336.  
    337.             if (it == 2)
    338.                 e->setRobustKernel(0);
    339.         }
    340.  
    341.         if (optimizer.edges().size() < 10)
    342.             break;
    343.     }
    344.  
    345.     // Recover optimized pose and return number of inliers
    346.     // Step 5 得到优化后的当前帧的位姿
    347.     g2o::VertexSE3Expmap *vSE3_recov = static_cast(optimizer.vertex(0));
    348.     g2o::SE3Quat SE3quat_recov = vSE3_recov->estimate();
    349.     cv::Mat pose = Converter::toCvMat(SE3quat_recov);
    350.     pFrame->SetPose(pose);
    351.  
    352.     // 并且返回内点数目
    353.     return nInitialCorrespondences - nBad;
    354. }

    使用上一关键帧+当前帧的视觉信息和IMU信息，优化当前帧位姿
    可分为以下几个步骤：

    • Step 1：创建g2o优化器，初始化顶点和边
    • Step 2：启动多轮优化，剔除外点
    • Step 3：更新当前帧位姿、速度、IMU偏置
    • Step 4：记录当前帧的优化状态，包括参数信息和对应的海森矩阵
    1. int Optimizer::PoseInertialOptimizationLastKeyFrame(Frame *pFrame, bool bRecInit)
    2. {
    3.     // 1. 创建g2o优化器，初始化顶点和边
    4.     //构建一个稀疏求解器
    5.     g2o::SparseOptimizer optimizer;
    6.     g2o::BlockSolverX::LinearSolverType *linearSolver;
    7.  
    8.     // 使用dense的求解器，（常见非dense求解器有cholmod线性求解器和shur补线性求解器）
    9.     linearSolver = new g2o::LinearSolverDense();
    10.  
    11.     g2o::BlockSolverX *solver_ptr = new g2o::BlockSolverX(linearSolver);
    12.     //使用高斯牛顿求解器
    13.     g2o::OptimizationAlgorithmGaussNewton *solver = new g2o::OptimizationAlgorithmGaussNewton(solver_ptr);
    14.     optimizer.setVerbose(false);
    15.     optimizer.setAlgorithm(solver);
    16.  
    17.     //当前帧单（左）目地图点数目
    18.     int nInitialMonoCorrespondences = 0;
    19.     int nInitialStereoCorrespondences = 0;
    20.     //上面两项的和，即参与优化的地图点总数目
    21.     int nInitialCorrespondences = 0;
    22.  
    23.     // Set Frame vertex
    24.     // 2. 确定节点
    25.     // 当前帧的位姿，旋转+平移，6-dim
    26.     VertexPose *VP = new VertexPose(pFrame);
    27.     VP->setId(0);
    28.     VP->setFixed(false);
    29.     optimizer.addVertex(VP);
    30.     //当前帧的速度，3-dim
    31.     VertexVelocity *VV = new VertexVelocity(pFrame);
    32.     VV->setId(1);
    33.     VV->setFixed(false);
    34.     optimizer.addVertex(VV);
    35.     //当前帧的陀螺仪偏置，3-dim
    36.     VertexGyroBias *VG = new VertexGyroBias(pFrame);
    37.     VG->setId(2);
    38.     VG->setFixed(false);
    39.     optimizer.addVertex(VG);
    40.     //当前帧的加速度偏置，3-dim
    41.     VertexAccBias *VA = new VertexAccBias(pFrame);
    42.     VA->setId(3);
    43.     VA->setFixed(false);
    44.     optimizer.addVertex(VA);
    45.     // setFixed(false)这个设置使以上四个顶点（15个参数）在优化时更新
    46.  
    47.     // Set MapPoint vertices
    48.     // 当前帧的特征点总数
    49.     const int N = pFrame->N;
    50.     // 当前帧左目的特征点总数
    51.     const int Nleft = pFrame->Nleft;
    52.     // 当前帧是否存在右目（即是否为双目）
    53.     const bool bRight = (Nleft != -1);
    54.  
    55.     vector vpEdgesMono;
    56.     vector vpEdgesStereo;
    57.     vector<size_t> vnIndexEdgeMono;
    58.     vector<size_t> vnIndexEdgeStereo;
    59.     vpEdgesMono.reserve(N);
    60.     vpEdgesStereo.reserve(N);
    61.     vnIndexEdgeMono.reserve(N);
    62.     vnIndexEdgeStereo.reserve(N);
    63.  
    64.     // 自由度为2的卡方分布，显著性水平为0.05，对应的临界阈值5.991
    65.     const float thHuberMono = sqrt(5.991);
    66.     // 自由度为3的卡方分布，显著性水平为0.05，对应的临界阈值7.815
    67.     const float thHuberStereo = sqrt(7.815);
    68.  
    69.     {
    70.         // 锁定地图点。由于需要使用地图点来构造顶点和边,因此不希望在构造的过程中部分地图点被改写造成不一致甚至是段错误
    71.         // 3. 投影边
    72.         unique_lock lock(MapPoint::mGlobalMutex);
    73.  
    74.         for (int i = 0; i < N; i++)
    75.         {
    76.             MapPoint *pMP = pFrame->mvpMapPoints[i];
    77.             if (pMP)
    78.             {
    79.                 cv::KeyPoint kpUn;
    80.  
    81.                 // Left monocular observation
    82.                 // 这里说的Left monocular包含两种情况：1.单目情况 2.双目情况下的左目
    83.                 if ((!bRight && pFrame->mvuRight[i] < 0) || i < Nleft)
    84.                 {
    85.                     //如果是双目情况下的左目
    86.                     if (i < Nleft) // pair left-right
    87.                         //使用未畸变校正的特征点
    88.                         kpUn = pFrame->mvKeys[i];
    89.                     //如果是单目
    90.                     else
    91.                         //使用畸变校正过的特征点
    92.                         kpUn = pFrame->mvKeysUn[i];
    93.  
    94.                     //单目地图点计数增加
    95.                     nInitialMonoCorrespondences++;
    96.                     //当前地图点默认设置为不是外点
    97.                     pFrame->mvbOutlier[i] = false;
    98.  
    99.                     Eigen::Matrix<double, 2, 1> obs;
    100.                     obs << kpUn.pt.x, kpUn.pt.y;
    101.  
    102.                     //第一种边(视觉重投影约束)：地图点投影到该帧图像的坐标与特征点坐标偏差尽可能小
    103.                     EdgeMonoOnlyPose *e = new EdgeMonoOnlyPose(pMP->GetWorldPos(), 0);
    104.  
    105.                     //将位姿作为第一个顶点
    106.                     e->setVertex(0, VP);
    107.  
    108.                     //设置观测值，即去畸变后的像素坐标
    109.                     e->setMeasurement(obs);
    110.  
    111.                     // Add here uncerteinty
    112.                     // 获取不确定度，这里调用uncertainty2返回固定值1.0
    113.                     // ?这里的1.0是作为缺省值的意思吗？是否可以根据对视觉信息的信任度动态修改这个值，比如标定的误差？
    114.                     const float unc2 = pFrame->mpCamera->uncertainty2(obs);
    115.  
    116.                     // invSigma2 = (Inverse(协方差矩阵))^2，表明该约束在各个维度上的可信度
    117.                     //  图像金字塔层数越高，可信度越差
    118.                     const float invSigma2 = pFrame->mvInvLevelSigma2[kpUn.octave] / unc2;
    119.                     //设置该约束的信息矩阵
    120.                     e->setInformation(Eigen::Matrix2d::Identity() * invSigma2);
    121.  
    122.                     g2o::RobustKernelHuber *rk = new g2o::RobustKernelHuber;
    123.                     // 设置鲁棒核函数，避免其误差的平方项出现数值过大的增长 注：后续在优化2次后会用e->setRobustKernel(0)禁掉鲁棒核函数
    124.                     e->setRobustKernel(rk);
    125.  
    126.                     //重投影误差的自由度为2，设置对应的卡方阈值
    127.                     rk->setDelta(thHuberMono);
    128.  
    129.                     //将第一种边加入优化器
    130.                     optimizer.addEdge(e);
    131.  
    132.                     //将第一种边加入vpEdgesMono
    133.                     vpEdgesMono.push_back(e);
    134.                     //将对应的特征点索引加入vnIndexEdgeMono
    135.                     vnIndexEdgeMono.push_back(i);
    136.                 }
    137.                 // Stereo observation
    138.                 else if (!bRight)
    139.                 {
    140.                     nInitialStereoCorrespondences++;
    141.                     pFrame->mvbOutlier[i] = false;
    142.  
    143.                     kpUn = pFrame->mvKeysUn[i];
    144.                     const float kp_ur = pFrame->mvuRight[i];
    145.                     Eigen::Matrix<double, 3, 1> obs;
    146.                     obs << kpUn.pt.x, kpUn.pt.y, kp_ur;
    147.  
    148.                     EdgeStereoOnlyPose *e = new EdgeStereoOnlyPose(pMP->GetWorldPos());
    149.  
    150.                     e->setVertex(0, VP);
    151.                     e->setMeasurement(obs);
    152.  
    153.                     // Add here uncerteinty
    154.                     const float unc2 = pFrame->mpCamera->uncertainty2(obs.head(2));
    155.  
    156.                     const float &invSigma2 = pFrame->mvInvLevelSigma2[kpUn.octave] / unc2;
    157.                     e->setInformation(Eigen::Matrix3d::Identity() * invSigma2);
    158.  
    159.                     g2o::RobustKernelHuber *rk = new g2o::RobustKernelHuber;
    160.                     e->setRobustKernel(rk);
    161.                     rk->setDelta(thHuberStereo);
    162.  
    163.                     optimizer.addEdge(e);
    164.  
    165.                     vpEdgesStereo.push_back(e);
    166.                     vnIndexEdgeStereo.push_back(i);
    167.                 }
    168.  
    169.                 // Right monocular observation
    170.                 if (bRight && i >= Nleft)
    171.                 {
    172.                     nInitialMonoCorrespondences++;
    173.                     pFrame->mvbOutlier[i] = false;
    174.  
    175.                     kpUn = pFrame->mvKeysRight[i - Nleft];
    176.                     Eigen::Matrix<double, 2, 1> obs;
    177.                     obs << kpUn.pt.x, kpUn.pt.y;
    178.  
    179.                     EdgeMonoOnlyPose *e = new EdgeMonoOnlyPose(pMP->GetWorldPos(), 1);
    180.  
    181.                     e->setVertex(0, VP);
    182.                     e->setMeasurement(obs);
    183.  
    184.                     // Add here uncerteinty
    185.                     const float unc2 = pFrame->mpCamera->uncertainty2(obs);
    186.  
    187.                     const float invSigma2 = pFrame->mvInvLevelSigma2[kpUn.octave] / unc2;
    188.                     e->setInformation(Eigen::Matrix2d::Identity() * invSigma2);
    189.  
    190.                     g2o::RobustKernelHuber *rk = new g2o::RobustKernelHuber;
    191.                     e->setRobustKernel(rk);
    192.                     rk->setDelta(thHuberMono);
    193.  
    194.                     optimizer.addEdge(e);
    195.  
    196.                     vpEdgesMono.push_back(e);
    197.                     vnIndexEdgeMono.push_back(i);
    198.                 }
    199.             }
    200.         }
    201.     }
    202.     //统计参与优化的地图点总数目
    203.     nInitialCorrespondences = nInitialMonoCorrespondences + nInitialStereoCorrespondences;
    204.  
    205.     // 4. 上一个关键帧节点
    206.     // pKF为上一关键帧
    207.     KeyFrame *pKF = pFrame->mpLastKeyFrame;
    208.  
    209.     //上一关键帧的位姿，旋转+平移，6-dim
    210.     VertexPose *VPk = new VertexPose(pKF);
    211.     VPk->setId(4);
    212.     VPk->setFixed(true);
    213.     optimizer.addVertex(VPk);
    214.     //上一关键帧的速度，3-dim
    215.     VertexVelocity *VVk = new VertexVelocity(pKF);
    216.     VVk->setId(5);
    217.     VVk->setFixed(true);
    218.     optimizer.addVertex(VVk);
    219.     //上一关键帧的陀螺仪偏置，3-dim
    220.     VertexGyroBias *VGk = new VertexGyroBias(pKF);
    221.     VGk->setId(6);
    222.     VGk->setFixed(true);
    223.     optimizer.addVertex(VGk);
    224.     //上一关键帧的加速度偏置，3-dim
    225.     VertexAccBias *VAk = new VertexAccBias(pKF);
    226.     VAk->setId(7);
    227.     VAk->setFixed(true);
    228.     optimizer.addVertex(VAk);
    229.     // setFixed(true)这个设置使以上四个顶点（15个参数）的值在优化时保持固定
    230.     //既然被选为关键帧，就不能太善变
    231.  
    232.     // 5. 第二种边（IMU预积分约束）：两帧之间位姿的变化量与IMU预积分的值偏差尽可能小
    233.     EdgeInertial *ei = new EdgeInertial(pFrame->mpImuPreintegrated);
    234.  
    235.     //将上一关键帧四个顶点（P、V、BG、BA）和当前帧两个顶点（P、V）加入第二种边
    236.     ei->setVertex(0, VPk);
    237.     ei->setVertex(1, VVk);
    238.     ei->setVertex(2, VGk);
    239.     ei->setVertex(3, VAk);
    240.     ei->setVertex(4, VP);
    241.     ei->setVertex(5, VV);
    242.     //把第二种边加入优化器
    243.     optimizer.addEdge(ei);
    244.  
    245.     // 6. 第三种边（陀螺仪随机游走约束）：陀螺仪的随机游走值在相近帧间不会相差太多  residual=VG-VGk
    246.     // 用大白话来讲就是用固定的VGK拽住VG，防止VG在优化中放飞自我
    247.     EdgeGyroRW *egr = new EdgeGyroRW();
    248.  
    249.     // 将上一关键帧的BG加入第三种边
    250.     egr->setVertex(0, VGk);
    251.     //将当前帧的BG加入第三种边
    252.     egr->setVertex(1, VG);
    253.     // C值在预积分阶段更新，range(9,12)对应陀螺仪偏置的协方差，最终cvInfoG值为inv(∑(GyroRW^2/freq))
    254.     cv::Mat cvInfoG = pFrame->mpImuPreintegrated->C.rowRange(9, 12).colRange(9, 12).inv(cv::DECOMP_SVD);
    255.     Eigen::Matrix3d InfoG;
    256.     for (int r = 0; r < 3; r++)
    257.         for (int c = 0; c < 3; c++)
    258.             InfoG(r, c) = cvInfoG.at<float>(r, c);
    259.  
    260.     // 设置信息矩阵
    261.     egr->setInformation(InfoG);
    262.     // 把第三种边加入优化器
    263.     optimizer.addEdge(egr);
    264.  
    265.     // 7. 第四种边（加速度随机游走约束）：加速度的随机游走值在相近帧间不会相差太多  residual=VA-VAk
    266.     // 用大白话来讲就是用固定的VAK拽住VA，防止VA在优化中放飞自我
    267.     EdgeAccRW *ear = new EdgeAccRW();
    268.     // 将上一关键帧的BA加入第四种边
    269.     ear->setVertex(0, VAk);
    270.     // 将当前帧的BA加入第四种边
    271.     ear->setVertex(1, VA);
    272.     // C值在预积分阶段更新，range(12,15)对应加速度偏置的协方差，最终cvInfoG值为inv(∑(AccRW^2/freq))
    273.     cv::Mat cvInfoA = pFrame->mpImuPreintegrated->C.rowRange(12, 15).colRange(12, 15).inv(cv::DECOMP_SVD);
    274.     Eigen::Matrix3d InfoA;
    275.     for (int r = 0; r < 3; r++)
    276.         for (int c = 0; c < 3; c++)
    277.             InfoA(r, c) = cvInfoA.at<float>(r, c);
    278.     // 设置信息矩阵
    279.     ear->setInformation(InfoA);
    280.     // 把第四种边加入优化器
    281.     optimizer.addEdge(ear);
    282.  
    283.     // 8. 启动多轮优化，剔除外点
    284.  
    285.     // We perform 4 optimizations, after each optimization we classify observation as inlier/outlier
    286.     // At the next optimization, outliers are not included, but at the end they can be classified as inliers again.
    287.     // 卡方检验值呈递减趋势，目的是让检验越来越苛刻
    288.     float chi2Mono[4] = {12, 7.5, 5.991, 5.991};
    289.     float chi2Stereo[4] = {15.6, 9.8, 7.815, 7.815};
    290.     // 4次优化的迭代次数都为10
    291.     int its[4] = {10, 10, 10, 10};
    292.  
    293.     // 坏点数
    294.     int nBad = 0;
    295.     // 单目坏点数
    296.     int nBadMono = 0;
    297.     // 双目坏点数
    298.     int nBadStereo = 0;
    299.     // 单目内点数
    300.     int nInliersMono = 0;
    301.     // 双目内点数
    302.     int nInliersStereo = 0;
    303.     // 内点数
    304.     int nInliers = 0;
    305.     bool bOut = false;
    306.  
    307.     // 进行4次优化
    308.     for (size_t it = 0; it < 4; it++)
    309.     {
    310.         // 初始化优化器,这里的参数0代表只对level为0的边进行优化（不传参数默认也是0）
    311.         optimizer.initializeOptimization(0);
    312.         // 每次优化迭代十次
    313.         optimizer.optimize(its[it]);
    314.  
    315.         // 每次优化都重新统计各类点的数目
    316.         nBad = 0;
    317.         nBadMono = 0;
    318.         nBadStereo = 0;
    319.         nInliers = 0;
    320.         nInliersMono = 0;
    321.         nInliersStereo = 0;
    322.  
    323.         // 使用1.5倍的chi2Mono作为“近点”的卡方检验值，意味着地图点越近，检验越宽松
    324.         // 地图点如何定义为“近点”在下面的代码中有解释
    325.         float chi2close = 1.5 * chi2Mono[it];
    326.  
    327.         // For monocular observations
    328.         for (size_t i = 0, iend = vpEdgesMono.size(); i < iend; i++)
    329.         {
    330.             EdgeMonoOnlyPose *e = vpEdgesMono[i];
    331.  
    332.             const size_t idx = vnIndexEdgeMono[i];
    333.  
    334.             // 如果这条误差边是来自于outlier
    335.             if (pFrame->mvbOutlier[idx])
    336.             {
    337.                 // 计算这条边上次优化后的误差
    338.                 e->computeError();
    339.             }
    340.  
    341.             // 就是error*\Omega*error，表示了这条边考虑置信度以后的误差大小
    342.             const float chi2 = e->chi2();
    343.  
    344.             // 当地图点在当前帧的深度值小于10时，该地图点属于close（近点）
    345.             // mTrackDepth是在Frame.cc的isInFrustum函数中计算出来的
    346.             bool bClose = pFrame->mvpMapPoints[idx]->mTrackDepth < 10.f;
    347.  
    348.             // 判断某地图点为外点的条件有以下三种：
    349.             // 1.该地图点不是近点并且误差大于卡方检验值chi2Mono[it]
    350.             // 2.该地图点是近点并且误差大于卡方检验值chi2close
    351.             // 3.深度不为正
    352.             // 每次优化后，用更小的卡方检验值，原因是随着优化的进行，对划分为内点的信任程度越来越低
    353.             if ((chi2 > chi2Mono[it] && !bClose) || (bClose && chi2 > chi2close) || !e->isDepthPositive())
    354.             {
    355.                 // 将该点设置为外点
    356.                 pFrame->mvbOutlier[idx] = true;
    357.                 // 外点不参与下一轮优化
    358.                 e->setLevel(1);
    359.                 // 单目坏点数+1
    360.                 nBadMono++;
    361.             }
    362.             else
    363.             {
    364.                 // 将该点设置为内点（暂时）
    365.                 pFrame->mvbOutlier[idx] = false;
    366.                 // 内点继续参与下一轮优化
    367.                 e->setLevel(0);
    368.                 // 单目内点数+1
    369.                 nInliersMono++;
    370.             }
    371.  
    372.             // 从第三次优化开始就不设置鲁棒核函数了，原因是经过两轮优化已经趋向准确值，不会有太大误差
    373.             if (it == 2)
    374.                 e->setRobustKernel(0);
    375.         }
    376.  
    377.         // For stereo observations
    378.         for (size_t i = 0, iend = vpEdgesStereo.size(); i < iend; i++)
    379.         {
    380.             EdgeStereoOnlyPose *e = vpEdgesStereo[i];
    381.  
    382.             const size_t idx = vnIndexEdgeStereo[i];
    383.  
    384.             if (pFrame->mvbOutlier[idx])
    385.             {
    386.                 e->computeError();
    387.             }
    388.  
    389.             const float chi2 = e->chi2();
    390.  
    391.             if (chi2 > chi2Stereo[it])
    392.             {
    393.                 pFrame->mvbOutlier[idx] = true;
    394.                 e->setLevel(1); // not included in next optimization
    395.                 nBadStereo++;
    396.             }
    397.             else
    398.             {
    399.                 pFrame->mvbOutlier[idx] = false;
    400.                 e->setLevel(0);
    401.                 nInliersStereo++;
    402.             }
    403.  
    404.             if (it == 2)
    405.                 e->setRobustKernel(0);
    406.         }
    407.  
    408.         // 内点总数=单目内点数+双目内点数
    409.         nInliers = nInliersMono + nInliersStereo;
    410.         // 坏点数=单目坏点数+双目坏点数
    411.         nBad = nBadMono + nBadStereo;
    412.  
    413.         if (optimizer.edges().size() < 10)
    414.         {
    415.             cout << "PIOLKF: NOT ENOUGH EDGES" << endl;
    416.             break;
    417.         }
    418.     }
    419.  
    420.     // If not too much tracks, recover not too bad points
    421.     // 9. 若4次优化后内点数小于30，尝试恢复一部分不那么糟糕的坏点
    422.     if ((nInliers < 30) && !bRecInit)
    423.     {
    424.         //重新从0开始统计坏点数
    425.         nBad = 0;
    426.         //单目可容忍的卡方检验最大值（如果误差比这还大就不要挣扎了...）
    427.         const float chi2MonoOut = 18.f;
    428.         const float chi2StereoOut = 24.f;
    429.         EdgeMonoOnlyPose *e1;
    430.         EdgeStereoOnlyPose *e2;
    431.         //遍历所有单目特征点
    432.         for (size_t i = 0, iend = vnIndexEdgeMono.size(); i < iend; i++)
    433.         {
    434.             const size_t idx = vnIndexEdgeMono[i];
    435.             //获取这些特征点对应的边
    436.             e1 = vpEdgesMono[i];
    437.             e1->computeError();
    438.             //判断误差值是否超过单目可容忍的卡方检验最大值，是的话就把这个点保下来
    439.             if (e1->chi2() < chi2MonoOut)
    440.                 pFrame->mvbOutlier[idx] = false;
    441.             else
    442.                 nBad++;
    443.         }
    444.         for (size_t i = 0, iend = vnIndexEdgeStereo.size(); i < iend; i++)
    445.         {
    446.             const size_t idx = vnIndexEdgeStereo[i];
    447.             e2 = vpEdgesStereo[i];
    448.             e2->computeError();
    449.             if (e2->chi2() < chi2StereoOut)
    450.                 pFrame->mvbOutlier[idx] = false;
    451.             else
    452.                 nBad++;
    453.         }
    454.     }
    455.  
    456.     // 10. 更新当前帧位姿、速度、IMU偏置
    457.  
    458.     // Recover optimized pose, velocity and biases
    459.     // 给当前帧设置优化后的旋转、位移、速度，用来更新位姿
    460.     pFrame->SetImuPoseVelocity(Converter::toCvMat(VP->estimate().Rwb), Converter::toCvMat(VP->estimate().twb), Converter::toCvMat(VV->estimate()));
    461.     Vector6d b;
    462.     b << VG->estimate(), VA->estimate();
    463.     // 给当前帧设置优化后的bg，ba
    464.     pFrame->mImuBias = IMU::Bias(b[3], b[4], b[5], b[0], b[1], b[2]);
    465.  
    466.     // 11. 记录当前帧的优化状态，包括参数信息和对应的海森矩阵
    467.     // Recover Hessian, marginalize keyFframe states and generate new prior for frame
    468.     Eigen::Matrix<double, 15, 15> H;
    469.     H.setZero();
    470.  
    471.     // H(x)=J(x).t()*info*J(x)
    472.  
    473.     // J(x)取的是EdgeInertial中的_jacobianOplus[4]和_jacobianOplus[5]，即EdgeInertial::computeError计算出来的er,ev,ep对当前帧Pose和Velocity的偏导
    474.     //因此ei->GetHessian2的结果为：
    475.     // H(∂er/∂r) H(∂er/∂t) H(∂er/∂v)
    476.     // H(∂ev/∂r) H(∂ev/∂t) H(∂ev/∂v)
    477.     // H(∂ep/∂r) H(∂ep/∂t) H(∂ep/∂v)
    478.     //每项H都是3x3，故GetHessian2的结果是9x9
    479.     H.block<9, 9>(0, 0) += ei->GetHessian2();
    480.  
    481.     // J(x)取的是EdgeGyroRW中的_jacobianOplusXj，即EdgeGyroRW::computeError计算出来的_error(ebg)对当前帧bg的偏导
    482.     //因此egr->GetHessian2的结果为：
    483.     // H(∂ebg/∂bg) ，3x3
    484.     H.block<3, 3>(9, 9) += egr->GetHessian2();
    485.  
    486.     // J(x)取的是EdgeAccRW中的_jacobianOplusXj，即EdgeAccRW::computeError计算出来的_error(ebg)对当前帧ba的偏导
    487.     //因此ear->GetHessian2的结果为：
    488.     // H(∂eba/∂ba)  ，3x3
    489.     H.block<3, 3>(12, 12) += ear->GetHessian2();
    490.  
    491.     //经过前面几步，Hessian Matrix长这个样子（注：省略了->GetHessian2()）
    492.     // ei ei ei ei ei ei ei ei ei   0      0     0    0     0    0
    493.     // ei ei ei ei ei ei ei ei ei   0      0     0    0     0    0
    494.     // ei ei ei ei ei ei ei ei ei   0      0     0    0     0    0
    495.     // ei ei ei ei ei ei ei ei ei   0      0     0    0     0    0
    496.     // ei ei ei ei ei ei ei ei ei   0      0     0    0     0    0
    497.     // ei ei ei ei ei ei ei ei ei   0      0     0    0     0    0
    498.     // ei ei ei ei ei ei ei ei ei   0      0     0    0     0    0
    499.     // ei ei ei ei ei ei ei ei ei   0      0     0    0     0    0
    500.     // ei ei ei ei ei ei ei ei ei   0      0     0    0     0    0
    501.     // 0  0  0  0  0  0  0   0  0  egr egr egr  0     0     0
    502.     // 0  0  0  0  0  0  0   0  0  egr egr egr  0     0     0
    503.     // 0  0  0  0  0  0  0   0  0  egr egr egr  0     0     0
    504.     // 0  0  0  0  0  0  0   0  0    0     0      0  ear ear ear
    505.     // 0  0  0  0  0  0  0   0  0    0     0      0  ear ear ear
    506.     // 0  0  0  0  0  0  0   0  0    0     0      0  ear ear ear
    507.  
    508.     int tot_in = 0, tot_out = 0;
    509.     for (size_t i = 0, iend = vpEdgesMono.size(); i < iend; i++)
    510.     {
    511.         EdgeMonoOnlyPose *e = vpEdgesMono[i];
    512.  
    513.         const size_t idx = vnIndexEdgeMono[i];
    514.  
    515.         if (!pFrame->mvbOutlier[idx])
    516.         {
    517.             H.block<6, 6>(0, 0) += e->GetHessian();
    518.             tot_in++;
    519.         }
    520.         else
    521.             tot_out++;
    522.     }
    523.  
    524.     for (size_t i = 0, iend = vpEdgesStereo.size(); i < iend; i++)
    525.     {
    526.         EdgeStereoOnlyPose *e = vpEdgesStereo[i];
    527.  
    528.         const size_t idx = vnIndexEdgeStereo[i];
    529.  
    530.         if (!pFrame->mvbOutlier[idx])
    531.         {
    532.             H.block<6, 6>(0, 0) += e->GetHessian();
    533.             tot_in++;
    534.         }
    535.         else
    536.             tot_out++;
    537.     }
    538.  
    539.     //设eie = ei->GetHessian2()+∑(e->GetHessian())
    540.     //则最终Hessian Matrix长这样
    541.     // eie eie eie eie eie eie         ei ei ei   0      0     0    0     0    0
    542.     // eie eie eie eie eie eie         ei ei ei   0      0     0    0     0    0
    543.     // eie eie eie eie eie eie         ei ei ei   0      0     0    0     0    0
    544.     // eie eie eie eie eie eie         ei ei ei   0      0     0    0     0    0
    545.     // eie eie eie eie eie eie         ei ei ei   0      0     0    0     0    0
    546.     // eie eie eie eie eie eie         ei ei ei   0      0     0    0     0    0
    547.     // ei    ei    ei    ei   ei   ei  ei ei ei   0      0     0    0     0    0
    548.     // ei    ei    ei    ei   ei   ei  ei ei ei   0      0     0    0     0    0
    549.     // ei    ei    ei    ei   ei   ei  ei ei ei   0      0     0    0     0    0
    550.     //  0     0     0     0     0    0   0   0  0  egr egr egr  0     0     0
    551.     //  0     0     0     0     0    0   0   0  0  egr egr egr  0     0     0
    552.     //  0     0     0     0     0    0   0   0  0  egr egr egr  0     0     0
    553.     //  0     0     0     0     0    0   0   0  0    0     0      0  ear ear ear
    554.     //  0     0     0     0     0    0   0   0  0    0     0      0  ear ear ear
    555.     //  0     0     0     0     0    0   0   0  0    0     0      0  ear ear ear
    556.  
    557.     //构造一个ConstraintPoseImu对象，为下一帧提供先验约束
    558.     //构造对象所使用的参数是当前帧P、V、BG、BA的估计值和H矩阵
    559.     pFrame->mpcpi = new ConstraintPoseImu(VP->estimate().Rwb, VP->estimate().twb, VV->estimate(), VG->estimate(), VA->estimate(), H);
    560.     //在PoseInertialOptimizationLastFrame函数中，会将ConstraintPoseImu信息作为“上一帧先验约束”生成一条优化边
    561.  
    562.     //返回值：内点数 = 总地图点数目 - 坏点（外点）数目
    563.     return nInitialCorrespondences - nBad;
    564. }

    使用上一帧+当前帧的视觉信息和IMU信息，优化当前帧位姿
    可分为以下几个步骤：

    • Step 1：创建g2o优化器，初始化顶点和边
    • Step 2：启动多轮优化，剔除外点
    • Step 3：更新当前帧位姿、速度、IMU偏置
    • Step 4：记录当前帧的优化状态，包括参数信息和边缘化后的海森矩阵
    1. int Optimizer::PoseInertialOptimizationLastFrame(Frame *pFrame, bool bRecInit)
    2. {
    3.     // Step 1：创建g2o优化器，初始化顶点和边
    4.     //构建一个稀疏求解器
    5.     g2o::SparseOptimizer optimizer;
    6.     g2o::BlockSolverX::LinearSolverType *linearSolver;
    7.  
    8.     // 使用dense的求解器，（常见非dense求解器有cholmod线性求解器和shur补线性求解器）
    9.     linearSolver = new g2o::LinearSolverDense();
    10.  
    11.     g2o::BlockSolverX *solver_ptr = new g2o::BlockSolverX(linearSolver);
    12.  
    13.     //使用高斯牛顿求解器
    14.     g2o::OptimizationAlgorithmGaussNewton *solver = new g2o::OptimizationAlgorithmGaussNewton(solver_ptr);
    15.     optimizer.setAlgorithm(solver);
    16.     optimizer.setVerbose(false);
    17.  
    18.     //当前帧单（左）目地图点数目
    19.     int nInitialMonoCorrespondences = 0;
    20.     int nInitialStereoCorrespondences = 0;
    21.     int nInitialCorrespondences = 0;
    22.  
    23.     // Set Current Frame vertex
    24.     //当前帧的位姿，旋转+平移，6-dim
    25.     VertexPose *VP = new VertexPose(pFrame);
    26.     VP->setId(0);
    27.     VP->setFixed(false); //需要优化，所以不固定
    28.     optimizer.addVertex(VP);
    29.     //当前帧的速度，3-dim
    30.     VertexVelocity *VV = new VertexVelocity(pFrame);
    31.     VV->setId(1);
    32.     VV->setFixed(false);
    33.     optimizer.addVertex(VV);
    34.     //当前帧的陀螺仪偏置，3-dim
    35.     VertexGyroBias *VG = new VertexGyroBias(pFrame);
    36.     VG->setId(2);
    37.     VG->setFixed(false);
    38.     optimizer.addVertex(VG);
    39.     //当前帧的加速度偏置，3-dim
    40.     VertexAccBias *VA = new VertexAccBias(pFrame);
    41.     VA->setId(3);
    42.     VA->setFixed(false);
    43.     optimizer.addVertex(VA);
    44.  
    45.     // Set MapPoint vertices
    46.     //当前帧的特征点总数 N = Nleft + Nright
    47.     //对于单目 N!=0, Nleft=-1，Nright=-1
    48.     //对于双目 N!=0, Nleft!=-1，Nright!=-1
    49.     const int N = pFrame->N;
    50.     //当前帧左目的特征点总数
    51.     const int Nleft = pFrame->Nleft;
    52.     //当前帧是否存在右目（即是否为双目），存在为true
    53.     //?感觉可以更直接点啊 bRight = (Nright!=-1)
    54.     const bool bRight = (Nleft != -1);
    55.  
    56.     vector vpEdgesMono;
    57.     vector vpEdgesStereo;
    58.     vector<size_t> vnIndexEdgeMono;
    59.     vector<size_t> vnIndexEdgeStereo;
    60.     vpEdgesMono.reserve(N);
    61.     vpEdgesStereo.reserve(N);
    62.     vnIndexEdgeMono.reserve(N);
    63.     vnIndexEdgeStereo.reserve(N);
    64.  
    65.     // 自由度为2的卡方分布，显著性水平为0.05，对应的临界阈值5.991
    66.     const float thHuberMono = sqrt(5.991);
    67.     // 自由度为3的卡方分布，显著性水平为0.05，对应的临界阈值7.815
    68.     const float thHuberStereo = sqrt(7.815);
    69.  
    70.     {
    71.         // 锁定地图点。由于需要使用地图点来构造顶点和边,因此不希望在构造的过程中部分地图点被改写造成不一致甚至是段错误
    72.         unique_lock lock(MapPoint::mGlobalMutex);
    73.  
    74.         for (int i = 0; i < N; i++)
    75.         {
    76.             MapPoint *pMP = pFrame->mvpMapPoints[i];
    77.             if (pMP)
    78.             {
    79.                 cv::KeyPoint kpUn;
    80.                 // Left monocular observation
    81.                 // 这里说的Left monocular包含两种情况：1.单目情况 2.双目情况下的左目
    82.                 if ((!bRight && pFrame->mvuRight[i] < 0) || i < Nleft)
    83.                 {
    84.                     //如果是双目情况下的左目
    85.                     if (i < Nleft) // pair left-right
    86.                         //使用未畸变校正的特征点
    87.                         kpUn = pFrame->mvKeys[i];
    88.                     //如果是单目
    89.                     else
    90.                         //使用畸变校正过的特征点
    91.                         kpUn = pFrame->mvKeysUn[i];
    92.  
    93.                     //单目地图点计数增加
    94.                     nInitialMonoCorrespondences++;
    95.                     //当前地图点默认设置为不是外点
    96.                     pFrame->mvbOutlier[i] = false;
    97.                     // 观测的特征点
    98.                     Eigen::Matrix<double, 2, 1> obs;
    99.                     obs << kpUn.pt.x, kpUn.pt.y;
    100.  
    101.                     //第一种边(视觉重投影约束)：地图点投影到该帧图像的坐标与特征点坐标偏差尽可能小
    102.                     EdgeMonoOnlyPose *e = new EdgeMonoOnlyPose(pMP->GetWorldPos(), 0);
    103.  
    104.                     //将位姿作为第一个顶点
    105.                     e->setVertex(0, VP);
    106.  
    107.                     //设置观测值，即去畸变后的像素坐标
    108.                     e->setMeasurement(obs);
    109.  
    110.                     // Add here uncerteinty
    111.                     // 获取不确定度，这里调用uncertainty2返回固定值1.0
    112.                     // ?这里返回1.0是作为缺省值，是否可以根据对视觉信息的信任度动态修改这个值，比如标定的误差？
    113.                     const float unc2 = pFrame->mpCamera->uncertainty2(obs);
    114.  
    115.                     // invSigma2 = (Inverse(协方差矩阵))^2，表明该约束在各个维度上的可信度
    116.                     //  图像金字塔层数越高，可信度越差
    117.                     const float invSigma2 = pFrame->mvInvLevelSigma2[kpUn.octave] / unc2;
    118.                     //设置该约束的信息矩阵
    119.                     e->setInformation(Eigen::Matrix2d::Identity() * invSigma2);
    120.  
    121.                     g2o::RobustKernelHuber *rk = new g2o::RobustKernelHuber;
    122.                     // 设置鲁棒核函数，避免其误差的平方项出现数值过大的增长 注：后续在优化2次后会用e->setRobustKernel(0)禁掉鲁棒核函数
    123.                     e->setRobustKernel(rk);
    124.                     //重投影误差的自由度为2，设置对应的卡方阈值
    125.                     rk->setDelta(thHuberMono);
    126.  
    127.                     //将第一种边加入优化器
    128.                     optimizer.addEdge(e);
    129.  
    130.                     //将第一种边加入vpEdgesMono
    131.                     vpEdgesMono.push_back(e);
    132.                     //将对应的特征点索引加入vnIndexEdgeMono
    133.                     vnIndexEdgeMono.push_back(i);
    134.                 }
    135.                 // Stereo observation
    136.                 else if (!bRight) //? 为什么是双目
    137.                 {
    138.                     nInitialStereoCorrespondences++;
    139.                     pFrame->mvbOutlier[i] = false;
    140.  
    141.                     kpUn = pFrame->mvKeysUn[i];
    142.                     const float kp_ur = pFrame->mvuRight[i];
    143.                     Eigen::Matrix<double, 3, 1> obs;
    144.                     obs << kpUn.pt.x, kpUn.pt.y, kp_ur;
    145.  
    146.                     EdgeStereoOnlyPose *e = new EdgeStereoOnlyPose(pMP->GetWorldPos());
    147.  
    148.                     e->setVertex(0, VP);
    149.                     e->setMeasurement(obs);
    150.  
    151.                     // Add here uncerteinty
    152.                     const float unc2 = pFrame->mpCamera->uncertainty2(obs.head(2));
    153.  
    154.                     const float &invSigma2 = pFrame->mvInvLevelSigma2[kpUn.octave] / unc2;
    155.                     e->setInformation(Eigen::Matrix3d::Identity() * invSigma2);
    156.  
    157.                     g2o::RobustKernelHuber *rk = new g2o::RobustKernelHuber;
    158.                     e->setRobustKernel(rk);
    159.                     rk->setDelta(thHuberStereo);
    160.  
    161.                     optimizer.addEdge(e);
    162.  
    163.                     vpEdgesStereo.push_back(e);
    164.                     vnIndexEdgeStereo.push_back(i);
    165.                 }
    166.  
    167.                 // Right monocular observation
    168.                 if (bRight && i >= Nleft)
    169.                 {
    170.                     nInitialMonoCorrespondences++;
    171.                     pFrame->mvbOutlier[i] = false;
    172.  
    173.                     kpUn = pFrame->mvKeysRight[i - Nleft];
    174.                     Eigen::Matrix<double, 2, 1> obs;
    175.                     obs << kpUn.pt.x, kpUn.pt.y;
    176.  
    177.                     EdgeMonoOnlyPose *e = new EdgeMonoOnlyPose(pMP->GetWorldPos(), 1);
    178.  
    179.                     e->setVertex(0, VP);
    180.                     e->setMeasurement(obs);
    181.  
    182.                     // Add here uncerteinty
    183.                     const float unc2 = pFrame->mpCamera->uncertainty2(obs);
    184.  
    185.                     const float invSigma2 = pFrame->mvInvLevelSigma2[kpUn.octave] / unc2;
    186.                     e->setInformation(Eigen::Matrix2d::Identity() * invSigma2);
    187.  
    188.                     g2o::RobustKernelHuber *rk = new g2o::RobustKernelHuber;
    189.                     e->setRobustKernel(rk);
    190.                     rk->setDelta(thHuberMono);
    191.  
    192.                     optimizer.addEdge(e);
    193.  
    194.                     vpEdgesMono.push_back(e);
    195.                     vnIndexEdgeMono.push_back(i);
    196.                 }
    197.             }
    198.         }
    199.     }
    200.     //统计参与优化的地图点总数目
    201.     nInitialCorrespondences = nInitialMonoCorrespondences + nInitialStereoCorrespondences;
    202.  
    203.     // Set Previous Frame Vertex
    204.     // pFp为上一帧
    205.     Frame *pFp = pFrame->mpPrevFrame;
    206.  
    207.     //上一帧的位姿，旋转+平移，6-dim
    208.     VertexPose *VPk = new VertexPose(pFp);
    209.     VPk->setId(4);
    210.     VPk->setFixed(false);
    211.     optimizer.addVertex(VPk);
    212.     //上一帧的速度，3-dim
    213.     VertexVelocity *VVk = new VertexVelocity(pFp);
    214.     VVk->setId(5);
    215.     VVk->setFixed(false);
    216.     optimizer.addVertex(VVk);
    217.     //上一帧的陀螺仪偏置，3-dim
    218.     VertexGyroBias *VGk = new VertexGyroBias(pFp);
    219.     VGk->setId(6);
    220.     VGk->setFixed(false);
    221.     optimizer.addVertex(VGk);
    222.     //上一帧的加速度偏置，3-dim
    223.     VertexAccBias *VAk = new VertexAccBias(pFp);
    224.     VAk->setId(7);
    225.     VAk->setFixed(false);
    226.     optimizer.addVertex(VAk);
    227.     // setFixed(false)这个设置使以上四个顶点（15个参数）的值随优化而变，这样做会给上一帧再提供一些优化空间
    228.     //但理论上不应该优化过多，否则会有不良影响，故后面的代码会用第五种边来约束上一帧的变化量
    229.  
    230.     //第二种边（IMU预积分约束）：两帧之间位姿的变化量与IMU预积分的值偏差尽可能小
    231.     EdgeInertial *ei = new EdgeInertial(pFrame->mpImuPreintegratedFrame);
    232.  
    233.     //将上一帧四个顶点（P、V、BG、BA）和当前帧两个顶点（P、V）加入第二种边
    234.     ei->setVertex(0, VPk);
    235.     ei->setVertex(1, VVk);
    236.     ei->setVertex(2, VGk);
    237.     ei->setVertex(3, VAk);
    238.     ei->setVertex(4, VP);
    239.     ei->setVertex(5, VV);
    240.     //把第二种边加入优化器
    241.     optimizer.addEdge(ei);
    242.  
    243.     //第三种边（陀螺仪随机游走约束）：陀螺仪的随机游走值在相邻帧间不会相差太多  residual=VG-VGk
    244.     //用大白话来讲就是用固定的VGK拽住VG，防止VG在优化中放飞自我
    245.     EdgeGyroRW *egr = new EdgeGyroRW();
    246.     //将上一帧的BG加入第三种边
    247.     egr->setVertex(0, VGk);
    248.     //将当前帧的BG加入第三种边
    249.     egr->setVertex(1, VG);
    250.     // C值在预积分阶段更新，range(9,12)对应陀螺仪偏置的协方差，最终cvInfoG值为inv(∑(GyroRW^2/freq))
    251.     cv::Mat cvInfoG = pFrame->mpImuPreintegratedFrame->C.rowRange(9, 12).colRange(9, 12).inv(cv::DECOMP_SVD);
    252.     Eigen::Matrix3d InfoG;
    253.     for (int r = 0; r < 3; r++)
    254.         for (int c = 0; c < 3; c++)
    255.             InfoG(r, c) = cvInfoG.at<float>(r, c);
    256.  
    257.     //设置信息矩阵
    258.     egr->setInformation(InfoG);
    259.     //把第三种边加入优化器
    260.     optimizer.addEdge(egr);
    261.  
    262.     //第四种边（加速度随机游走约束）：加速度的随机游走值在相近帧间不会相差太多  residual=VA-VAk
    263.     //用大白话来讲就是用固定的VAK拽住VA，防止VA在优化中放飞自我
    264.     EdgeAccRW *ear = new EdgeAccRW();
    265.     //将上一帧的BA加入第四种边
    266.     ear->setVertex(0, VAk);
    267.     //将当前帧的BA加入第四种边
    268.     ear->setVertex(1, VA);
    269.     // C值在预积分阶段更新，range(12,15)对应加速度偏置的协方差，最终cvInfoG值为inv(∑(AccRW^2/freq))
    270.     cv::Mat cvInfoA = pFrame->mpImuPreintegratedFrame->C.rowRange(12, 15).colRange(12, 15).inv(cv::DECOMP_SVD);
    271.     Eigen::Matrix3d InfoA;
    272.     for (int r = 0; r < 3; r++)
    273.         for (int c = 0; c < 3; c++)
    274.             InfoA(r, c) = cvInfoA.at<float>(r, c);
    275.     //设置信息矩阵
    276.     ear->setInformation(InfoA);
    277.     //把第四种边加入优化器
    278.     optimizer.addEdge(ear);
    279.  
    280.     // ?既然有判空的操作，可以区分一下有先验信息（五条边）和无先验信息（四条边）的情况
    281.     if (!pFp->mpcpi)
    282.         Verbose::PrintMess("pFp->mpcpi does not exist!!!\nPrevious Frame " + to_string(pFp->mnId), Verbose::VERBOSITY_NORMAL);
    283.  
    284.     //第五种边（先验约束）：上一帧信息随优化的改变量要尽可能小
    285.     EdgePriorPoseImu *ep = new EdgePriorPoseImu(pFp->mpcpi);
    286.  
    287.     //将上一帧的四个顶点（P、V、BG、BA）加入第五种边
    288.     ep->setVertex(0, VPk);
    289.     ep->setVertex(1, VVk);
    290.     ep->setVertex(2, VGk);
    291.     ep->setVertex(3, VAk);
    292.     g2o::RobustKernelHuber *rkp = new g2o::RobustKernelHuber;
    293.     ep->setRobustKernel(rkp);
    294.     rkp->setDelta(5);
    295.     //把第五种边加入优化器
    296.     optimizer.addEdge(ep);
    297.  
    298.     // Step 2：启动多轮优化，剔除外点
    299.  
    300.     // We perform 4 optimizations, after each optimization we classify observation as inlier/outlier
    301.     // At the next optimization, outliers are not included, but at the end they can be classified as inliers again.
    302.     //与PoseInertialOptimizationLastKeyFrame函数对比，区别在于：在优化过程中保持卡方阈值不变
    303.     // 以下参数的解释
    304.     // 自由度为2的卡方分布，显著性水平为0.05，对应的临界阈值5.991
    305.     // 自由度为3的卡方分布，显著性水平为0.05，对应的临界阈值7.815
    306.     // 自由度为3的卡方分布，显著性水平为0.02，对应的临界阈值9.8
    307.     // 自由度为3的卡方分布，显著性水平为0.001，对应的临界阈值15.6
    308.     // 计算方法：https://stattrek.com/online-calculator/chi-square.aspx
    309.     const float chi2Mono[4] = {5.991, 5.991, 5.991, 5.991};
    310.     const float chi2Stereo[4] = {15.6f, 9.8f, 7.815f, 7.815f};
    311.     // 4次优化的迭代次数都为10
    312.     const int its[4] = {10, 10, 10, 10};
    313.  
    314.     //坏点数
    315.     int nBad = 0;
    316.     //单目坏点数
    317.     int nBadMono = 0;
    318.     //双目坏点数
    319.     int nBadStereo = 0;
    320.     //单目内点数
    321.     int nInliersMono = 0;
    322.     //双目内点数
    323.     int nInliersStereo = 0;
    324.     //内点数
    325.     int nInliers = 0;
    326.     for (size_t it = 0; it < 4; it++)
    327.     {
    328.         // 初始化优化器,这里的参数0代表只对level为0的边进行优化（不传参数默认也是0）
    329.         optimizer.initializeOptimization(0);
    330.         //每次优化迭代十次
    331.         optimizer.optimize(its[it]);
    332.  
    333.         //每次优化都重新统计各类点的数目
    334.         nBad = 0;
    335.         nBadMono = 0;
    336.         nBadStereo = 0;
    337.         nInliers = 0;
    338.         nInliersMono = 0;
    339.         nInliersStereo = 0;
    340.         //使用1.5倍的chi2Mono作为“近点”的卡方检验值，意味着地图点越近，检验越宽松
    341.         //地图点如何定义为“近点”在下面的代码中有解释
    342.         float chi2close = 1.5 * chi2Mono[it];
    343.  
    344.         for (size_t i = 0, iend = vpEdgesMono.size(); i < iend; i++)
    345.         {
    346.             EdgeMonoOnlyPose *e = vpEdgesMono[i];
    347.  
    348.             const size_t idx = vnIndexEdgeMono[i];
    349.  
    350.             //当地图点在当前帧的深度值小于10时，该地图点属于close（近点）
    351.             // mTrackDepth是在Frame.cc的isInFrustum函数中计算出来的
    352.             bool bClose = pFrame->mvpMapPoints[idx]->mTrackDepth < 10.f;
    353.  
    354.             // 如果这条误差边是来自于outlier
    355.             if (pFrame->mvbOutlier[idx])
    356.             {
    357.                 //计算本次优化后的误差
    358.                 e->computeError();
    359.             }
    360.  
    361.             // 就是error*\Omega*error，表示了这条边考虑置信度以后的误差大小
    362.             const float chi2 = e->chi2();
    363.  
    364.             //判断某地图点为外点的条件有以下三种：
    365.             // 1.该地图点不是近点并且误差大于卡方检验值chi2Mono[it]
    366.             // 2.该地图点是近点并且误差大于卡方检验值chi2close
    367.             // 3.深度不为正
    368.             //每次优化后，用更小的卡方检验值，原因是随着优化的进行，对划分为内点的信任程度越来越低
    369.             if ((chi2 > chi2Mono[it] && !bClose) || (bClose && chi2 > chi2close) || !e->isDepthPositive())
    370.             {
    371.                 //将该点设置为外点
    372.                 pFrame->mvbOutlier[idx] = true;
    373.                 //外点不参与下一轮优化
    374.                 e->setLevel(1);
    375.                 //单目坏点数+1
    376.                 nBadMono++;
    377.             }
    378.             else
    379.             {
    380.                 //将该点设置为内点（暂时）
    381.                 pFrame->mvbOutlier[idx] = false;
    382.                 //内点继续参与下一轮优化
    383.                 e->setLevel(0);
    384.                 //单目内点数+1
    385.                 nInliersMono++;
    386.             }
    387.  
    388.             //从第三次优化开始就不设置鲁棒核函数了，原因是经过两轮优化已经趋向准确值，不会有太大误差
    389.             if (it == 2)
    390.                 e->setRobustKernel(0);
    391.         }
    392.  
    393.         for (size_t i = 0, iend = vpEdgesStereo.size(); i < iend; i++)
    394.         {
    395.             EdgeStereoOnlyPose *e = vpEdgesStereo[i];
    396.  
    397.             const size_t idx = vnIndexEdgeStereo[i];
    398.  
    399.             if (pFrame->mvbOutlier[idx])
    400.             {
    401.                 e->computeError();
    402.             }
    403.  
    404.             const float chi2 = e->chi2();
    405.  
    406.             if (chi2 > chi2Stereo[it])
    407.             {
    408.                 pFrame->mvbOutlier[idx] = true;
    409.                 e->setLevel(1);
    410.                 nBadStereo++;
    411.             }
    412.             else
    413.             {
    414.                 pFrame->mvbOutlier[idx] = false;
    415.                 e->setLevel(0);
    416.                 nInliersStereo++;
    417.             }
    418.  
    419.             if (it == 2)
    420.                 e->setRobustKernel(0);
    421.         }
    422.  
    423.         //内点总数=单目内点数+双目内点数
    424.         nInliers = nInliersMono + nInliersStereo;
    425.         //坏点数=单目坏点数+双目坏点数
    426.         nBad = nBadMono + nBadStereo;
    427.  
    428.         if (optimizer.edges().size() < 10)
    429.         {
    430.             cout << "PIOLF: NOT ENOUGH EDGES" << endl;
    431.             break;
    432.         }
    433.     }
    434.  
    435.     //若4次优化后内点数小于30，尝试恢复一部分不那么糟糕的坏点
    436.     if ((nInliers < 30) && !bRecInit)
    437.     {
    438.         //重新从0开始统计坏点数
    439.         nBad = 0;
    440.         //单目可容忍的卡方检验最大值（如果误差比这还大就不要挣扎了...）
    441.         const float chi2MonoOut = 18.f;
    442.         const float chi2StereoOut = 24.f;
    443.         EdgeMonoOnlyPose *e1;
    444.         EdgeStereoOnlyPose *e2;
    445.         //遍历所有单目特征点
    446.         for (size_t i = 0, iend = vnIndexEdgeMono.size(); i < iend; i++)
    447.         {
    448.             const size_t idx = vnIndexEdgeMono[i];
    449.             //获取这些特征点对应的边
    450.             e1 = vpEdgesMono[i];
    451.             e1->computeError();
    452.             if (e1->chi2() < chi2MonoOut)
    453.                 pFrame->mvbOutlier[idx] = false;
    454.             else
    455.                 nBad++;
    456.         }
    457.         for (size_t i = 0, iend = vnIndexEdgeStereo.size(); i < iend; i++)
    458.         {
    459.             const size_t idx = vnIndexEdgeStereo[i];
    460.             e2 = vpEdgesStereo[i];
    461.             e2->computeError();
    462.             if (e2->chi2() < chi2StereoOut)
    463.                 pFrame->mvbOutlier[idx] = false;
    464.             else
    465.                 nBad++;
    466.         }
    467.     }
    468.  
    469.     // ?多此一举？优化过程中nInliers这个值已经计算过了，nInliersMono和nInliersStereo在后续代码中一直保持不变
    470.     nInliers = nInliersMono + nInliersStereo;
    471.  
    472.     // Step 3：更新当前帧位姿、速度、IMU偏置
    473.  
    474.     // Recover optimized pose, velocity and biases
    475.     //给当前帧设置优化后的旋转、位移、速度，用来更新位姿
    476.     pFrame->SetImuPoseVelocity(Converter::toCvMat(VP->estimate().Rwb), Converter::toCvMat(VP->estimate().twb), Converter::toCvMat(VV->estimate()));
    477.     Vector6d b;
    478.     b << VG->estimate(), VA->estimate();
    479.     //给当前帧设置优化后的bg，ba
    480.     pFrame->mImuBias = IMU::Bias(b[3], b[4], b[5], b[0], b[1], b[2]);
    481.  
    482.     // Step 4：记录当前帧的优化状态，包括参数信息和边缘化后的海森矩阵
    483.     // Recover Hessian, marginalize previous frame states and generate new prior for frame
    484.     //包含本次优化所有信息矩阵的和，它代表本次优化对确定性的影响
    485.     Eigen::Matrix<double, 30, 30> H;
    486.     H.setZero();
    487.  
    488.     //第1步，加上EdgeInertial（IMU预积分约束）的海森矩阵
    489.     // ei的定义
    490.     // EdgeInertial* ei = new EdgeInertial(pFrame->mpImuPreintegratedFrame);
    491.     // ei->setVertex(0, VPk);
    492.     // ei->setVertex(1, VVk);
    493.     // ei->setVertex(2, VGk);
    494.     // ei->setVertex(3, VAk);
    495.     // ei->setVertex(4, VP);  // VertexPose* VP = new VertexPose(pFrame);
    496.     // ei->setVertex(5, VV);  // VertexVelocity* VV = new VertexVelocity(pFrame);
    497.     // ei->GetHessian()  =  J.t * J 下同，不做详细标注了
    498.     // J
    499.     //       rn + tn  vn    gn   an   rn+1 + tn+1  vn+1
    500.     // er      Jp1    Jv1  Jg1   Ja1      Jp2      Jv2
    501.     // 角标1表示上一帧，2表示当前帧
    502.     //      6            3             3           3            6            3
    503.     // Jp1.t * Jp1  Jp1.t * Jv1  Jp1.t * Jg1  Jp1.t * Ja1  Jp1.t * Jp2  Jp1.t * Jv2     6
    504.     // Jv1.t * Jp1  Jv1.t * Jv1  Jv1.t * Jg1  Jv1.t * Ja1  Jv1.t * Jp2  Jv1.t * Jv2     3
    505.     // Jg1.t * Jp1  Jg1.t * Jv1  Jg1.t * Jg1  Jg1.t * Ja1  Jg1.t * Jp2  Jg1.t * Jv2     3
    506.     // Ja1.t * Jp1  Ja1.t * Jv1  Ja1.t * Jg1  Ja1.t * Ja1  Ja1.t * Jp2  Ja1.t * Jv2     3
    507.     // Jp2.t * Jp1  Jp2.t * Jv1  Jp2.t * Jg1  Jp2.t * Ja1  Jp2.t * Jp2  Jp2.t * Jv2     6
    508.     // Jv2.t * Jp1  Jv2.t * Jv1  Jv2.t * Jg1  Jv2.t * Ja1  Jv2.t * Jp2  Jv2.t * Jv2     3
    509.     // 所以矩阵是24*24 的
    510.     H.block<24, 24>(0, 0) += ei->GetHessian();
    511.     // 经过这步H变成了
    512.     // 列数 6            3             3           3            6           3        6
    513.     // ---------------------------------------------------------------------------------- 行数
    514.     // Jp1.t * Jp1  Jp1.t * Jv1  Jp1.t * Jg1  Jp1.t * Ja1  Jp1.t * Jp2  Jp1.t * Jv2   0 |  6
    515.     // Jv1.t * Jp1  Jv1.t * Jv1  Jv1.t * Jg1  Jv1.t * Ja1  Jv1.t * Jp2  Jv1.t * Jv2   0 |  3
    516.     // Jg1.t * Jp1  Jg1.t * Jv1  Jg1.t * Jg1  Jg1.t * Ja1  Jg1.t * Jp2  Jg1.t * Jv2   0 |  3
    517.     // Ja1.t * Jp1  Ja1.t * Jv1  Ja1.t * Jg1  Ja1.t * Ja1  Ja1.t * Jp2  Ja1.t * Jv2   0 |  3
    518.     // Jp2.t * Jp1  Jp2.t * Jv1  Jp2.t * Jg1  Jp2.t * Ja1  Jp2.t * Jp2  Jp2.t * Jv2   0 |  6
    519.     // Jv2.t * Jp1  Jv2.t * Jv1  Jv2.t * Jg1  Jv2.t * Ja1  Jv2.t * Jp2  Jv2.t * Jv2   0 |  3
    520.     //     0            0            0            0            0           0          0 |  6
    521.     // ----------------------------------------------------------------------------------
    522.  
    523.     //第2步，加上EdgeGyroRW（陀螺仪随机游走约束）的信息矩阵：
    524.     //|   0~8   |       9~11     | 12~23 |     24~26     |27~29
    525.     // 9~11是上一帧的bg(3-dim)，24~26是当前帧的bg(3-dim)
    526.     //  egr的定义
    527.     //  EdgeGyroRW* egr = new EdgeGyroRW();
    528.     //  egr->setVertex(0, VGk);
    529.     //  egr->setVertex(1, VG);
    530.     Eigen::Matrix<double, 6, 6> Hgr = egr->GetHessian();
    531.     H.block<3, 3>(9, 9) += Hgr.block<3, 3>(0, 0);   // Jgr1.t * Jgr1
    532.     H.block<3, 3>(9, 24) += Hgr.block<3, 3>(0, 3);  // Jgr1.t * Jgr2
    533.     H.block<3, 3>(24, 9) += Hgr.block<3, 3>(3, 0);  // Jgr2.t * Jgr1
    534.     H.block<3, 3>(24, 24) += Hgr.block<3, 3>(3, 3); // Jgr2.t * Jgr2
    535.     // 经过这步H变成了
    536.     // 列数 6            3                    3                      3            6           3             3         3
    537.     // ----------------------------------------------------------------------------------------------------------------- 行数
    538.     // Jp1.t * Jp1  Jp1.t * Jv1         Jp1.t * Jg1           Jp1.t * Ja1  Jp1.t * Jp2  Jp1.t * Jv2        0         0 |  6
    539.     // Jv1.t * Jp1  Jv1.t * Jv1         Jv1.t * Jg1           Jv1.t * Ja1  Jv1.t * Jp2  Jv1.t * Jv2        0         0 |  3
    540.     // Jg1.t * Jp1  Jg1.t * Jv1  Jg1.t * Jg1 + Jgr1.t * Jgr1  Jg1.t * Ja1  Jg1.t * Jp2  Jg1.t * Jv2  Jgr1.t * Jgr2   0 |  3
    541.     // Ja1.t * Jp1  Ja1.t * Jv1         Ja1.t * Jg1           Ja1.t * Ja1  Ja1.t * Jp2  Ja1.t * Jv2        0         0 |  3
    542.     // Jp2.t * Jp1  Jp2.t * Jv1         Jp2.t * Jg1           Jp2.t * Ja1  Jp2.t * Jp2  Jp2.t * Jv2        0         0 |  6
    543.     // Jv2.t * Jp1  Jv2.t * Jv1         Jv2.t * Jg1           Jv2.t * Ja1  Jv2.t * Jp2  Jv2.t * Jv2        0         0 |  3
    544.     //     0            0             Jgr2.t * Jgr1                 0            0           0       Jgr2.t * Jgr2   0 |  3
    545.     //     0            0                    0                      0            0           0             0         0 |  3
    546.     // -----------------------------------------------------------------------------------------------------------------
    547.  
    548.     //第3步，加上EdgeAccRW（加速度随机游走约束）的信息矩阵：
    549.     //|   0~11   |      12~14    | 15~26 |     27~29     |30
    550.     // 12~14是上一帧的ba(3-dim)，27~29是当前帧的ba(3-dim)
    551.     // ear的定义
    552.     // EdgeAccRW* ear = new EdgeAccRW();
    553.     // ear->setVertex(0, VAk);
    554.     // ear->setVertex(1, VA);
    555.     Eigen::Matrix<double, 6, 6> Har = ear->GetHessian();
    556.     H.block<3, 3>(12, 12) += Har.block<3, 3>(0, 0); // Jar1.t * Jar1
    557.     H.block<3, 3>(12, 27) += Har.block<3, 3>(0, 3); // Jar1.t * Jar2
    558.     H.block<3, 3>(27, 12) += Har.block<3, 3>(3, 0); // Jar2.t * Jar1
    559.     H.block<3, 3>(27, 27) += Har.block<3, 3>(3, 3); // Jar2.t * Jar2
    560.     // 经过这步H变成了
    561.     // 列数 6            3                    3                            3                         6           3             3              3
    562.     // --------------------------------------------------------------------------------------------------------------------------------------------------- 行数
    563.     // |  Jp1.t * Jp1  Jp1.t * Jv1         Jp1.t * Jg1                 Jp1.t * Ja1            |  Jp1.t * Jp2  Jp1.t * Jv2        0              0        |  6
    564.     // |  Jv1.t * Jp1  Jv1.t * Jv1         Jv1.t * Jg1                 Jv1.t * Ja1            |  Jv1.t * Jp2  Jv1.t * Jv2        0              0        |  3
    565.     // |  Jg1.t * Jp1  Jg1.t * Jv1  Jg1.t * Jg1 + Jgr1.t * Jgr1        Jg1.t * Ja1            |  Jg1.t * Jp2  Jg1.t * Jv2  Jgr1.t * Jgr2        0        |  3
    566.     // |  Ja1.t * Jp1  Ja1.t * Jv1         Ja1.t * Jg1           Ja1.t * Ja1 + Jar1.t * Jar1  |  Ja1.t * Jp2  Ja1.t * Jv2        0       Jar1.t * Jar2   |  3
    567.     // |--------------------------------------------------------------------------------------------------------------------------------------------------
    568.     // |  Jp2.t * Jp1  Jp2.t * Jv1         Jp2.t * Jg1                 Jp2.t * Ja1            |  Jp2.t * Jp2  Jp2.t * Jv2        0              0        |  6
    569.     // |  Jv2.t * Jp1  Jv2.t * Jv1         Jv2.t * Jg1                 Jv2.t * Ja1            |  Jv2.t * Jp2  Jv2.t * Jv2        0              0        |  3
    570.     // |      0            0              Jgr2.t * Jgr1                      0                |        0           0       Jgr2.t * Jgr2        0        |  3
    571.     // |      0            0                    0                     Jar2.t * Jar1           |        0           0             0        Jar2.t * Jar2  |  3
    572.     // ---------------------------------------------------------------------------------------------------------------------------------------------------
    573.  
    574.     //第4步，加上EdgePriorPoseImu（先验约束）的信息矩阵：
    575.     //|   0~14   |  15~29
    576.     // 0~14是上一帧的P(6-dim)、V(3-dim)、BG(3-dim)、BA(3-dim)
    577.     // ep定义
    578.     // EdgePriorPoseImu* ep = new EdgePriorPoseImu(pFp->mpcpi);
    579.     // ep->setVertex(0, VPk);
    580.     // ep->setVertex(1, VVk);
    581.     // ep->setVertex(2, VGk);
    582.     // ep->setVertex(3, VAk);
    583.     //      6            3             3           3
    584.     // Jp1.t * Jp1  Jp1.t * Jv1  Jp1.t * Jg1  Jp1.t * Ja1     6
    585.     // Jv1.t * Jp1  Jv1.t * Jv1  Jv1.t * Jg1  Jv1.t * Ja1     3
    586.     // Jg1.t * Jp1  Jg1.t * Jv1  Jg1.t * Jg1  Jg1.t * Ja1     3
    587.     // Ja1.t * Jp1  Ja1.t * Jv1  Ja1.t * Jg1  Ja1.t * Ja1     3
    588.     H.block<15, 15>(0, 0) += ep->GetHessian(); // 上一帧 的H矩阵，矩阵太大了不写了。。。总之就是加到下标为1相关的了
    589.  
    590.     int tot_in = 0, tot_out = 0;
    591.     // 第5步 关于位姿的海森
    592.     for (size_t i = 0, iend = vpEdgesMono.size(); i < iend; i++)
    593.     {
    594.         EdgeMonoOnlyPose *e = vpEdgesMono[i];
    595.  
    596.         const size_t idx = vnIndexEdgeMono[i];
    597.  
    598.         if (!pFrame->mvbOutlier[idx])
    599.         {
    600.             //  0~14  |     15~20   | 21~29
    601.             // 15~20是当前帧的P(6-dim)
    602.             H.block<6, 6>(15, 15) += e->GetHessian(); // 当前帧的H矩阵，矩阵太大了不写了。。。总之就是加到p2相关的了
    603.             tot_in++;
    604.         }
    605.         else
    606.             tot_out++;
    607.     }
    608.  
    609.     for (size_t i = 0, iend = vpEdgesStereo.size(); i < iend; i++)
    610.     {
    611.         EdgeStereoOnlyPose *e = vpEdgesStereo[i];
    612.  
    613.         const size_t idx = vnIndexEdgeStereo[i];
    614.  
    615.         if (!pFrame->mvbOutlier[idx])
    616.         {
    617.             //  0~14  |     15~20   | 21~29
    618.             H.block<6, 6>(15, 15) += e->GetHessian();
    619.             tot_in++;
    620.         }
    621.         else
    622.             tot_out++;
    623.     }
    624.  
    625.     // H  = |B  E.t| ------> |0             0|
    626.     //      |E    A|         |0 A-E*B.inv*E.t|
    627.     H = Marginalize(H, 0, 14);
    628.     /*
    629.     Marginalize里的函数在此处的调用等效于：
    630.     Eigen::JacobiSVD svd(H.block(0, 0, 15, 15), Eigen::ComputeThinU | Eigen::ComputeThinV);
    631.     Eigen::JacobiSVD::SingularValuesType singularValues_inv = svd.singularValues();
    632.     for (int i = 0; i < 15; ++i)
    633.     {
    634.         if (singularValues_inv(i) > 1e-6)
    635.             singularValues_inv(i) = 1.0 / singularValues_inv(i);
    636.         else
    637.             singularValues_inv(i) = 0;
    638.     }
    639.     Eigen::MatrixXd invHb = svd.matrixV() * singularValues_inv.asDiagonal() * svd.matrixU().transpose();
    640.     H.block(15, 15, 15, 15) = H.block(15, 15, 15, 15) - H.block(15, 0, 15, 15) * invHb - H.block(0, 15, 15, 15);
    641.     */
    642.  
    643.     //构造一个ConstraintPoseImu对象，为下一帧提供先验约束
    644.     //构造对象所使用的参数是当前帧P、V、BG、BA的估计值和边缘化后的H矩阵
    645.     pFrame->mpcpi = new ConstraintPoseImu(VP->estimate().Rwb, VP->estimate().twb, VV->estimate(), VG->estimate(), VA->estimate(), H.block<15, 15>(15, 15));
    646.     //下一帧使用的EdgePriorPoseImu参数来自于此
    647.     delete pFp->mpcpi;
    648.     pFp->mpcpi = NULL;
    649.  
    650.     //返回值：内点数 = 总地图点数目 - 坏点（外点）数目
    651.     return nInitialCorrespondences - nBad;
    652. }

    参考文献

    主要内容来自

    【SLAM学习笔记】6-ORB_SLAM3关键源码分析④ Optimizer（一）单帧优化_口哨糖youri的博客-CSDN博客

    重写了一些说明，增加了注释。

  • 相关阅读:
    一文读懂VR数字展览会，从沉浸式体验到市场竞争力的全方位提升
    设计接口应该考虑的因素以及遵循的原则
    常用数学点回顾
    科普一下：抖音视频不能保存本地是怎么回事？
    【操作系统】8/35进程间通信
    2023年中国肠胃炎用药行业现状分析：随着老龄化进程明显加速，市场规模同比增长7%[图]
    typescript55-泛型约束
    智能问答技术在百度搜索中的应用
    亚马逊测评掉评、留不上评是怎么回事呢？要如何应对？
    《暴走IT》第5话：“裸泳”的办公软件
  • 原文地址：https://blog.csdn.net/xhtchina/article/details/126945261