/***单帧优化***/
单帧的位姿的优化 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)
MergeInertialBA
visual inertial BA。 * LoopClosing::MergeLocal2 中用到。化目标:相关帧的位姿,速度,偏置,还有涉及点的坐标,可以理解为跨地图的局部窗口优化
@param[in] pCurrKF 当前关键帧
[in] pMergeKF 融合帧
[in] pbStopFlag 是否优化
[in] pMap 当前地图
[out] corrPoses 所有的Sim3 矫正
void Optimizer::MergeInertialBA(
KeyFrame *pCurrKF, KeyFrame *pMergeKF, bool *pbStopFlag, Map *pMap, LoopClosing::KeyFrameAndPose &corrPoses)