ORB_SLAM2： Optimizer::PoseOptimization

ORB_SLAM2中的位姿优化问题可以形容为：已知一个地图点的三维世界坐标X_w，已知它在当前图像中的二维像素坐标(u,v)，把"当前相机位姿T_cw"作为唯一待优化变量，通过最小化3D点投影后的像素位置与实际观测像素位置之间的误差，优化当前帧的相机位姿。

一、建立优化概念

函数含义

cpp 复制代码

int Optimizer::PoseOptimization(Frame *pFrame)

优化目标 ：仅优化当前帧的6自由度位姿 Tcw（固定所有地图点的3D坐标）

输入：Frame* pFrame（包含特征点、地图点关联、相机参数等）

输出：优化后的位姿（直接修改pFrame），返回值是有效内点数量

阶段一：初始化给g2o优化器

cpp 复制代码

    g2o::SparseOptimizer optimizer;   //SparseOptimizer：g2o的核心图容器，管理所有的顶点和边
    //BlockSolver_6_3: 求解器模板参数；6：位姿顶点维度；3：地图点顶点维度（3D坐标，此处未使用）
    g2o::BlockSolver_6_3::LinearSolverType * linearSolver;

    //使用稠密矩阵求解线性系统，（适合小规模问题）
    linearSolver = new g2o::LinearSolverDense<g2o::BlockSolver_6_3::PoseMatrixType>(); 

    g2o::BlockSolver_6_3 * solver_ptr = new g2o::BlockSolver_6_3(linearSolver);

    //使用Levenberg-Marquardt算法进行优化
    //（结合高斯牛顿和梯度下降的优点）
    g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr);
    optimizer.setAlgorithm(solver);

变量	类型	作用
`SparseOptimizer`	g2o核心容器	管理所有顶点和边，执行优化
`BlockSolver_6_3`	求解器模板	6 ：位姿顶点维度（SE(3)，6自由度） 3：地图点顶点维度（3D坐标，此处未使用）
`LinearSolverDense`	线性求解器	使用稠密矩阵求解增量方程（适合小规模问题）（若大规模BA会用`LinearSolverCSparse`）
`OptimizationAlgorithmLevenberg`	优化算法	Levenberg-Marquardt 算法结合高斯牛顿（快）和梯度下降（稳）的优点

为什么用稠密求解器？

此处只优化1个位姿顶点，问题规模极小（6×6矩阵）
稠密求解器简单快速，稀疏求解器反而有额外开销

为什么用LM算法？

高斯牛顿法在接近最优解时收敛快，但初始值不好时容易发散
梯度下降法稳健但收敛慢
LM算法自动调节两者权重，既稳健又高效

阶段二：添加位姿顶点

cpp 复制代码

g2o::VertexSE3Expmap * vSE3 = new g2o::VertexSE3Expmap();

在优化变量中我们只添加了一个顶点：VertexSE3Expmap这个顶点代表一个6自由度的相机位姿（3个平移+3个旋转）。SE3是特使欧几里得群，Expmap表示它用"李代数"来存储和更新。

cpp 复制代码

vSE3->setEstimate(Converter::toSE3Quat(pFrame->mTcw));

setEstimate：给顶点一个初始值

cpp 复制代码

vSE3->setId(0);  //顶点ID为0
vSE3->setFixed(false);  //告诉优化器，这个顶点是可移动的（需要被优化）
optimizer.addVertex(vSE3);

阶段三：准备边的容器

cpp 复制代码

vector<g2o::EdgeSE3ProjectXYZOnlyPose*> vpEdgesMono;
vector<size_t> vnIndexEdgeMono;
vpEdgesMono.reserve(N);
vnIndexEdgeMono.reserve(N);

vector<g2o::EdgeStereoSE3ProjectXYZOnlyPose*> vpEdgesStereo;
vector<size_t> vnIndexEdgeStereo;
vpEdgesStereo.reserve(N);
vnIndexEdgeStereo.reserve(N);

const float deltaMono = sqrt(5.991);   // 单目Huber核阈值
const float deltaStereo = sqrt(7.815); // 双目Huber核阈值

容器	存储内容	用途
`vpEdgesMono`	单目边指针	存储所有单目观测的优化边
`vnIndexEdgeMono`	特征点索引	记录每条边对应的特征点索引，方便后续更新外点标志
`vpEdgesStereo`	双目边指针	存储所有双目观测的优化边
`vnIndexEdgeStereo`	特征点索引	记录双目边的特征点索引

Huber核阈值（卡方分布）

观测类型	自由度	阈值	物理意义
单目	2（u, v）	`sqrt(5.991)` ≈ 2.45	卡方分布95%置信区间
双目	3（u, v, u_right）	`sqrt(7.815)` ≈ 2.80	卡方分布95%置信区

Huber核的作用：当残差大于阈值时，从二次函数切换为线性函数，降低外点的影响。

阶段四：创建边

4.1 加锁保护地图点

cpp 复制代码

unique_lock<mutex> lock(MapPoint::mGlobalMutex);

地图点可能被其他线程（如局部建图线程）修改
使用互斥锁保证数据一致性

4.2 单目观测边

cpp 复制代码

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

mvKeysUn 是当前帧的去畸变后的关键点。取出关键点的像素坐标并放入Eigen向量。这是实际图像的观测值。

cpp 复制代码

g2o::EdgeSE3ProjectXYZOnlyPose* e = new g2o::EdgeSE3ProjectXYZOnlyPose();

创建一个EdgeSE3ProjectXYZOnlyPose，可以把它理解成：一个"3D地图点 → 2D图像像素"的单目重投影误差约束。

cpp 复制代码

e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(0)));

optimizer.vertex(0)：优化器中的顶点（当前相机的位姿）
e->setVertex(0, ...)：把这条边的第0个顶点连接到当前相机的位姿

cpp 复制代码

e->setMeasurement(obs);

设置观测值

cpp 复制代码

const float invSigma2 = pFrame->mvInvLevelSigma2[kpUn.octave];
e->setInformation(Eigen::Matrix2d::Identity()*invSigma2);

设置尺度对应的不确定性+信息矩阵（可以理解为kalman中的R阵）

这一行与 ORB 的图像金字塔有关。ORB 特征通常是在不同尺度上提取的：

复制代码

Level 0
Level 1
Level 2
Level 3
...

每个关键点的 kpUn.octave 记录它来自哪个金字塔层。

例如：kpUn.octave = 0表示：原始图像尺度。如果：kpUn.octave =3 表示：在更低分辨率的图像层提取出来的。

cpp 复制代码

g2o::RobustKernelHuber* rk =new g2o::RobustKernelHuber;
e->setRobustKernel(rk);  //将Huber核添加到边
rk->setDelta(deltaMono);  //设置Huber的阈值

创建Huber鲁棒核

cpp 复制代码

e->fx = pFrame->fx; 
e->fy = pFrame->fy;
e->cx = pFrame->cx;
e->cy = pFrame->cy;

设置相机内参

cpp 复制代码

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

设置地图点世界坐标

cpp 复制代码

optimizer.addEdge(e);
vpEdgesMono.push_back(e);
vnIndexEdgeMono.push_back(i);

把边加入优化器
保存指针

4.3 双目观测边

双目观测边有三个观测量，单目观测有2个观测量。

为什么双目只有三个观测量？

ORB-SLAM2 使用的是：经过立体校正（Rectification）的双目图像。

校正以后

复制代码

左图                     右图

● 特征点                  ● 特征点
(uL,v)                    (uR,v)
   │                         │
   │                         │
   └──────── 同一水平线 ──────┘

因此：vL≈vR 。所以只需要：三个量。

单目和双目区别

阶段五：匹配点检测

cpp 复制代码

if(nInitialCorrespondences<3)
        return 0;

为什么至少需要 3 个？

因为这里需要估计相机的 6DoF 位姿：Tcw∈SE(3)包括：tx,ty,tz 以及：rx,ry,rz,如果对应点太少，位姿约束不足。

阶段六：定义单目/双目卡方阈值、迭代次数

cpp 复制代码

const float chi2Mono[4]={5.991,5.991,5.991,5.991};
const float chi2Stereo[4]={7.815,7.815,7.815, 7.815};
const int its[4]={10,10,10,10};

阶段七：进入四次迭代优化

cpp 复制代码

vSE3->setEstimate(Converter::toSE3Quat(pFrame->mTcw)); //重置初始估计值

为什么每轮都要重新设置？

轮次	目的	原因
第1轮	使用初始位姿	还没有任何优化结果
第2-4轮	使用更新后的位姿	`pFrame->mTcw` 在上一轮优化后已被更新（最后一行 `pFrame->SetPose(pose)`）

注意：pFrame->mTcw 在优化后被更新，所以下一轮开始前需要将新的位姿重新设置到顶点中，确保优化从当前最佳估计开始。

cpp 复制代码

optimizer.initializeOptimization(0);

参数 0 的含义 ：指定要优化的层（Level）。

g2o 的层级机制：

setLevel(0)：该边参与优化（内点）
setLevel(1)：该边不参与优化（外点）

initializeOptimization(0) 的作用：

收集所有 setLevel(0) 的边
构建优化问题的稀疏结构（雅可比矩阵、海森矩阵）
准备好增量方程，等待求解

cpp 复制代码

optimizer.optimize(its[it]);  // its[it] = 10

optimizer.optimize(its $it$ )真正开始执行优化

cpp 复制代码

for(size_t i=0, iend=vpEdgesMono.size(); i<iend; i++)
{
     g2o::EdgeSE3ProjectXYZOnlyPose* e = vpEdgesMono[i];

     const size_t idx = vnIndexEdgeMono[i];

     if(pFrame->mvbOutlier[idx])
     {
         e->computeError();
     }
     const float chi2 = e->chi2();  //这个观测的归一化重投影误差平方。
     if(chi2>chi2Mono[it])  //判断是不是坏点
     {                
          pFrame->mvbOutlier[idx]=true;
          e->setLevel(1);
          nBad++;
      }
      else
      {
           pFrame->mvbOutlier[idx]=false;
           e->setLevel(0);
      }
      if(it==2)
           e->setRobustKernel(0);  //移除鲁棒核
}
//后边是相同的原理对双目误差边处理

阶段八：结果存储

cpp 复制代码

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