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 bFixedVel=false, bool bGauss=false, 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(vector<KeyFrame*> 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: 传感器是否为单目的标记位,在调用时设置为true;
covInertial: 惯导协方差矩阵(貌似无用);
bFixedVel: 是否固定关键帧速度不优化的标记位,false;
bGuass: 惯导噪声是否符合高斯分布的标记位,false;
priorG: 陀螺仪偏置优化权重;
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 vector<KeyFrame *> vpKFs = pMap->GetAllKeyFrames(); // 获取所有关键帧 // Setup optimizer // 1. 构建优化器 g2o::SparseOptimizer optimizer; g2o::BlockSolverX::LinearSolverType *linearSolver; linearSolver = new g2o::LinearSolverEigen<g2o::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::BaseVertex<6, ImuCamPose> VP->setId(pKFi->mnId); VP->setFixed(true); optimizer.addVertex(VP); VertexVelocity *VV = new VertexVelocity(pKFi); // 继承于public g2o::BaseVertex<3, 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_cast<g2o::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_cast<g2o::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信息链接重力方向与尺度信息 vector<EdgeInertialGS *> vpei; // 后面虽然加入了边,但是没有用到,应该调试用的 vpei.reserve(vpKFs.size()); vector<pair<KeyFrame *, 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_cast<g2o::OptimizableGraph::Vertex *>(VP1)); ei->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex *>(VV1)); ei->setVertex(2, dynamic_cast<g2o::OptimizableGraph::Vertex *>(VG)); ei->setVertex(3, dynamic_cast<g2o::OptimizableGraph::Vertex *>(VA)); ei->setVertex(4, dynamic_cast<g2o::OptimizableGraph::Vertex *>(VP2)); ei->setVertex(5, dynamic_cast<g2o::OptimizableGraph::Vertex *>(VV2)); ei->setVertex(6, dynamic_cast<g2o::OptimizableGraph::Vertex *>(VGDir)); ei->setVertex(7, dynamic_cast<g2o::OptimizableGraph::Vertex *>(VS)); vpei.push_back(ei); vppUsedKF.push_back(make_pair(pKFi->mPrevKF, pKFi)); optimizer.addEdge(ei); } } // Compute error for different scales std::set<g2o::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_cast<VertexGyroBias *>(optimizer.vertex(maxKFid * 2 + 2)); VA = static_cast<VertexAccBias *>(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_cast<VertexVelocity *>(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. InertialOptimization
LoopClosing::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 vector<KeyFrame *> vpKFs = pMap->GetAllKeyFrames(); // Setup optimizer // 1. 构建优化器 g2o::SparseOptimizer optimizer; g2o::BlockSolverX::LinearSolverType *linearSolver; linearSolver = new g2o::LinearSolverEigen<g2o::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_cast<g2o::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_cast<g2o::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 vector<EdgeInertialGS *> vpei; vpei.reserve(vpKFs.size()); vector<pair<KeyFrame *, 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_cast<g2o::OptimizableGraph::Vertex *>(VP1)); ei->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex *>(VV1)); ei->setVertex(2, dynamic_cast<g2o::OptimizableGraph::Vertex *>(VG)); ei->setVertex(3, dynamic_cast<g2o::OptimizableGraph::Vertex *>(VA)); ei->setVertex(4, dynamic_cast<g2o::OptimizableGraph::Vertex *>(VP2)); ei->setVertex(5, dynamic_cast<g2o::OptimizableGraph::Vertex *>(VV2)); ei->setVertex(6, dynamic_cast<g2o::OptimizableGraph::Vertex *>(VGDir)); ei->setVertex(7, dynamic_cast<g2o::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_cast<VertexGyroBias *>(optimizer.vertex(maxKFid * 2 + 2)); VA = static_cast<VertexAccBias *>(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_cast<VertexVelocity *>(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 vector<KeyFrame *> vpKFs = pMap->GetAllKeyFrames(); // 1. 构建优化器 // Setup optimizer g2o::SparseOptimizer optimizer; g2o::BlockSolverX::LinearSolverType *linearSolver; linearSolver = new g2o::LinearSolverEigen<g2o::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_cast<g2o::OptimizableGraph::Vertex *>(VP1)); ei->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex *>(VV1)); ei->setVertex(2, dynamic_cast<g2o::OptimizableGraph::Vertex *>(VG)); ei->setVertex(3, dynamic_cast<g2o::OptimizableGraph::Vertex *>(VA)); ei->setVertex(4, dynamic_cast<g2o::OptimizableGraph::Vertex *>(VP2)); ei->setVertex(5, dynamic_cast<g2o::OptimizableGraph::Vertex *>(VV2)); ei->setVertex(6, dynamic_cast<g2o::OptimizableGraph::Vertex *>(VGDir)); ei->setVertex(7, dynamic_cast<g2o::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-SLAM3:InertialOptimization()代码分析_Zirong.的博客-CSDN博客
ORB_SLAM3与VINS_MONO的惯导部分比较以及尺度因子优化分析_Leon Goretzka的博客-CSDN博客