前言
g2o是比较流行的图优化库,在视觉slam中的应用非常广泛,关于如何利用g2o求解BA优化问题,在此作个笔记,以供后面的学习中方便查阅。
一、g2o进行非线性优化
以《视觉slam十四讲》中的求解最小二乘问题为例:
原函数:
二范数:
误差函数:
构建图模型:顶点为待优化变量,边为误差。图的模型大致为下图(有点丑。。。)。
因为我们需要求的量只有一个,即:vector3d(a,b,c),因此,在这个图中,顶点只有一个,我们通过随机生成n组 x,y,他们之间的误差构成了许许多多的边。构建出了图之后,下面以原书程序进行讲解整个优化的编程实现过程:
// 构建图优化,先设定g2o
typedef g2o::BlockSolver<g2o::BlockSolverTraits<3, 1>> BlockSolverType; // 定义图的类型,顶点为三维,边为一维
typedef g2o::LinearSolverDense<BlockSolverType::PoseMatrixType> LinearSolverType; // 线性求解器类型
// 梯度下降方法,可以从GN, LM, DogLeg 中选
auto solver = new g2o::OptimizationAlgorithmGaussNewton(
g2o::make_unique<BlockSolverType>(g2o::make_unique<LinearSolverType>()));
g2o::SparseOptimizer optimizer; // 图模型
optimizer.setAlgorithm(solver); // 设置求解器
optimizer.setVerbose(true); // 打开调试输出
// 往图中增加顶点
CurveFittingVertex *v = new CurveFittingVertex();
v->setEstimate(Eigen::Vector3d(ae, be, ce));
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);
}
// 执行优化
cout << "start optimization" << endl;
optimizer.initializeOptimization();
optimizer.optimize(10);
cout << "solve time cost = " << time_used.count() << " seconds. " << endl;
// 输出优化值
Eigen::Vector3d abc_estimate = v->estimate();
cout << "estimated model: " << abc_estimate.transpose() << endl;
其实整个图优化的过程大致都是这个框架,下面主要说下注意部分:
v->setId(0);
这句程序是对顶点进行编号,里面的0你可以写成任意的正整数,但是后面设置edge连接顶点时,必须要和这个一致。
// 往图中增加边
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);
}
该部分程序需要注意的是edge->setVertex(0, v); 其中的0为该边连接的第一个顶点,即前面编号为0的顶点v,因为是一元边,因此只需这一句。根据函数公式:
可知,y为观测值,故有edge->setMeasurement(y_data[i])。图构建出之后,即可进行迭代求解。
二、g2o进行前端位姿优化
在《视觉slam十四讲》中,前端的位姿优化主要讲了3D-3D的ICP、3D-2D的PNP位姿求解,本文主要讲解ICP的g2o求解。其实,这和前面的非线性优化很相似,两帧图像之间匹配出的3D点的差值构成了该图的一元边,顶点即为两帧之间的运动估计T,也就是相机的位姿,两帧之间优化出一个相机位姿T。
二范数:
图模型:
下面为g2o构建图过程以及图优化的代码编写,同样以十四讲书中的程序为例。
// 设定g2o,构建图模型
typedef g2o::BlockSolverX BlockSolverType;
typedef g2o::LinearSolverDense<BlockSolverType::PoseMatrixType> LinearSolverType; // 线性求解器类型
// 选择优化方法
auto solver = new g2o::OptimizationAlgorithmLevenberg(
g2o::make_unique<BlockSolverType>(g2o::make_unique<LinearSolverType>()));
g2o::SparseOptimizer optimizer; // 图模型
optimizer.setAlgorithm(solver); // 设置求解器
optimizer.setVerbose(true); // 打开调试输出
// vertex
VertexPose *pose = new VertexPose(); // camera pose
//两帧优化出一个最优pose 一个edge连接一个vertex
pose->setId(0);
pose->setEstimate(Sophus::SE3d()); //李群T
optimizer.addVertex(pose);
边的部分
//边的值即为两帧之间匹配点的3D坐标之差
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);
// P1 = T12P2,故观测值为pts1
edge->setMeasurement(Eigen::Vector3d(
pts1[i].x, pts1[i].y, pts1[i].z));
edge->setInformation(Eigen::Matrix3d::Identity());
optimizer.addEdge(edge);
}
对构建的图进行优化
chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
optimizer.initializeOptimization();
optimizer.optimize(10);
chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);
cout << "optimization costs time: " << time_used.count() << " seconds." << endl;
cout << endl << "after optimization:" << endl;
cout << "T=\n" << pose->estimate().matrix() << endl;
需要注意的是,因为设置的观测值为p1,所以优化的pose为p2到p1的坐标变换。 如果想要求得一个图像序列中,两两帧之间的T,那么只需要循环的调用
void bundleAdjustment(const vector<Point3f> &pts1,const vector<Point3f> &pts2, Mat &R, Mat &t)
这个函数,传入的参数为两帧之间匹配的3D点对即可。
三、g2o进行后端位姿优化
高翔博士的一起做RGBD-SLAM中有一个是利用g2o对vo进行了一个后端优化的讲解,我们也以此为例。
在前端的vo求解中,利用PNP或者ICP求出了两帧之间的相机运动(局部最优),连接起来即为视觉里程计,但这个过程中存在着误差累积,且一旦出现了错误匹配,里程计的误差将会逐渐增加,将整体的位姿进行优化,有助于误差的消除,达到全局的最优。
原函数:
二范数:
xi∗表示xi的估计值。
图模型:
与前面图模型不同的是,后端优化是将所有的pose相连,以第一个pose为初始值,整体进行迭代优化,edge为二元边。
图优化程序如下:
顶点部分:初始化第一个顶点
/*******************************
// 新增:有关g2o的初始化
*******************************/
typedef g2o::BlockSolver<g2o::BlockSolverTraits<6,6>> SlamBlockSolver; //图类型(顶点6维,边6维)
typedef g2o::LinearSolverCSparse< SlamBlockSolver::PoseMatrixType > SlamLinearSolver; //线性求解器类型
//选择优化方法
auto solver = new g2o::OptimizationAlgorithmLevenberg(g2o::make_unique<SlamBlockSolver>(g2o::make_unique<SlamLinearSolver>()));
g2o::SparseOptimizer globalOptimizer;
globalOptimizer.setAlgorithm( solver );
// 不要输出调试信息
globalOptimizer.setVerbose( false );
// 向globalOptimizer增加第一个顶点
g2o::VertexSE3* v = new g2o::VertexSE3();
//第一个顶点为初始值,编号为初始帧currIndex
v->setId( currIndex );
v->setEstimate( Eigen::Isometry3d::Identity() ); //估计为单位矩阵
v->setFixed( true ); //第一个顶点固定,不用优化
globalOptimizer.addVertex( v );
边部分:即为PNP估计出的T,也即相机位姿。
初始化第二个顶点,为当前帧的编号
这部分代码是在一个for循环中,主要就是向图中逐个的加入边和顶点。
// 向g2o中增加这个顶点与上一帧联系的边
// 顶点部分
// 顶点只需设定id即可
g2o::VertexSE3 *v = new g2o::VertexSE3();
v->setId( currIndex );
v->setEstimate( Eigen::Isometry3d::Identity() );
globalOptimizer.addVertex(v);
// 边部分
g2o::EdgeSE3* edge = new g2o::EdgeSE3();
// 连接此边的两个顶点id
edge->vertices() [0] = globalOptimizer.vertex( lastIndex );
edge->vertices() [1] = globalOptimizer.vertex( currIndex );
// 信息矩阵
Eigen::Matrix<double, 6, 6> information = Eigen::Matrix< double, 6,6 >::Identity();
// 信息矩阵是协方差矩阵的逆,表示我们对边的精度的预先估计
// 因为pose为6D的,信息矩阵是6*6的阵,假设位置和角度的估计精度均为0.1且互相独立
// 那么协方差则为对角为0.01的矩阵,信息阵则为100的矩阵
information(0,0) = information(1,1) = information(2,2) = 100;
information(3,3) = information(4,4) = information(5,5) = 100;
// 也可以将角度设大一些,表示对角度的估计更加准确
edge->setInformation( information );
// 边的估计即是pnp求解之结果
edge->setMeasurement( T );
// 将此边加入图中
globalOptimizer.addEdge(edge);
因为是后端的整体优化,所以需要将全部的相机位姿通过二元边进行连接起来,然后对构建的图进行优化。
edge->vertices() [0] = globalOptimizer.vertex( lastIndex );
edge->vertices() [1] = globalOptimizer.vertex( currIndex );
该边连接的第一个顶点为上一帧的T,第二个为当前帧的T。
globalOptimizer.initializeOptimization();
globalOptimizer.optimize( 100 ); //可以指定优化步数
globalOptimizer.clear();
总结
值得注意的是,在利用g2o进行图优化时,我们优化的公式为
但我们在进行点云拼接时,会想求这样的公式
中的T,所以需要获得pose->estimate().inverse()进行点云拼接。
参考资料: