ORB-SLAM3 从理论到代码实现（四）：Optimizer 尺度与重力优化

1. 前言InertialOptimization共有4个重载// Inertial pose-graph void static InertialOptimization(Map *pMap, Eigen::Matrix3d Rwg, double scale, Eigen::Vector3d bg, Eigen::Vector3d ba, bool bMono, Eigen::MatrixXd covInertial, bool bFixedVelfalse, bool bGaussfalse, float priorG 1e2, float priorA 1e6); void static InertialOptimization(Map *pMap, Eigen::Vector3d bg, Eigen::Vector3d ba, float priorG 1e2, float priorA 1e6); void static InertialOptimization(vectorKeyFrame* vpKFs, Eigen::Vector3d bg, Eigen::Vector3d ba, float priorG 1e2, float priorA 1e6); void static InertialOptimization(Map *pMap, Eigen::Matrix3d Rwg, double scale);此处理只介绍其中两个与尺度与重力优化相关的。2. 代码分析2.1. InertialOptimization参数pMap: 当前地图Rwg: 待优化的重力方向scale: 待优化的尺度因子bg: 待优化的陀螺仪偏置 (gyro bias)ba: 待优化的加速计偏置 (accel bias)bMono: 传感器是否为单目的标记位在调用时设置为truecovInertial: 惯导协方差矩阵貌似无用bFixedVel: 是否固定关键帧速度不优化的标记位falsebGuass: 惯导噪声是否符合高斯分布的标记位falsepriorG: 陀螺仪偏置优化权重priorA: 加速计偏置优化权重。void Optimizer::InertialOptimization(Map *pMap, Eigen::Matrix3d Rwg, double scale, Eigen::Vector3d bg, Eigen::Vector3d ba, bool bMono, Eigen::MatrixXd covInertial, bool bFixedVel, bool bGauss, float priorG, float priorA) { Verbose::PrintMess(inertial optimization, Verbose::VERBOSITY_NORMAL); int its 200; // Check number of iterations long unsigned int maxKFid pMap-GetMaxKFid(); const vectorKeyFrame * vpKFs pMap-GetAllKeyFrames(); // 获取所有关键帧 // Setup optimizer // 1. 构建优化器 g2o::SparseOptimizer optimizer; g2o::BlockSolverX::LinearSolverType *linearSolver; linearSolver new g2o::LinearSolverEigeng2o::BlockSolverX::PoseMatrixType(); g2o::BlockSolverX *solver_ptr new g2o::BlockSolverX(linearSolver); g2o::OptimizationAlgorithmLevenberg *solver new g2o::OptimizationAlgorithmLevenberg(solver_ptr); if (priorG ! 0.f) solver-setUserLambdaInit(1e3); optimizer.setAlgorithm(solver); // Set KeyFrame vertices (fixed poses and optimizable velocities) // 2. 确定关键帧节点锁住的位姿及可优化的速度 for (size_t i 0; i vpKFs.size(); i) { KeyFrame *pKFi vpKFs[i]; // 跳过id大于当前地图最大id的关键帧 if (pKFi-mnId maxKFid) continue; VertexPose *VP new VertexPose(pKFi); // 继承于public g2o::BaseVertex6, ImuCamPose VP-setId(pKFi-mnId); VP-setFixed(true); optimizer.addVertex(VP); VertexVelocity *VV new VertexVelocity(pKFi); // 继承于public g2o::BaseVertex3, Eigen::Vector3d VV-setId(maxKFid (pKFi-mnId) 1); if (bFixedVel) VV-setFixed(true); else VV-setFixed(false); optimizer.addVertex(VV); } // Biases // 3. 确定偏置节点陀螺仪与加速度计 VertexGyroBias *VG new VertexGyroBias(vpKFs.front()); VG-setId(maxKFid * 2 2); if (bFixedVel) VG-setFixed(true); else VG-setFixed(false); optimizer.addVertex(VG); VertexAccBias *VA new VertexAccBias(vpKFs.front()); VA-setId(maxKFid * 2 3); if (bFixedVel) VA-setFixed(true); else VA-setFixed(false); optimizer.addVertex(VA); // prior acc bias // 4. 添加关于加速度计与陀螺仪偏置的边这两个边加入是保证第一帧的偏置为0 EdgePriorAcc *epa new EdgePriorAcc(cv::Mat::zeros(3, 1, CV_32F)); epa-setVertex(0, dynamic_castg2o::OptimizableGraph::Vertex *(VA)); double infoPriorA priorA; epa-setInformation(infoPriorA * Eigen::Matrix3d::Identity()); optimizer.addEdge(epa); EdgePriorGyro *epg new EdgePriorGyro(cv::Mat::zeros(3, 1, CV_32F)); epg-setVertex(0, dynamic_castg2o::OptimizableGraph::Vertex *(VG)); double infoPriorG priorG; epg-setInformation(infoPriorG * Eigen::Matrix3d::Identity()); optimizer.addEdge(epg); // Gravity and scale // 5. 添加关于重力方向与尺度的节点 VertexGDir *VGDir new VertexGDir(Rwg); VGDir-setId(maxKFid * 2 4); VGDir-setFixed(false); optimizer.addVertex(VGDir); VertexScale *VS new VertexScale(scale); VS-setId(maxKFid * 2 5); VS-setFixed(!bMono); // Fixed for stereo case optimizer.addVertex(VS); // Graph edges // IMU links with gravity and scale // 6. imu信息链接重力方向与尺度信息 vectorEdgeInertialGS * vpei; // 后面虽然加入了边但是没有用到应该调试用的 vpei.reserve(vpKFs.size()); vectorpairKeyFrame *, KeyFrame * vppUsedKF; vppUsedKF.reserve(vpKFs.size()); // 后面虽然加入了关键帧但是没有用到应该调试用的 std::cout build optimization graph std::endl; for (size_t i 0; i vpKFs.size(); i) { KeyFrame *pKFi vpKFs[i]; if (pKFi-mPrevKF pKFi-mnId maxKFid) { if (pKFi-isBad() || pKFi-mPrevKF-mnId maxKFid) continue; if (!pKFi-mpImuPreintegrated) std::cout Not preintegrated measurement std::endl; // 到这里的条件是pKFi是好的并且它有上一个关键帧且他们的id要小于最大id // 6.1 检查节点指针是否为空 // 将pKFi偏置设定为上一关键帧的偏置 pKFi-mpImuPreintegrated-SetNewBias(pKFi-mPrevKF-GetImuBias()); g2o::HyperGraph::Vertex *VP1 optimizer.vertex(pKFi-mPrevKF-mnId); g2o::HyperGraph::Vertex *VV1 optimizer.vertex(maxKFid (pKFi-mPrevKF-mnId) 1); g2o::HyperGraph::Vertex *VP2 optimizer.vertex(pKFi-mnId); g2o::HyperGraph::Vertex *VV2 optimizer.vertex(maxKFid (pKFi-mnId) 1); g2o::HyperGraph::Vertex *VG optimizer.vertex(maxKFid * 2 2); g2o::HyperGraph::Vertex *VA optimizer.vertex(maxKFid * 2 3); g2o::HyperGraph::Vertex *VGDir optimizer.vertex(maxKFid * 2 4); g2o::HyperGraph::Vertex *VS optimizer.vertex(maxKFid * 2 5); if (!VP1 || !VV1 || !VG || !VA || !VP2 || !VV2 || !VGDir || !VS) { cout Error VP1 , VV1 , VG , VA , VP2 , VV2 , VGDir , VS endl; continue; } // 6.2 这是一个大边。。。。包含了上面所有信息注意到前面的两个偏置也做了两个一元边加入 EdgeInertialGS *ei new EdgeInertialGS(pKFi-mpImuPreintegrated); ei-setVertex(0, dynamic_castg2o::OptimizableGraph::Vertex *(VP1)); ei-setVertex(1, dynamic_castg2o::OptimizableGraph::Vertex *(VV1)); ei-setVertex(2, dynamic_castg2o::OptimizableGraph::Vertex *(VG)); ei-setVertex(3, dynamic_castg2o::OptimizableGraph::Vertex *(VA)); ei-setVertex(4, dynamic_castg2o::OptimizableGraph::Vertex *(VP2)); ei-setVertex(5, dynamic_castg2o::OptimizableGraph::Vertex *(VV2)); ei-setVertex(6, dynamic_castg2o::OptimizableGraph::Vertex *(VGDir)); ei-setVertex(7, dynamic_castg2o::OptimizableGraph::Vertex *(VS)); vpei.push_back(ei); vppUsedKF.push_back(make_pair(pKFi-mPrevKF, pKFi)); optimizer.addEdge(ei); } } // Compute error for different scales std::setg2o::HyperGraph::Edge * setEdges optimizer.edges(); std::cout start optimization std::endl; optimizer.initializeOptimization(); optimizer.setVerbose(false); optimizer.optimize(its); std::cout end optimization std::endl; // 7. 取数 scale VS-estimate(); // Recover optimized data // Biases VG static_castVertexGyroBias *(optimizer.vertex(maxKFid * 2 2)); VA static_castVertexAccBias *(optimizer.vertex(maxKFid * 2 3)); Vector6d vb; vb VG-estimate(), VA-estimate(); bg VG-estimate(); ba VA-estimate(); scale VS-estimate(); IMU::Bias b(vb[3], vb[4], vb[5], vb[0], vb[1], vb[2]); Rwg VGDir-estimate().Rwg; cv::Mat cvbg Converter::toCvMat(bg); // Keyframes velocities and biases const int N vpKFs.size(); for (size_t i 0; i N; i) { KeyFrame *pKFi vpKFs[i]; if (pKFi-mnId maxKFid) continue; VertexVelocity *VV static_castVertexVelocity *(optimizer.vertex(maxKFid (pKFi-mnId) 1)); Eigen::Vector3d Vw VV-estimate(); // Velocity is scaled after pKFi-SetVelocity(Converter::toCvMat(Vw)); if (cv::norm(pKFi-GetGyroBias() - cvbg) 0.01) { pKFi-SetNewBias(b); if (pKFi-mpImuPreintegrated) pKFi-mpImuPreintegrated-Reintegrate(); } else pKFi-SetNewBias(b); } }2.2. InertialOptimizationLoopClosing::MergeLocal2 中使用跟参数最多的那个同名函数不同的地方在于很多节点不可选是否固定优化的目标有速度偏置void Optimizer::InertialOptimization(Map *pMap, Eigen::Vector3d bg, Eigen::Vector3d ba, float priorG, float priorA) { int its 200; long unsigned int maxKFid pMap-GetMaxKFid(); const vectorKeyFrame * vpKFs pMap-GetAllKeyFrames(); // Setup optimizer // 1. 构建优化器 g2o::SparseOptimizer optimizer; g2o::BlockSolverX::LinearSolverType *linearSolver; linearSolver new g2o::LinearSolverEigeng2o::BlockSolverX::PoseMatrixType(); g2o::BlockSolverX *solver_ptr new g2o::BlockSolverX(linearSolver); g2o::OptimizationAlgorithmLevenberg *solver new g2o::OptimizationAlgorithmLevenberg(solver_ptr); solver-setUserLambdaInit(1e3); optimizer.setAlgorithm(solver); // Set KeyFrame vertices (fixed poses and optimizable velocities) // 2. 构建节点固定rt for (size_t i 0; i vpKFs.size(); i) { KeyFrame *pKFi vpKFs[i]; if (pKFi-mnId maxKFid) continue; VertexPose *VP new VertexPose(pKFi); VP-setId(pKFi-mnId); VP-setFixed(true); optimizer.addVertex(VP); VertexVelocity *VV new VertexVelocity(pKFi); VV-setId(maxKFid (pKFi-mnId) 1); VV-setFixed(false); optimizer.addVertex(VV); } // Biases // 3. 第一个关键帧的偏置 VertexGyroBias *VG new VertexGyroBias(vpKFs.front()); VG-setId(maxKFid * 2 2); VG-setFixed(false); optimizer.addVertex(VG); VertexAccBias *VA new VertexAccBias(vpKFs.front()); VA-setId(maxKFid * 2 3); VA-setFixed(false); optimizer.addVertex(VA); // prior acc bias // 4. 先验边让偏置趋向于0 EdgePriorAcc *epa new EdgePriorAcc(cv::Mat::zeros(3, 1, CV_32F)); epa-setVertex(0, dynamic_castg2o::OptimizableGraph::Vertex *(VA)); double infoPriorA priorA; epa-setInformation(infoPriorA * Eigen::Matrix3d::Identity()); optimizer.addEdge(epa); EdgePriorGyro *epg new EdgePriorGyro(cv::Mat::zeros(3, 1, CV_32F)); epg-setVertex(0, dynamic_castg2o::OptimizableGraph::Vertex *(VG)); double infoPriorG priorG; epg-setInformation(infoPriorG * Eigen::Matrix3d::Identity()); optimizer.addEdge(epg); // Gravity and scale // 5. 注意这里固定了尺度与重力方向所以只优化了偏置与速度 VertexGDir *VGDir new VertexGDir(Eigen::Matrix3d::Identity()); VGDir-setId(maxKFid * 2 4); VGDir-setFixed(true); optimizer.addVertex(VGDir); VertexScale *VS new VertexScale(1.0); VS-setId(maxKFid * 2 5); VS-setFixed(true); // Fixed since scale is obtained from already well initialized map optimizer.addVertex(VS); // Graph edges // IMU links with gravity and scale vectorEdgeInertialGS * vpei; vpei.reserve(vpKFs.size()); vectorpairKeyFrame *, KeyFrame * vppUsedKF; vppUsedKF.reserve(vpKFs.size()); // 6. 设置残差边 for (size_t i 0; i vpKFs.size(); i) { KeyFrame *pKFi vpKFs[i]; if (pKFi-mPrevKF pKFi-mnId maxKFid) { if (pKFi-isBad() || pKFi-mPrevKF-mnId maxKFid) continue; pKFi-mpImuPreintegrated-SetNewBias(pKFi-mPrevKF-GetImuBias()); g2o::HyperGraph::Vertex *VP1 optimizer.vertex(pKFi-mPrevKF-mnId); g2o::HyperGraph::Vertex *VV1 optimizer.vertex(maxKFid (pKFi-mPrevKF-mnId) 1); g2o::HyperGraph::Vertex *VP2 optimizer.vertex(pKFi-mnId); g2o::HyperGraph::Vertex *VV2 optimizer.vertex(maxKFid (pKFi-mnId) 1); g2o::HyperGraph::Vertex *VG optimizer.vertex(maxKFid * 2 2); g2o::HyperGraph::Vertex *VA optimizer.vertex(maxKFid * 2 3); g2o::HyperGraph::Vertex *VGDir optimizer.vertex(maxKFid * 2 4); g2o::HyperGraph::Vertex *VS optimizer.vertex(maxKFid * 2 5); if (!VP1 || !VV1 || !VG || !VA || !VP2 || !VV2 || !VGDir || !VS) { cout Error VP1 , VV1 , VG , VA , VP2 , VV2 , VGDir , VS endl; continue; } EdgeInertialGS *ei new EdgeInertialGS(pKFi-mpImuPreintegrated); ei-setVertex(0, dynamic_castg2o::OptimizableGraph::Vertex *(VP1)); ei-setVertex(1, dynamic_castg2o::OptimizableGraph::Vertex *(VV1)); ei-setVertex(2, dynamic_castg2o::OptimizableGraph::Vertex *(VG)); ei-setVertex(3, dynamic_castg2o::OptimizableGraph::Vertex *(VA)); ei-setVertex(4, dynamic_castg2o::OptimizableGraph::Vertex *(VP2)); ei-setVertex(5, dynamic_castg2o::OptimizableGraph::Vertex *(VV2)); ei-setVertex(6, dynamic_castg2o::OptimizableGraph::Vertex *(VGDir)); ei-setVertex(7, dynamic_castg2o::OptimizableGraph::Vertex *(VS)); vpei.push_back(ei); vppUsedKF.push_back(make_pair(pKFi-mPrevKF, pKFi)); optimizer.addEdge(ei); } } // 7. 优化 // Compute error for different scales optimizer.setVerbose(false); optimizer.initializeOptimization(); optimizer.optimize(its); // 8. 取数 // Recover optimized data // Biases VG static_castVertexGyroBias *(optimizer.vertex(maxKFid * 2 2)); VA static_castVertexAccBias *(optimizer.vertex(maxKFid * 2 3)); Vector6d vb; vb VG-estimate(), VA-estimate(); bg VG-estimate(); ba VA-estimate(); IMU::Bias b(vb[3], vb[4], vb[5], vb[0], vb[1], vb[2]); cv::Mat cvbg Converter::toCvMat(bg); // Keyframes velocities and biases const int N vpKFs.size(); for (size_t i 0; i N; i) { KeyFrame *pKFi vpKFs[i]; if (pKFi-mnId maxKFid) continue; VertexVelocity *VV static_castVertexVelocity *(optimizer.vertex(maxKFid (pKFi-mnId) 1)); Eigen::Vector3d Vw VV-estimate(); pKFi-SetVelocity(Converter::toCvMat(Vw)); if (cv::norm(pKFi-GetGyroBias() - cvbg) 0.01) { pKFi-SetNewBias(b); if (pKFi-mpImuPreintegrated) pKFi-mpImuPreintegrated-Reintegrate(); } else pKFi-SetNewBias(b); } }优化重力方向与尺度LocalMapping::ScaleRefinement()中使用优化目标有重力方向与尺度void Optimizer::InertialOptimization(Map *pMap, Eigen::Matrix3d Rwg, double scale) { int its 10; long unsigned int maxKFid pMap-GetMaxKFid(); const vectorKeyFrame * vpKFs pMap-GetAllKeyFrames(); // 1. 构建优化器 // Setup optimizer g2o::SparseOptimizer optimizer; g2o::BlockSolverX::LinearSolverType *linearSolver; linearSolver new g2o::LinearSolverEigeng2o::BlockSolverX::PoseMatrixType(); g2o::BlockSolverX *solver_ptr new g2o::BlockSolverX(linearSolver); g2o::OptimizationAlgorithmGaussNewton *solver new g2o::OptimizationAlgorithmGaussNewton(solver_ptr); optimizer.setAlgorithm(solver); // Set KeyFrame vertices (all variables are fixed) // 2. 添加帧节点其中包括位姿速度两个偏置 for (size_t i 0; i vpKFs.size(); i) { KeyFrame *pKFi vpKFs[i]; if (pKFi-mnId maxKFid) continue; VertexPose *VP new VertexPose(pKFi); VP-setId(pKFi-mnId); VP-setFixed(true); optimizer.addVertex(VP); VertexVelocity *VV new VertexVelocity(pKFi); VV-setId(maxKFid 1 (pKFi-mnId)); VV-setFixed(true); optimizer.addVertex(VV); // Vertex of fixed biases VertexGyroBias *VG new VertexGyroBias(vpKFs.front()); VG-setId(2 * (maxKFid 1) (pKFi-mnId)); VG-setFixed(true); optimizer.addVertex(VG); VertexAccBias *VA new VertexAccBias(vpKFs.front()); VA-setId(3 * (maxKFid 1) (pKFi-mnId)); VA-setFixed(true); optimizer.addVertex(VA); } // 3. 添加重力方向与尺度的节点为优化对象 // Gravity and scale VertexGDir *VGDir new VertexGDir(Rwg); VGDir-setId(4 * (maxKFid 1)); VGDir-setFixed(false); optimizer.addVertex(VGDir); VertexScale *VS new VertexScale(scale); VS-setId(4 * (maxKFid 1) 1); VS-setFixed(false); optimizer.addVertex(VS); // Graph edges // 4. 添加边 for (size_t i 0; i vpKFs.size(); i) { KeyFrame *pKFi vpKFs[i]; if (pKFi-mPrevKF pKFi-mnId maxKFid) { if (pKFi-isBad() || pKFi-mPrevKF-mnId maxKFid) continue; g2o::HyperGraph::Vertex *VP1 optimizer.vertex(pKFi-mPrevKF-mnId); g2o::HyperGraph::Vertex *VV1 optimizer.vertex((maxKFid 1) pKFi-mPrevKF-mnId); g2o::HyperGraph::Vertex *VP2 optimizer.vertex(pKFi-mnId); g2o::HyperGraph::Vertex *VV2 optimizer.vertex((maxKFid 1) pKFi-mnId); g2o::HyperGraph::Vertex *VG optimizer.vertex(2 * (maxKFid 1) pKFi-mPrevKF-mnId); g2o::HyperGraph::Vertex *VA optimizer.vertex(3 * (maxKFid 1) pKFi-mPrevKF-mnId); g2o::HyperGraph::Vertex *VGDir optimizer.vertex(4 * (maxKFid 1)); g2o::HyperGraph::Vertex *VS optimizer.vertex(4 * (maxKFid 1) 1); if (!VP1 || !VV1 || !VG || !VA || !VP2 || !VV2 || !VGDir || !VS) { Verbose::PrintMess(Error to_string(VP1-id()) , to_string(VV1-id()) , to_string(VG-id()) , to_string(VA-id()) , to_string(VP2-id()) , to_string(VV2-id()) , to_string(VGDir-id()) , to_string(VS-id()), Verbose::VERBOSITY_NORMAL); continue; } EdgeInertialGS *ei new EdgeInertialGS(pKFi-mpImuPreintegrated); ei-setVertex(0, dynamic_castg2o::OptimizableGraph::Vertex *(VP1)); ei-setVertex(1, dynamic_castg2o::OptimizableGraph::Vertex *(VV1)); ei-setVertex(2, dynamic_castg2o::OptimizableGraph::Vertex *(VG)); ei-setVertex(3, dynamic_castg2o::OptimizableGraph::Vertex *(VA)); ei-setVertex(4, dynamic_castg2o::OptimizableGraph::Vertex *(VP2)); ei-setVertex(5, dynamic_castg2o::OptimizableGraph::Vertex *(VV2)); ei-setVertex(6, dynamic_castg2o::OptimizableGraph::Vertex *(VGDir)); ei-setVertex(7, dynamic_castg2o::OptimizableGraph::Vertex *(VS)); optimizer.addEdge(ei); } } // 5. 优化 // Compute error for different scales optimizer.setVerbose(false); optimizer.initializeOptimization(); optimizer.optimize(its); // Recover optimized data // 6. 取数 scale VS-estimate(); Rwg VGDir-estimate().Rwg; }参考文献【SLAM学习笔记】9-ORB_SLAM3关键源码分析⑦ Optimizer四尺度与重力优化_口哨糖youri的博客-CSDN博客ORB-SLAM3InertialOptimization()代码分析_Zirong.的博客-CSDN博客ORB_SLAM3与VINS_MONO的惯导部分比较以及尺度因子优化分析_Leon Goretzka的博客-CSDN博客