G2o顶点编程-优快云博客

本文链接：https://blog.youkuaiyun.com/qq_44691564/article/details/146213088

稀疏优化器原理：

稀疏优化器是基于图的优化框架，主要用于处理大规模稀疏问题。它将优化问题建模为一个图，图中的节点表示优化变量（顶点），边表示变量之间的约束关系（误差项）。通过最小化所有误差项的加权和，求解变量的最优估计值。在计算机视觉和机器人领域，稀疏优化被广泛应用于_bundle adjustment_（捆绑调整）

在Slam的图优化中，顶点包括相机的位姿（世界坐标系中的位置和方向）以及场景中物体的三维坐标。
边表示3D点与相机之间的投影关系，每条边连接一个3D点顶点和一个相机位姿顶点，表示该3D点在相机成像平面上的投影观测。边的优化目标是最小化观测值（图像上的2D点）与根据当前估计的相机位姿和3D点投影得到的2D点之间的误差。

通俗的说，我们通过相机去估计真实场景中物体的位置，那么，我们的目标就是让我们估计的3D点位置映射到相机成像平面的结果尽可能和我们的相机成像平面上的观测位置接近，这意味着我们估计的3D点位置与它的真实位置无限靠近。

根据之前学习的相机成像模型：

在相机坐标系下：
在这里插入图片描述
转换为图像坐标系下：

边的优化就是最小化实际得到的相机位姿与根据3D点投影得到2D坐标的误差

现在我们展示代码(代码来自计算机视觉life)，题目为：

给定一组世界坐标系下的3D点(p3d.txt)以及它在相机中对应的坐标(p2d.txt)，以及相机的内参矩阵。使用bundle adjustment 方法（g2o库实现）来估计相机的位姿T。初始位姿T为单位矩阵。
代码：

void bundleAdjustment (
        const vector< Point3f > points_3d,
        const vector< Point2f > points_2d,
        Mat& K   )
{
    // creat g2o
    // new g2o version. Ref:https://www.cnblogs.com/xueyuanaichiyu/p/7921382.html

    // pose 维度为 6, landmark 维度为 3,相机的位姿（pose）由旋转和平移组成。
    // 旋转可以用3个参数（如欧拉角）表示，平移也需要3个参数，因此总共有6个自由度。
    using Block = g2o::BlockSolver< g2o::BlockSolverTraits<6,3> > ;  
    
    // 第1步：创建一个线性求解器LinearSolver
    Block::LinearSolverType* linearSolver = new g2o::LinearSolverCSparse<Block::PoseMatrixType>();

    // 第2步：创建BlockSolver。并用上面定义的线性求解器初始化
    Block* solver_ptr = new Block (  std::unique_ptr<Block::LinearSolverType>(linearSolver) );

    // 第3步：创建总求解器solver。并从GN, LM, DogLeg 中选一个，再用上述块求解器BlockSolver初始化
    g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg ( std::unique_ptr<Block>(solver_ptr) );

    // 第4步：创建稀疏优化器
    g2o::SparseOptimizer optimizer;
    optimizer.setAlgorithm ( solver );

    // 第5步：定义图的顶点和边。并添加到SparseOptimizer中

    // 添加相机位姿顶点，初始位姿为单位矩阵
    g2o::VertexSE3Expmap* pose = new g2o::VertexSE3Expmap();
    pose->setId(0);
    pose->setEstimate(Eigen::Isometry3d::Identity());
    optimizer.addVertex(pose);

    // 添加3D点顶点
    for (size_t i = 0; i < points_3d.size(); ++i) {
        g2o::VertexSBAPointXYZ* point = new g2o::VertexSBAPointXYZ();
        point->setId(i + 1); // 顶点ID从1开始
        point->setEstimate(Eigen::Vector3d(points_3d[i].x, points_3d[i].y, points_3d[i].z));
        optimizer.addVertex(point);
    }

    // 设置相机内参
    g2o::CameraParameters* camera = new g2o::CameraParameters (
            K.at<double> ( 0,0 ), Eigen::Vector2d ( K.at<double> ( 0,2 ), K.at<double> ( 1,2 ) ), 0);
    camera->setId ( 0 );
    optimizer.addParameter ( camera );

    // 设置边
    for (size_t i = 0; i < points_2d.size(); ++i) {
    
        // EdgeProjectXYZ2UV 是一个自定义边类，用于表示从三维点到二维图像平面的投影关系。
        // 它的作用是将三维点通过相机的投影模型映射到二维图像平面上。
        g2o::EdgeProjectXYZ2UV* edge = new g2o::EdgeProjectXYZ2UV();  
        
        edge->setId(i);
        edge->setVertex(0, dynamic_cast<g2o::VertexSBAPointXYZ*>(optimizer.vertex(i + 1))); // 对应的3D点顶点
        edge->setVertex(1, pose); // 相机位姿顶点
        edge->setMeasurement(Eigen::Vector2d(points_2d[i].x, points_2d[i].y)); // 设置观测值
        edge->setParameterId(0, 0); // 设置相机内参参数ID
        edge->setInformation(Eigen::Matrix2d::Identity()); // 设置信息矩阵，这里用单位矩阵
        optimizer.addEdge(edge);
    }

    // 第6步：设置优化参数，开始执行优化
    optimizer.setVerbose ( false );
    optimizer.initializeOptimization();
    optimizer.optimize ( 100 );

    // 输出优化结果
    cout<<endl<<"after optimization:"<<endl;
    cout<<"T="<<endl<<Eigen::Isometry3d ( pose->estimate() ).matrix() <<endl;
}

顶点的类型

VertexSE2 : public BaseVertex<3, SE2>  //2D pose Vertex, (x,y,theta)
VertexSE3 : public BaseVertex<6, Isometry3>  //6d vector (x,y,z,qx,qy,qz) (note that we leave out the w part of the quaternion)
VertexPointXY : public BaseVertex<2, Vector2>
VertexPointXYZ : public BaseVertex<3, Vector3>
VertexSBAPointXYZ : public BaseVertex<3, Vector3>

// SE3 Vertex parameterized internally with a transformation matrix and externally with its exponential map
VertexSE3Expmap : public BaseVertex<6, SE3Quat>

// SBACam Vertex, (x,y,z,qw,qx,qy,qz),(x,y,z,qx,qy,qz) (note that we leave out the w part of the quaternion.
// qw is assumed to be positive, otherwise there is an ambiguity in qx,qy,qz as a rotation
VertexCam : public BaseVertex<6, SBACam>

// Sim3 Vertex, (x,y,z,qw,qx,qy,qz),7d vector,(x,y,z,qx,qy,qz) (note that we leave out the w part of the quaternion.
VertexSim3Expmap : public BaseVertex<7, Sim3>

VertexSE2
应用场景：用于二维空间中的位姿估计，例如二维机器人定位或二维地图构建。它表示一个2D位姿，包含平移（x, y）和旋转（theta）。
常见用途：在2D SLAM（Simultaneous Localization and Mapping）中，用于表示机器人（相机位姿）的位置和方向。
VertexSE3
应用场景：用于三维空间中的位姿估计，例如三维机器人定位或三维地图构建。它表示一个3D位姿，包含平移（x, y, z）和旋转（用四元数的三个分量表示，qw部分通过模长为1约束计算）。
常见用途：在3D SLAM中，用于表示机器人（相机位姿）的位置和方向。
VertexPointXY
应用场景：用于二维空间中的点估计，例如二维地图中的地标点或路径规划中的二维点。
常见用途：在2D SLAM中，用于表示地图中的固定点。
VertexPointXYZ
应用场景：用于三维空间中的点估计，例如三维地图中的地标点或物体的三维位置。
常见用途：在3D SLAM或三维重建中，用于表示地图中的固定点。
VertexSBAPointXYZ
应用场景：与VertexPointXYZ类似，但通常用于稀疏bundle adjustment（稀疏光束法优化）场景中，用于表示三维点。
常见用途：在视觉SLAM或三维重建中，用于优化相机和三维点之间的关系。
VertexSE3Expmap
应用场景：与VertexSE3类似，但内部使用变换矩阵参数化，外部使用指数映射参数化。这种方式在某些优化算法中可能更高效。
常见用途：在3D SLAM中，用于优化位姿，特别是在需要高效计算的场景中。
VertexSim3Expmap
应用场景：用于表示相似变换（Sim3），包含平移、旋转和缩放。它通常用于需要考虑尺度变化的优化问题。
常见用途：在多视图几何或三维重建中，用于处理不同尺度下的位姿优化。
VertexCam
应用场景：用于表示相机的位姿和内参，通常用于视觉SLAM或视觉定位中。它包含平移（x, y, z）和旋转（四元数表示），但省略了四元数的w部分。
常见用途：在视觉SLAM中，用于优化相机的位姿和内参。