初学SLAM,因为ubuntu中不能使用onenote,所以决定用CSDN来记录自己的学习笔记,发表的目的是想和大家一起分享交流进步,笔记内容多来自各大佬的心得,我也会附上链接,如果有侵权烦请告知。
我们要求的增量的方程的形式是:H△X=-b,通常情况下想到的方法就是直接求逆,也就是△X=-H^-1*b。但是当H的维度较大时,矩阵求逆变得很困难,此时我们就需要一些特殊的方法对矩阵进行求逆。
LinearSolverCholmod :使用sparse cholesky分解法。继承自LinearSolverCCS
LinearSolverCSparse:使用CSparse法。继承自LinearSolverCCS
LinearSolverPCG :使用preconditioned conjugate gradient 法,继承自LinearSolver
LinearSolverDense :使用dense cholesky分解法。继承自LinearSolver
LinearSolverEigen: 依赖项只有eigen,使用eigen中sparse Cholesky 求解,性能和CSparse差不多。继承自LinearSolver
BlockSolver 内部包含 LinearSolver,用上面我们定义的线性求解器LinearSolver来初始化。它的定义在如下文件夹内:
g2o/g2o/core/block_solver.h
BlockSolver有两种定义方式
1.一种是指定的固定变量的solver,定义如下
using BlockSolverPL = BlockSolver< BlockSolverTraits<p, l> >;
其中p代表pose的维度(注意一定是流形manifold下的最小表示),l表示landmark的维度
2.另一种是可变尺寸的solver,定义如下
using BlockSolverX = BlockSolverPL<Eigen::Dynamic, Eigen::Dynamic>;
block_solver.h的最后,预定义了比较常用的几种类型,如下所示:
BlockSolver_6_3 :表示pose 是6维,观测点是3维。用于3D SLAM中的BA
BlockSolver_7_3:在BlockSolver_6_3 的基础上多了一个scale
BlockSolver_3_2:表示pose 是3维,观测点是2维
在g2o/g2o/core/ 目录下,Solver的优化方法有三种:分别是高斯牛顿(GaussNewton)法,LM(Levenberg–Marquardt)法、Dogleg法,如下图所示,也和前面的图相匹配而GN、 LM、 Doglet算法内部,都继承自同一个类:OptimizationWithHessian。
而 OptimizationAlgorithmWithHessian,又继承自OptimizationAlgorithm。
总之,在该阶段,我们可以选则三种方法:
g2o::OptimizationAlgorithmGaussNewton
g2o::OptimizationAlgorithmLevenberg
g2o::OptimizationAlgorithmDogleg
创建稀疏优化器
g2o::SparseOptimizer optimizer;
用前面定义好的求解器作为求解方法:
SparseOptimizer::setAlgorithm(OptimizationAlgorithm* algorithm)
其中setVerbose是设置优化过程输出信息用的
SparseOptimizer::setVerbose(bool verbose)
先来看看上图中和vertex有关的第①个类: HyperGraph::Vertex,,它在路径为
g2o/core/hyper_graph.h
这个 HyperGraph::Vertex 是个abstract vertex,必须通过派生来使用。如下图所示
然后我们看g2o 类结构图中第②个类,我们看到HyperGraph::Vertex 是通过类OptimizableGraph 来继承的, 而OptimizableGraph的定义在
g2o/core/optimizable_graph.h
我们找到vertex定义,发现果然,OptimizableGraph 继承自 HyperGraph,如下图所示
不过,这个OptimizableGraph::Vertex 也非常底层,具体使用时一般都会进行扩展,因此g2o中提供了一个比较通用的适合大部分情况的模板。就是g2o 类结构图中 对应的第③个类:
BaseVertex
它的路径为:
g2o/core/base_vertex.h
D是int 类型的,表示vertex的最小维度,比如3D空间中旋转是3维的,那么这里 D = 3
T是待估计vertex的数据类型,比如用四元数表达三维旋转的话,T就是Quaternion 类型
如何自己定义顶点?
g2o本身内部定义了一些常用的顶点类型:
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顶点的内参数化与外参数化及其指数映射
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为正值,否则qx,qy,qz中存在一个不明确的旋转
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>
重新定义顶点一般需要考虑重写如下函数:
virtual bool read(std::istream& is);
virtual bool write(std::ostream& os) const;
virtual void oplusImpl(const number_t* update);
virtual void setToOriginImpl();
read,write:分别是读盘、存盘函数,一般情况下不需要进行读/写操作的话,仅仅声明一下就可以
setToOriginImpl:顶点重置函数,设定被优化变量的原始值。
oplusImpl:顶点更新函数。非常重要的一个函数,主要用于优化过程中增量△x 的计算。我们根据增量方程计算出增量之后,就是通过这个函数对估计值进行调整的,因此这个函数的内容一定要重视。
先看一个简单例子,来自十四讲中的曲线拟合,来源如下
ch6/g2o_curve_fitting/main.cpp
// 曲线模型的顶点,模板参数:优化变量维度和数据类型
class CurveFittingVertex: public g2o::BaseVertex<3, Eigen::Vector3d>
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
virtual void setToOriginImpl() // 重置
{
_estimate << 0,0,0;
}
virtual void oplusImpl( const double* update ) // 更新
{
_estimate += Eigen::Vector3d(update);
}
// 存盘和读盘:留空
virtual bool read( istream& in ) {
}
virtual bool write( ostream& out ) const {
}
};
代码中顶点初值设置为0,更新时因为 x + △x 直接把更新量 update 加上去的(因为顶点类型是Eigen::Vector3d,属于向量,是可以通过加法来更新)
但是有些例子就不行,比如下面这个复杂点例子:李代数表示位姿VertexSE3Expmap
来自g2o官网
g2o/types/sba/types_six_dof_expmap.h
//brief SE3 Vertex parameterized internally with a transformation matrix and externally with its exponential map
class G2O_TYPES_SBA_API VertexSE3Expmap : public BaseVertex<6, SE3Quat>{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
VertexSE3Expmap();
bool read(std::istream& is);
bool write(std::ostream& os) const;
virtual void setToOriginImpl() {
_estimate = SE3Quat();
}
virtual void oplusImpl(const number_t* update_) {
Eigen::Map<const Vector6> update(update_);
setEstimate(SE3Quat::exp(update)*estimate()); //更新方式
}
};
这个里面的6, SE3Quat 分别为:
第一个参数6 表示内部存储的优化变量维度,这是个6维的李代数
第二个参数是优化变量的类型,这里使用了g2o定义的相机位姿类型:SE3Quat。
在这里可以具体查看g2o/types/slam3d/se3quat.h
它内部使用了四元数表达旋转,然后加上位移来存储位姿,同时支持李代数上的运算,比如对数映射(log函数)、李代数上增量(update函数)等操作
如何向图中添加顶点?
往图中增加顶点比较简单,我们还是先看看第一个曲线拟合的例子,setEstimate(type) 函数来设定初始值;setId(int) 定义节点编号
// 往图中增加顶点
CurveFittingVertex* v = new CurveFittingVertex();
v->setEstimate( Eigen::Vector3d(0,0,0) );
v->setId(0);
optimizer.addVertex( v );
这个是添加 VertexSBAPointXYZ 的例子,都很容易看懂
/ch7/pose_estimation_3d2d.cpp
int index = 1;
for ( const Point3f p:points_3d ) // landmarks
{
g2o::VertexSBAPointXYZ* point = new g2o::VertexSBAPointXYZ();
point->setId ( index++ );
point->setEstimate ( Eigen::Vector3d ( p.x, p.y, p.z ) );
point->setMarginalized ( true );
optimizer.addVertex ( point );
}
初步认识g2o的边
与边有关的头文件:
g2o/g2o/core/hyper_graph.h
g2o/g2o/core/optimizable_graph.h
g2o/g2o/core/base_edge.h
BaseUnaryEdge,BaseBinaryEdge,BaseMultiEdge 分别表示一元边,两元边,多元边。
一元边可以理解为一条边只连接一个顶点,两元边理解为一条边连接两个顶点,多元边理解为一条边可以连接多个(3个以上)顶点
它们的主要参数为:D, E, VertexXi, VertexXj,他们的分别代表:
D 是 int 型,表示测量值的维度 (dimension)
E 表示测量值的数据类型
VertexXi,VertexXj 分别表示不同顶点的类型
比如我们用边表示三维点投影到图像平面的重投影误差,就可以设置输入参数如下:
BaseBinaryEdge<2, Vector2D, VertexSBAPointXYZ, VertexSE3Expmap>
第1个2是说测量值是2维的,也就是图像像素坐标x,y的差值,对应测量值的类型是Vector2D,两个顶点也就是优化变量分别是三维点 VertexSBAPointXYZ,和李群位姿VertexSE3Expmap
定义边我们通常需要复写一些重要的成员函数,边主要有以下几个重要的成员函数
virtual bool read(std::istream& is);
virtual bool write(std::ostream& os) const;
virtual void computeError();
virtual void linearizeOplus();
下面简单解释一下
read,write:分别是读盘、存盘函数,一般情况下不需要进行读/写操作的话,仅仅声明一下就可以
computeError函数:非常重要,是使用当前顶点的值计算的测量值与真实的测量值之间的误差
linearizeOplus函数:非常重要,是在当前顶点的值下,该误差对优化变量的偏导数,也就是我们说的Jacobian
除了上面几个成员函数,还有几个重要的成员变量和函数也一并解释一下:
_measurement:存储观测值
_error:存储computeError() 函数计算的误差
_vertices[]:存储顶点信息,比如二元边的话,_vertices[] 的大小为2,存储顺序和调用setVertex(int, vertex) 是设定的int 有关(0 或1)
setId(int):来定义边的编号(决定了在H矩阵中的位置)
setMeasurement(type) 函数来定义观测值
setVertex(int, vertex) 来定义顶点
setInformation() 来定义协方差矩阵的逆
如何自定义g2o的边?
基本上定义g2o中的边,就是如下套路:
class myEdge: public g2o::BaseBinaryEdge<errorDim, errorType, Vertex1Type, Vertex2Type>
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
myEdge(){
}
virtual bool read(istream& in) {
}
virtual bool write(ostream& out) const {
}
virtual void computeError() override
{
// ...
_error = _measurement - Something;
}
virtual void linearizeOplus() override
{
_jacobianOplusXi(pos, pos) = something;
// ...
/*
_jocobianOplusXj(pos, pos) = something;
...
*/
}
private:
// data
}
我们可以发现,最重要的就是computeError(),linearizeOplus()两个函数了.
我们先来看一个简单例子,地址在https://github.com/gaoxiang12/slambook/blob/master/ch6/g2o_curve_fitting/main.cpp
这个是个一元边,主要是定义误差函数了,如下所示,你可以发现这个例子基本就是上面例子的一丢丢扩展
// 误差模型 模板参数:观测值维度,类型,连接顶点类型
class CurveFittingEdge: public g2o::BaseUnaryEdge<1,double,CurveFittingVertex>
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
CurveFittingEdge( double x ): BaseUnaryEdge(), _x(x) {
}
// 计算曲线模型误差
void computeError()
{
const CurveFittingVertex* v = static_cast<const CurveFittingVertex*> (_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 bool read( istream& in ) {
}
virtual bool write( ostream& out ) const {
}
public:
double _x; // x 值, y 值为 _measurement
};
下面是一个复杂一点例子,3D-2D点的PnP 问题,也就是最小化重投影误差问题,这个问题非常常见,使用最常见的二元边,弄懂了这个基本跟边相关的代码也差不多都一通百通了
//继承了BaseBinaryEdge类,观测值是2维,类型Vector2D,顶点分别是三维点、李群位姿
class G2O_TYPES_SBA_API EdgeProjectXYZ2UV : public BaseBinaryEdge<2, Vector2D, VertexSBAPointXYZ, VertexSE3Expmap>{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
//1. 默认初始化
EdgeProjectXYZ2UV();
//2. 计算误差
void computeError() {
//李群相机位姿v1
const VertexSE3Expmap* v1 = static_cast<const VertexSE3Expmap*>(_vertices[1]);
// 顶点v2
const VertexSBAPointXYZ* v2 = static_cast<const VertexSBAPointXYZ*>(_vertices[0]);
//相机参数
const CameraParameters * cam
= static_cast<const CameraParameters *>(parameter(0));
//误差计算,测量值减去估计值,也就是重投影误差obs-cam
//估计值计算方法是T*p,得到相机坐标系下坐标,然后在利用camera2pixel()函数得到像素坐标。
Vector2D obs(_measurement);
_error = obs-cam->cam_map(v1->estimate().map(v2->estimate()));
//误差 = 观测 - 投影
}
//3. 线性增量函数,也就是雅克比矩阵J的计算方法
virtual void linearizeOplus();
//4. 相机参数
CameraParameters * _cam;
bool read(std::istream& is);
bool write(std::ostream& os) const;
};
如何向图中添加边?
我们还是先从最简单的 例子说起:一元边的添加方法
下面代码来自GitHub上,仍然是前面曲线拟合的例子
slambook/ch6/g2o_curve_fitting/main.cpp
// 往图中增加边
for ( int i=0; i<N; i++ )
{
CurveFittingEdge* edge = new CurveFittingEdge( x_data[i] );
edge->setId(i);
edge->setVertex( 0, v ); // 设置连接的顶点
edge->setMeasurement( y_data[i] ); // 观测数值
edge->setInformation( Eigen::Matrix<double,1,1>::Identity()*1/(w_sigma*w_sigma) ); // 信息矩阵:协方差矩阵之逆
optimizer.addEdge( edge );
}
对于这个曲线拟合,观测值就是实际观测到的数据点。对于视觉SLAM来说,通常就是我们我们观测到的特征点坐标,下面就是一个例子。这个例子比刚才的复杂一点,因为它是二元边,需要用边连接两个顶点
代码来自GitHub上
slambook/ch7/pose_estimation_3d2d.cpp
index = 1;
for ( const Point2f p:points_2d )
{
g2o::EdgeProjectXYZ2UV* edge = new g2o::EdgeProjectXYZ2UV();
edge->setId ( index );
edge->setVertex ( 0, dynamic_cast<g2o::VertexSBAPointXYZ*> ( optimizer.vertex ( index ) ) );
edge->setVertex ( 1, pose );
edge->setMeasurement ( Eigen::Vector2d ( p.x, p.y ) );
edge->setParameterId ( 0,0 );
edge->setInformation ( Eigen::Matrix2d::Identity() );
optimizer.addEdge ( edge );
index++;
}
这里的setMeasurement函数里的p来自向量points_2d,也就是特征点的图像坐标(x,y)。
setVertex 有两个一个是 0 和 VertexSBAPointXYZ 类型的顶点,一个是1 和pose。
_vertices[0] 对应的是 VertexSBAPointXYZ 类型的顶点,也就是三维点,_vertices[1] 对应的是VertexSE3Expmap 类型的顶点,也就是位姿pose
设置SparseOptimizer的初始化、迭代次数、保存结果等。
初始化
SparseOptimizer::initializeOptimization(HyperGraph::EdgeSet& eset)
设置迭代次数,然后就开始执行图优化了。
SparseOptimizer::optimize(int iterations, bool online)
——————————————————————————————
typedef g2o::BlockSolver< g2o::BlockSolverTraits<3,1> > Block; // 每个误差项优化变量维度为3,误差值维度为1
// 第1步:创建一个线性求解器LinearSolver
Block::LinearSolverType* linearSolver = new g2o::LinearSolverDense<Block::PoseMatrixType>();
// 第2步:创建BlockSolver。并用上面定义的线性求解器初始化
Block* solver_ptr = new Block( linearSolver );
// 第3步:创建总求解器solver。并从GN, LM, DogLeg 中选一个,再用上述块求解器BlockSolver初始化
g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg( solver_ptr );
// 第4步:创建终极大boss 稀疏优化器(SparseOptimizer)
g2o::SparseOptimizer optimizer; // 图模型
optimizer.setAlgorithm( solver ); // 设置求解器
optimizer.setVerbose( true ); // 打开调试输出
// 第5步:定义图的顶点和边。并添加到SparseOptimizer中
CurveFittingVertex* v = new CurveFittingVertex(); //往图中增加顶点
v->setEstimate( Eigen::Vector3d(0,0,0) );
v->setId(0);
optimizer.addVertex( v );
for ( int i=0; i<N; i++ ) // 往图中增加边
{
CurveFittingEdge* edge = new CurveFittingEdge( x_data[i] );
edge->setId(i);
edge->setVertex( 0, v ); // 设置连接的顶点
edge->setMeasurement( y_data[i] ); // 观测数值
edge->setInformation( Eigen::Matrix<double,1,1>::Identity()*1/(w_sigma*w_sigma) ); // 信息矩阵:协方差矩阵之逆
optimizer.addEdge( edge );
}
// 第6步:设置优化参数,开始执行优化
optimizer.initializeOptimization();
optimizer.optimize(100);
参考资料:
高翔《视觉SLAM十四讲》
https://blog.csdn.net/electech6/article/details/88018481
https://www.cnblogs.com/CV-life/p/10286037.html
https://www.cnblogs.com/CV-life/archive/2019/03/13/10525579.html