ORB_SLAM3_优化方法 Pose优化

火柴的初心

已于 2023-07-24 22:32:06 修改

阅读量646

点赞数

分类专栏： ORB_SLAM3_源码解析文章标签： SLAM 自动驾驶计算机视觉算法图像处理

于 2023-04-16 14:49:57 首次发布

本文链接：https://blog.csdn.net/He3he3he/article/details/130182435

版权

ORB_SLAM3_源码解析专栏收录该内容

20 篇文章 10 订阅

订阅专栏

PoseOptimization

PoseOptimization主要的作用是利用重投影优化单帧的位姿，主要用在Tracking的几种跟踪模式TrackWithMotionModel、TrackReferenceKeyFrame、 TrackLocalMap、Relocalization中

输入

优化变量		观测
帧的Pose	帧的MapPoint	帧的KeyPoint

初始化

	//创建优化器  
    g2o::SparseOptimizer optimizer;  
    g2o::BlockSolver_6_3::LinearSolverType * linearSolver;

	//创建线性求解器
    linearSolver = new g2o::LinearSolverDense<g2o::BlockSolver_6_3::PoseMatrixType>();
	//创建块求解器 6 位姿 3 地图点
    g2o::BlockSolver_6_3 * solver_ptr = new g2o::BlockSolver_6_3(linearSolver);

	//设置优化算法
    g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr);
    optimizer.setAlgorithm(solver);

设置vertex

VertexSE3Expmap
- 设置估计值： $T_{cw}$
- 设置Id
- 是否固定：False

    g2o::VertexSE3Expmap * vSE3 = new g2o::VertexSE3Expmap();
    Sophus::SE3<float> Tcw = pFrame->GetPose();
    //需要将Tcw转换为SE3Quat
    vSE3->setEstimate(g2o::SE3Quat(Tcw.unit_quaternion().cast<double>(),Tcw.translation().cast<double>()));
    vSE3->setId(0);
    vSE3->setFixed(false);
    optimizer.addVertex(vSE3);

设置edge

对于每一对地图点-特征点添加重投影残差边：

EdgeSE3ProjectXYZOnlyPose
- 设置vertex：g2o::VertexSE3Expmap
- 设置观测obs：keyPoint
- 设置信息矩阵
- 设置鲁棒核函数：huber核
- 设置huber核的的δ
- 设置相机内参
- 设置地图点

	Eigen::Matrix<double,2,1> obs;  
	const cv::KeyPoint &kpUn = pFrame->mvKeysUn[i];  
	obs << kpUn.pt.x, kpUn.pt.y;

	// 新建节点，只优化位姿  
   ORB_SLAM3::EdgeSE3ProjectXYZOnlyPose* e = new ORB_SLAM3::EdgeSE3ProjectXYZOnlyPose();

	//设置vertex和观测
   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);

	//设置huber核函数
	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);

优化

title: 优化策略
* 分4次优化，每次迭代10次
* 每次优化，评估每条重投影边的残差
	* 如果大于阈值，设置为`level = 1`，不再参与优化
	* 如果小于阈值，设置为`level = 0`
* 从第**3**次开始，不再使用鲁棒核函数`e->setRobustKernel(0)`

恢复

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);

PoseInertialOptimizationLastKeyFrame

PoseInertialOptimizationLastKeyFrame主要的作用是利用上一关键帧和当前关键帧的视觉以及IMU信息来优化当前帧的位姿，主要用在Tracking的TrackLocalMap
在这里插入图片描述

输入

优化变量	fixed
当前帧的 $T_{wb}$	上一关键帧的 $T_{wb}$	当前帧的地图点
当前帧的 $b_{a}$	上一关键帧的 $b_{a}$
当前帧的 $b_{g}$	上一关键帧的 $b_{g}$
当前帧的速度v	上一关键帧的速度v

初始化

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);

title: 块求解器
此处的块求解器为`BlockSolverX`
```cpp
typedef BlockSolver< BlockSolverTraits<Eigen::Dynamic, Eigen::Dynamic> > BlockSolverX;

设置Vertex

当前帧

Pose：VertexPose

    VertexPose* VP = new VertexPose(pFrame);
    VP->setId(0);
    VP->setFixed(false);
    optimizer.addVertex(VP);

速度：VertexVelocity

    VertexVelocity* VV = new VertexVelocity(pFrame);
    VV->setId(1);
    VV->setFixed(false);
    optimizer.addVertex(VV);

陀螺仪bias：VertexGyroBias

    VertexGyroBias* VG = new VertexGyroBias(pFrame);
    VG->setId(2);
    VG->setFixed(false);
    optimizer.addVertex(VG);

加速度bias：VertexAccBias

    VertexAccBias* VA = new VertexAccBias(pFrame);
    VA->setId(3);
    VA->setFixed(false);
    optimizer.addVertex(VA);

上一关键帧

    KeyFrame* pKF = pFrame->mpLastKeyFrame;
    VertexPose* VPk = new VertexPose(pKF);
    VPk->setId(4);
    VPk->setFixed(true);
    optimizer.addVertex(VPk);
    
    VertexVelocity* VVk = new VertexVelocity(pKF);
    VVk->setId(5);
    VVk->setFixed(true);
    optimizer.addVertex(VVk);
    
    VertexGyroBias* VGk = new VertexGyroBias(pKF);
    VGk->setId(6);
    VGk->setFixed(true);
    optimizer.addVertex(VGk);
    
    VertexAccBias* VAk = new VertexAccBias(pKF);
    VAk->setId(7);
    VAk->setFixed(true);
    optimizer.addVertex(VAk);

设置edge

EdgeMonoOnlyPose
- 设置vertex：VertexPose
- 设置观测值
- 设置信息矩阵
- 设置鲁棒核函数

//观测
Eigen::Matrix<double,2,1> obs;  
obs << kpUn.pt.x, kpUn.pt.y;  
  
//创建边(地图点， 相机id)
EdgeMonoOnlyPose* e = new EdgeMonoOnlyPose(pMP->GetWorldPos(),0);  
  
//设置vertex
e->setVertex(0,VP);  
  
//设置观测值
e->setMeasurement(obs);  
  
// 获取不确定度 
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;  
e->setRobustKernel(rk);  
  
//重投影误差的自由度为2，设置对应的卡方阈值  
rk->setDelta(thHuberMono);  
  
//将第一种边加入优化器  
optimizer.addEdge(e);

EdgeInertial
- 设置vertex:
  - 当前帧：VP, VV
  - 上一帧：VPk, VVk, VGk, VAk

//创建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);

EdgeGyroRW
- 设置vertex:
  - 上一帧：VGk
  - 当前帧：VG
- 设置信息矩阵：噪声更新中的矩阵C

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);

EdgeAccRW
- 设置vertex:
  - 上一帧：VGk
  - 当前帧：VG
- 设置信息矩阵：噪声更新中的矩阵C

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);

优化策略

分4次优化，每次迭代10次
每次优化，评估每条重投影边的残差
- 如果大于阈值，设置为level = 1，不再参与优化
- 如果小于阈值，设置为level = 0
从第3次开始，不再使用鲁棒核函数e->setRobustKernel(0)
如果4次优化完后内点数目太少，恢复一些不太糟糕的点

恢复

pFrame->SetImuPoseVelocity(Converter::toCvMat(VP->estimate().Rwb),Converter::toCvMat(VP->estimate().twb),Converter::toCvMat(VV->estimate()));  
Vector6d b;  
b << VG->estimate(), VA->estimate();  
//给当前帧设置优化后的bg，ba  
pFrame->mImuBias = IMU::Bias(b[3],b[4],b[5],b[0],b[1],b[2]);

计算先验信息

A.对于当前帧的Pose

$\frac{\partial \mathbf{r}_{ij}}{\partial \mathbf{T}_{j}}$
$\frac{\partial \mathbf{r}_{ij}}{\partial \mathbf{T}_{j}} =\begin{bmatrix} \frac{\partial \mathbf{r}_{\Delta \mathbf{R}_{i j}}}{\partial \delta \vec{\phi}_{j}} & \frac{\partial \mathbf{r}_{\Delta \mathbf{R}_{i j}}}{\partial \delta \mathbf{p}_{j}}\\ \frac{\partial \mathbf{r}_{\Delta v_{i j}}}{\partial \delta \vec{\phi}_{j}} & \frac{\partial \mathbf{r}_{\Delta \mathbf{v}_{i j}}}{\partial \delta \mathbf{p}_{j}}\\ \frac{\partial \mathbf{r}_{\Delta \mathbf{p}_{i j}}}{\partial \delta \vec{\phi}_{j}} & \frac{\partial \mathbf{r}_{\Delta \mathbf{p}_{i j}}}{\partial \delta \mathbf{p}_{j}} \end{bmatrix}=\begin{bmatrix} \mathbf{J}_{r}^{-1}\left(\mathbf{r}_{\Delta \mathbf{R}_{i j}}\right) & 0\\ 0 & 0\\ 0 & \mathbf{R}_{i}^{T} \mathbf{R}_{j} \end{bmatrix}_{9\times 6}$
$\frac{\partial \mathbf{r}_{ij}}{\partial \delta \mathbf{v}_{j} }$
$\frac{\partial \mathbf{r}_{ij}}{\partial \delta \mathbf{v}_{j} } =\begin{bmatrix} \frac{\partial \mathbf{r}_{\Delta \mathbf{R}_{i j}}}{\partial \delta \mathbf{v}_{j}} \\ \frac{\partial \mathbf{r}_{\Delta \mathbf{v}_{i j}}}{\partial \delta \mathbf{v}_{j}} \\ \frac{\partial \mathbf{r}_{\Delta \mathbf{p}_{i j}}}{\partial \delta \mathbf{v}_{j}} \end{bmatrix} = \begin{bmatrix} \mathbf{0} \\ \mathbf{R}_{i}^{T} \\ \mathbf{0} \end{bmatrix}_{9\times 3}$
$\mathbf{J}$
$\mathbf{J} = \begin{bmatrix} {\frac{\partial \mathbf{r}_{ij}}{\partial \mathbf{T}_{j}}}_{9 \times 6} & {\frac{\partial \mathbf{r}_{ij}}{\partial \delta \mathbf{v}_{j} }} _{9 \times 3}\end{bmatrix}_{9 \times 9}$
$\mathbf{H}$