视觉SLAM学习笔记：g2o位姿优化实战

🔧 一、g2o位姿优化核心概念

1. 图优化模型

   • 顶点（Vertex）：待优化的变量（如相机位姿 SE3、三维点坐标 PointXYZ）。
   • 边（Edge）：表示顶点间的约束关系（如重投影误差、里程计相对位姿）。
   • 示例：在SLAM中，相机位姿是顶点，重投影误差（3D点投影到2D的偏差）构成边。

2. 优化目标
   最小化所有边的误差平方和：
   $$\min \sum _i\Vert {\text{error}}_i{\Vert }^2$$
   常用算法：高斯-牛顿法（Gauss-Newton）或列文伯格-马夸尔特法（Levenberg-Marquardt）。

🛠️ 二、实战步骤详解（以单目BA为例）

步骤1：定义顶点

• 相机位姿顶点（6自由度，SE3）：

  class VertexPose : public g2o::BaseVertex<6, Sophus::SE3d> {
       
       
  public:
      virtual void setToOriginImpl