g2o中Bundle Adjustment阶段如何优化相机内参(fx,fy,cx,cy)

在Bundle Adjustment（BA）流程中，EdgeProjectXYZ2UV、EdgeSE3ProjectXYZ这类预定义边被广泛使用，但它们仅支持固定相机参数的设置。我想知道是否可以设置可优化的相机内参。

我在g2o/types/sba/vertex_intrinsics.h中找到了VertexIntrinsics顶点，它的oplusImpl()函数已经实现，说明这个顶点是可以被优化的：

/**
 *  brief Vertex encoding the intrinsics of the camera fx, fy, cx, xy, baseline;
 */
class G2O_TYPES_SBA_API VertexIntrinsics : public BaseVertex<4, Eigen::Matrix<number_t, 5, 1, Eigen::ColMajor> >

但我找不到能连接该顶点、相机位姿顶点和3D世界点顶点的三元边，也没有找到适配该顶点的其他边类型。希望有人能分享这个顶点的使用示例、原理说明，或是g2o中优化相机内参的其他方法。

可行解决方案 #

1. 自定义适配VertexIntrinsics的三元边 #

g2o默认没有提供直接关联VertexIntrinsics、相机位姿顶点（如VertexSE3Expmap）和3D点顶点（如VertexSBAPointXYZ）的三元边，你可以自行实现：

• 边需继承BaseMultiEdge，模板参数设为误差维度（单目设为2，对应UV坐标）和误差类型（如Eigen::Vector2d）
• 在computeError()函数中，分别获取三个顶点的当前估计值，按照相机投影模型计算预测的UV坐标，与观测值做差得到误差
• 在linearizeOplus()函数中，推导并实现三个顶点对应的雅可比矩阵（可参考EdgeProjectXYZ2UV的雅可比推导逻辑，扩展内参的导数部分）

以下是一个简化的自定义边示例：

class EdgeXYZIntrinsicsSE3UV : public BaseMultiEdge<2, Eigen::Vector2d> {
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    virtual void computeError() override {
        // 获取三个顶点的估计值
        const VertexSBAPointXYZ* v_xyz = static_cast<const VertexSBAPointXYZ*>(_vertices[0]);
        const VertexSE3Expmap* v_se3 = static_cast<const VertexSE3Expmap*>(_vertices[1]);
        const VertexIntrinsics* v_intr = static_cast<const VertexIntrinsics*>(_vertices[2]);
        
        // 3D点从世界坐标系转换到相机坐标系
        Eigen::Vector3d xyz_cam = v_se3->estimate().map(v_xyz->estimate());
        
        // 提取内参参数
        double fx = v_intr->estimate()(0);
        double fy = v_intr->estimate()(1);
        double cx = v_intr->estimate()(2);
        double cy = v_intr->estimate()(3);
        
        // 计算投影后的UV坐标
        Eigen::Vector2d uv_pred(fx * xyz_cam(0)/xyz_cam(2) + cx, fy * xyz_cam(1)/xyz_cam(2) + cy);
        // 计算误差
        _error = uv_pred - _measurement;
    }

    virtual void linearizeOplus() override {
        // 手动推导并实现对三个顶点的雅可比矩阵
        // 参考g2o内置投影边的雅可比实现，补充内参部分的导数
    }
};

2. 使用g2o内置的集成式相机顶点 #

如果不想自定义边，可以直接使用g2o针对SBA场景提供的集成顶点：

• 单目相机：使用VertexCam（定义在g2o/types/sba/types_sba.h），该顶点同时包含相机位姿和内参，搭配EdgeProjectP2MC等边即可实现位姿、内参与3D点的联合优化
• 双目相机：使用VertexStereoCam，同样集成了位姿和内参，搭配EdgeStereoProjXYZ边完成双目场景的内参优化

3. VertexIntrinsics的基础使用步骤 #

若坚持使用VertexIntrinsics，可按以下步骤操作：

1. 添加内参顶点到优化器：

// 初始化内参：fx, fy, cx, cy，最后一位是baseline（单目设为0）
VertexIntrinsics* v_intrinsics = new VertexIntrinsics();
v_intrinsics->setEstimate(Eigen::Matrix<double,5,1>(800.0, 800.0, 320.0, 240.0, 0.0));
v_intrinsics->setId(0); // 分配唯一ID
optimizer.addVertex(v_intrinsics);

2. 创建并添加自定义三元边：

EdgeXYZIntrinsicsSE3UV* edge = new EdgeXYZIntrinsicsSE3UV();
// 关联3D点、位姿、内参三个顶点
edge->setVertex(0, dynamic_cast<VertexSBAPointXYZ*>(optimizer.vertex(point_id)));
edge->setVertex(1, dynamic_cast<VertexSE3Expmap*>(optimizer.vertex(pose_id)));
edge->setVertex(2, dynamic_cast<VertexIntrinsics*>(optimizer.vertex(0)));
// 设置观测到的UV坐标
edge->setMeasurement(Eigen::Vector2d(uv_x, uv_y));
// 设置信息矩阵
edge->setInformation(Eigen::Matrix2d::Identity());
optimizer.addEdge(edge);

3. 配置优化器并执行优化：

optimizer.setVerbose(true);
optimizer.initializeOptimization();
optimizer.optimize(20);

内容的提问来源于stack exchange，提问作者vicky Lin