1. BundleAdjustment
（1）顶点与边
全局优化函数BundleAdjustment主要用于优化所有关键帧的位姿和地图点。因此，其顶点是待优化的所有关键帧的位姿和所有地图点。并且，以第0个关键帧位姿作为参考基准，不参与优化。顶点中关键帧位姿的类型为
g2o::VertexSE3Expmap，顶点中地图点的类型为g2o::VertexSBAPointXYZ。
由于地图点和观测到它的关键帧存在投影约束关系，因此全局优化中的边是二元边。边的两个顶点分别是地图点坐标以及观测到它的关键帧的位姿。
对于单目相机模式，边的类型为g2o::EdgeSE3ProjectXYZ。对于双目/RGB-D相机模式，边的类型为
g2o::EdgeStereoSE3ProjectXYZ。
（2）全局优化与局部优化的区别
- 相同点：二者的顶点类型与边类型均相同、优化变量均为关键帧位姿与地图点、误差定义均为重投影误差。
- 不同点：局部优化中使用的是局部关键帧以及局部地图点，全局优化中使用的是所有的关键帧和地图点。
（3）BundleAdjustment整体流程
- 初始化g2o优化器。
- 向优化器中添加顶点：所有的关键帧位姿和所有的地图点。
- 向优化器中添加边：所有的地图点和观测到它的关键帧的投影关系，单目和双目模式下有所不同。
- 开始优化，迭代10次。
- 将优化后的结果保存起来，注意，没有直接原位替换更新。
（4）BundleAdjustment代码实现
/**
* @brief bundle adjustment 优化过程
* 1. Vertex: g2o::VertexSE3Expmap()，即当前帧的Tcw
* g2o::VertexSBAPointXYZ()，MapPoint的mWorldPos
* 2. Edge:
* - g2o::EdgeSE3ProjectXYZ()，BaseBinaryEdge
* + Vertex：待优化当前帧的Tcw
* + Vertex：待优化MapPoint的mWorldPos
* + measurement：MapPoint在当前帧中的二维位置(u,v)
* + InfoMatrix: invSigma2(与特征点所在的尺度有关)
*
* @param[in] vpKFs 参与BA的所有关键帧
* @param[in] vpMP 参与BA的所有地图点
* @param[in] nIterations 优化迭代次数
* @param[in] pbStopFlag 外部控制BA结束标志
* @param[in] nLoopKF 形成闭环的当前关键帧的id
* @param[in] bRobust 是否使用核函数
*/
void Optimizer::BundleAdjustment(const vector<KeyFrame *> &vpKFs,
const vector<MapPoint *> &vpMP,
int nIterations, bool* pbStopFlag,
const unsigned long nLoopKF, const bool bRobust)
{
// vbNotIncludedMP: 存储不参与优化的地图点
vector<bool> vbNotIncludedMP;
vbNotIncludedMP.resize(vpMP.size());
// Step 1 初始化g2o优化器
g2o::SparseOptimizer optimizer;
g2o::BlockSolver_6_3::LinearSolverType * linearSolver;
linearSolver = new g2o::LinearSolverEigen<g2o::BlockSolver_6_3::PoseMatrixType>();
g2o::BlockSolver_6_3 * solver_ptr = new g2o::BlockSolver_6_3(linearSolver);
// 使用LM算法优化
g2o::OptimizationAlgorithmLevenberg* solver = new
g2o::OptimizationAlgorithmLevenberg(solver_ptr);
optimizer.setAlgorithm(solver);
// 如果这个时候外部请求终止，那就结束. 注意这句执行之后，如果外部再请求结束BA，那么无法结束不了
if(pbStopFlag)
optimizer.setForceStopFlag(pbStopFlag);
// 记录添加到优化器中的关键帧位姿顶点的最大关键帧id
// 以便后续是将优化器中地图点顶点放在关键帧位姿顶点的后面
long unsigned int maxKFid = 0;
// Step 2 向优化器添加顶点
// Step 2.1 ：向优化器添加关键帧位姿顶点
// 遍历当前地图中的所有关键帧
for(size_t i=0; i<vpKFs.size(); i++)
{
// 获取遍历到的当前关键帧
KeyFrame* pKF = vpKFs[i];
// 跳过无效关键帧
if(pKF->isBad())
continue;
// 对于每一个能用的关键帧构造SE3顶点,其实就是当前关键帧的位姿
g2o::VertexSE3Expmap * vSE3 = new g2o::VertexSE3Expmap();
vSE3->setEstimate(Converter::toSE3Quat(pKF->GetPose()));
// 顶点的id就是关键帧在所有关键帧中的id
vSE3->setId(pKF->mnId);
// 只有第0帧关键帧不优化（参考基准）
vSE3->setFixed(pKF->mnId==0);
// 向优化器中添加顶点，并且更新maxKFid
optimizer.addVertex(vSE3);
if(pKF->mnId>maxKFid)
maxKFid=pKF->mnId;
}
// 卡方分布 95% 以上可信度时候的阈值
const float thHuber2D = sqrt(5.99); // 自由度为2
const float thHuber3D = sqrt(7.815); // 自由度为3
// Step 2.2：向优化器添加地图点作为顶点
// 遍历地图中的所有地图点
for(size_t i=0; i<vpMP.size(); i++)
{
// 获取遍历到的当前地图点
MapPoint* pMP = vpMP[i];
// 跳过无效地图点
if(pMP->isBad())
continue;
// 创建类型为g2o::VertexSBAPointXYZ的地图点顶点
g2o::VertexSBAPointXYZ* vPoint = new g2o::VertexSBAPointXYZ();
// 注意由于地图点的位置是使用cv::Mat数据类型表示的,这里需要转换成为Eigen::Vector3d类型
vPoint->setEstimate(Converter::toVector3d(pMP->GetWorldPos()));
// 前面记录maxKFid 是在这里使用的，将地图点顶点排在关键帧位姿顶点后面
const int id = pMP->mnId+maxKFid+1;
vPoint->setId(id);
/*
注意g2o在做BA的优化时必须将其所有地图点全部schur掉，否则会出错。
原因是使用了g2o::LinearSolver<BalBlockSolver::PoseMatrixType>这个类型来指定linearsolver,
其中模板参数当中的位姿矩阵类型在程序中为相机姿态参数的维度，于是BA当中schur消元后解得线性方程组必须是只含有相机姿态变量。
Ceres库则没有这样的限制
疑问：这里说的还没有明白
*/
vPoint->setMarginalized(true);
// 将顶点添加到优化器中
optimizer.addVertex(vPoint);
// 取出地图点和关键帧之间观测的关系
const map<KeyFrame*,size_t> observations = pMP->GetObservations();
// 边计数
int nEdges = 0;
// Step 3：向优化器添加投影边（是在遍历地图点、添加地图点的顶点的时候顺便添加的）
// 遍历观察到当前地图点的所有关键帧
for(map<KeyFrame*,size_t>::const_iterator mit=observations.begin();
mit!=observations.end(); mit++)
{
// 获取当前遍历到的关键帧
KeyFrame* pKF = mit->first;
// 跳过不合法的关键帧
if(pKF->isBad() || pKF->mnId>maxKFid)
continue;
nEdges++; // 合法的边要计数累加
// 取出该地图点对应该关键帧的2D特征点
const cv::KeyPoint &kpUn = pKF->mvKeysUn[mit->second];
if(pKF->mvuRight[mit->second]<0) // 单目模式
{
// 构造观测
Eigen::Matrix<double,2,1> obs;
obs << kpUn.pt.x, kpUn.pt.y;
// 创建边
g2o::EdgeSE3ProjectXYZ* e = new g2o::EdgeSE3ProjectXYZ();
// 边连接的第0号顶点对应的是第id个地图点
e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>
(optimizer.vertex(id)));
// 边连接的第1号顶点对应的是第mnId个关键帧
e->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex*>
(optimizer.vertex(pKF->mnId)));
// 设置观测
e->setMeasurement(obs);
// 信息矩阵，其可信程度与特征点在图像金字塔中的图层有关，图层越高，可信度越差
// 为了避免出现信息矩阵中元素为负数的情况，这里使用的是sigma^(-2)
const float &invSigma2 = pKF->mvInvLevelSigma2[kpUn.octave];
e->setInformation(Eigen::Matrix2d::Identity()*invSigma2);
// 使用鲁棒核函数
if(bRobust)
{
g2o::RobustKernelHuber* rk = new g2o::RobustKernelHuber;
e->setRobustKernel(rk);
// 重投影误差由2个平方项的和组成，因此使用卡方分布自由度为2时的阈值
rk->setDelta(thHuber2D);
}
// 设置相机内参
e->fx = pKF->fx;
e->fy = pKF->fy;
e->cx = pKF->cx;
e->cy = pKF->cy;
// 将边添加到优化器中
optimizer.addEdge(e);
}
else // 双目/RGB-D模式下
{
// 观测数据：投影点的x坐标，投影点的y坐标，和投影点在右目中的x坐标(默认y方向上已对齐)
Eigen::Matrix<double,3,1> obs;
const float kp_ur = pKF->mvuRight[mit->second];
obs << kpUn.pt.x, kpUn.pt.y, kp_ur;
// 定义双目误差边
g2o::EdgeStereoSE3ProjectXYZ* e = new g2o::EdgeStereoSE3ProjectXYZ();
// 边连接的第0号顶点对应的是第id个地图点
e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>
(optimizer.vertex(id)));
// 边连接的第1号顶点对应的是第mnId个关键帧
e->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex*>
(optimizer.vertex(pKF->mnId)));
// 设置观测
e->setMeasurement(obs);
// 信息矩阵与单目模式相同，考虑的是左目特征点的所在图层
const float &invSigma2 = pKF->mvInvLevelSigma2[kpUn.octave];
Eigen::Matrix3d Info = Eigen::Matrix3d::Identity()*invSigma2;
e->setInformation(Info);
// 如果要使用鲁棒核函数
if(bRobust)
{
g2o::RobustKernelHuber* rk = new g2o::RobustKernelHuber;
e->setRobustKernel(rk);
// 重投影误差由3个平方项的和组成，因此使用卡方分布自由度为3时的阈值
rk->setDelta(thHuber3D);
}
// 设置相机内参
e->fx = pKF->fx;
e->fy = pKF->fy;
e->cx = pKF->cx;
e->cy = pKF->cy;
e->bf = pKF->mbf;
// 将边添加到优化器中
optimizer.addEdge(e);
}
}
// 如果没有关键帧观测到这个地图点，那么删掉这个顶点，且这个地图点不参与优化
if(nEdges==0)
{
optimizer.removeVertex(vPoint);
vbNotIncludedMP[i]=true;
}
else
{
vbNotIncludedMP[i]=false;
}
}
// Step 4：开始优化
optimizer.initializeOptimization();
optimizer.optimize(nIterations);
// Step 5：得到优化的结果
// Step 5.1 遍历所有的关键帧
for(size_t i=0; i<vpKFs.size(); i++)
{
KeyFrame* pKF = vpKFs[i];
if(pKF->isBad())
continue;
// 获取到当前关键帧pKF优化后的位姿
g2o::VertexSE3Expmap* vSE3 = static_cast<g2o::VertexSE3Expmap*>
(optimizer.vertex(pKF->mnId));
g2o::SE3Quat SE3quat = vSE3->estimate();
if(nLoopKF==0)
{
// 原则上来讲不会出现"当前闭环关键帧是第0帧"的情况,如果这种情况出现
// 只能够说明是在创建初始地图点的时候调用的这个全局BA函数.
// 这个时候,地图中就只有两个关键帧,其中优化后的位姿数据可以直接写入到帧的成员变量中
pKF->SetPose(Converter::toCvMat(SE3quat));
}
else
{
// 正常的操作,先把优化后的位姿写入到帧的一个专门的成员变量mTcwGBA中备用
pKF->mTcwGBA.create(4,4,CV_32F);
Converter::toCvMat(SE3quat).copyTo(pKF->mTcwGBA);
pKF->mnBAGlobalForKF = nLoopKF;
}
}
// Step 5.2 遍历所有地图点,去除其中没有参与优化过程的地图点
for(size_t i=0; i<vpMP.size(); i++)
{
// 没有参与优化的地图点就直接跳过
if(vbNotIncludedMP[i])
continue;
MapPoint* pMP = vpMP[i];
if(pMP->isBad()) // Bad的地图点也直接跳过
continue;
// 获取优化之后的地图点的位置
g2o::VertexSBAPointXYZ* vPoint = static_cast<g2o::VertexSBAPointXYZ*>
(optimizer.vertex(pMP->mnId+maxKFid+1));
// 和上面对关键帧的操作一样
if(nLoopKF==0)
{
// 如果这个GBA是在创建初始地图时调用的话, 那么地图点的位姿也可以直接写入到对应的成员变量中
pMP->SetWorldPos(Converter::toCvMat(vPoint->estimate()));
// 更新当前地图点的平均观测方向和观测距离范围等属性
pMP->UpdateNormalAndDepth();
}
else
{
// 反之,如果是正常的闭环过程调用,就先临时保存一下
pMP->mPosGBA.create(3,1,CV_32F);
Converter::toCvMat(vPoint->estimate()).copyTo(pMP->mPosGBA);
pMP->mnBAGlobalForKF = nLoopKF;
}
}
}
202
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
