需要解决的问题:
如何自定义Vertex和Edge?
如何选择Edge类型?一元还是二元?
如何赋值信息矩阵?
如何设置鲁棒核函数的阈值?
如何选择Vertex设置为固定?
如何边缘化以便稀疏化求解?
如何处理优化结束后outliner
?
如何设置根据卡方分布的临界值表对Edge的chi2
设置阈值?
1. g2o提供的顶点vertex
1) 李代数位姿
class VertexSE3Expmap : public BaseVertex<6, SE3Quat>
继承于BaseVertex
这个模板类
需要设置的模板参数:
- 参数
6
:SE3Quat
类型为六维,三维旋转,三维平移 - 参数
SE3Quat
:该类型旋转在前,平移在后,注意:类型内部使用的其实是四元数,不是李代数
该顶点需要设置的参数:
g2o::VertexSE3Expmap * vSE3 = new g2o::VertexSE3Expmap();
//【1】设置待优化位姿(这是粗略位姿)
vSE3->setEstimate(Converter::toSE3Quat(pKFi->GetPose()));
//【2】设置Id号
vSE3->setId(pKFi->mnId);
//【3】设置是否固定,第一帧固定
vSE3->setFixed(pKFi->mnId==0);
2) 空间点位置
class VertexSBAPointXYZ : public BaseVertex<3, Vector3d>
该顶点需要设置的参数:
g2o::VertexSBAPointXYZ* vPoint = new g2o::VertexSBAPointXYZ();
//【1】设置待优化空间点3D位置XYZ
vPoint->setEstimate(Converter::toVector3d(pMP->GetWorldPos()));
//【2】设置Id号
vPoint->setId(id);
//【3】是否边缘化(以便稀疏化求解)
vPoint->setMarginalized(true);
2. g2o提供的边edge
1) Point-Pose 二元边(
-SE3边)
即要优化MapPoints的位置,又要优化相机的位姿
class EdgeSE3ProjectXYZ: public BaseBinaryEdge<2, Vector2d, VertexSBAPointXYZ, VertexSE3Expmap>
继承于BaseBinaryEdge
这个二元边模板类
需要设置的模板参数:
- 参数
2
:观测值(这里是3D点在像素坐标系下的投影坐标)的维度 - 参数
Vector
:观测值类型,piexl.x,piexl.y - 参数
VertexSBAPointXYZ
:第一个顶点类型 - 参数
VertexSE3Expmap
:第二个顶点类型
该边需要设置的参数:
g2o::EdgeSE3ProjectXYZ* e = new g2o::EdgeSE3ProjectXYZ();
//【1】设置第一个顶点,注意该顶点类型与模板参数第一个顶点类型对应
e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(id)));
//【2】设置第二个顶点
e->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(pKFi->mnId)));
//【3】设置观测值,类型与模板参数对应
e->setMeasurement(obs);
const float &invSigma2 = pKFi->mvInvLevelSigma2[kpUn.octave];
//【4】设置信息矩阵,协方差
e->setInformation(Eigen::Matrix2d::Identity()*invSigma2);
//【5】设置鲁棒核函数
g2o::RobustKernelHuber* rk = new g2o::RobustKernelHuber;
e->setRobustKernel(rk);
rk->setDelta(thHuberMono);
//【6】设置相机内参
e->fx = pKFi->fx;
e->fy = pKFi->fy;
e->cx = pKFi->cx;
e->cy = pKFi->cy;
2) Pose 一元边(SE3)
仅优化相机位姿,为了构造出投影方程,需要按下面的方式把MapPoints的位置作为常量加入
class EdgeSE3ProjectXYZOnlyPose: public BaseUnaryEdge<2, Vector2d, VertexSE3Expmap>
该继承于BaseUnaryEdge
这个一元边模板类,需要设置的模板参数如上
该边需要设置的参数:
g2o::EdgeSE3ProjectXYZOnlyPose* e = new g2o::EdgeSE3ProjectXYZOnlyPose();
// 注意这里只设置一个顶点,其它一样
e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(0)));
e->setMeasurement(obs);
const float invSigma2 = pFrame->mvInvLevelSigma2[kpUn.octave];
e->setInformation(Eigen::Matrix2d::Identity()*invSigma2);
g2o::RobustKernelHuber* rk = new g2o::RobustKernelHuber;
e->setRobustKernel(rk);
rk->setDelta(deltaMono); /** @attention 设置阈值,卡方自由度为2,内点概率95%对应的临界值*/
e->fx = pFrame->fx;
e->fy = pFrame->fy;
e->cx = pFrame->cx;
e->cy = pFrame->cy;
/** @attention 需要在这里设置<不做优化>的MapPoints的位置*/
cv::Mat Xw = pMP->GetWorldPos();
e->Xw[0] = Xw.at<float>(0);
e->Xw[1] = Xw.at<float>(1);
e->Xw[2] = Xw.at<float>(2);
3) Pose-Pose 二元边(SE3-SE3边)
class G2O_TYPES_SBA_API EdgeSE3Expmap : public BaseBinaryEdge<6, SE3Quat, VertexSE3Expmap, VertexSE3Expmap>
@todo 有点复杂,后续补充
3. g2o优化
//【1】指定pose维度为6, landmark维度为3
typedef g2o::BlockSolver< g2o::BlockSolverTraits<6,3> > Block;
//【2】线性方程求解器,使用CSparse分解
// 还有g2o::LinearSolverDense,使用cholesky分解
Block::LinearSolverType* linearSolver = new g2o::LinearSolverCSparse<Block::PoseMatrixType>();
//【3】矩阵块求解器
Block* solver_ptr = new Block ( linearSolver );
//【4】梯度下降方法,从GN, LM, DogLeg 中选
g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg ( solver_ptr );
// 图模型
g2o::SparseOptimizer optimizer;
//【5】设置求解器
optimizer.setAlgorithm ( solver );
......
......
......
//【6】打开调试输出
optimizer.setVerbose ( true );
optimizer.initializeOptimization();
//【7】指定迭代次数:100次
optimizer.optimize ( 100 );
g2o中经常使用的BlockSolver_6_3
、BlockSolver_7_3
即是上面程序中【1】所作的事,如下:
// solver for BA/3D SLAM
typedef BlockSolver< BlockSolverTraits<6, 3> > BlockSolver_6_3;
// solver fo BA with scale
typedef BlockSolver< BlockSolverTraits<7, 3> > BlockSolver_7_3;
4. 检测outliner
优化完成后,对每一条边都进行检查,剔除误差较大的边(认为是错误的边),并设置setLevel
为0,即下次不再对该边进行优化
第二次优化完成后,会对连接偏差比较大的边,在关键帧中剔除对该MapPoint的观测,在MapPoint中剔除对该关键帧的观测,具体实现参考orb-slam源码Optimizer::LocalBundleAdjustment
optimizer.optimize ( 100 );
// 优化完成后,进行Edge的检查
for(size_t i=0, iend=vpEdgesMono.size(); i<iend;i++)
{
g2o::EdgeSE3ProjectXYZ* e = vpEdgesMono[i];
MapPoint* pMP = vpMapPointEdgeMono[i];
if(pMP->isBad())
continue;
// 基于卡方检验计算出的阈值(假设测量有一个像素的偏差)
// 第二个判断条件,用于检查构成该边的MapPoint在该相机坐标系下的深度是否为正?
if(e->chi2()>5.991 || !e->isDepthPositive())
{
e->setLevel(1);// 不优化
}
// 因为剔除了错误的边,所以下次优化不再使用核函数
e->setRobustKernel(0);
}
5. 加入对相机内参的优化
g2o::CameraParameters* camera = new g2o::CameraParameters (
K.at<double> ( 0,0 ), Eigen::Vector2d ( K.at<double> ( 0,2 ), K.at<double> ( 1,2 ) ), 0
);
// 设置Id号为0即可
camera->setId ( 0 );
optimizer.addParameter ( camera );
如果不想优化相机内参,则内参按照第二步中二元边中的程序demo中设置
6. 实践:自定义PoseGraphy优化
g2o/types/slam3d/中的SE3位姿,它实质上使用的是四元数而非李代数。接下来使用Sophus表示的李代数来构造g2o中顶点和边。
自定义g2o用到的顶点和边
Vertex:关键帧的位姿
Edge:测量出来的两个关键帧位姿节点之间的相对运动
,可以来自wheel odometry、或者IMU、或者orb-slam中Optimizer::OptimizeEssentialGraph
函数中的方式
1)自定义李代数顶点
class VertexSE3LieAlgebra : public g2o::BaseVertex<6, SE3>
{
......
}
VertexSE3LieAlgebra
是自定义的类名,继承于g2o::BaseVertex
两个模板参数分别表示:
- 参数
6
:顶点的维度,这里是Sopuse
的SE3
的维度 - 参数
SE3
:顶点的类型,这里是Sopuse
的SE3
2)自定义两个李代数顶点之边
class EdgeSE3LieAlgebra: public g2o::BaseBinaryEdge<6, SE3, VertexSE3LieAlgebra, VertexSE3LieAlgebra>
{
......
}
EdgeSE3LieAlgebra
是自定义的类名,继承于g2o::BaseBinaryEdge
模板类(二元边)
四个模板参数分别表示:
- 参数
6
:观测值的维度,这里是第二个参数SE3
的维度 - 参数
SE3
:观测值的类型,这里是Sopuse
的SE3
,两个位姿节点之间的相对运动 ,即相邻两关键帧之间的位姿变化 - 参数
VertexSE3LieAlgebra
: 第一个顶点的类型,上一步自定义的关于相机位姿的顶点 - 参数
VertexSE3LieAlgebra
: 第二个顶点的类型
<完>
@LeatherWang