引子
g2o (General Graph Optimization) 是基于图优化的库，不同于 ceres 的“自动求导+残差块”不同，g2o从”图论“的视角来描述优化问题
核心架构
g2o的求解器初始化非常繁琐
![[g2o.png]]
先从整体上看，整个 g2o 框架可以分为上下两部分，以 SparseOpyimizer 为核心
配置一个 g2o 求解器，需要由内向外来进行
LinearSolver（线性求解器）- 负责求解 H Δ x = − b H\Delta x=-b HΔx=−b
- 求解方法：
LinearSolverCholmod，LinearSolverCSparse，LinearSolverEigen，LinearSolverDense - 通常选用
Cholmod和CSparse处理稀疏矩阵
BlockSolver（块求解器）
- 负责求解 H H H 和 b b b 的矩阵块（雅可比乘法）
- 定义：BlockSolver<BlockSolverTraits<PoseDim, LandmarkDim>>
- 参数：位姿维度和路标维度
- BA :<6, 3>（6 维位姿，3维点），也可以使用BlockSolver_6_3
- 位姿图优化：<6, 6>（只有位姿）OptimizationAlgorithm（迭代算法）
- 确定下降策略（GN，LM，Dogleg）
- 类型：OptimizationAlgorithmGaussNewton，OptimizationAlgorithmLevenberg，OptimizationAlgorithmDoglegSparseOptimizer（稀疏优化器）
- 最终的图模型
重要的类和函数
顶点 (Vertex)
继承自 BaseVertex<D, T>
D：变量维度（int）T：变量的数据类型（Eigen::Vector3d，Sophus::SE3d）
需要实现的方法：setToOriginImpl()：重置顶点为初始值oplusImpl(const double* update)：更新策略。定义优化过程中增量 Δ x \Delta x Δx 的计算（比如使用普通加法或者李代数乘法）
边（Edge）
继承自 BaseUnaryEdge （一元边）、BaseBinaryEdge （二元边）、BaseMultiEdge （多元边）
以最常见的二元边为例，其参数有：
D：观测值维度（int）E：观测值类型VertexXi、VertexXj：不同顶点的类型
需要实现的方法：compute：计算残差。 e = meas − f ( x ) e=\text{meas}-f(x) e=meas−f(x)linearizeOplus：计算雅可比
代码实例
曲线拟合
#include <iostream>
#include <eigen3/Eigen/Core>
#include <vector>
#include <opencv4/opencv2/core.hpp>
#include <cmath>
#include <chrono>
#include <g2o/core/g2o_core_api.h>
#include <g2o/core/base_unary_edge.h>
#include <g2o/core/base_vertex.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/solvers/dense/linear_solver_dense.h>
// 1.定义顶点
class CurveFittingVertex : public g2o::BaseVertex<3, Eigen::Vector3d> {
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
// 重置
virtual void setToOriginImpl() override
{
_estimate << 0, 0, 0;
}
// 更新 x_new = x_old + update
virtual void oplusImpl(const double* update) override
{
_estimate += Eigen::Vector3d(update);
}
// 读写
virtual bool read(std::istream& in) override { return true; }
virtual bool write(std::ostream& out) const override { return true; }
};
// 2.定义边
class CurveFittingEdge : public g2o::BaseUnaryEdge<1, double, CurveFittingVertex> {
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
CurveFittingEdge(double x) : BaseUnaryEdge(), _x(x) {}
// 计算残差函数
virtual void computeError() override
{
// 获取连接的顶点
const CurveFittingVertex* v = static_cast<const CurveFittingVertex*>(_vertices[0]);
const Eigen::Vector3d& abc = v->estimate();
// 误差 = 观测值 - 预测值
_error(0, 0) = _measurement - std::exp(abc(0) * _x * _x + abc(1) * _x + abc(2));
}
// 计算雅可比矩阵
virtual void linearizeOplus() override
{
const CurveFittingVertex* v = static_cast<const CurveFittingVertex*>(_vertices[0]);
const Eigen::Vector3d& abc = v->estimate();
double y = std::exp(abc(0) * _x * _x + abc(1) * _x + abc(2));
_jacobianOplusXi(0, 0) = -_x * _x * y; // 对a求导
_jacobianOplusXi(0, 1) = -_x * y; // 对b求导
_jacobianOplusXi(0, 2) = -y; // 对c求导
}
virtual bool read(std::istream& in) override { return true; }
virtual bool write(std::ostream& out) const override { return true; }
public:
double _x;
};
int main()
{
double ar = 1.0, br = 2.0, cr = 1.0; // 真实参数值
double ae = 2.0, be = -1.0, ce = 5.0; // 估计参数初值
int N = 100; // 数据点
double w_sigma = 1.0; // 噪声Sigma值
double inv_sigma = 1.0 / w_sigma;
cv::RNG rng; // OpenCV随机数产生器
std::vector<double> x_data, y_data;
for (int i = 0; i < N; i++)
{
double x = i / 100.0;
x_data.push_back(x);
double y = std::exp(ar * x * x + br * x + cr) + rng.gaussian(w_sigma * w_sigma);
y_data.push_back(y);
}
// 构建图优化
// 1. 构建 BlockSolver 块求解器
typedef g2o::BlockSolver<g2o::BlockSolverTraits<3, 1>> BlockSolverType;
// 2. 构建线性求解器
typedef g2o::LinearSolverDense<BlockSolverType::PoseMatrixType> LinearSolverType;
// 3. 设定算法 Algorithm
auto solver = std::make_unique<g2o::OptimizationAlgorithmLevenberg>(
std::make_unique<BlockSolverType>(std::make_unique<LinearSolverType>())
);
// 4. 创建稀疏优化器
g2o::SparseOptimizer optimizer;
optimizer.setAlgorithm(solver.release());
optimizer.setVerbose(true); // 打开调试输出
// 5. 添加顶点
CurveFittingVertex* v = std::make_unique<CurveFittingVertex>().release();
v->setId(0);
v->setEstimate(Eigen::Vector3d(ae, be, ce)); // 设置初始值
optimizer.addVertex(v);
// 6. 添加边
for (int i = 0; i < N; i++)
{
CurveFittingEdge* edge = std::make_unique<CurveFittingEdge>(x_data[i]).release();
edge->setId(i);
edge->setVertex(0, optimizer.vertex(0)); // 连接顶点
edge->setMeasurement(y_data[i]); // 观测值
edge->setInformation(Eigen::Matrix<double, 1 ,1>::Identity() * inv_sigma * inv_sigma); // 信息矩阵
optimizer.addEdge(edge);
}
// 7. 执行优化
std::cout << "Start optimization" << std::endl;
std::chrono::steady_clock::time_point t1 = std::chrono::steady_clock::now();
optimizer.initializeOptimization();
optimizer.optimize(100);
std::chrono::steady_clock::time_point t2 = std::chrono::steady_clock::now();
std::chrono::duration<double> time_used = std::chrono::duration_cast<std::chrono::duration<double>>(t2 - t1);
std::cout << "Optimization costs time: " << time_used.count() << " seconds." << std::endl;
std::cout << "Estimated model: " << v->estimate().transpose() << std::endl;
}
PnP
#include <iostream>
#include <eigen3/Eigen/Core>
#include <vector>
#include <opencv4/opencv2/core.hpp>
#include <sophus/se3.hpp>
#include <g2o/core/g2o_core_api.h>
#include <g2o/core/base_unary_edge.h>
#include <g2o/core/base_vertex.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/solvers/dense/linear_solver_dense.h>
// 相机内参
double fx = 520.9, fy = 521.0, cx = 325.1, cy = 249.7;
// 1. 定义顶点：相机位姿
class VertexPose : public g2o::BaseVertex<6, Sophus::SE3d> {
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
virtual void setToOriginImpl() override
{
_estimate = Sophus::SE3d();
}
virtual void oplusImpl(const double* update) override
{
Eigen::Map<const Eigen::Matrix<double, 6, 1>> update_eigen(update);
// T_new = exp(update) * T_old
_estimate = Sophus::SE3d::exp(update_eigen) * _estimate;
}
virtual bool read(std::istream& in) override { return true; }
virtual bool write(std::ostream& out) const override { return true; }
};
// 2. 定义边：重投影误差
class EdgeProjection : public g2o::BaseUnaryEdge<2, Eigen::Vector2d, VertexPose> {
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
EdgeProjection(const Eigen::Vector3d& pos) : _pos3d(pos) {}
virtual void computeError() override
{
const VertexPose* v = static_cast<const VertexPose*>(_vertices[0]);
Sophus::SE3d T = v->estimate();
// 世界坐标系转换到相机坐标系
Eigen::Vector3d pos_cam = T * _pos3d;
// 投影到像素平面
pos_cam /= pos_cam[2];
double u = fx * pos_cam[0] + cx;
double v_pix = fy * pos_cam[1] + cy;
// 误差 = 观测 - 预测
_error = _measurement - Eigen::Vector2d(u, v_pix);
}
// 计算雅可比
virtual void linearizeOplus() override
{
const VertexPose* pose_vertex = static_cast<const VertexPose*>(_vertices[0]);
Sophus::SE3d T = pose_vertex->estimate();
Eigen::Vector3d pos_cam = T * _pos3d;
double X = pos_cam[0];
double Y = pos_cam[1];
double Z = pos_cam[2];
double Z2 = Z * Z;
_jacobianOplusXi <<
-fx / Z, 0, fx * X / Z2, fx * X * Y / Z2, -fx - fx * X * X / Z2, fx * Y / Z,
0, -fy / Z, fy * Y / Z2, fy + fy * Y * Y / Z2, -fy * X * Y / Z2, -fy * X / Z;
}
virtual bool read(std::istream& in) override { return true; }
virtual bool write(std::ostream& out) const override { return true; }
private:
Eigen::Vector3d _pos3d; // 3D点在世界坐标系中的位置
};
int main()
{
std::vector<Eigen::Vector3d> points_3d;
std::vector<Eigen::Vector2d> points_2d;
Eigen::Matrix3d K;
K << fx, 0, cx,
0, fy, cy,
0, 0, 1;
// 真实位姿
Sophus::SE3d pose_true(Eigen::Quaterniond(0.92388, 0, 0, 0.38268), Eigen::Vector3d(0.5, 0, 1.0));
// 生成观测点
for (int x = -2; x <= 2; x++)
{
for (int y = -2; y <= 2; y++)
{
for (int z = 2; z <= 4; z++)
{
Eigen::Vector3d p_w(x, y, z);
Eigen::Vector3d p_c = pose_true * p_w;
// 投影
Eigen::Vector2d p_uv( fx * p_c[0] / p_c[2] + cx,
fy * p_c[1] / p_c[2] + cy );
points_3d.push_back(p_w);
points_2d.push_back(p_uv);
}
}
}
// 构建g2o优化器
// 1. BlockSolver
// PoseDim = 6, LandMarkDim = 1(没有优化点，所以是一元边)
typedef g2o::BlockSolver<g2o::BlockSolverTraits<6, 1>> BlockSolverType;
// 2. LinearSolver
typedef g2o::LinearSolverDense<BlockSolverType::PoseMatrixType> LinearSolverType;
// 3. 算法配置
auto solver = std::make_unique<g2o::OptimizationAlgorithmLevenberg>(
std::make_unique<BlockSolverType>(std::make_unique<LinearSolverType>())
);
// 4. 优化器
g2o::SparseOptimizer optimizer;
optimizer.setAlgorithm(solver.release());
optimizer.setVerbose(true);
// 5. 添加位姿顶点
VertexPose* vertex_pose = std::make_unique<VertexPose>().release();
vertex_pose->setId(0);
vertex_pose->setEstimate(Sophus::SE3d(Eigen::Quaterniond(1,0,0,0), Eigen::Vector3d(0,0,0))); // 初始值设为单位元
optimizer.addVertex(vertex_pose);
// 6. 添加边
for (int i = 0; i < points_3d.size(); i++)
{
EdgeProjection* edge = std::make_unique<EdgeProjection>(points_3d[i]).release();
edge->setId(i);
edge->setVertex(0, vertex_pose);
edge->setMeasurement(points_2d[i]);
edge->setInformation(Eigen::Matrix2d::Identity());
optimizer.addEdge(edge);
}
// 7. 优化
std::cout << "Start optimization" << std::endl;
optimizer.initializeOptimization();
optimizer.optimize(100);
// 输出结果
Sophus::SE3d pose_estimated = vertex_pose->estimate();
std::cout << "Estimated pose: \n" << pose_estimated.matrix() << std::endl;
std::cout << "True pose: \n" << pose_true.matrix() << std::endl;
}
BA
#include <iostream>
#include <eigen3/Eigen/Core>
#include <vector>
#include <opencv4/opencv2/core.hpp>
#include <sophus/se3.hpp>
#include <g2o/core/g2o_core_api.h>
#include <g2o/core/base_binary_edge.h>
#include <g2o/core/base_vertex.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/solvers/dense/linear_solver_dense.h>
double fx = 520.9, fy = 521.0, cx = 325.1, cy = 249.7;
// 1. 定义顶点：相机位姿
class VertexPose : public g2o::BaseVertex<6, Sophus::SE3d> {
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
virtual void setToOriginImpl() override
{
_estimate = Sophus::SE3d();
}
// T_new = exp(update) * T_old
virtual void oplusImpl(const double* update) override
{
Eigen::Map<const Eigen::Matrix<double, 6, 1>> update_eigen(update);
_estimate = Sophus::SE3d::exp(update_eigen) * _estimate;
}
virtual bool read(std::istream& in) override { return true; }
virtual bool write(std::ostream& out) const override { return true; }
};
// 2. 定义路标点顶点
class VertexPoint : public g2o::BaseVertex<3, Eigen::Vector3d> {
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
virtual void setToOriginImpl() override
{
_estimate = Eigen::Vector3d::Zero();
}
// P_new = P_old + update
virtual void oplusImpl(const double* update) override
{
Eigen::Map<const Eigen::Vector3d> update_eigen(update);
_estimate += update_eigen;
}
virtual bool read(std::istream& in) override { return true; }
virtual bool write(std::ostream& out) const override { return true; }
};
class EdgeProjection : public g2o::BaseBinaryEdge<2, Eigen::Vector2d, VertexPose, VertexPoint> {
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
virtual void computeError() override
{
const VertexPose* v0 = static_cast<const VertexPose*>(_vertices[0]);
const VertexPoint* v1 = static_cast<const VertexPoint*>(_vertices[1]);
Sophus::SE3d T = v0->estimate();
Eigen::Vector3d pos_3d = v1->estimate();
// 1. 转换到相机坐标系
Eigen::Vector3d pos_cam = T * pos_3d;
// 2. 投影到像素平面
double inv_z = 1.0 / pos_cam[2];
double u = fx * pos_cam[0] * inv_z + cx;
double v = fy * pos_cam[1] * inv_z + cy;
// 3. 计算误差
_error = _measurement - Eigen::Vector2d(u, v);
}
// 手写雅可比
// _jacobianOplusXi: 位姿顶点的雅可比(2*6)
// _jacobianOplusXj: 路标点顶点的雅可比(2*3)
virtual void linearizeOplus() override
{
const VertexPose* v0 = static_cast<const VertexPose*>(_vertices[0]);
const VertexPoint* v1 = static_cast<const VertexPoint*>(_vertices[1]);
Sophus::SE3d T = v0->estimate();
Eigen::Vector3d pos_3d = v1->estimate();
Eigen::Vector3d pos_cam = T * pos_3d;
double X = pos_cam[0];
double Y = pos_cam[1];
double Z = pos_cam[2];
double Z2 = Z * Z;
double inv_Z = 1.0 / Z;
double inv_Z2 = inv_Z * inv_Z;
// 位姿顶点雅可比(平移在前，旋转在后)
_jacobianOplusXi <<
-fx * inv_Z, 0, fx * X * inv_Z2, fx * X * Y * inv_Z2, -fx - fx * X * X * inv_Z2, fx * Y * inv_Z,
0, -fy * inv_Z, fy * Y * inv_Z2, fy + fy * Y * Y * inv_Z2, -fy * X * Y * inv_Z2, -fy * X * inv_Z;
// 路标点顶点雅可比
Eigen::Matrix<double, 2, 3> j_cam_point;
j_cam_point <<
-fx * inv_Z, 0, fx * X * inv_Z2,
0, -fy * inv_Z, fy * Y * inv_Z2;
Eigen::Matrix3d R = T.rotationMatrix();
_jacobianOplusXj = j_cam_point * R;
}
virtual bool read(std::istream& in) override { return true; }
virtual bool write(std::ostream& out) const override { return true; }
};
int main()
{
// 1. 数据生成 (模拟 3 个相机观测 20 个点)
std::vector<Eigen::Vector3d> true_points;
std::vector<Sophus::SE3d> true_poses; // g2o 内部使用的位姿类型
// 生成 3 个相机位姿 (围绕原点半圆分布)
for (int i = 0; i < 3; i++)
{
double theta = i * M_PI / 4; // 0, 45, 90 度
Eigen::Matrix3d R;
// 相机朝向原点：简单的 LookAt 逻辑 (这里简化为绕Y轴旋转)
R = Eigen::AngleAxisd(theta - M_PI/2, Eigen::Vector3d(0,1,0)).toRotationMatrix();
Eigen::Vector3d t(5 * cos(theta), 0, 5 * sin(theta));
true_poses.push_back(Sophus::SE3d(R, t));
}
// 生成 20 个随机 3D 点 (在原点附近)
cv::RNG rng;
for (int i = 0; i < 20; ++i)
{
double x = rng.uniform(-1.0, 1.0);
double y = rng.uniform(-1.0, 1.0);
double z = rng.uniform(-1.0, 1.0);
true_points.push_back(Eigen::Vector3d(x, y, z));
}
// 2. 构建 g2o 优化器
typedef g2o::BlockSolver< g2o::BlockSolverTraits<6, 3> > BlockSolverType; // pose:6, landmark:3
typedef g2o::LinearSolverDense<BlockSolverType::PoseMatrixType> LinearSolverType;
auto solver = std::make_unique<g2o::OptimizationAlgorithmLevenberg>(
std::make_unique<BlockSolverType>(std::make_unique<LinearSolverType>())
);
g2o::SparseOptimizer optimizer;
optimizer.setAlgorithm(solver.release());
optimizer.setVerbose(true);
// 3. 添加顶点
// 添加相机位姿顶点
for (size_t i = 0; i < true_poses.size(); ++i)
{
VertexPose* v = std::make_unique<VertexPose>().release();
v->setId(i);
v->setEstimate(true_poses[i]);
if (i == 0) // 固定第一个相机位姿作为参考
v->setFixed(true);
optimizer.addVertex(v);
}
// 添加路标点顶点
for (size_t i = 0; i < true_points.size(); ++i)
{
VertexPoint* v = std::make_unique<VertexPoint>().release();
v->setId(static_cast<int>(i + true_poses.size())); // 路标点 ID 从相机数量开始编号
v->setEstimate(true_points[i]);
v->setMarginalized(true); // 路标点边缘化
optimizer.addVertex(v);
}
// 4. 添加边
for (size_t i = 0; i < true_poses.size(); ++i)
{
for (size_t j = 0; j < true_points.size(); ++j)
{
EdgeProjection* edge = std::make_unique<EdgeProjection>().release();
edge->setVertex(0, optimizer.vertex(static_cast<int>(i))); // 相机位姿顶点
edge->setVertex(1, optimizer.vertex(static_cast<int>(j + true_poses.size()))); // 路标点顶点
edge->setMeasurement(Eigen::Vector2d(100, 100));
edge->setInformation(Eigen::Matrix2d::Identity());
optimizer.addEdge(edge);
}
}
optimizer.initializeOptimization();
optimizer.optimize(10);
};
也可以直接#include <g2o/types/sba/types_six_dof_expmap.h> 调用官方预制的 BA 库
扫一扫
1万+
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
