认真看代码ORB_SLAM3===2 优化器

Caleb tam

已于 2023-05-31 19:56:03 修改

阅读量516

点赞数

分类专栏： slam 文章标签：算法

于 2022-11-21 11:01:24 首次发布

本文链接：https://blog.csdn.net/t_home/article/details/127959827

版权

slam 专栏收录该内容

5 篇文章 0 订阅

订阅专栏

/单帧优化/

单帧的位姿的优化 PoseOptimization

位姿优化，纯视觉时使用。@param pFrame 待优化的帧

int Optimizer::PoseOptimization(Frame *pFrame)

当前帧位姿优化 PoseInertialOptimizationLastKeyFrame

使用上一关键帧+当前帧的视觉信息和IMU信息，优化当前帧位姿

Step 1：创建g2o优化器，初始化顶点和边

Step 2：启动多轮优化，剔除外点

Step 3：更新当前帧位姿、速度、IMU偏置

Step 4：记录当前帧的优化状态，包括参数信息和对应的海森矩阵

[in] pFrame 当前帧，也是待优化的帧

[in] bRecInit 调用这个函数的位置并没有传这个参数，因此它的值默认为false

return int 返回优化后的内点数

int Optimizer::PoseInertialOptimizationLastKeyFrame(Frame *pFrame, bool bRecInit)

/局部地图优化/

纯视觉BA LocalBundleAdjustment

@brief Local Bundle Adjustment LocalMapping::Run() 使用，纯视觉

pKF KeyFrame

pbStopFlag 是否停止优化的标志

pMap 在优化后，更新状态时需要用到Map的互斥量mMutexMapUpdate

与ORBSLAM2的不同 : 前面操作基本一样，但优化时2代去掉了误差大的点又进行优化了，3代只是统计但没有去掉继续优化，而后都是将误差大的点干掉

void Optimizer::LocalBundleAdjustment(KeyFrame *pKF, bool *pbStopFlag, Map *pMap, int &num_fixedKF, int &num_OptKF, int &num_MPs, int &num_edges)

局部地图＋惯导BA LocalInertialBA

@brief 局部地图＋惯导BA LocalMapping IMU中使用，地图经过imu初始化时用这个函数代替LocalBundleAdjustment

[in] pKF //关键帧

[in] pbStopFlag //是否停止的标志

[in] pMap //地图

[in] num_fixedKF //固定不优化关键帧的数目

[in] num_OptKF [in] num_MPs [in] num_edges

[in] bLarge 成功跟踪匹配的点数是否足够多

[in] bRecInit 经过BA2初始化后这个变量为false

void Optimizer::LocalInertialBA( KeyFrame *pKF, bool *pbStopFlag, Map *pMap, int &num_fixedKF, int &num_OptKF, int &num_MPs, int &num_edges, bool bLarge, bool bRecInit)

/以下为全局优化/

所有的MapPoints和关键帧BA GlobalBundleAdjustemnt

全局BA： pMap中所有的MapPoints和关键帧做bundle adjustment优化，这个全局BA优化在本程序中有两个地方使用：1、单目初始化：CreateInitialMapMonocular函数，2、闭环优化：RunGlobalBundleAdjustment函数

[in] pMap 地图点 [in] nIterations 迭代次数

[in] pbStopFlag 外部控制BA结束标志

[in] nLoopKF 形成了闭环的当前关键帧的id

[in] bRobust 是否使用鲁棒核函数

void Optimizer::GlobalBundleAdjustemnt(Map *pMap, int nIterations, bool *pbStopFlag, const unsigned long nLoopKF, const bool bRobust)

BA优化过程 BundleAdjustment

bundle adjustment 优化过程

[in] vpKFs 参与BA的所有关键帧

[in] vpMP 参与BA的所有地图点

[in] nIterations 优化迭代次数

[in] pbStopFlag 外部控制BA结束标志

[in] nLoopKF 形成了闭环的当前关键帧的id

[in] bRobust 是否使用核函数

void Optimizer::BundleAdjustment( const vector<KeyFrame *> &vpKFs, const vector<MapPoint *> &vpMP, int nIterations, bool *pbStopFlag, const unsigned long nLoopKF, const bool bRobust)

imu初始化优化 FullInertialBA

@brief imu初始化优化，LocalMapping::InitializeIMU中使用 LoopClosing::RunGlobalBundleAdjustment 地图全部做BA。也就是imu版的GlobalBundleAdjustemnt

@param pMap 地图 its 迭代次数 bFixLocal 是否固定局部，为false nLoopId 回环id pbStopFlag 是否停止的标志 bInit 提供priorG时为true，此时偏置只优化最后一帧的至0，然后所有关键帧的偏置都赋值为优化后的值，若为false，则建立每两帧之间的偏置边，优化使其相差为0 priorG 陀螺仪偏置的信息矩阵系数，主动设置时一般bInit为true，也就是只优化最后一帧的偏置，这个数会作为计算信息矩阵时使用 priorA 加速度计偏置的信息矩阵系数 vSingVal 没用，估计调试用的 bHess 没用，估计调试用的

void Optimizer::FullInertialBA( Map *pMap, int its, const bool bFixLocal, const long unsigned int nLoopId, bool *pbStopFlag, bool bInit, float priorG, float priorA, Eigen::VectorXd *vSingVal, bool *bHess)

/尺度与重力优化/

imu初始化优化1 InertialOptimization

* @brief LocalMapping::InitializeIMU中使用，其中kf的位姿是固定不变的，优化目标重力方向，尺度，速度与偏置

@param pMap 地图 Rwg 重力方向到速度方向的转角 scale 尺度（输出cout用） bg 陀螺仪偏置（输出cout用） ba 加速度计偏置（输出cout用） bMono 是否为单目 covInertial 惯导协方差矩阵(暂时没用，9*9的0矩阵) bFixedVel 是否固定速度不优化，false bGauss 没用，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)

imu初始化优化2 InertialOptimization

区别在于很多节点不可选是否固定，优化的目标有：速度，偏置

@param pMap 地图 bg 陀螺仪偏置 ba 加速度计偏置 priorG 陀螺仪偏置的信息矩阵系数 priorA 加速度计偏置的信息矩阵系数

void Optimizer::InertialOptimization(Map *pMap, Eigen::Vector3d &bg, Eigen::Vector3d &ba, float priorG, float priorA)

重力方向与尺度优化 InertialOptimization

@brief LocalMapping::ScaleRefinement()中使用，优化目标有：重力方向与尺度

@param pMap 地图 Rwg 重力方向到速度方向的转角 scale 尺度

void Optimizer::InertialOptimization(Map *pMap, Eigen::Matrix3d &Rwg, double &scale)

/sim3优化/

OptimizeSim3

@brief LoopClosing::DetectAndReffineSim3FromLastKF使用 1投2， 2投1这么来只优化g2oS12

@param pKF1 当前关键帧 pKF2 候选关键帧 vpMatches1 当前关键帧与地图匹配上的MP，中间会有NULL，与当前关键帧的特征点一一对应 g2oS12 相似变换矩阵 th2 误差上限的平方 bFixScale 是否固定尺度，单目或者单目imu未做到3阶段初始化为false mAcumHessian 计算累计海森矩阵（没啥用） bAllPoints 是否计算所有点（都为true）

int Optimizer::OptimizeSim3( KeyFrame *pKF1, KeyFrame *pKF2, vector<MapPoint *> &vpMatches1, g2o::Sim3 &g2oS12, const float th2, const bool bFixScale, Eigen::Matrix<double, 7, 7> &mAcumHessian, const bool bAllPoints)

/地图回环优化/

纯视觉，全局本质图优化 OptimizeEssentialGraph

@brief LoopClosing::CorrectLoop() 回环矫正时使用，纯视觉，全局本质图优化优化目标：地图中所有MP与关键帧

pMap 当前的map

pLoopKF mpLoopMatchedKF 与 mpCurrentKF 匹配的关键帧

pCurKF mpCurrentKF 当前关键帧

NonCorrectedSim3 通过pKFi->GetPose()计算的放NonCorrectedSim3也就是回环前的位姿这里面的帧只是与mpCurrentKF相关联的

CorrectedSim3 通过mg2oLoopScw计算的放CorrectedSim3 这里面的帧只是与mpCurrentKF相关联的

LoopConnections 因为回环而建立的新的帧与帧的连接关系，里面的key 为pCurKF共视帧 value为共视帧的共视帧

bFixScale 单目imu且未到第三次imu初始化为false

void Optimizer::OptimizeEssentialGraph( Map *pMap, KeyFrame *pLoopKF, KeyFrame *pCurKF,

const LoopClosing::KeyFrameAndPose &NonCorrectedSim3, const LoopClosing::KeyFrameAndPose &CorrectedSim3, const map<KeyFrame *, set<KeyFrame *>> &LoopConnections, const bool &bFixScale)

OptimizeEssentialGraph4DoF

@brief LoopClosing::CorrectLoop() 回环矫正时使用，IMU加视觉，全局本质图优化，流程基本与上面 OptimizeEssentialGraph 一模一样，同样有严重BUG 优化目标：地图中所有MP与关键帧

@param pMap 当前的map

pLoopKF mpLoopMatchedKF 与 mpCurrentKF 匹配的关键帧

pCurKF mpCurrentKF 当前关键帧

NonCorrectedSim3 通过pKFi->GetPose()计算的放NonCorrectedSim3也就是回环前的位姿这里面的帧只是与mpCurrentKF相关联的

CorrectedSim3 通过mg2oLoopScw计算的放CorrectedSim3 这里面的帧只是与mpCurrentKF共视的

LoopConnections 因为回环而建立的新的帧与帧的连接关系，里面的key 全部为pCurKF的共视帧与其本身

void Optimizer::OptimizeEssentialGraph4DoF( Map *pMap, KeyFrame *pLoopKF, KeyFrame *pCurKF, const LoopClosing::KeyFrameAndPose &NonCorrectedSim3, const LoopClosing::KeyFrameAndPose &CorrectedSim3, const map<KeyFrame *, set<KeyFrame *>> &LoopConnections)

OptimizeEssentialGraph

LoopClosing::MergeLocal() 融合地图时使用，优化当前帧没有参与融合的元素，本质图优化. 优化目标： 1. vpNonFixedKFs; 2.vpNonCorrectedMPs

@param pCurKF mpCurrentKF 融合时当前关键帧

vpFixedKFs vpMergeConnectedKFs 融合地图中的关键帧，也就是上面函数中的 vpFixedKF

vpFixedCorrectedKFs vpLocalCurrentWindowKFs 当前地图中经过矫正的关键帧，也就是Optimizer::LocalBundleAdjustment中优化过的vpAdjustKF

vpNonFixedKFs vpCurrentMapKFs 当前地图中剩余的关键帧，待优化

vpNonCorrectedMPs vpCurrentMapMPs 当前地图中剩余的MP点，待优化

void Optimizer::OptimizeEssentialGraph( KeyFrame *pCurKF, vector<KeyFrame *> &vpFixedKFs, vector<KeyFrame *> &vpFixedCorrectedKFs, vector<KeyFrame *> &vpNonFixedKFs, vector<MapPoint *> &vpNonCorrectedMPs)