g2o实践笔记
本文旨在学习通过《SLAM十四讲》中曲线拟合和pnp的对比，梳理g2o求解优化问题的实现步骤和具体操作，对具体原理不做探讨，感兴趣的可以移步：
g2o学习笔记 - (jianshu.com)
理解图优化，一步步带你看懂g2o代码 - 云+社区 - 腾讯云 (tencent.com)
深入理解图优化与g2o：g2o篇 - 半闲居士 - 博客园 (cnblogs.com)
图优化分为9步：
- 重写顶点类
- 重写边类
- 创建BlockSolver
- 创建LinearSolver
- 创建总求解器Solver
- 创建稀疏优化器
- 在图中添加顶点
- 在图中添加边
- 设置优化参数，开始执行优化
图中包含顶点和边，简单地讲，顶点就是待优化的变量的，边就是链接顶点的约束条件
1.重写顶点类
1.1源代码对比
曲线拟合顶点定义：
class CurveFittingVertex : public g2o::BaseVertex<3, Eigen::Vector3d> {
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
// 重置
virtual void setToOriginImpl() override {
_estimate << 0, 0, 0;
}
// 更新
virtual void oplusImpl(const double *update) override {
_estimate += Eigen::Vector3d(update);
}
// 存盘和读盘：留空
virtual bool read(istream &in) {}
virtual bool write(ostream &out) const {}
};
pnp顶点定义：
class VertexPose : public g2o::BaseVertex<6, Sophus::SE3d> {
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
virtual void setToOriginImpl() override {
_estimate = Sophus::SE3d();
}
/// left multiplication on SE3
virtual void oplusImpl(const double *update) override {
Eigen::Matrix update_eigen;
update_eigen << update[0], update[1], update[2], update[3], update[4], update[5];
_estimate = Sophus::SE3d::exp(update_eigen) * _estimate;
}
virtual bool read(istream &in) override {}
virtual bool write(ostream &out) const override {}
};
可以看到，对于定义顶点我们要做的四件事：
1.2定义顶点要重写四个函数
- virtual void setToOriginImpl()
- virtual void oplusImpl(const double *update)
- virtual bool read(istream &in)
- virtual bool write(ostream &out)
一般情况下，write和read置空即可，我们重点讨论setToOriginImpl()和oplusImpl（）
1.2.1setToOriginImpl()
setToOriginImpl()函数的用途就是初始化估计值_estimate,初始化的具体形式随_estimate数据类型而定，_estimate类型可以通过指定BaseVertex模板参数指定
在曲线拟合中class CurveFittingVertex : public g2o::BaseVertex<3, Eigen::Vector3d> 指定了_estimate是一个Eigen::Vector3d ，3是指Eigen::Vector3d可以由3个量来确定。
在pnp中，class VertexPose : public g2o::BaseVertex<6, Sophus::SE3d> 指定了_estimate是一个Sophus::SE3d ，6是指Sophus::SE3d>可以由6个量来确定。
不同的类型决定了不同的初始化方式，在曲线拟合中是初始化一个向量，在pnp中则是初始化一个位姿。
1.2.2oplusImpl()
oplusImpl()函数的作用是定义增量，以及每一次优化迭代的“加法”，这是很有必要的，我们知道，在优化问题中，我们需要计算，这个公式在三维空间是很好解决的，就是向量的每一维相加即可，但如果是在李群上（李群没有加法），这个“+”好就变成了左乘，因此我们必须对oplusImpl()重写。
在曲线拟合中_estimate += Eigen::Vector3d(update); ，就是普通的向量相加。
在pnp中，由于_estimate是一个李群，因而加法就变成了左乘_estimate = Sophus::SE3d::exp(update_eigen) * _estimate;
2.重写边
边就是链接节点之间的约束条件，在曲线拟合问题中，定义为单边，在pnp中定义为双边
2.1源代码对比
曲线拟合问题：
// 误差模型 模板参数：观测值维度，类型，连接顶点类型
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 (_vertices[0]);
const Eigen::Vector3d abc = v->estimate();
_error(0, 0) = _measurement - std::exp(abc(0, 0) * _x * _x + abc(1, 0) * _x + abc(2, 0));
}
// 计算雅可比矩阵
virtual void linearizeOplus() override {
const CurveFittingVertex *v = static_cast (_vertices[0]);
const Eigen::Vector3d abc = v->estimate();
double y = exp(abc[0] * _x * _x + abc[1] * _x + abc[2]);
_jacobianOplusXi[0] = -_x * _x * y;
_jacobianOplusXi[1] = -_x * y;
_jacobianOplusXi[2] = -y;
}
virtual bool read(istream &in) {}
virtual bool write(ostream &out) const {}
public:
double _x; // x 值， y 值为 _measurement
};
pnp问题：
class EdgeProjection : public g2o::BaseUnaryEdge<2, Eigen::Vector2d, VertexPose> {
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
EdgeProjection(const Eigen::Vector3d &pos, const Eigen::Matrix3d &K) : _pos3d(pos), _K(K) {}
virtual void computeError() override {
const VertexPose *v = static_cast (_vertices[0]);
Sophus::SE3d T = v->estimate();
Eigen::Vector3d pos_pixel = _K * (T * _pos3d);
pos_pixel /= pos_pixel[2];
_error = _measurement - pos_pixel.head<2>();
}
virtual void linearizeOplus() override {
const VertexPose *v = static_cast (_vertices[0]);
Sophus::SE3d T = v->estimate();
Eigen::Vector3d pos_cam = T * _pos3d;
double fx = _K(0, 0);
double fy = _K(1, 1);
double cx = _K(0, 2);
double cy = _K(1, 2);
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 / (Z * Z), fy + fy * Y * Y / Z2, -fy * X * Y / Z2, -fy * X / Z;
}
2.2如何确定父类边的模板参数
在重写边的过程中，要继承边，分为单边，双边，多边，这里结合具体代码介绍一下单边。
2.2.1单边BaseUnaryEdge
首先讲什么情况下选择单边，仅优化一个变量地时候选择单边，在曲线拟合中仅优化向量（a,b,c）,在pnp中仅优化相机位姿pose
- D即为观测值维度
- E为观测值的类型
- VertexXi为边链接的节点类型
具体结合曲线拟合来说：
曲线拟合是要求解这样一个优化问题，
我们的观测值是,这是一个一维的double类型，并且便连接的只有一种节点，所以模板参数定义为：BaseUnaryEdge<1, double, CurveFittingVertex>,这也就是后面程序使用的_measurement。
具体结合pnp来说：
pnp是要求解这样一个优化问题，
我们要求解T的估计，因为选用单边，我们的观测值是，即像素坐标，是一个2维的double向量，这也就是后面程序使用的_measurement，并且图中含有两种顶点，所以声明为BaseUnaryEdge<2, Eigen::Vector2d, VertexPose>，请注意，我们根据仅优化一个变量****VertexPose ,而选择了单边 BaseUnaryEdge ，而非根据图中具有的顶点种类。
2.3重写边类中的五个函数
一般边内需要重写5个函数
- 边的构造函数
- virtual void computeError()
- virtual void linearizeOplus()
- virtual bool read(istream &in)
- virtual bool write(ostream &out)
其中，read以及write函数，置空即可，重点探讨剩下的三个函数
2.3.1边的构造函数
构造函数关键在于需要传入什么参数构造边。
在曲线拟合中，_estimate代表(a,b,c)的估计,_measurement代表，那么为了估计这个问题
我们还需要知道,因此我们选择在构造函数中传入x：CurveFittingEdge(double x) : BaseUnaryEdge(), _x(x) {}
在pnp问题中，_estimate代表T的估计，_measurment代表，那么为了估计这个问题
我们还需要知道，因此需要在构造函数中传入：EdgeProjection(const Eigen::Vector3d &pos, const Eigen::Matrix3d &K) : _pos3d(pos), _K(K){} ,pos代表三维空间点，K代表相机参数
2.3.2virtual void computeError()
这个函数需要计算估计和测量值的差，也就是最小二乘问题中的。
在曲线拟合问题中，表现为，因此函数表示为
virtual void computeError() override {
//1、定义要估计的顶点
const CurveFittingVertex *v = static_cast (_vertices[0]);
//2、取得顶点的估计
const Eigen::Vector3d abc = v->estimate();
//3、计算误差e
_error(0, 0) = _measurement - std::exp(abc(0, 0) * _x * _x + abc(1, 0) * _x + abc(2, 0));
}
在pnp问题中，表现为,因此函数表示为
virtual void computeError() override {
//1、定义要估计的顶点
const VertexPose *v = static_cast (_vertices[0]);
//2、取得顶点的估计
Sophus::SE3d T = v->estimate();
//3、计算误差e
Eigen::Vector3d pos_pixel = _K * (T * _pos3d);
pos_pixel /= pos_pixel[2];
_error = _measurement - pos_pixel.head<2>();
}
2.3.3virtual void linearizeOplus()
这个函数我们计算关于估计量的雅可比，如果我们没有给出雅可比，g2o也会进行数值求导，但是会比显示给出雅可比慢。
在曲线拟合问题中，根据e关于估计量（a,b,c）的雅可比，我们可以写出这个virtual void linearizeOplus()
// 计算雅可比矩阵
virtual void linearizeOplus() override {
const CurveFittingVertex *v = static_cast (_vertices[0]);
const Eigen::Vector3d abc = v->estimate();
double y = exp(abc[0] * _x * _x + abc[1] * _x + abc[2]);
_jacobianOplusXi[0] = -_x * _x * y;
_jacobianOplusXi[1] = -_x * y;
_jacobianOplusXi[2] = -y;
}
同样的，在pnp问题中，virtual void linearizeOplus()的形式是
virtual void linearizeOplus() override {
const VertexPose *v = static_cast (_vertices[0]);
Sophus::SE3d T = v->estimate();
Eigen::Vector3d pos_cam = T * _pos3d;
double fx = _K(0, 0);
double fy = _K(1, 1);
double cx = _K(0, 2);
double cy = _K(1, 2);
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 / (Z * Z), fy + fy * Y * Y / Z2, -fy * X * Y / Z2, -fy * X / Z;
}
3.创建BlockSolver
3.1源码对比
在曲线拟合问题中：
typedef g2o::BlockSolver> BlockSolverType;
在pnp问题中
typedef g2o::BlockSolver> BlockSolverType;
g2o::BlockSolverTraits
g2o还提供了其他的块求解器：
- BlockSolver_6_3 ：表示pose 是6维，观测点是3维，用于BA。
- BlockSolver_7_3：在BlockSolver_6_3 的基础上多了一个scale。
- BlockSolver_3_2：表示pose 是3维，观测点是2维。
特别地，当你不想了解到底是多少维，可以使用typedef g2o::BlockSolverX BlockSolverType; 他会自定推测数据的维度。
4.创建线性求解器LinearSolver
这一步中我们可以选择不同的求解方式来求解线性方程
，g2o中提供的求解方式主要有：
- LinearSolverCholmod ：使用sparse cholesky分解法，继承自LinearSolverCCS。
- LinearSolverCSparse：使用CSparse法，继承自LinearSolverCCS。
- LinearSolverPCG ：使用preconditioned conjugate gradient 法，继承自LinearSolver。
- LinearSolverDense ：使用dense cholesky分解法，继承自LinearSolver。
- LinearSolverEigen：依赖项只有eigen，使用eigen中sparse Cholesky 求解，因此编译好后可以方便的在其他地方使用，性能和CSparse差不多，继承自LinearSolver。
4.1源码对比
曲线拟合：
typedef g2o::LinearSolverDense LinearSolverType;
pnp：
typedef g2o::LinearSolverDense LinearSolverType;
5.创建总求解器solver
有三种求解器可以选择
- g2o::OptimizationAlgorithmGaussNewton
- g2o::OptimizationAlgorithmLevenberg
- g2o::OptimizationAlgorithmDogleg
5.1源码对比：
曲线拟合：
auto solver = new g2o::OptimizationAlgorithmGaussNewton(
g2o::make_unique(g2o::make_unique()));
pnp:
auto solver = new g2o::OptimizationAlgorithmGaussNewton(
g2o::make_unique(g2o::make_unique()));
6.创建图优化的核心：稀疏优化器
创建稀疏优化器的过程有以下三步：
- 建立图模型
g2o::SparseOptimizer optimizer;
- 设置求解方法：
SparseOptimizer::setAlgorithm(OptimizationAlgorithm* algorithm)；
- 设置优化过程输出信息：
SparseOptimizer::setVerbose(bool verbose)；
6.1源码对比
曲线拟合：
g2o::SparseOptimizer optimizer; // 图模型
optimizer.setAlgorithm(solver); // 设置求解器
optimizer.setVerbose(true); // 打开调试输出
pnp:
g2o::SparseOptimizer optimizer; // 图模型
optimizer.setAlgorithm(solver); // 设置求解器
optimizer.setVerbose(true); // 打开调试输出
7.在图中添加顶点
添加顶点分为三步：
- 创建点并初始化
- 定义节点编号
- 把节点添加到图中
7.1源码对比
曲线拟合：
CurveFittingVertex *v = new CurveFittingVertex();//1.创建点
v->setEstimate(Eigen::Vector3d(ae, be, ce));//1.设定初值
v->setId(0);//2.定义节点标号
optimizer.addVertex(v);//3.添加点到图中
pnp:
VertexPose *vertex_pose = new VertexPose(); //1.创建点
vertex_pose->setId(0);//2.定义节点标号
vertex_pose->setEstimate(Sophus::SE3d());//1.设定初值
optimizer.addVertex(vertex_pose);//3.添加点到图中
8.向图中添加边
添加边总共有五步：
- 创建边
- 设置边id
- 设置连接的顶点
- 设置观测值（_measurement）
- 设置信息矩阵
- 向图中添加边
8.1源码对比
曲线拟合：
for (int i = 0; i < N; i++) {
CurveFittingEdge *edge = new CurveFittingEdge(x_data[i]);//1.创建边
edge->setId(i); //2.设置边的ID
edge->setVertex(0, v); //3.设置连接的顶点
edge->setMeasurement(y_data[i]); //4.设置观测数值
edge->setInformation(Eigen::Matrix::Identity() * 1 / (w_sigma * w_sigma)); //5.设置信息矩阵：协方差矩阵之逆
optimizer.addEdge(edge); //6.向图中添加边
}
pnp:
int index = 1;
for (size_t i = 0; i < points_2d.size(); ++i) {
auto p2d = points_2d[i];
auto p3d = points_3d[i];
EdgeProjection *edge = new EdgeProjection(p3d, K_eigen);//1.创建边
edge->setId(index); //2.设置边的ID
edge->setVertex(0, vertex_pose); //3.设置连接的顶点
edge->setMeasurement(p2d); //4.设置观测值
edge->setInformation(Eigen::Matrix2d::Identity());//5.设置信息矩阵
optimizer.addEdge(edge); //6.向图中添加边
index++;
}
9.设置优化参数，开始执行优化
设置SparseOptimizer的初始化、迭代次数、保存结果等。
- 初始化：
SparseOptimizer::initializeOptimization(HyperGraph::EdgeSet& eset)；
- 设置迭代次数：
SparseOptimizer::optimize(int iterations,bool online)；
9.1源码对比
曲线拟合：
optimizer.initializeOptimization(); //1.初始化
optimizer.optimize(10); //2.设置迭代次数
pnp:
optimizer.initializeOptimization(); //1.初始化
optimizer.optimize(10); //2.设置迭代次数
参考：
SLAM从0到1——图优化g2o：从看懂代码到动手编写（长文）-技术圈 (proginn.com)