我看到高博的好多例子都用到g2o,因此我找了一些博客来学习g2o和图优化的基本知识,这篇博客就是一个简单的总结。
图优化
SLAM的核心是根据已有的观测数据,计算机器人的运动轨迹和地图。假设在时刻 k,机器人在位置 xk 处,用传感器进行了一次观测,得到了数据 zk,该传感器的观测方程为:zk = h(xk)。
由于误差的存在,zk不可能精确地等于 h(xk),于是就有了误差:ek = zk - h(xk)。如果我们以 xk 为优化变量,以 min Fk(xk) = |ek| 为目标函数,就可以求得 xk 的最优值。
SLAM问题的处理方法主要分为滤波和图优化两类,滤波的方法常见的是扩展卡尔曼滤波、粒子滤波、信息滤波等。而图优化,就是把一个常规的优化问题,以图的形式来表述。图(Graph)是由顶点(Vertex)和边(Edge)组成的结构。
记一个图G={V,E},其中V为顶点集,E为边集。边可以是有向的或是无向的,对应的图称为有向图或无向图。边可以连接一个顶点(Unary Edge,一元边)、两个顶点(Binary Edge,二元边)或多个顶点(Hyper Edge,多元边)。当一个图中存在连接两个以上顶点的边时,称这个图为超图(Hyper Graph)。在图中,以顶点表示优化变量,以边表示观测方程。一个顶点和多条一元边的示意图如下:
SLAM问题可以表示成一个超图,其中观测方程有很多形式,下面是几个常用的观测方程和其对应的图优化表示:
1.机器人两个Pose之间的变换:一条二元边,两个顶点为位姿,边的方程是位姿变换:T1=ΔT*T2。
2.机器人在某个Pose处用激光测量到了某个空间点,得到了它离自己的距离与角度:二元边,两个顶点分别是一个2D Pose:[x,y,𝜃θ]和一个二维点[λx,λy],观测数据是距离r和角度b,那么观测方程为:
3.机器人在某个Pose处用相机观测到了某个空间点,得到了它的像素坐标:二元边,顶点分别为一个3D Pose T和一个空间P=[x,y,z],观测数据为像素坐标u=[u,v],则观测方程为:u=K(RP+t),其中K为相机内参,R,t表示旋转和平移。
图优化的目标函数:
e表示误差。xk可以指一个顶点、两个顶点或多个顶点,取决于边的实际类型。zk 是测量值。Ωk 表示该误差的权重矩阵,也就是边的信息矩阵。图优化里每一条边的信息矩阵就是测量协防差矩阵的逆,协防差越小,表示这次测量越准越值得相信,信息权重就越大。
图优化的流程:
1.选择你想要的图里的节点与边的类型,确定它们的参数化形式;
2.往图里加入实际的节点和边;
3.选择初值,开始迭代;
3.每一步迭代中,计算对应于当前估计值的雅可比矩阵和海塞矩阵;
4.求解稀疏线性方程HkΔx = −bk,得到梯度方向;
5.继续用高斯牛顿法(GN)或列文列文伯格-马夸尔特法(LM)进行迭代。
6.如果迭代结束,返回优化值。
图优化库g2o可以帮我们做好第3-6步。关于GN和LM算法,之前有做过笔记<非线性最小二乘求解方法>。
g2o
g2o(General Graphic Optimization),是一个基于图优化的库。之前也有记录过g2o的安装过程<Ceres和g2o的配置和使用>。下图是g2o基本的类结构:
上图左边的节点SparseOptimizer类是我们要维护的东西,是一个Optimizable Graph,从而也是一个Hyper Graph。一个 SparseOptimizer 含有很多个顶点 (继承自 Base Vertex)和很多条边(继承自 BaseUnaryEdge, BaseBinaryEdge或BaseMultiEdge)。这些 Base Vertex 和 Base Edge 都是抽象的基类,而实际用的顶点和边,都是它们的派生类。用 SparseOptimizer.addVertex() 和 SparseOptimizer.addEdge() 向一个图中添加顶点和边,最后调用 SparseOptimizer.optimize() 完成优化。
在优化之前,需要指定求解器(Solver)和迭代算法(OptimizationAlgorithm)。从图中下半部分可以看到,一个 SparseOptimizer 拥有一个Optimization Algorithm,继承自Gauss-Newton, Levernberg-Marquardt, Powell's dogleg 三者之一。同时,这个 Optimization Algorithm 拥有一个Solver,它是一个块求解器(BlockSolver),它含有两个部分:一个是 SparseBlockMatrix ,用于计算稀疏的雅可比和海塞; LinearSolver,一个线性方程的求解器,是用于计算迭代过程中 HΔx = −b,LinearSolver可以从 PCG, CSparse, Choldmod 三者选一。
作为 g2o 的用户,我们要做的事主要包含以下步骤:
1. 定义顶点和边的类型。
2. 构建图。
3. 选择优化算法。包括选择LinearSolver,BlockSolver,OptimizationAlgorithm。
4. 调用 g2o 进行优化,返回结果。
有时候我们需要自定义顶点和边,这需要继承g2o中对应的基类。自定义顶点需要覆盖如下方法:
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()的功能是更新,根据增量方程计算出增量之后,通过该函数对估计值进行调整。
自定义边如要覆盖如下方法:
virtual bool read(std::istream& is); virtual bool write(std::ostream& os) const; virtual void computeError(); virtual void linearizeOplus();
Read()和write()函数同上。computeError()函数是使用当前顶点的值计算的测量值与真实的测量值之间的误差。linearizeOplus()函数是在当前顶点的值下,该误差对优化变量的偏导数,即计算雅可比矩阵。
代码实现
上篇博文<ICP算法>里,使用高斯牛顿法对ICP求解的结果进行优化,这里改用g2o优化。
首先定义顶点:
/* HyperGraph::Vertex 是个抽象类 OptimizableGraph::Vertex继承自HyperGraph::Vertex 而BaseVertex继承自OptimizableGraph::Vertex */ //自定义顶点,继承自BaseVertex类模板,模板参数:顶点的最小维度,顶点的数据类型 //第一个参数D并非是顶点的维度,而是其在流形空间(manifold)的最小表示,这里是李代数的维度 class VertexPose : public g2o::BaseVertex<6, Sophus::SE3d> { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW; //重置优化变量 virtual void setToOriginImpl() override { _estimate = Sophus::SE3d();//_estimate被优化变量 } //根据增量方程计算处增量*updata后,该函数对估计值_estimate进行更新,这里估计值通过左乘扰动量(增量)进行更新 virtual void oplusImpl(const double *update) override { Eigen::Matrix<double, 6, 1> 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 {} };
然后定义一元边:
/*常用变量的定义: BaseUnaryEdge定义的变量如下: JacobianXiOplusType _jacobianOplusXi;//雅可比矩阵 BaseUnaryEdge继承自BaseEdge,BaseEdge类中定义的变量如下: Measurement _measurement;//存储观测值 InformationType _information;//存储信息矩阵 ErrorVector _error; //存储计算的误差 BaseEdge继承自OptimizableGraph::Edge,Edge类定义的变量如下: int _dimension; OptimizableGraph::Edge继承自HyperGraph::Edge,里面定义的变量如下: VertexContainer _vertices;//存储边连接的顶点 */ //自定义边,继承自一元边BaseUnaryEdge模板类,模板参数:观测方程的维度,类型,顶点类型 class EdgeProjectXYZRGBDPoseOnly : public g2o::BaseUnaryEdge<3, Eigen::Vector3d, VertexPose> { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW; //构造函数,传入用于计算估计值的点 EdgeProjectXYZRGBDPoseOnly(const Eigen::Vector3d &point) : _point(point) {} //使用当前顶点的值计算的测量值与真实的测量值之间的误差。 virtual void computeError() override { //获得当前顶点,顶点由setVertex(int, vertex)设置 const VertexPose *pose = static_cast<const VertexPose *> ( _vertices[0] ); //计算观测值与估计值之差,观测值由setMeasurement(type)设置 _error = _measurement - pose->estimate() * _point; } //在当前顶点的值下,该误差对优化变量的偏导数,即计算雅可比矩阵。 virtual void linearizeOplus() override { //获得顶点 VertexPose *pose = static_cast<VertexPose *>(_vertices[0]); //获得当前位姿 Sophus::SE3d T = pose->estimate(); //计算估计点 Eigen::Vector3d xyz_trans = T * _point; //计算雅可比 _jacobianOplusXi.block<3, 3>(0, 0) = -Eigen::Matrix3d::Identity(); _jacobianOplusXi.block<3, 3>(0, 3) = Sophus::SO3d::hat(xyz_trans); } //读存盘 bool read(istream &in) {} bool write(ostream &out) const {} protected: Eigen::Vector3d _point; };
接下来,就是定义图,选择优化算法,并进行优化:
void bundleAdjustment( const vector<Point3f> &pts1, const vector<Point3f> &pts2, Mat &R, Mat &t) { //定义可变尺寸的blocksolver, //它的定义:using BlockSolverX = BlockSolverPL<Eigen::Dynamic, Eigen::Dynamic>; typedef g2o::BlockSolverX BlockSolverType; //创建线性求解器类型,这里选择LinearSolverDense /*可选择的线性求解器 LinearSolverCholmod :使用sparse cholesky分解法。继承自LinearSolverCCS LinearSolverCSparse:使用CSparse法。继承自LinearSolverCCS LinearSolverPCG :使用preconditioned conjugate gradient 法,继承自LinearSolver LinearSolverDense :使用dense cholesky分解法。继承自LinearSolver LinearSolverEigen: 依赖项只有eigen,使用eigen中sparse Cholesky 求解,因此编译好后可以方便的在其他地方使用,性能和CSparse差不多。继承自LinearSolver */ typedef g2o::LinearSolverDense<BlockSolverType::PoseMatrixType> LinearSolverType; //创建总的求解器, 梯度下降方法可以从GN, LM, DogLeg 中选,这里选择LM /*可选迭代算法,均继承自OptimizationWithHessian,而OptimizationAlgorithmWithHessian继承自OptimizationAlgorithm g2o::OptimizationAlgorithmGaussNewton g2o::OptimizationAlgorithmLevenberg g2o::OptimizationAlgorithmDogleg */ auto solver = new g2o::OptimizationAlgorithmLevenberg( g2o::make_unique<BlockSolverType>(g2o::make_unique<LinearSolverType>())//创建blockSolver ); //稀疏优化器 g2o::SparseOptimizer optimizer; // 图模型 optimizer.setAlgorithm(solver); // 设置求解器 optimizer.setVerbose(true); // 打开调试输出 //添加顶点 VertexPose *pose = new VertexPose(); pose->setId(0); pose->setEstimate(Sophus::SE3d()); optimizer.addVertex(pose); //添加边 for (size_t i = 0; i < pts1.size(); i++) { EdgeProjectXYZRGBDPoseOnly *edge = new EdgeProjectXYZRGBDPoseOnly(Eigen::Vector3d(pts2[i].x, pts2[i].y, pts2[i].z)); edge->setVertex(0, pose);//设置顶点 edge->setMeasurement(Eigen::Vector3d(pts1[i].x, pts1[i].y, pts1[i].z));//设置观测值 edge->setInformation(Eigen::Matrix3d::Identity());//设置信息矩阵,协方差矩阵之逆 optimizer.addEdge(edge);//添加边 } //初始化参数, optimizer.initializeOptimization(); //进行优化,迭代10次 optimizer.optimize(10); //取出优化后的结果 cout << endl << "after optimization:" << endl; cout << "T=\n" << pose->estimate().matrix() << endl; // convert to cv::Mat Eigen::Matrix3d R_ = pose->estimate().rotationMatrix(); Eigen::Vector3d t_ = pose->estimate().translation(); R = (Mat_<double>(3, 3) << R_(0, 0), R_(0, 1), R_(0, 2), R_(1, 0), R_(1, 1), R_(1, 2), R_(2, 0), R_(2, 1), R_(2, 2) ); t = (Mat_<double>(3, 1) << t_(0, 0), t_(1, 0), t_(2, 0)); }
参考文献
[0]半闲居士.深入理解图优化与g2o:图优化篇.https://www.cnblogs.com/gaoxiang12/p/5244828.html .2016-03-07
[1]白巧克力亦唯心.graph slam tutorial : 从推导到应用1.https://blog.csdn.net/heyijia0327/article/details/47686523.2015-10-12
[2]半闲居士.深入理解图优化与g2o:g2o篇.https://www.cnblogs.com/gaoxiang12/p/5304272.html.2016-03-22
[3]Feifanren.g2o学习——g2o整体框架.https://www.cnblogs.com/feifanrensheng/articles/8681952.html.2018-03-31
[4] 计算机视觉life.从零开始一起学习SLAM | 理解图优化,一步步带你看懂g2o代码. https://mp.weixin.qq.com/s?__biz=MzIxOTczOTM4NA==&mid=2247486858&idx=1&sn=ce458d5eb6b1ad11b065d71899e31a04&chksm=97d7e81da0a0610b1e3e12415b6de1501329920c3074ab5b48e759edbb33d264a73f1a9f9faf&scene=21#wechat_redirect. 2019-01-17
[5]计算机视觉life.从零开始一起学习SLAM | 掌握g2o顶点编程套路. https://mp.weixin.qq.com/s?__biz=MzIxOTczOTM4NA==&mid=2247486992&idx=1&sn=ecb7c3ef9bd968e51914c2f5b767428d&chksm=97d7eb87a0a062912a9db9fb16a08129f373791fd3918952342d5db46c0bc4880326a7933671&scene=21#wechat_redirect. 2019-02-22
[6]视觉SLAM14讲