【SLAM学习笔记】6-ORB_SLAM3关键源码分析④ Optimizer（一）单帧优化
2024-05-13 13:12:57
2021SC@SDUSC
目录
- 1.前言
- 2.代码分析
1.前言
Optimizer是非常重要的代码文件！!
这一部分代码量巨大，查阅了很多资料结合来看的代码，将分为以下部分进行分析
1. 单帧优化
2. 局部地图优化
3. 全局优化
4. 尺度与重力优化
5. sim3优化
6. 地图回环优化
7. 地图融合优化
下面给出逐步注释分析
2.代码分析
位姿优化，纯视觉时使用。优化目标：单帧的位姿
int Optimizer::PoseOptimization(Frame *pFrame)
{// 该优化函数主要用于Tracking线程中：运动跟踪、参考帧跟踪、地图跟踪、重定位// Step 1：构造g2o优化器, BlockSolver_6_3表示：位姿 _PoseDim 为6维，路标点 _LandmarkDim 是3维g2o::SparseOptimizer optimizer;g2o::BlockSolver_6_3::LinearSolverType *linearSolver;linearSolver = new g2o::LinearSolverDense<g2o::BlockSolver_6_3::PoseMatrixType>();g2o::BlockSolver_6_3 *solver_ptr = new g2o::BlockSolver_6_3(linearSolver);g2o::OptimizationAlgorithmLevenberg *solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr);optimizer.setAlgorithm(solver);// 输入的帧中,有效的,参与优化过程的2D-3D点对int nInitialCorrespondences = 0;// Set Frame vertex// Step 2：添加顶点：待优化当前帧的Tcwg2o::VertexSE3Expmap *vSE3 = new g2o::VertexSE3Expmap();vSE3->setEstimate(Converter::toSE3Quat(pFrame->mTcw));// 设置id，保证本次优化过程中id独立即可vSE3->setId(0);// 要优化的变量，所以不能固定vSE3->setFixed(false);// 添加节点optimizer.addVertex(vSE3);// Set MapPoint vertices// 当前帧的特征点数量const int N = pFrame->N;vector<ORB_SLAM3::EdgeSE3ProjectXYZOnlyPose *> vpEdgesMono; // 存放单目边vector<ORB_SLAM3::EdgeSE3ProjectXYZOnlyPoseToBody *> vpEdgesMono_FHR; // 存放另一目的边vector<size_t> vnIndexEdgeMono, vnIndexEdgeRight; // 边对应特征点的idvpEdgesMono.reserve(N);vpEdgesMono_FHR.reserve(N);vnIndexEdgeMono.reserve(N);vnIndexEdgeRight.reserve(N);vector<g2o::EdgeStereoSE3ProjectXYZOnlyPose *> vpEdgesStereo; // 存放双目边vector<size_t> vnIndexEdgeStereo;vpEdgesStereo.reserve(N);vnIndexEdgeStereo.reserve(N);// 自由度为2的卡方分布，显著性水平为0.05，对应的临界阈值5.991// 可以理解为卡方值高于5.991 95%的几率维外点const float deltaMono = sqrt(5.991);// 自由度为3的卡方分布，显著性水平为0.05，对应的临界阈值7.815const float deltaStereo = sqrt(7.815);// Step 3：添加一元边{// 锁定地图点。由于需要使用地图点来构造顶点和边,因此不希望在构造的过程中部分地图点被改写造成不一致甚至是段错误unique_lock<mutex> lock(MapPoint::mGlobalMutex);// 遍历当前帧中的所有地图点for (int i = 0; i < N; i++){MapPoint *pMP = pFrame->mvpMapPoints[i];// 如果这个地图点还存在没有被剔除掉if (pMP){// Conventional SLAM// 不存在相机2，则有可能为单目与双目if (!pFrame->mpCamera2){// Monocular observation// 单目情况，因为双目模式下pFrame->mvuRight[i]会大于0if (pFrame->mvuRight[i] < 0){nInitialCorrespondences++;pFrame->mvbOutlier[i] = false;// 对这个地图点的观测，也就是去畸变像素坐标Eigen::Matrix<double, 2, 1> obs;const cv::KeyPoint &kpUn = pFrame->mvKeysUn[i];obs << kpUn.pt.x, kpUn.pt.y;// 新建节点,注意这个节点的只是优化位姿PoseORB_SLAM3::EdgeSE3ProjectXYZOnlyPose *e = new ORB_SLAM3::EdgeSE3ProjectXYZOnlyPose();e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex *>(optimizer.vertex(0)));e->setMeasurement(obs);// 这个点的可信程度和特征点所在的图层有关，层数约高invSigma2越小，信息矩阵越小，表示误差越大，优化时考虑的比较少const float invSigma2 = pFrame->mvInvLevelSigma2[kpUn.octave];e->setInformation(Eigen::Matrix2d::Identity() * invSigma2);// 在这里使用了鲁棒核函数g2o::RobustKernelHuber *rk = new g2o::RobustKernelHuber;e->setRobustKernel(rk);rk->setDelta(deltaMono);// 设置相机e->pCamera = pFrame->mpCamera;// 地图点的空间位置,作为迭代的初始值，因为是一元边，所以不以节点的形式出现cv::Mat Xw = pMP->GetWorldPos();e->Xw[0] = Xw.at<float>(0);e->Xw[1] = Xw.at<float>(1);e->Xw[2] = Xw.at<float>(2);optimizer.addEdge(e);vpEdgesMono.push_back(e);vnIndexEdgeMono.push_back(i);}else // 双目{nInitialCorrespondences++;pFrame->mvbOutlier[i] = false;// SET EDGE// 观测多了一项右目的坐标Eigen::Matrix<double, 3, 1> obs;const cv::KeyPoint &kpUn = pFrame->mvKeysUn[i];const float &kp_ur = pFrame->mvuRight[i];obs << kpUn.pt.x, kpUn.pt.y, kp_ur;// 新建节点,注意这里也是只优化位姿g2o::EdgeStereoSE3ProjectXYZOnlyPose *e = new g2o::EdgeStereoSE3ProjectXYZOnlyPose();e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex *>(optimizer.vertex(0)));e->setMeasurement(obs);// 置信程度主要是看左目特征点所在的图层const float invSigma2 = pFrame->mvInvLevelSigma2[kpUn.octave];Eigen::Matrix3d Info = Eigen::Matrix3d::Identity() * invSigma2;e->setInformation(Info);g2o::RobustKernelHuber *rk = new g2o::RobustKernelHuber;e->setRobustKernel(rk);rk->setDelta(deltaStereo);// 双目下没有加入相机e->fx = pFrame->fx;e->fy = pFrame->fy;e->cx = pFrame->cx;e->cy = pFrame->cy;e->bf = pFrame->mbf;cv::Mat Xw = pMP->GetWorldPos();e->Xw[0] = Xw.at<float>(0);e->Xw[1] = Xw.at<float>(1);e->Xw[2] = Xw.at<float>(2);optimizer.addEdge(e);vpEdgesStereo.push_back(e);vnIndexEdgeStereo.push_back(i);}}// SLAM with respect a rigid bodyelse // 两个相机{nInitialCorrespondences++;cv::KeyPoint kpUn;// 总数N = 相机1所有特征点数量+相机2所有特征点数量if (i < pFrame->Nleft){ // Left camera observationkpUn = pFrame->mvKeys[i];pFrame->mvbOutlier[i] = false;Eigen::Matrix<double, 2, 1> obs;obs << kpUn.pt.x, kpUn.pt.y;ORB_SLAM3::EdgeSE3ProjectXYZOnlyPose *e = new ORB_SLAM3::EdgeSE3ProjectXYZOnlyPose();e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex *>(optimizer.vertex(0)));e->setMeasurement(obs);const float invSigma2 = pFrame->mvInvLevelSigma2[kpUn.octave];e->setInformation(Eigen::Matrix2d::Identity() * invSigma2);g2o::RobustKernelHuber *rk = new g2o::RobustKernelHuber;e->setRobustKernel(rk);rk->setDelta(deltaMono);e->pCamera = pFrame->mpCamera;cv::Mat Xw = pMP->GetWorldPos();e->Xw[0] = Xw.at<float>(0);e->Xw[1] = Xw.at<float>(1);e->Xw[2] = Xw.at<float>(2);optimizer.addEdge(e);vpEdgesMono.push_back(e);vnIndexEdgeMono.push_back(i);}else{ // Right camera observationkpUn = pFrame->mvKeysRight[i - pFrame->Nleft];Eigen::Matrix<double, 2, 1> obs;obs << kpUn.pt.x, kpUn.pt.y;pFrame->mvbOutlier[i] = false;ORB_SLAM3::EdgeSE3ProjectXYZOnlyPoseToBody *e = new ORB_SLAM3::EdgeSE3ProjectXYZOnlyPoseToBody();e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex *>(optimizer.vertex(0)));e->setMeasurement(obs);const float invSigma2 = pFrame->mvInvLevelSigma2[kpUn.octave];e->setInformation(Eigen::Matrix2d::Identity() * invSigma2);g2o::RobustKernelHuber *rk = new g2o::RobustKernelHuber;e->setRobustKernel(rk);rk->setDelta(deltaMono);e->pCamera = pFrame->mpCamera2;cv::Mat Xw = pMP->GetWorldPos();e->Xw[0] = Xw.at<float>(0);e->Xw[1] = Xw.at<float>(1);e->Xw[2] = Xw.at<float>(2);e->mTrl = Converter::toSE3Quat(pFrame->mTrl);optimizer.addEdge(e);vpEdgesMono_FHR.push_back(e);vnIndexEdgeRight.push_back(i);}}}}}// 如果没有足够的匹配点,那么就只好放弃了// cout << "PO: vnIndexEdgeMono.size() = " << vnIndexEdgeMono.size() << " vnIndexEdgeRight.size() = " << vnIndexEdgeRight.size() << endl;if (nInitialCorrespondences < 3)return 0;// We perform 4 optimizations, after each optimization we classify observation as inlier/outlier// At the next optimization, outliers are not included, but at the end they can be classified as inliers again.// Step 4：开始优化，总共优化四次，每次优化迭代10次,每次优化后，将观测分为outlier和inlier，outlier不参与下次优化// 由于每次优化后是对所有的观测进行outlier和inlier判别，因此之前被判别为outlier有可能变成inlier，反之亦然// 基于卡方检验计算出的阈值（假设测量有一个像素的偏差）const float chi2Mono[4] = {5.991, 5.991, 5.991, 5.991}; // 单目const float chi2Stereo[4] = {7.815, 7.815, 7.815, 7.815}; // 双目const int its[4] = {10, 10, 10, 10}; // 四次迭代，每次迭代的次数// bad 的地图点个数int nBad = 0;// 一共进行四次优化，每次会剔除外点for (size_t it = 0; it < 4; it++){vSE3->setEstimate(Converter::toSE3Quat(pFrame->mTcw));// 其实就是初始化优化器,这里的参数0就算是不填写,默认也是0,也就是只对level为0的边进行优化optimizer.initializeOptimization(0);// 开始优化，优化10次optimizer.optimize(its[it]);nBad = 0;// 优化结束,开始遍历参与优化的每一条误差边(单目或双相机的相机1)for (size_t i = 0, iend = vpEdgesMono.size(); i < iend; i++){ORB_SLAM3::EdgeSE3ProjectXYZOnlyPose *e = vpEdgesMono[i];const size_t idx = vnIndexEdgeMono[i];// 如果这条误差边是来自于outlier，也就是上一次优化去掉的// 重新计算，因为上一次外点这一次可能成为内点if (pFrame->mvbOutlier[idx]){e->computeError();}// 就是error*\Omega*error,表征了这个点的误差大小(考虑置信度以后)const float chi2 = e->chi2();if (chi2 > chi2Mono[it]){pFrame->mvbOutlier[idx] = true;e->setLevel(1); // 设置为outlier , level 1 对应为外点,上面的过程中我们设置其为不优化nBad++;}else{pFrame->mvbOutlier[idx] = false;e->setLevel(0); // 设置为inlier, level 0 对应为内点,上面的过程中我们就是要优化这些关系}if (it == 2)e->setRobustKernel(0); // 除了前两次优化需要RobustKernel以外, 其余的优化都不需要 -- 因为重投影的误差已经有明显的下降了}// 对于相机2for (size_t i = 0, iend = vpEdgesMono_FHR.size(); i < iend; i++){ORB_SLAM3::EdgeSE3ProjectXYZOnlyPoseToBody *e = vpEdgesMono_FHR[i];const size_t idx = vnIndexEdgeRight[i];if (pFrame->mvbOutlier[idx]){e->computeError();}const float chi2 = e->chi2();if (chi2 > chi2Mono[it]){pFrame->mvbOutlier[idx] = true;e->setLevel(1);nBad++;}else{pFrame->mvbOutlier[idx] = false;e->setLevel(0);}if (it == 2)e->setRobustKernel(0);}// 对于双目for (size_t i = 0, iend = vpEdgesStereo.size(); i < iend; i++){g2o::EdgeStereoSE3ProjectXYZOnlyPose *e = vpEdgesStereo[i];const size_t idx = vnIndexEdgeStereo[i];if (pFrame->mvbOutlier[idx]){e->computeError();}const float chi2 = e->chi2();if (chi2 > chi2Stereo[it]){pFrame->mvbOutlier[idx] = true;e->setLevel(1);nBad++;}else{e->setLevel(0);pFrame->mvbOutlier[idx] = false;}if (it == 2)e->setRobustKernel(0);}if (optimizer.edges().size() < 10)break;}// Recover optimized pose and return number of inliers// Step 5 得到优化后的当前帧的位姿g2o::VertexSE3Expmap *vSE3_recov = static_cast<g2o::VertexSE3Expmap *>(optimizer.vertex(0));g2o::SE3Quat SE3quat_recov = vSE3_recov->estimate();cv::Mat pose = Converter::toCvMat(SE3quat_recov);pFrame->SetPose(pose);// 并且返回内点数目return nInitialCorrespondences - nBad;
}
使用上一关键帧+当前帧的视觉信息和IMU信息，优化当前帧位姿
可分为以下几个步骤：
- Step 1：创建g2o优化器，初始化顶点和边
- Step 2：启动多轮优化，剔除外点
- Step 3：更新当前帧位姿、速度、IMU偏置
- Step 4：记录当前帧的优化状态，包括参数信息和对应的海森矩阵
int Optimizer::PoseInertialOptimizationLastKeyFrame(Frame *pFrame, bool bRecInit)
{// 1. 创建g2o优化器，初始化顶点和边//构建一个稀疏求解器g2o::SparseOptimizer optimizer;g2o::BlockSolverX::LinearSolverType *linearSolver;// 使用dense的求解器，（常见非dense求解器有cholmod线性求解器和shur补线性求解器）linearSolver = new g2o::LinearSolverDense<g2o::BlockSolverX::PoseMatrixType>();g2o::BlockSolverX *solver_ptr = new g2o::BlockSolverX(linearSolver);//使用高斯牛顿求解器g2o::OptimizationAlgorithmGaussNewton *solver = new g2o::OptimizationAlgorithmGaussNewton(solver_ptr);optimizer.setVerbose(false);optimizer.setAlgorithm(solver);//当前帧单（左）目地图点数目int nInitialMonoCorrespondences = 0;int nInitialStereoCorrespondences = 0;//上面两项的和，即参与优化的地图点总数目int nInitialCorrespondences = 0;// Set Frame vertex// 2. 确定节点// 当前帧的位姿，旋转+平移，6-dimVertexPose *VP = new VertexPose(pFrame);VP->setId(0);VP->setFixed(false);optimizer.addVertex(VP);//当前帧的速度，3-dimVertexVelocity *VV = new VertexVelocity(pFrame);VV->setId(1);VV->setFixed(false);optimizer.addVertex(VV);//当前帧的陀螺仪偏置，3-dimVertexGyroBias *VG = new VertexGyroBias(pFrame);VG->setId(2);VG->setFixed(false);optimizer.addVertex(VG);//当前帧的加速度偏置，3-dimVertexAccBias *VA = new VertexAccBias(pFrame);VA->setId(3);VA->setFixed(false);optimizer.addVertex(VA);// setFixed(false)这个设置使以上四个顶点（15个参数）在优化时更新// Set MapPoint vertices// 当前帧的特征点总数const int N = pFrame->N;// 当前帧左目的特征点总数const int Nleft = pFrame->Nleft;// 当前帧是否存在右目（即是否为双目）const bool bRight = (Nleft != -1);vector<EdgeMonoOnlyPose *> vpEdgesMono;vector<EdgeStereoOnlyPose *> vpEdgesStereo;vector<size_t> vnIndexEdgeMono;vector<size_t> vnIndexEdgeStereo;vpEdgesMono.reserve(N);vpEdgesStereo.reserve(N);vnIndexEdgeMono.reserve(N);vnIndexEdgeStereo.reserve(N);// 自由度为2的卡方分布，显著性水平为0.05，对应的临界阈值5.991const float thHuberMono = sqrt(5.991);// 自由度为3的卡方分布，显著性水平为0.05，对应的临界阈值7.815const float thHuberStereo = sqrt(7.815);{// 锁定地图点。由于需要使用地图点来构造顶点和边,因此不希望在构造的过程中部分地图点被改写造成不一致甚至是段错误// 3. 投影边unique_lock<mutex> lock(MapPoint::mGlobalMutex);for (int i = 0; i < N; i++){MapPoint *pMP = pFrame->mvpMapPoints[i];if (pMP){cv::KeyPoint kpUn;// Left monocular observation// 这里说的Left monocular包含两种情况：1.单目情况 2.双目情况下的左目if ((!bRight && pFrame->mvuRight[i] < 0) || i < Nleft){//如果是双目情况下的左目if (i < Nleft) // pair left-right//使用未畸变校正的特征点kpUn = pFrame->mvKeys[i];//如果是单目else//使用畸变校正过的特征点kpUn = pFrame->mvKeysUn[i];//单目地图点计数增加nInitialMonoCorrespondences++;//当前地图点默认设置为不是外点pFrame->mvbOutlier[i] = false;Eigen::Matrix<double, 2, 1> obs;obs << kpUn.pt.x, kpUn.pt.y;//第一种边(视觉重投影约束)：地图点投影到该帧图像的坐标与特征点坐标偏差尽可能小EdgeMonoOnlyPose *e = new EdgeMonoOnlyPose(pMP->GetWorldPos(), 0);//将位姿作为第一个顶点e->setVertex(0, VP);//设置观测值，即去畸变后的像素坐标e->setMeasurement(obs);// Add here uncerteinty// 获取不确定度，这里调用uncertainty2返回固定值1.0// ?这里的1.0是作为缺省值的意思吗？是否可以根据对视觉信息的信任度动态修改这个值，比如标定的误差？const float unc2 = pFrame->mpCamera->uncertainty2(obs);// invSigma2 = (Inverse(协方差矩阵))^2，表明该约束在各个维度上的可信度// 图像金字塔层数越高，可信度越差const float invSigma2 = pFrame->mvInvLevelSigma2[kpUn.octave] / unc2;//设置该约束的信息矩阵e->setInformation(Eigen::Matrix2d::Identity() * invSigma2);g2o::RobustKernelHuber *rk = new g2o::RobustKernelHuber;// 设置鲁棒核函数，避免其误差的平方项出现数值过大的增长 注：后续在优化2次后会用e->setRobustKernel(0)禁掉鲁棒核函数e->setRobustKernel(rk);//重投影误差的自由度为2，设置对应的卡方阈值rk->setDelta(thHuberMono);//将第一种边加入优化器optimizer.addEdge(e);//将第一种边加入vpEdgesMonovpEdgesMono.push_back(e);//将对应的特征点索引加入vnIndexEdgeMonovnIndexEdgeMono.push_back(i);}// Stereo observationelse if (!bRight){nInitialStereoCorrespondences++;pFrame->mvbOutlier[i] = false;kpUn = pFrame->mvKeysUn[i];const float kp_ur = pFrame->mvuRight[i];Eigen::Matrix<double, 3, 1> obs;obs << kpUn.pt.x, kpUn.pt.y, kp_ur;EdgeStereoOnlyPose *e = new EdgeStereoOnlyPose(pMP->GetWorldPos());e->setVertex(0, VP);e->setMeasurement(obs);// Add here uncerteintyconst float unc2 = pFrame->mpCamera->uncertainty2(obs.head(2));const float &invSigma2 = pFrame->mvInvLevelSigma2[kpUn.octave] / unc2;e->setInformation(Eigen::Matrix3d::Identity() * invSigma2);g2o::RobustKernelHuber *rk = new g2o::RobustKernelHuber;e->setRobustKernel(rk);rk->setDelta(thHuberStereo);optimizer.addEdge(e);vpEdgesStereo.push_back(e);vnIndexEdgeStereo.push_back(i);}// Right monocular observationif (bRight && i >= Nleft){nInitialMonoCorrespondences++;pFrame->mvbOutlier[i] = false;kpUn = pFrame->mvKeysRight[i - Nleft];Eigen::Matrix<double, 2, 1> obs;obs << kpUn.pt.x, kpUn.pt.y;EdgeMonoOnlyPose *e = new EdgeMonoOnlyPose(pMP->GetWorldPos(), 1);e->setVertex(0, VP);e->setMeasurement(obs);// Add here uncerteintyconst float unc2 = pFrame->mpCamera->uncertainty2(obs);const float invSigma2 = pFrame->mvInvLevelSigma2[kpUn.octave] / unc2;e->setInformation(Eigen::Matrix2d::Identity() * invSigma2);g2o::RobustKernelHuber *rk = new g2o::RobustKernelHuber;e->setRobustKernel(rk);rk->setDelta(thHuberMono);optimizer.addEdge(e);vpEdgesMono.push_back(e);vnIndexEdgeMono.push_back(i);}}}}//统计参与优化的地图点总数目nInitialCorrespondences = nInitialMonoCorrespondences + nInitialStereoCorrespondences;// 4. 上一个关键帧节点// pKF为上一关键帧KeyFrame *pKF = pFrame->mpLastKeyFrame;//上一关键帧的位姿，旋转+平移，6-dimVertexPose *VPk = new VertexPose(pKF);VPk->setId(4);VPk->setFixed(true);optimizer.addVertex(VPk);//上一关键帧的速度，3-dimVertexVelocity *VVk = new VertexVelocity(pKF);VVk->setId(5);VVk->setFixed(true);optimizer.addVertex(VVk);//上一关键帧的陀螺仪偏置，3-dimVertexGyroBias *VGk = new VertexGyroBias(pKF);VGk->setId(6);VGk->setFixed(true);optimizer.addVertex(VGk);//上一关键帧的加速度偏置，3-dimVertexAccBias *VAk = new VertexAccBias(pKF);VAk->setId(7);VAk->setFixed(true);optimizer.addVertex(VAk);// setFixed(true)这个设置使以上四个顶点（15个参数）的值在优化时保持固定//既然被选为关键帧，就不能太善变// 5. 第二种边（IMU预积分约束）：两帧之间位姿的变化量与IMU预积分的值偏差尽可能小EdgeInertial *ei = new EdgeInertial(pFrame->mpImuPreintegrated);//将上一关键帧四个顶点（P、V、BG、BA）和当前帧两个顶点（P、V）加入第二种边ei->setVertex(0, VPk);ei->setVertex(1, VVk);ei->setVertex(2, VGk);ei->setVertex(3, VAk);ei->setVertex(4, VP);ei->setVertex(5, VV);//把第二种边加入优化器optimizer.addEdge(ei);// 6. 第三种边（陀螺仪随机游走约束）：陀螺仪的随机游走值在相近帧间不会相差太多 residual=VG-VGk// 用大白话来讲就是用固定的VGK拽住VG，防止VG在优化中放飞自我EdgeGyroRW *egr = new EdgeGyroRW();// 将上一关键帧的BG加入第三种边egr->setVertex(0, VGk);//将当前帧的BG加入第三种边egr->setVertex(1, VG);// C值在预积分阶段更新，range(9,12)对应陀螺仪偏置的协方差，最终cvInfoG值为inv(∑(GyroRW^2/freq))cv::Mat cvInfoG = pFrame->mpImuPreintegrated->C.rowRange(9, 12).colRange(9, 12).inv(cv::DECOMP_SVD);Eigen::Matrix3d InfoG;for (int r = 0; r < 3; r++)for (int c = 0; c < 3; c++)InfoG(r, c) = cvInfoG.at<float>(r, c);// 设置信息矩阵egr->setInformation(InfoG);// 把第三种边加入优化器optimizer.addEdge(egr);// 7. 第四种边（加速度随机游走约束）：加速度的随机游走值在相近帧间不会相差太多 residual=VA-VAk// 用大白话来讲就是用固定的VAK拽住VA，防止VA在优化中放飞自我EdgeAccRW *ear = new EdgeAccRW();// 将上一关键帧的BA加入第四种边ear->setVertex(0, VAk);// 将当前帧的BA加入第四种边ear->setVertex(1, VA);// C值在预积分阶段更新，range(12,15)对应加速度偏置的协方差，最终cvInfoG值为inv(∑(AccRW^2/freq))cv::Mat cvInfoA = pFrame->mpImuPreintegrated->C.rowRange(12, 15).colRange(12, 15).inv(cv::DECOMP_SVD);Eigen::Matrix3d InfoA;for (int r = 0; r < 3; r++)for (int c = 0; c < 3; c++)InfoA(r, c) = cvInfoA.at<float>(r, c);// 设置信息矩阵ear->setInformation(InfoA);// 把第四种边加入优化器optimizer.addEdge(ear);// 8. 启动多轮优化，剔除外点// We perform 4 optimizations, after each optimization we classify observation as inlier/outlier// At the next optimization, outliers are not included, but at the end they can be classified as inliers again.// 卡方检验值呈递减趋势，目的是让检验越来越苛刻float chi2Mono[4] = {12, 7.5, 5.991, 5.991};float chi2Stereo[4] = {15.6, 9.8, 7.815, 7.815};// 4次优化的迭代次数都为10int its[4] = {10, 10, 10, 10};// 坏点数int nBad = 0;// 单目坏点数int nBadMono = 0;// 双目坏点数int nBadStereo = 0;// 单目内点数int nInliersMono = 0;// 双目内点数int nInliersStereo = 0;// 内点数int nInliers = 0;bool bOut = false;// 进行4次优化for (size_t it = 0; it < 4; it++){// 初始化优化器,这里的参数0代表只对level为0的边进行优化（不传参数默认也是0）optimizer.initializeOptimization(0);// 每次优化迭代十次optimizer.optimize(its[it]);// 每次优化都重新统计各类点的数目nBad = 0;nBadMono = 0;nBadStereo = 0;nInliers = 0;nInliersMono = 0;nInliersStereo = 0;// 使用1.5倍的chi2Mono作为“近点”的卡方检验值，意味着地图点越近，检验越宽松// 地图点如何定义为“近点”在下面的代码中有解释float chi2close = 1.5 * chi2Mono[it];// For monocular observationsfor (size_t i = 0, iend = vpEdgesMono.size(); i < iend; i++){EdgeMonoOnlyPose *e = vpEdgesMono[i];const size_t idx = vnIndexEdgeMono[i];// 如果这条误差边是来自于outlierif (pFrame->mvbOutlier[idx]){// 计算这条边上次优化后的误差e->computeError();}// 就是error*\Omega*error，表示了这条边考虑置信度以后的误差大小const float chi2 = e->chi2();// 当地图点在当前帧的深度值小于10时，该地图点属于close（近点）// mTrackDepth是在Frame.cc的isInFrustum函数中计算出来的bool bClose = pFrame->mvpMapPoints[idx]->mTrackDepth < 10.f;// 判断某地图点为外点的条件有以下三种：// 1.该地图点不是近点并且误差大于卡方检验值chi2Mono[it]// 2.该地图点是近点并且误差大于卡方检验值chi2close// 3.深度不为正// 每次优化后，用更小的卡方检验值，原因是随着优化的进行，对划分为内点的信任程度越来越低if ((chi2 > chi2Mono[it] && !bClose) || (bClose && chi2 > chi2close) || !e->isDepthPositive()){// 将该点设置为外点pFrame->mvbOutlier[idx] = true;// 外点不参与下一轮优化e->setLevel(1);// 单目坏点数+1nBadMono++;}else{// 将该点设置为内点（暂时）pFrame->mvbOutlier[idx] = false;// 内点继续参与下一轮优化e->setLevel(0);// 单目内点数+1nInliersMono++;}// 从第三次优化开始就不设置鲁棒核函数了，原因是经过两轮优化已经趋向准确值，不会有太大误差if (it == 2)e->setRobustKernel(0);}// For stereo observationsfor (size_t i = 0, iend = vpEdgesStereo.size(); i < iend; i++){EdgeStereoOnlyPose *e = vpEdgesStereo[i];const size_t idx = vnIndexEdgeStereo[i];if (pFrame->mvbOutlier[idx]){e->computeError();}const float chi2 = e->chi2();if (chi2 > chi2Stereo[it]){pFrame->mvbOutlier[idx] = true;e->setLevel(1); // not included in next optimizationnBadStereo++;}else{pFrame->mvbOutlier[idx] = false;e->setLevel(0);nInliersStereo++;}if (it == 2)e->setRobustKernel(0);}// 内点总数=单目内点数+双目内点数nInliers = nInliersMono + nInliersStereo;// 坏点数=单目坏点数+双目坏点数nBad = nBadMono + nBadStereo;if (optimizer.edges().size() < 10){cout << "PIOLKF: NOT ENOUGH EDGES" << endl;break;}}// If not too much tracks, recover not too bad points// 9. 若4次优化后内点数小于30，尝试恢复一部分不那么糟糕的坏点if ((nInliers < 30) && !bRecInit){//重新从0开始统计坏点数nBad = 0;//单目可容忍的卡方检验最大值（如果误差比这还大就不要挣扎了...）const float chi2MonoOut = 18.f;const float chi2StereoOut = 24.f;EdgeMonoOnlyPose *e1;EdgeStereoOnlyPose *e2;//遍历所有单目特征点for (size_t i = 0, iend = vnIndexEdgeMono.size(); i < iend; i++){const size_t idx = vnIndexEdgeMono[i];//获取这些特征点对应的边e1 = vpEdgesMono[i];e1->computeError();//判断误差值是否超过单目可容忍的卡方检验最大值，是的话就把这个点保下来if (e1->chi2() < chi2MonoOut)pFrame->mvbOutlier[idx] = false;elsenBad++;}for (size_t i = 0, iend = vnIndexEdgeStereo.size(); i < iend; i++){const size_t idx = vnIndexEdgeStereo[i];e2 = vpEdgesStereo[i];e2->computeError();if (e2->chi2() < chi2StereoOut)pFrame->mvbOutlier[idx] = false;elsenBad++;}}// 10. 更新当前帧位姿、速度、IMU偏置// Recover optimized pose, velocity and biases// 给当前帧设置优化后的旋转、位移、速度，用来更新位姿pFrame->SetImuPoseVelocity(Converter::toCvMat(VP->estimate().Rwb), Converter::toCvMat(VP->estimate().twb), Converter::toCvMat(VV->estimate()));Vector6d b;b << VG->estimate(), VA->estimate();// 给当前帧设置优化后的bg，bapFrame->mImuBias = IMU::Bias(b[3], b[4], b[5], b[0], b[1], b[2]);// 11. 记录当前帧的优化状态，包括参数信息和对应的海森矩阵// Recover Hessian, marginalize keyFframe states and generate new prior for frameEigen::Matrix<double, 15, 15> H;H.setZero();// H(x)=J(x).t()*info*J(x)// J(x)取的是EdgeInertial中的_jacobianOplus[4]和_jacobianOplus[5]，即EdgeInertial::computeError计算出来的er,ev,ep对当前帧Pose和Velocity的偏导//因此ei->GetHessian2的结果为：// H(∂er/∂r) H(∂er/∂t) H(∂er/∂v)// H(∂ev/∂r) H(∂ev/∂t) H(∂ev/∂v)// H(∂ep/∂r) H(∂ep/∂t) H(∂ep/∂v)//每项H都是3x3，故GetHessian2的结果是9x9H.block<9, 9>(0, 0) += ei->GetHessian2();// J(x)取的是EdgeGyroRW中的_jacobianOplusXj，即EdgeGyroRW::computeError计算出来的_error(ebg)对当前帧bg的偏导//因此egr->GetHessian2的结果为：// H(∂ebg/∂bg) ，3x3H.block<3, 3>(9, 9) += egr->GetHessian2();// J(x)取的是EdgeAccRW中的_jacobianOplusXj，即EdgeAccRW::computeError计算出来的_error(ebg)对当前帧ba的偏导//因此ear->GetHessian2的结果为：// H(∂eba/∂ba) ，3x3H.block<3, 3>(12, 12) += ear->GetHessian2();//经过前面几步，Hessian Matrix长这个样子（注：省略了->GetHessian2()）// ei ei ei ei ei ei ei ei ei 0 0 0 0 0 0// ei ei ei ei ei ei ei ei ei 0 0 0 0 0 0// ei ei ei ei ei ei ei ei ei 0 0 0 0 0 0// ei ei ei ei ei ei ei ei ei 0 0 0 0 0 0// ei ei ei ei ei ei ei ei ei 0 0 0 0 0 0// ei ei ei ei ei ei ei ei ei 0 0 0 0 0 0// ei ei ei ei ei ei ei ei ei 0 0 0 0 0 0// ei ei ei ei ei ei ei ei ei 0 0 0 0 0 0// ei ei ei ei ei ei ei ei ei 0 0 0 0 0 0// 0 0 0 0 0 0 0 0 0 egr egr egr 0 0 0// 0 0 0 0 0 0 0 0 0 egr egr egr 0 0 0// 0 0 0 0 0 0 0 0 0 egr egr egr 0 0 0// 0 0 0 0 0 0 0 0 0 0 0 0 ear ear ear// 0 0 0 0 0 0 0 0 0 0 0 0 ear ear ear// 0 0 0 0 0 0 0 0 0 0 0 0 ear ear earint tot_in = 0, tot_out = 0;for (size_t i = 0, iend = vpEdgesMono.size(); i < iend; i++){EdgeMonoOnlyPose *e = vpEdgesMono[i];const size_t idx = vnIndexEdgeMono[i];if (!pFrame->mvbOutlier[idx]){H.block<6, 6>(0, 0) += e->GetHessian();tot_in++;}elsetot_out++;}for (size_t i = 0, iend = vpEdgesStereo.size(); i < iend; i++){EdgeStereoOnlyPose *e = vpEdgesStereo[i];const size_t idx = vnIndexEdgeStereo[i];if (!pFrame->mvbOutlier[idx]){H.block<6, 6>(0, 0) += e->GetHessian();tot_in++;}elsetot_out++;}//设eie = ei->GetHessian2()+∑(e->GetHessian())//则最终Hessian Matrix长这样// eie eie eie eie eie eie ei ei ei 0 0 0 0 0 0// eie eie eie eie eie eie ei ei ei 0 0 0 0 0 0// eie eie eie eie eie eie ei ei ei 0 0 0 0 0 0// eie eie eie eie eie eie ei ei ei 0 0 0 0 0 0// eie eie eie eie eie eie ei ei ei 0 0 0 0 0 0// eie eie eie eie eie eie ei ei ei 0 0 0 0 0 0// ei ei ei ei ei ei ei ei ei 0 0 0 0 0 0// ei ei ei ei ei ei ei ei ei 0 0 0 0 0 0// ei ei ei ei ei ei ei ei ei 0 0 0 0 0 0// 0 0 0 0 0 0 0 0 0 egr egr egr 0 0 0// 0 0 0 0 0 0 0 0 0 egr egr egr 0 0 0// 0 0 0 0 0 0 0 0 0 egr egr egr 0 0 0// 0 0 0 0 0 0 0 0 0 0 0 0 ear ear ear// 0 0 0 0 0 0 0 0 0 0 0 0 ear ear ear// 0 0 0 0 0 0 0 0 0 0 0 0 ear ear ear//构造一个ConstraintPoseImu对象，为下一帧提供先验约束//构造对象所使用的参数是当前帧P、V、BG、BA的估计值和H矩阵pFrame->mpcpi = new ConstraintPoseImu(VP->estimate().Rwb, VP->estimate().twb, VV->estimate(), VG->estimate(), VA->estimate(), H);//在PoseInertialOptimizationLastFrame函数中，会将ConstraintPoseImu信息作为“上一帧先验约束”生成一条优化边//返回值：内点数 = 总地图点数目 - 坏点（外点）数目return nInitialCorrespondences - nBad;
}
使用上一帧+当前帧的视觉信息和IMU信息，优化当前帧位姿
可分为以下几个步骤：
- Step 1：创建g2o优化器，初始化顶点和边
- Step 2：启动多轮优化，剔除外点
- Step 3：更新当前帧位姿、速度、IMU偏置
- Step 4：记录当前帧的优化状态，包括参数信息和边缘化后的海森矩阵
int Optimizer::PoseInertialOptimizationLastFrame(Frame *pFrame, bool bRecInit)
{// Step 1：创建g2o优化器，初始化顶点和边//构建一个稀疏求解器g2o::SparseOptimizer optimizer;g2o::BlockSolverX::LinearSolverType *linearSolver;// 使用dense的求解器，（常见非dense求解器有cholmod线性求解器和shur补线性求解器）linearSolver = new g2o::LinearSolverDense<g2o::BlockSolverX::PoseMatrixType>();g2o::BlockSolverX *solver_ptr = new g2o::BlockSolverX(linearSolver);//使用高斯牛顿求解器g2o::OptimizationAlgorithmGaussNewton *solver = new g2o::OptimizationAlgorithmGaussNewton(solver_ptr);optimizer.setAlgorithm(solver);optimizer.setVerbose(false);//当前帧单（左）目地图点数目int nInitialMonoCorrespondences = 0;int nInitialStereoCorrespondences = 0;int nInitialCorrespondences = 0;// Set Current Frame vertex//当前帧的位姿，旋转+平移，6-dimVertexPose *VP = new VertexPose(pFrame);VP->setId(0);VP->setFixed(false); //需要优化，所以不固定optimizer.addVertex(VP);//当前帧的速度，3-dimVertexVelocity *VV = new VertexVelocity(pFrame);VV->setId(1);VV->setFixed(false);optimizer.addVertex(VV);//当前帧的陀螺仪偏置，3-dimVertexGyroBias *VG = new VertexGyroBias(pFrame);VG->setId(2);VG->setFixed(false);optimizer.addVertex(VG);//当前帧的加速度偏置，3-dimVertexAccBias *VA = new VertexAccBias(pFrame);VA->setId(3);VA->setFixed(false);optimizer.addVertex(VA);// Set MapPoint vertices//当前帧的特征点总数 N = Nleft + Nright//对于单目 N!=0, Nleft=-1，Nright=-1//对于双目 N!=0, Nleft!=-1，Nright!=-1const int N = pFrame->N;//当前帧左目的特征点总数const int Nleft = pFrame->Nleft;//当前帧是否存在右目（即是否为双目），存在为true//?感觉可以更直接点啊 bRight = (Nright!=-1)const bool bRight = (Nleft != -1);vector<EdgeMonoOnlyPose *> vpEdgesMono;vector<EdgeStereoOnlyPose *> vpEdgesStereo;vector<size_t> vnIndexEdgeMono;vector<size_t> vnIndexEdgeStereo;vpEdgesMono.reserve(N);vpEdgesStereo.reserve(N);vnIndexEdgeMono.reserve(N);vnIndexEdgeStereo.reserve(N);// 自由度为2的卡方分布，显著性水平为0.05，对应的临界阈值5.991const float thHuberMono = sqrt(5.991);// 自由度为3的卡方分布，显著性水平为0.05，对应的临界阈值7.815const float thHuberStereo = sqrt(7.815);{// 锁定地图点。由于需要使用地图点来构造顶点和边,因此不希望在构造的过程中部分地图点被改写造成不一致甚至是段错误unique_lock<mutex> lock(MapPoint::mGlobalMutex);for (int i = 0; i < N; i++){MapPoint *pMP = pFrame->mvpMapPoints[i];if (pMP){cv::KeyPoint kpUn;// Left monocular observation// 这里说的Left monocular包含两种情况：1.单目情况 2.双目情况下的左目if ((!bRight && pFrame->mvuRight[i] < 0) || i < Nleft){//如果是双目情况下的左目if (i < Nleft) // pair left-right//使用未畸变校正的特征点kpUn = pFrame->mvKeys[i];//如果是单目else//使用畸变校正过的特征点kpUn = pFrame->mvKeysUn[i];//单目地图点计数增加nInitialMonoCorrespondences++;//当前地图点默认设置为不是外点pFrame->mvbOutlier[i] = false;// 观测的特征点Eigen::Matrix<double, 2, 1> obs;obs << kpUn.pt.x, kpUn.pt.y;//第一种边(视觉重投影约束)：地图点投影到该帧图像的坐标与特征点坐标偏差尽可能小EdgeMonoOnlyPose *e = new EdgeMonoOnlyPose(pMP->GetWorldPos(), 0);//将位姿作为第一个顶点e->setVertex(0, VP);//设置观测值，即去畸变后的像素坐标e->setMeasurement(obs);// Add here uncerteinty// 获取不确定度，这里调用uncertainty2返回固定值1.0// ?这里返回1.0是作为缺省值，是否可以根据对视觉信息的信任度动态修改这个值，比如标定的误差？const float unc2 = pFrame->mpCamera->uncertainty2(obs);// invSigma2 = (Inverse(协方差矩阵))^2，表明该约束在各个维度上的可信度// 图像金字塔层数越高，可信度越差const float invSigma2 = pFrame->mvInvLevelSigma2[kpUn.octave] / unc2;//设置该约束的信息矩阵e->setInformation(Eigen::Matrix2d::Identity() * invSigma2);g2o::RobustKernelHuber *rk = new g2o::RobustKernelHuber;// 设置鲁棒核函数，避免其误差的平方项出现数值过大的增长 注：后续在优化2次后会用e->setRobustKernel(0)禁掉鲁棒核函数e->setRobustKernel(rk);//重投影误差的自由度为2，设置对应的卡方阈值rk->setDelta(thHuberMono);//将第一种边加入优化器optimizer.addEdge(e);//将第一种边加入vpEdgesMonovpEdgesMono.push_back(e);//将对应的特征点索引加入vnIndexEdgeMonovnIndexEdgeMono.push_back(i);}// Stereo observationelse if (!bRight) //? 为什么是双目{nInitialStereoCorrespondences++;pFrame->mvbOutlier[i] = false;kpUn = pFrame->mvKeysUn[i];const float kp_ur = pFrame->mvuRight[i];Eigen::Matrix<double, 3, 1> obs;obs << kpUn.pt.x, kpUn.pt.y, kp_ur;EdgeStereoOnlyPose *e = new EdgeStereoOnlyPose(pMP->GetWorldPos());e->setVertex(0, VP);e->setMeasurement(obs);// Add here uncerteintyconst float unc2 = pFrame->mpCamera->uncertainty2(obs.head(2));const float &invSigma2 = pFrame->mvInvLevelSigma2[kpUn.octave] / unc2;e->setInformation(Eigen::Matrix3d::Identity() * invSigma2);g2o::RobustKernelHuber *rk = new g2o::RobustKernelHuber;e->setRobustKernel(rk);rk->setDelta(thHuberStereo);optimizer.addEdge(e);vpEdgesStereo.push_back(e);vnIndexEdgeStereo.push_back(i);}// Right monocular observationif (bRight && i >= Nleft){nInitialMonoCorrespondences++;pFrame->mvbOutlier[i] = false;kpUn = pFrame->mvKeysRight[i - Nleft];Eigen::Matrix<double, 2, 1> obs;obs << kpUn.pt.x, kpUn.pt.y;EdgeMonoOnlyPose *e = new EdgeMonoOnlyPose(pMP->GetWorldPos(), 1);e->setVertex(0, VP);e->setMeasurement(obs);// Add here uncerteintyconst float unc2 = pFrame->mpCamera->uncertainty2(obs);const float invSigma2 = pFrame->mvInvLevelSigma2[kpUn.octave] / unc2;e->setInformation(Eigen::Matrix2d::Identity() * invSigma2);g2o::RobustKernelHuber *rk = new g2o::RobustKernelHuber;e->setRobustKernel(rk);rk->setDelta(thHuberMono);optimizer.addEdge(e);vpEdgesMono.push_back(e);vnIndexEdgeMono.push_back(i);}}}}//统计参与优化的地图点总数目nInitialCorrespondences = nInitialMonoCorrespondences + nInitialStereoCorrespondences;// Set Previous Frame Vertex// pFp为上一帧Frame *pFp = pFrame->mpPrevFrame;//上一帧的位姿，旋转+平移，6-dimVertexPose *VPk = new VertexPose(pFp);VPk->setId(4);VPk->setFixed(false);optimizer.addVertex(VPk);//上一帧的速度，3-dimVertexVelocity *VVk = new VertexVelocity(pFp);VVk->setId(5);VVk->setFixed(false);optimizer.addVertex(VVk);//上一帧的陀螺仪偏置，3-dimVertexGyroBias *VGk = new VertexGyroBias(pFp);VGk->setId(6);VGk->setFixed(false);optimizer.addVertex(VGk);//上一帧的加速度偏置，3-dimVertexAccBias *VAk = new VertexAccBias(pFp);VAk->setId(7);VAk->setFixed(false);optimizer.addVertex(VAk);// setFixed(false)这个设置使以上四个顶点（15个参数）的值随优化而变，这样做会给上一帧再提供一些优化空间//但理论上不应该优化过多，否则会有不良影响，故后面的代码会用第五种边来约束上一帧的变化量//第二种边（IMU预积分约束）：两帧之间位姿的变化量与IMU预积分的值偏差尽可能小EdgeInertial *ei = new EdgeInertial(pFrame->mpImuPreintegratedFrame);//将上一帧四个顶点（P、V、BG、BA）和当前帧两个顶点（P、V）加入第二种边ei->setVertex(0, VPk);ei->setVertex(1, VVk);ei->setVertex(2, VGk);ei->setVertex(3, VAk);ei->setVertex(4, VP);ei->setVertex(5, VV);//把第二种边加入优化器optimizer.addEdge(ei);//第三种边（陀螺仪随机游走约束）：陀螺仪的随机游走值在相邻帧间不会相差太多 residual=VG-VGk//用大白话来讲就是用固定的VGK拽住VG，防止VG在优化中放飞自我EdgeGyroRW *egr = new EdgeGyroRW();//将上一帧的BG加入第三种边egr->setVertex(0, VGk);//将当前帧的BG加入第三种边egr->setVertex(1, VG);// C值在预积分阶段更新，range(9,12)对应陀螺仪偏置的协方差，最终cvInfoG值为inv(∑(GyroRW^2/freq))cv::Mat cvInfoG = pFrame->mpImuPreintegratedFrame->C.rowRange(9, 12).colRange(9, 12).inv(cv::DECOMP_SVD);Eigen::Matrix3d InfoG;for (int r = 0; r < 3; r++)for (int c = 0; c < 3; c++)InfoG(r, c) = cvInfoG.at<float>(r, c);//设置信息矩阵egr->setInformation(InfoG);//把第三种边加入优化器optimizer.addEdge(egr);//第四种边（加速度随机游走约束）：加速度的随机游走值在相近帧间不会相差太多 residual=VA-VAk//用大白话来讲就是用固定的VAK拽住VA，防止VA在优化中放飞自我EdgeAccRW *ear = new EdgeAccRW();//将上一帧的BA加入第四种边ear->setVertex(0, VAk);//将当前帧的BA加入第四种边ear->setVertex(1, VA);// C值在预积分阶段更新，range(12,15)对应加速度偏置的协方差，最终cvInfoG值为inv(∑(AccRW^2/freq))cv::Mat cvInfoA = pFrame->mpImuPreintegratedFrame->C.rowRange(12, 15).colRange(12, 15).inv(cv::DECOMP_SVD);Eigen::Matrix3d InfoA;for (int r = 0; r < 3; r++)for (int c = 0; c < 3; c++)InfoA(r, c) = cvInfoA.at<float>(r, c);//设置信息矩阵ear->setInformation(InfoA);//把第四种边加入优化器optimizer.addEdge(ear);// ?既然有判空的操作，可以区分一下有先验信息（五条边）和无先验信息（四条边）的情况if (!pFp->mpcpi)Verbose::PrintMess("pFp->mpcpi does not exist!!!\nPrevious Frame " + to_string(pFp->mnId), Verbose::VERBOSITY_NORMAL);//第五种边（先验约束）：上一帧信息随优化的改变量要尽可能小EdgePriorPoseImu *ep = new EdgePriorPoseImu(pFp->mpcpi);//将上一帧的四个顶点（P、V、BG、BA）加入第五种边ep->setVertex(0, VPk);ep->setVertex(1, VVk);ep->setVertex(2, VGk);ep->setVertex(3, VAk);g2o::RobustKernelHuber *rkp = new g2o::RobustKernelHuber;ep->setRobustKernel(rkp);rkp->setDelta(5);//把第五种边加入优化器optimizer.addEdge(ep);// Step 2：启动多轮优化，剔除外点// We perform 4 optimizations, after each optimization we classify observation as inlier/outlier// At the next optimization, outliers are not included, but at the end they can be classified as inliers again.//与PoseInertialOptimizationLastKeyFrame函数对比，区别在于：在优化过程中保持卡方阈值不变// 以下参数的解释// 自由度为2的卡方分布，显著性水平为0.05，对应的临界阈值5.991// 自由度为3的卡方分布，显著性水平为0.05，对应的临界阈值7.815// 自由度为3的卡方分布，显著性水平为0.02，对应的临界阈值9.8// 自由度为3的卡方分布，显著性水平为0.001，对应的临界阈值15.6// 计算方法：https://stattrek.com/online-calculator/chi-square.aspxconst float chi2Mono[4] = {5.991, 5.991, 5.991, 5.991};const float chi2Stereo[4] = {15.6f, 9.8f, 7.815f, 7.815f};// 4次优化的迭代次数都为10const int its[4] = {10, 10, 10, 10};//坏点数int nBad = 0;//单目坏点数int nBadMono = 0;//双目坏点数int nBadStereo = 0;//单目内点数int nInliersMono = 0;//双目内点数int nInliersStereo = 0;//内点数int nInliers = 0;for (size_t it = 0; it < 4; it++){// 初始化优化器,这里的参数0代表只对level为0的边进行优化（不传参数默认也是0）optimizer.initializeOptimization(0);//每次优化迭代十次optimizer.optimize(its[it]);//每次优化都重新统计各类点的数目nBad = 0;nBadMono = 0;nBadStereo = 0;nInliers = 0;nInliersMono = 0;nInliersStereo = 0;//使用1.5倍的chi2Mono作为“近点”的卡方检验值，意味着地图点越近，检验越宽松//地图点如何定义为“近点”在下面的代码中有解释float chi2close = 1.5 * chi2Mono[it];for (size_t i = 0, iend = vpEdgesMono.size(); i < iend; i++){EdgeMonoOnlyPose *e = vpEdgesMono[i];const size_t idx = vnIndexEdgeMono[i];//当地图点在当前帧的深度值小于10时，该地图点属于close（近点）// mTrackDepth是在Frame.cc的isInFrustum函数中计算出来的bool bClose = pFrame->mvpMapPoints[idx]->mTrackDepth < 10.f;// 如果这条误差边是来自于outlierif (pFrame->mvbOutlier[idx]){//计算本次优化后的误差e->computeError();}// 就是error*\Omega*error，表示了这条边考虑置信度以后的误差大小const float chi2 = e->chi2();//判断某地图点为外点的条件有以下三种：// 1.该地图点不是近点并且误差大于卡方检验值chi2Mono[it]// 2.该地图点是近点并且误差大于卡方检验值chi2close// 3.深度不为正//每次优化后，用更小的卡方检验值，原因是随着优化的进行，对划分为内点的信任程度越来越低if ((chi2 > chi2Mono[it] && !bClose) || (bClose && chi2 > chi2close) || !e->isDepthPositive()){//将该点设置为外点pFrame->mvbOutlier[idx] = true;//外点不参与下一轮优化e->setLevel(1);//单目坏点数+1nBadMono++;}else{//将该点设置为内点（暂时）pFrame->mvbOutlier[idx] = false;//内点继续参与下一轮优化e->setLevel(0);//单目内点数+1nInliersMono++;}//从第三次优化开始就不设置鲁棒核函数了，原因是经过两轮优化已经趋向准确值，不会有太大误差if (it == 2)e->setRobustKernel(0);}for (size_t i = 0, iend = vpEdgesStereo.size(); i < iend; i++){EdgeStereoOnlyPose *e = vpEdgesStereo[i];const size_t idx = vnIndexEdgeStereo[i];if (pFrame->mvbOutlier[idx]){e->computeError();}const float chi2 = e->chi2();if (chi2 > chi2Stereo[it]){pFrame->mvbOutlier[idx] = true;e->setLevel(1);nBadStereo++;}else{pFrame->mvbOutlier[idx] = false;e->setLevel(0);nInliersStereo++;}if (it == 2)e->setRobustKernel(0);}//内点总数=单目内点数+双目内点数nInliers = nInliersMono + nInliersStereo;//坏点数=单目坏点数+双目坏点数nBad = nBadMono + nBadStereo;if (optimizer.edges().size() < 10){cout << "PIOLF: NOT ENOUGH EDGES" << endl;break;}}//若4次优化后内点数小于30，尝试恢复一部分不那么糟糕的坏点if ((nInliers < 30) && !bRecInit){//重新从0开始统计坏点数nBad = 0;//单目可容忍的卡方检验最大值（如果误差比这还大就不要挣扎了...）const float chi2MonoOut = 18.f;const float chi2StereoOut = 24.f;EdgeMonoOnlyPose *e1;EdgeStereoOnlyPose *e2;//遍历所有单目特征点for (size_t i = 0, iend = vnIndexEdgeMono.size(); i < iend; i++){const size_t idx = vnIndexEdgeMono[i];//获取这些特征点对应的边e1 = vpEdgesMono[i];e1->computeError();if (e1->chi2() < chi2MonoOut)pFrame->mvbOutlier[idx] = false;elsenBad++;}for (size_t i = 0, iend = vnIndexEdgeStereo.size(); i < iend; i++){const size_t idx = vnIndexEdgeStereo[i];e2 = vpEdgesStereo[i];e2->computeError();if (e2->chi2() < chi2StereoOut)pFrame->mvbOutlier[idx] = false;elsenBad++;}}// ?多此一举？优化过程中nInliers这个值已经计算过了，nInliersMono和nInliersStereo在后续代码中一直保持不变nInliers = nInliersMono + nInliersStereo;// Step 3：更新当前帧位姿、速度、IMU偏置// Recover optimized pose, velocity and biases//给当前帧设置优化后的旋转、位移、速度，用来更新位姿pFrame->SetImuPoseVelocity(Converter::toCvMat(VP->estimate().Rwb), Converter::toCvMat(VP->estimate().twb), Converter::toCvMat(VV->estimate()));Vector6d b;b << VG->estimate(), VA->estimate();//给当前帧设置优化后的bg，bapFrame->mImuBias = IMU::Bias(b[3], b[4], b[5], b[0], b[1], b[2]);// Step 4：记录当前帧的优化状态，包括参数信息和边缘化后的海森矩阵// Recover Hessian, marginalize previous frame states and generate new prior for frame//包含本次优化所有信息矩阵的和，它代表本次优化对确定性的影响Eigen::Matrix<double, 30, 30> H;H.setZero();//第1步，加上EdgeInertial（IMU预积分约束）的海森矩阵// ei的定义// EdgeInertial* ei = new EdgeInertial(pFrame->mpImuPreintegratedFrame);// ei->setVertex(0, VPk);// ei->setVertex(1, VVk);// ei->setVertex(2, VGk);// ei->setVertex(3, VAk);// ei->setVertex(4, VP); // VertexPose* VP = new VertexPose(pFrame);// ei->setVertex(5, VV); // VertexVelocity* VV = new VertexVelocity(pFrame);// ei->GetHessian() = J.t * J 下同，不做详细标注了// J// rn + tn vn gn an rn+1 + tn+1 vn+1// er Jp1 Jv1 Jg1 Ja1 Jp2 Jv2// 角标1表示上一帧，2表示当前帧// 6 3 3 3 6 3// Jp1.t * Jp1 Jp1.t * Jv1 Jp1.t * Jg1 Jp1.t * Ja1 Jp1.t * Jp2 Jp1.t * Jv2 6// Jv1.t * Jp1 Jv1.t * Jv1 Jv1.t * Jg1 Jv1.t * Ja1 Jv1.t * Jp2 Jv1.t * Jv2 3// Jg1.t * Jp1 Jg1.t * Jv1 Jg1.t * Jg1 Jg1.t * Ja1 Jg1.t * Jp2 Jg1.t * Jv2 3// Ja1.t * Jp1 Ja1.t * Jv1 Ja1.t * Jg1 Ja1.t * Ja1 Ja1.t * Jp2 Ja1.t * Jv2 3// Jp2.t * Jp1 Jp2.t * Jv1 Jp2.t * Jg1 Jp2.t * Ja1 Jp2.t * Jp2 Jp2.t * Jv2 6// Jv2.t * Jp1 Jv2.t * Jv1 Jv2.t * Jg1 Jv2.t * Ja1 Jv2.t * Jp2 Jv2.t * Jv2 3// 所以矩阵是24*24 的H.block<24, 24>(0, 0) += ei->GetHessian();// 经过这步H变成了// 列数 6 3 3 3 6 3 6// ---------------------------------------------------------------------------------- 行数// Jp1.t * Jp1 Jp1.t * Jv1 Jp1.t * Jg1 Jp1.t * Ja1 Jp1.t * Jp2 Jp1.t * Jv2 0 | 6// Jv1.t * Jp1 Jv1.t * Jv1 Jv1.t * Jg1 Jv1.t * Ja1 Jv1.t * Jp2 Jv1.t * Jv2 0 | 3// Jg1.t * Jp1 Jg1.t * Jv1 Jg1.t * Jg1 Jg1.t * Ja1 Jg1.t * Jp2 Jg1.t * Jv2 0 | 3// Ja1.t * Jp1 Ja1.t * Jv1 Ja1.t * Jg1 Ja1.t * Ja1 Ja1.t * Jp2 Ja1.t * Jv2 0 | 3// Jp2.t * Jp1 Jp2.t * Jv1 Jp2.t * Jg1 Jp2.t * Ja1 Jp2.t * Jp2 Jp2.t * Jv2 0 | 6// Jv2.t * Jp1 Jv2.t * Jv1 Jv2.t * Jg1 Jv2.t * Ja1 Jv2.t * Jp2 Jv2.t * Jv2 0 | 3// 0 0 0 0 0 0 0 | 6// ----------------------------------------------------------------------------------//第2步，加上EdgeGyroRW（陀螺仪随机游走约束）的信息矩阵：//| 0~8 | 9~11 | 12~23 | 24~26 |27~29// 9~11是上一帧的bg(3-dim)，24~26是当前帧的bg(3-dim)// egr的定义// EdgeGyroRW* egr = new EdgeGyroRW();// egr->setVertex(0, VGk);// egr->setVertex(1, VG);Eigen::Matrix<double, 6, 6> Hgr = egr->GetHessian();H.block<3, 3>(9, 9) += Hgr.block<3, 3>(0, 0); // Jgr1.t * Jgr1H.block<3, 3>(9, 24) += Hgr.block<3, 3>(0, 3); // Jgr1.t * Jgr2H.block<3, 3>(24, 9) += Hgr.block<3, 3>(3, 0); // Jgr2.t * Jgr1H.block<3, 3>(24, 24) += Hgr.block<3, 3>(3, 3); // Jgr2.t * Jgr2// 经过这步H变成了// 列数 6 3 3 3 6 3 3 3// ----------------------------------------------------------------------------------------------------------------- 行数// Jp1.t * Jp1 Jp1.t * Jv1 Jp1.t * Jg1 Jp1.t * Ja1 Jp1.t * Jp2 Jp1.t * Jv2 0 0 | 6// Jv1.t * Jp1 Jv1.t * Jv1 Jv1.t * Jg1 Jv1.t * Ja1 Jv1.t * Jp2 Jv1.t * Jv2 0 0 | 3// 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// Ja1.t * Jp1 Ja1.t * Jv1 Ja1.t * Jg1 Ja1.t * Ja1 Ja1.t * Jp2 Ja1.t * Jv2 0 0 | 3// Jp2.t * Jp1 Jp2.t * Jv1 Jp2.t * Jg1 Jp2.t * Ja1 Jp2.t * Jp2 Jp2.t * Jv2 0 0 | 6// Jv2.t * Jp1 Jv2.t * Jv1 Jv2.t * Jg1 Jv2.t * Ja1 Jv2.t * Jp2 Jv2.t * Jv2 0 0 | 3// 0 0 Jgr2.t * Jgr1 0 0 0 Jgr2.t * Jgr2 0 | 3// 0 0 0 0 0 0 0 0 | 3// -----------------------------------------------------------------------------------------------------------------//第3步，加上EdgeAccRW（加速度随机游走约束）的信息矩阵：//| 0~11 | 12~14 | 15~26 | 27~29 |30// 12~14是上一帧的ba(3-dim)，27~29是当前帧的ba(3-dim)// ear的定义// EdgeAccRW* ear = new EdgeAccRW();// ear->setVertex(0, VAk);// ear->setVertex(1, VA);Eigen::Matrix<double, 6, 6> Har = ear->GetHessian();H.block<3, 3>(12, 12) += Har.block<3, 3>(0, 0); // Jar1.t * Jar1H.block<3, 3>(12, 27) += Har.block<3, 3>(0, 3); // Jar1.t * Jar2H.block<3, 3>(27, 12) += Har.block<3, 3>(3, 0); // Jar2.t * Jar1H.block<3, 3>(27, 27) += Har.block<3, 3>(3, 3); // Jar2.t * Jar2// 经过这步H变成了// 列数 6 3 3 3 6 3 3 3// --------------------------------------------------------------------------------------------------------------------------------------------------- 行数// | Jp1.t * Jp1 Jp1.t * Jv1 Jp1.t * Jg1 Jp1.t * Ja1 | Jp1.t * Jp2 Jp1.t * Jv2 0 0 | 6// | Jv1.t * Jp1 Jv1.t * Jv1 Jv1.t * Jg1 Jv1.t * Ja1 | Jv1.t * Jp2 Jv1.t * Jv2 0 0 | 3// | 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// | 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// |--------------------------------------------------------------------------------------------------------------------------------------------------// | Jp2.t * Jp1 Jp2.t * Jv1 Jp2.t * Jg1 Jp2.t * Ja1 | Jp2.t * Jp2 Jp2.t * Jv2 0 0 | 6// | Jv2.t * Jp1 Jv2.t * Jv1 Jv2.t * Jg1 Jv2.t * Ja1 | Jv2.t * Jp2 Jv2.t * Jv2 0 0 | 3// | 0 0 Jgr2.t * Jgr1 0 | 0 0 Jgr2.t * Jgr2 0 | 3// | 0 0 0 Jar2.t * Jar1 | 0 0 0 Jar2.t * Jar2 | 3// ---------------------------------------------------------------------------------------------------------------------------------------------------//第4步，加上EdgePriorPoseImu（先验约束）的信息矩阵：//| 0~14 | 15~29// 0~14是上一帧的P(6-dim)、V(3-dim)、BG(3-dim)、BA(3-dim)// ep定义// EdgePriorPoseImu* ep = new EdgePriorPoseImu(pFp->mpcpi);// ep->setVertex(0, VPk);// ep->setVertex(1, VVk);// ep->setVertex(2, VGk);// ep->setVertex(3, VAk);// 6 3 3 3// Jp1.t * Jp1 Jp1.t * Jv1 Jp1.t * Jg1 Jp1.t * Ja1 6// Jv1.t * Jp1 Jv1.t * Jv1 Jv1.t * Jg1 Jv1.t * Ja1 3// Jg1.t * Jp1 Jg1.t * Jv1 Jg1.t * Jg1 Jg1.t * Ja1 3// Ja1.t * Jp1 Ja1.t * Jv1 Ja1.t * Jg1 Ja1.t * Ja1 3H.block<15, 15>(0, 0) += ep->GetHessian(); // 上一帧 的H矩阵，矩阵太大了不写了。。。总之就是加到下标为1相关的了int tot_in = 0, tot_out = 0;// 第5步 关于位姿的海森for (size_t i = 0, iend = vpEdgesMono.size(); i < iend; i++){EdgeMonoOnlyPose *e = vpEdgesMono[i];const size_t idx = vnIndexEdgeMono[i];if (!pFrame->mvbOutlier[idx]){// 0~14 | 15~20 | 21~29// 15~20是当前帧的P(6-dim)H.block<6, 6>(15, 15) += e->GetHessian(); // 当前帧的H矩阵，矩阵太大了不写了。。。总之就是加到p2相关的了tot_in++;}elsetot_out++;}for (size_t i = 0, iend = vpEdgesStereo.size(); i < iend; i++){EdgeStereoOnlyPose *e = vpEdgesStereo[i];const size_t idx = vnIndexEdgeStereo[i];if (!pFrame->mvbOutlier[idx]){// 0~14 | 15~20 | 21~29H.block<6, 6>(15, 15) += e->GetHessian();tot_in++;}elsetot_out++;}// H = |B E.t| ------> |0 0|// |E A| |0 A-E*B.inv*E.t|H = Marginalize(H, 0, 14);/*Marginalize里的函数在此处的调用等效于：Eigen::JacobiSVD<Eigen::MatrixXd> svd(H.block(0, 0, 15, 15), Eigen::ComputeThinU | Eigen::ComputeThinV);Eigen::JacobiSVD<Eigen::MatrixXd>::SingularValuesType singularValues_inv = svd.singularValues();for (int i = 0; i < 15; ++i){if (singularValues_inv(i) > 1e-6)singularValues_inv(i) = 1.0 / singularValues_inv(i);elsesingularValues_inv(i) = 0;}Eigen::MatrixXd invHb = svd.matrixV() * singularValues_inv.asDiagonal() * svd.matrixU().transpose();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);*///构造一个ConstraintPoseImu对象，为下一帧提供先验约束//构造对象所使用的参数是当前帧P、V、BG、BA的估计值和边缘化后的H矩阵pFrame->mpcpi = new ConstraintPoseImu(VP->estimate().Rwb, VP->estimate().twb, VV->estimate(), VG->estimate(), VA->estimate(), H.block<15, 15>(15, 15));//下一帧使用的EdgePriorPoseImu参数来自于此delete pFp->mpcpi;pFp->mpcpi = NULL;//返回值：内点数 = 总地图点数目 - 坏点（外点）数目return nInitialCorrespondences - nBad;
}
【SLAM学习笔记】6-ORB_SLAM3关键源码分析④ Optimizer（一）单帧优化相关推荐
- 【SLAM学习笔记】12-ORB_SLAM3关键源码分析⑩ Optimizer（七）地图融合优化
2021SC@SDUSC 目录 1.前言 2.代码分析 1.前言 这一部分代码量巨大,查阅了很多资料结合来看的代码,将分为以下部分进行分析 单帧优化 局部地图优化 全局优化 尺度与重力优化 sim3优 ...
- 【SLAM学习笔记】11-ORB_SLAM3关键源码分析⑨ Optimizer（六）地图回环优化
2021SC@SDUSC 目录 1.前言 2.代码分析 1.前言 这一部分代码量巨大,查阅了很多资料结合来看的代码,将分为以下部分进行分析 单帧优化 局部地图优化 全局优化 尺度与重力优化 sim3优 ...
- Nginx学习笔记（五） 源码分析内存模块内存对齐
Nginx源码分析&内存模块 今天总结了下C语言的内存分配问题,那么就看看Nginx的内存分配相关模型的具体实现.还有内存对齐的内容~~不懂的可以看看~~ src/os/unix/Ngx_al ...
- Netty学习笔记(一)Netty客户端源码分析
最近在学些BIO,NIO相关的知识,也学习了下Netty和它的源码,做个记录,方便以后继续学习,如果有错误的地方欢迎指正 如果不了解BIO,NIO这些基础知识,可以看下我的如下博客 IO中的阻塞.非阻 ...
- Android学习笔记-常用的一些源码，防止忘记了
Android学习笔记-常用的一些源码,防止忘记了... 设置拨打电话 StringdialUri="tell:"+m_currentTelNumble; IntentcallIn ...
- Ceph 学习——OSD读写流程与源码分析（一）
消息从客户端发送而来,之前几节介绍了 客户端下 对象存储.块存储库的实现以及他们在客户端下API请求的发送过程(Ceph学习--Librados与Osdc实现源码解析 . Ceph学习--客户端读写操 ...
- Java的wait()、notify()学习三部曲之一：JVM源码分析
原文链接:https://blog.csdn.net/boling_cavalry/article/details/77793224 综述 Java的wait().notify()学习三部曲由三篇文章 ...
- 【安卓学习积累】IntentService的源码分析
今天主要总结一下IntentService的源码,里面是如何实现的,为什么IntentService在执行完耗时操作后会自动停止. 1.the service is started as needed ...
- Redis学习之intset整数集合源码分析
1.整数集合:整数的集合,升序排序,无重复元素 2.整数集合intset是集合键的底层实现之一,当一个集合只包含整数值的元素,并且这个集合的元素数量不多时,redis会使用整数集合作为集合键的底层实现 ...
最新文章
- php array_multisort对数据库结果多个字段进行排序
- HTML5 Canvas 绘制佛教万字
- idea里自动创建构造函数
- 玩转Google开源C++单元测试框架Google Test系列(gtest)之五 - 死亡测试
- /etc/sudoers中的含义
- LeetCode动态规划 使用最小花费爬楼梯
- Qt实现Word文档界面样式--QtitanRibbon
- NVIDIA TESLA M40
- 苹果iPhone SE 2概念视频放出：全面屏的小屏机
- HTTP权威指南-学习笔记（三）HTTP方法，状态码
- 总有人黑 JavaScript？那是不知道它有多好用吧！
- centos7 删除zabbix_Zabbix服务器端运行中显示为“不”的解决方式
- click和blur冲突的问题
- uni-app 封装请求
- 12.MongoDB之固定集合(Capped Collections)
- SpringBoot 配置文件说明
- linux 分区表 修复工具,介绍一个 GPL 的分区表修复工具 TestDisk
- 码云gitee 部署公钥匙
- 【mmdetection】mmdetection学习率设置
- Oracle 11g 停止正在运行的job