2021SC@SDUSC
1.前言
这一部分代码量巨大,查阅了很多资料结合来看的代码,将分为以下部分进行分析
- 单帧优化
- 局部地图优化
- 全局优化
- 尺度与重力优化
- sim3优化
- 地图回环优化
- 地图融合优化
下面给出逐步注释分析
2.代码分析
Local Bundle Adjustment LoopClosing::MergeLocal() 融合地图时使用,纯视觉
优化目标:
- vpAdjustKF
- 2.vpAdjustKF与vpFixedKF对应的MP点
优化所有的当前关键帧共视窗口里的关键帧和地图点, 固定所有融合帧共视窗口里的帧
在这里插入代码片void Optimizer::LocalBundleAdjustment(KeyFrame *pMainKF, vector<KeyFrame *> vpAdjustKF, vector<KeyFrame *> vpFixedKF, bool *pbStopFlag)
{
bool bShowImages = false;
// 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);
g2o::OptimizationAlgorithmLevenberg *solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr);
optimizer.setAlgorithm(solver);
optimizer.setVerbose(false);
if (pbStopFlag)
optimizer.setForceStopFlag(pbStopFlag);
long unsigned int maxKFid = 0;
set<KeyFrame *> spKeyFrameBA; // 存放关键帧,包含固定的与不固定的
vector<MapPoint *> vpMPs;
Map *pCurrentMap = pMainKF->GetMap();
// Set fixed KeyFrame vertices
// 2. 构建固定关键帧的节点,并储存对应的MP
for (KeyFrame *pKFi : vpFixedKF)
{
if (pKFi->isBad() || pKFi->GetMap() != pCurrentMap)
{
Verbose::PrintMess("ERROR LBA: KF is bad or is not in the current map", Verbose::VERBOSITY_NORMAL);
continue;
}
pKFi->mnBALocalForMerge = pMainKF->mnId; // 防止重复添加
g2o::VertexSE3Expmap *vSE3 = new g2o::VertexSE3Expmap();
vSE3->setEstimate(Converter::toSE3Quat(pKFi->GetPose()));
vSE3->setId(pKFi->mnId);
vSE3->setFixed(true);
optimizer.addVertex(vSE3);
if (pKFi->mnId > maxKFid)
maxKFid = pKFi->mnId;
set<MapPoint *> spViewMPs = pKFi->GetMapPoints();
for (MapPoint *pMPi : spViewMPs)
{
if (pMPi)
if (!pMPi->isBad() && pMPi->GetMap() == pCurrentMap)
if (pMPi->mnBALocalForMerge != pMainKF->mnId)
{
vpMPs.push_back(pMPi);
pMPi->mnBALocalForMerge = pMainKF->mnId;
}
}
spKeyFrameBA.insert(pKFi);
}
// Set non fixed Keyframe vertices
// 3. 构建不固定关键帧的节点,并储存对应的MP
set<KeyFrame *> spAdjustKF(vpAdjustKF.begin(), vpAdjustKF.end());
for (KeyFrame *pKFi : vpAdjustKF)
{
if (pKFi->isBad() || pKFi->GetMap() != pCurrentMap)
continue;
pKFi->mnBALocalForKF = pMainKF->mnId; // 防止重复添加
g2o::VertexSE3Expmap *vSE3 = new g2o::VertexSE3Expmap();
vSE3->setEstimate(Converter::toSE3Quat(pKFi->GetPose()));
vSE3->setId(pKFi->mnId);
optimizer.addVertex(vSE3);
if (pKFi->mnId > maxKFid)
maxKFid = pKFi->mnId;
set<MapPoint *> spViewMPs = pKFi->GetMapPoints();
for (MapPoint *pMPi : spViewMPs)
{
if (pMPi)
{
if (!pMPi->isBad() && pMPi->GetMap() == pCurrentMap)
{
if (pMPi->mnBALocalForMerge != pMainKF->mnId)
{
vpMPs.push_back(pMPi);
pMPi->mnBALocalForMerge = pMainKF->mnId;
}
}
}
}
spKeyFrameBA.insert(pKFi);
}
// 准备存放边的vector
const int nExpectedSize = (vpAdjustKF.size() + vpFixedKF.size()) * vpMPs.size();
vector<ORB_SLAM3::EdgeSE3ProjectXYZ *> vpEdgesMono;
vpEdgesMono.reserve(nExpectedSize);
vector<KeyFrame *> vpEdgeKFMono;
vpEdgeKFMono.reserve(nExpectedSize);
vector<MapPoint *> vpMapPointEdgeMono;
vpMapPointEdgeMono.reserve(nExpectedSize);
vector<g2o::EdgeStereoSE3ProjectXYZ *> vpEdgesStereo;
vpEdgesStereo.reserve(nExpectedSize);
vector<KeyFrame *> vpEdgeKFStereo;
vpEdgeKFStereo.reserve(nExpectedSize);
vector<MapPoint *> vpMapPointEdgeStereo;
vpMapPointEdgeStereo.reserve(nExpectedSize);
const float thHuber2D = sqrt(5.99);
const float thHuber3D = sqrt(7.815);
// Set MapPoint vertices
map<KeyFrame *, int> mpObsKFs; // 统计每个关键帧对应的MP点数,调试输出用
map<KeyFrame *, int> mpObsFinalKFs; // 统计每个MP对应的关键帧数,调试输出用
map<MapPoint *, int> mpObsMPs; // 统计每个MP被观测的图片数,双目就是两个,调试输出用
// 4. 确定MP节点与边的连接
for (unsigned int i = 0; i < vpMPs.size(); ++i)
{
MapPoint *pMPi = vpMPs[i];
if (pMPi->isBad())
continue;
g2o::VertexSBAPointXYZ *vPoint = new g2o::VertexSBAPointXYZ();
vPoint->setEstimate(Converter::toVector3d(pMPi->GetWorldPos()));
const int id = pMPi->mnId + maxKFid + 1;
vPoint->setId(id);
vPoint->setMarginalized(true);
optimizer.addVertex(vPoint);
const map<KeyFrame *, tuple<int, int>> observations = pMPi->GetObservations();
int nEdges = 0;
// SET EDGES
for (map<KeyFrame *, tuple<int, int>>::const_iterator mit = observations.begin(); mit != observations.end(); mit++)
{
KeyFrame *pKF = mit->first;
// 跳过的条件
// 1. 帧坏了
// 2. 帧靠后
// 3. 不在参与优化帧的里面
// 4. 在左相机上不存在这个三维点
if (pKF->isBad() || pKF->mnId > maxKFid || pKF->mnBALocalForMerge != pMainKF->mnId || !pKF->GetMapPoint(get<0>(mit->second)))
continue;
nEdges++;
const cv::KeyPoint &kpUn = pKF->mvKeysUn[get<0>(mit->second)];
// 注意!!!这里没有考虑相机2
if (pKF->mvuRight[get<0>(mit->second)] < 0) // Monocular
{
mpObsMPs[pMPi]++;
Eigen::Matrix<double, 2, 1> obs;
obs << kpUn.pt.x, kpUn.pt.y;
ORB_SLAM3::EdgeSE3ProjectXYZ *e = new ORB_SLAM3::EdgeSE3ProjectXYZ();
e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex *>(optimizer.vertex(id)));
e->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex *>(optimizer.vertex(pKF->mnId)));
e->setMeasurement(obs);
const float &invSigma2 = pKF->mvInvLevelSigma2[kpUn.octave];
e->setInformation(Eigen::Matrix2d::Identity() * invSigma2);
g2o::RobustKernelHuber *rk = new g2o::RobustKernelHuber;
e->setRobustKernel(rk);
rk->setDelta(thHuber2D);
e->pCamera = pKF->mpCamera;
optimizer.addEdge(e);
vpEdgesMono.push_back(e);
vpEdgeKFMono.push_back(pKF);
vpMapPointEdgeMono.push_back(pMPi);
mpObsKFs[pKF]++;
}
else // RGBD or Stereo
{
mpObsMPs[pMPi] += 2;
Eigen::Matrix<double, 3, 1> obs;
const float kp_ur = pKF->mvuRight[get<0>(mit->second)];
obs << kpUn.pt.x, kpUn.pt.y, kp_ur;
g2o::EdgeStereoSE3ProjectXYZ *e = new g2o::EdgeStereoSE3ProjectXYZ();
e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex *>(optimizer.vertex(id)));
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);
g2o::RobustKernelHuber *rk = new g2o::RobustKernelHuber;
e->setRobustKernel(rk);
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);
vpEdgesStereo.push_back(e);
vpEdgeKFStereo.push_back(pKF);
vpMapPointEdgeStereo.push_back(pMPi);
mpObsKFs[pKF]++;
}
}
}
// 这段没啥用,调试输出的,暂时不看
map<int, int> mStatsObs;
for (map<MapPoint *, int>::iterator it = mpObsMPs.begin(); it != mpObsMPs.end(); ++it)
{
MapPoint *pMPi = it->first;
int numObs = it->second;
mStatsObs[numObs]++;
}
if (pbStopFlag)
if (*pbStopFlag)
return;
// 5. 优化
optimizer.initializeOptimization();
optimizer.optimize(5);
bool bDoMore = true;
if (pbStopFlag)
if (*pbStopFlag)
bDoMore = false;
// 6. 剔除误差大的边
map<unsigned long int, int> mWrongObsKF;
if (bDoMore)
{
// Check inlier observations
int badMonoMP = 0, badStereoMP = 0;
for (size_t i = 0, iend = vpEdgesMono.size(); i < iend; i++)
{
ORB_SLAM3::EdgeSE3ProjectXYZ *e = vpEdgesMono[i];
MapPoint *pMP = vpMapPointEdgeMono[i];
if (pMP->isBad())
continue;
if (e->chi2() > 5.991 || !e->isDepthPositive())
{
e->setLevel(1);
badMonoMP++;
}
e->setRobustKernel(0);
}
for (size_t i = 0, iend = vpEdgesStereo.size(); i < iend; i++)
{
g2o::EdgeStereoSE3ProjectXYZ *e = vpEdgesStereo[i];
MapPoint *pMP = vpMapPointEdgeStereo[i];
if (pMP->isBad())
continue;
if (e->chi2() > 7.815 || !e->isDepthPositive())
{
e->setLevel(1);
badStereoMP++;
}
e->setRobustKernel(0);
}
Verbose::PrintMess("LBA: First optimization, there are " + to_string(badMonoMP) + " monocular and " + to_string(badStereoMP) + " sterero bad edges", Verbose::VERBOSITY_DEBUG);
// Optimize again without the outliers
optimizer.initializeOptimization(0);
optimizer.optimize(10);
}
// 7. 删除误差大的点与帧的连接关系
vector<pair<KeyFrame *, MapPoint *>> vToErase;
vToErase.reserve(vpEdgesMono.size() + vpEdgesStereo.size());
set<MapPoint *> spErasedMPs;
set<KeyFrame *> spErasedKFs;
// Check inlier observations
int badMonoMP = 0, badStereoMP = 0;
for (size_t i = 0, iend = vpEdgesMono.size(); i < iend; i++)
{
ORB_SLAM3::EdgeSE3ProjectXYZ *e = vpEdgesMono[i];
MapPoint *pMP = vpMapPointEdgeMono[i];
if (pMP->isBad())
continue;
if (e->chi2() > 5.991 || !e->isDepthPositive())
{
KeyFrame *pKFi = vpEdgeKFMono[i];
vToErase.push_back(make_pair(pKFi, pMP));
mWrongObsKF[pKFi->mnId]++;
badMonoMP++;
spErasedMPs.insert(pMP);
spErasedKFs.insert(pKFi);
}
}
for (size_t i = 0, iend = vpEdgesStereo.size(); i < iend; i++)
{
g2o::EdgeStereoSE3ProjectXYZ *e = vpEdgesStereo[i];
MapPoint *pMP = vpMapPointEdgeStereo[i];
if (pMP->isBad())
continue;
if (e->chi2() > 7.815 || !e->isDepthPositive())
{
KeyFrame *pKFi = vpEdgeKFStereo[i];
vToErase.push_back(make_pair(pKFi, pMP));
mWrongObsKF[pKFi->mnId]++;
badStereoMP++;
spErasedMPs.insert(pMP);
spErasedKFs.insert(pKFi);
}
}
Verbose::PrintMess("LBA: Second optimization, there are " + to_string(badMonoMP) + " monocular and " + to_string(badStereoMP) + " sterero bad edges", Verbose::VERBOSITY_DEBUG);
// Get Map Mutex
unique_lock<mutex> lock(pMainKF->GetMap()->mMutexMapUpdate);
if (!vToErase.empty())
{
map<KeyFrame *, int> mpMPs_in_KF;
for (KeyFrame *pKFi : spErasedKFs)
{
int num_MPs = pKFi->GetMapPoints().size();
mpMPs_in_KF[pKFi] = num_MPs;
}
// 剔除链接关系,其他均为调试
Verbose::PrintMess("LBA: There are " + to_string(vToErase.size()) + " observations whose will be deleted from the map", Verbose::VERBOSITY_DEBUG);
for (size_t i = 0; i < vToErase.size(); i++)
{
KeyFrame *pKFi = vToErase[i].first;
MapPoint *pMPi = vToErase[i].second;
pKFi->EraseMapPointMatch(pMPi);
pMPi->EraseObservation(pKFi);
}
Verbose::PrintMess("LBA: " + to_string(spErasedMPs.size()) + " MPs had deleted observations", Verbose::VERBOSITY_DEBUG);
Verbose::PrintMess("LBA: Current map is " + to_string(pMainKF->GetMap()->GetId()), Verbose::VERBOSITY_DEBUG);
int numErasedMP = 0;
for (MapPoint *pMPi : spErasedMPs)
{
if (pMPi->isBad())
{
Verbose::PrintMess("LBA: MP " + to_string(pMPi->mnId) + " has lost almost all the observations, its origin map is " + to_string(pMPi->mnOriginMapId), Verbose::VERBOSITY_DEBUG);
numErasedMP++;
}
}
Verbose::PrintMess("LBA: " + to_string(numErasedMP) + " MPs had deleted from the map", Verbose::VERBOSITY_DEBUG);
for (KeyFrame *pKFi : spErasedKFs)
{
int num_MPs = pKFi->GetMapPoints().size();
int num_init_MPs = mpMPs_in_KF[pKFi];
Verbose::PrintMess("LBA: Initially KF " + to_string(pKFi->mnId) + " had " + to_string(num_init_MPs) + ", at the end has " + to_string(num_MPs), Verbose::VERBOSITY_DEBUG);
}
}
// 没用,调试的
for (unsigned int i = 0; i < vpMPs.size(); ++i)
{
MapPoint *pMPi = vpMPs[i];
if (pMPi->isBad())
continue;
const map<KeyFrame *, tuple<int, int>> observations = pMPi->GetObservations();
for (map<KeyFrame *, tuple<int, int>>::const_iterator mit = observations.begin(); mit != observations.end(); mit++)
{
KeyFrame *pKF = mit->first;
if (pKF->isBad() || pKF->mnId > maxKFid || pKF->mnBALocalForKF != pMainKF->mnId || !pKF->GetMapPoint(get<0>(mit->second)))
continue;
const cv::KeyPoint &kpUn = pKF->mvKeysUn[get<0>(mit->second)];
if (pKF->mvuRight[get<0>(mit->second)] < 0) // Monocular
{
mpObsFinalKFs[pKF]++;
}
else // RGBD or Stereo
{
mpObsFinalKFs[pKF]++;
}
}
}
// Recover optimized data
// 8. 取出结果
// Keyframes
for (KeyFrame *pKFi : vpAdjustKF)
{
if (pKFi->isBad())
continue;
// 8.1 取出对应位姿
g2o::VertexSE3Expmap *vSE3 = static_cast<g2o::VertexSE3Expmap *>(optimizer.vertex(pKFi->mnId));
g2o::SE3Quat SE3quat = vSE3->estimate();
cv::Mat Tiw = Converter::toCvMat(SE3quat);
// 统计调试用
int numMonoBadPoints = 0, numMonoOptPoints = 0;
int numStereoBadPoints = 0, numStereoOptPoints = 0;
vector<MapPoint *> vpMonoMPsOpt, vpStereoMPsOpt; // 存放mp内点
vector<MapPoint *> vpMonoMPsBad, vpStereoMPsBad; // 存放mp外点
// 8.2 卡方检验,调试用的貌似没别的作用
for (size_t i = 0, iend = vpEdgesMono.size(); i < iend; i++)
{
ORB_SLAM3::EdgeSE3ProjectXYZ *e = vpEdgesMono[i];
MapPoint *pMP = vpMapPointEdgeMono[i];
KeyFrame *pKFedge = vpEdgeKFMono[i];
if (pKFi != pKFedge)
{
continue;
}
if (pMP->isBad())
continue;
if (e->chi2() > 5.991 || !e->isDepthPositive())
{
numMonoBadPoints++;
vpMonoMPsBad.push_back(pMP);
}
else
{
numMonoOptPoints++;
vpMonoMPsOpt.push_back(pMP);
}
}
for (size_t i = 0, iend = vpEdgesStereo.size(); i < iend; i++)
{
g2o::EdgeStereoSE3ProjectXYZ *e = vpEdgesStereo[i];
MapPoint *pMP = vpMapPointEdgeStereo[i];
KeyFrame *pKFedge = vpEdgeKFMono[i];
if (pKFi != pKFedge)
{
continue;
}
if (pMP->isBad())
continue;
if (e->chi2() > 7.815 || !e->isDepthPositive())
{
numStereoBadPoints++;
vpStereoMPsBad.push_back(pMP);
}
else
{
numStereoOptPoints++;
vpStereoMPsOpt.push_back(pMP);
}
}
if (numMonoOptPoints + numStereoOptPoints < 50)
{
Verbose::PrintMess("LBA ERROR: KF " + to_string(pKFi->mnId) + " has only " + to_string(numMonoOptPoints) + " monocular and " + to_string(numStereoOptPoints) + " stereo points", Verbose::VERBOSITY_DEBUG);
}
// 设置位姿
pKFi->SetPose(Tiw);
}
// 更新点的坐标
for (MapPoint *pMPi : vpMPs)
{
if (pMPi->isBad())
continue;
g2o::VertexSBAPointXYZ *vPoint = static_cast<g2o::VertexSBAPointXYZ *>(optimizer.vertex(pMPi->mnId + maxKFid + 1));
pMPi->SetWorldPos(Converter::toCvMat(vPoint->estimate()));
pMPi->UpdateNormalAndDepth();
}
}
LoopClosing::MergeLocal() 融合地图时使用,优化当前帧没有参与融合的元素,本质图优化
优化目标:
- vpNonFixedKFs
- 2.vpNonCorrectedMPs
void Optimizer::OptimizeEssentialGraph(KeyFrame *pCurKF, vector<KeyFrame *> &vpFixedKFs, vector<KeyFrame *> &vpFixedCorrectedKFs,
vector<KeyFrame *> &vpNonFixedKFs, vector<MapPoint *> &vpNonCorrectedMPs)
{
// 1. 优化器构建
g2o::SparseOptimizer optimizer;
optimizer.setVerbose(false);
g2o::BlockSolver_7_3::LinearSolverType *linearSolver =
new g2o::LinearSolverEigen<g2o::BlockSolver_7_3::PoseMatrixType>();
g2o::BlockSolver_7_3 *solver_ptr = new g2o::BlockSolver_7_3(linearSolver);
g2o::OptimizationAlgorithmLevenberg *solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr);
solver->setUserLambdaInit(1e-16);
optimizer.setAlgorithm(solver);
Map *pMap = pCurKF->GetMap();
const unsigned int nMaxKFid = pMap->GetMaxKFid();
vector<g2o::Sim3, Eigen::aligned_allocator<g2o::Sim3>> vScw(nMaxKFid + 1); // 存放每一帧优化前的sim3
vector<g2o::Sim3, Eigen::aligned_allocator<g2o::Sim3>> vCorrectedSwc(nMaxKFid + 1); // 存放每一帧优化后的sim3,调试输出用,没有实际作用
vector<g2o::VertexSim3Expmap *> vpVertices(nMaxKFid + 1); // 存放节点,没用,还占地方
const int minFeat = 100; // pKFi->GetCovisiblesByWeight(minFeat); essentialgraph 阈值就是共视大于100
// 2. 确定固定关键帧的节点
// vpMergeConnectedKFs 融合地图中的关键帧
for (KeyFrame *pKFi : vpFixedKFs)
{
if (pKFi->isBad())
continue;
g2o::VertexSim3Expmap *VSim3 = new g2o::VertexSim3Expmap();
const int nIDi = pKFi->mnId;
Eigen::Matrix<double, 3, 3> Rcw = Converter::toMatrix3d(pKFi->GetRotation());
Eigen::Matrix<double, 3, 1> tcw = Converter::toVector3d(pKFi->GetTranslation());
g2o::Sim3 Siw(Rcw, tcw, 1.0);
vScw[nIDi] = Siw;
vCorrectedSwc[nIDi] = Siw.inverse(); // This KFs mustn't be corrected 无实际作用
VSim3->setEstimate(Siw);
VSim3->setFixed(true);
VSim3->setId(nIDi);
VSim3->setMarginalized(false);
VSim3->_fix_scale = true;
optimizer.addVertex(VSim3);
vpVertices[nIDi] = VSim3;
}
Verbose::PrintMess("Opt_Essential: vpFixedKFs loaded", Verbose::VERBOSITY_DEBUG);
set<unsigned long> sIdKF;
// vpLocalCurrentWindowKFs 当前地图中经过矫正的关键帧
for (KeyFrame *pKFi : vpFixedCorrectedKFs)
{
if (pKFi->isBad())
continue;
g2o::VertexSim3Expmap *VSim3 = new g2o::VertexSim3Expmap();
const int nIDi = pKFi->mnId;
Eigen::Matrix<double, 3, 3> Rcw = Converter::toMatrix3d(pKFi->GetRotation());
Eigen::Matrix<double, 3, 1> tcw = Converter::toVector3d(pKFi->GetTranslation());
g2o::Sim3 Siw(Rcw, tcw, 1.0);
vCorrectedSwc[nIDi] = Siw.inverse(); // This KFs mustn't be corrected 无实际作用
VSim3->setEstimate(Siw);
cv::Mat Tcw_bef = pKFi->mTcwBefMerge;
Eigen::Matrix<double, 3, 3> Rcw_bef = Converter::toMatrix3d(Tcw_bef.rowRange(0, 3).colRange(0, 3));
Eigen::Matrix<double, 3, 1> tcw_bef = Converter::toVector3d(Tcw_bef.rowRange(0, 3).col(3));
vScw[nIDi] = g2o::Sim3(Rcw_bef, tcw_bef, 1.0);
VSim3->setFixed(true);
VSim3->setId(nIDi);
VSim3->setMarginalized(false);
optimizer.addVertex(VSim3);
vpVertices[nIDi] = VSim3;
sIdKF.insert(nIDi);
}
Verbose::PrintMess("Opt_Essential: vpFixedCorrectedKFs loaded", Verbose::VERBOSITY_DEBUG);
// 3. 确定待优化的关键帧节点
for (KeyFrame *pKFi : vpNonFixedKFs)
{
if (pKFi->isBad())
continue;
const int nIDi = pKFi->mnId;
if (sIdKF.count(nIDi)) // It has already added in the corrected merge KFs
continue;
g2o::VertexSim3Expmap *VSim3 = new g2o::VertexSim3Expmap();
Eigen::Matrix<double, 3, 3> Rcw = Converter::toMatrix3d(pKFi->GetRotation());
Eigen::Matrix<double, 3, 1> tcw = Converter::toVector3d(pKFi->GetTranslation());
g2o::Sim3 Siw(Rcw, tcw, 1.0);
vScw[nIDi] = Siw;
VSim3->setEstimate(Siw);
VSim3->setFixed(false);
VSim3->setId(nIDi);
VSim3->setMarginalized(false);
optimizer.addVertex(VSim3);
vpVertices[nIDi] = VSim3;
sIdKF.insert(nIDi);
}
Verbose::PrintMess("Opt_Essential: vpNonFixedKFs loaded", Verbose::VERBOSITY_DEBUG);
vector<KeyFrame *> vpKFs;
vpKFs.reserve(vpFixedKFs.size() + vpFixedCorrectedKFs.size() + vpNonFixedKFs.size());
vpKFs.insert(vpKFs.end(), vpFixedKFs.begin(), vpFixedKFs.end());
vpKFs.insert(vpKFs.end(), vpFixedCorrectedKFs.begin(), vpFixedCorrectedKFs.end());
vpKFs.insert(vpKFs.end(), vpNonFixedKFs.begin(), vpNonFixedKFs.end());
set<KeyFrame *> spKFs(vpKFs.begin(), vpKFs.end());
Verbose::PrintMess("Opt_Essential: List of KF loaded", Verbose::VERBOSITY_DEBUG);
const Eigen::Matrix<double, 7, 7> matLambda = Eigen::Matrix<double, 7, 7>::Identity();
// 4. 遍历所有帧
for (KeyFrame *pKFi : vpKFs)
{
int num_connections = 0; // 统计与pKFi连接的数量
const int nIDi = pKFi->mnId;
g2o::Sim3 Swi = vScw[nIDi].inverse();
KeyFrame *pParentKFi = pKFi->GetParent();
// Spanning tree edge
// 4.1 找到pKFi的父帧且在这批关键帧里面,添加与其关联的sim3边,相对约束
if (pParentKFi && spKFs.find(pParentKFi) != spKFs.end())
{
int nIDj = pParentKFi->mnId;
g2o::Sim3 Sjw = vScw[nIDj];
g2o::Sim3 Sji = Sjw * Swi;
g2o::EdgeSim3 *e = new g2o::EdgeSim3();
e->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex *>(optimizer.vertex(nIDj)));
e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex *>(optimizer.vertex(nIDi)));
e->setMeasurement(Sji);
e->information() = matLambda;
optimizer.addEdge(e);
num_connections++;
}
// Loop edges
// 4.2 添加回环的边
const set<KeyFrame *> sLoopEdges = pKFi->GetLoopEdges();
for (set<KeyFrame *>::const_iterator sit = sLoopEdges.begin(), send = sLoopEdges.end(); sit != send; sit++)
{
KeyFrame *pLKF = *sit;
if (spKFs.find(pLKF) != spKFs.end() && pLKF->mnId < pKFi->mnId)
{
g2o::Sim3 Slw = vScw[pLKF->mnId];
g2o::Sim3 Sli = Slw * Swi;
g2o::EdgeSim3 *el = new g2o::EdgeSim3();
el->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex *>(optimizer.vertex(pLKF->mnId)));
el->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex *>(optimizer.vertex(nIDi)));
el->setMeasurement(Sli);
el->information() = matLambda;
optimizer.addEdge(el);
num_connections++;
}
}
// Covisibility graph edges
// 4.3 建立essentialgraph(共视边)
const vector<KeyFrame *> vpConnectedKFs = pKFi->GetCovisiblesByWeight(minFeat);
for (vector<KeyFrame *>::const_iterator vit = vpConnectedKFs.begin(); vit != vpConnectedKFs.end(); vit++)
{
KeyFrame *pKFn = *vit;
// 1.这个帧存在且不是pKFi的父帧,防止重复添加
// 2.pKFn不为pKFi的子帧
// 3.pKFn不为回环帧
// 4.pKFn要在这批关键帧里面
// 防止重复添加
if (pKFn && pKFn != pParentKFi && !pKFi->hasChild(pKFn) && !sLoopEdges.count(pKFn) && spKFs.find(pKFn) != spKFs.end())
{
if (!pKFn->isBad() && pKFn->mnId < pKFi->mnId)
{
g2o::Sim3 Snw = vScw[pKFn->mnId];
g2o::Sim3 Sni = Snw * Swi;
g2o::EdgeSim3 *en = new g2o::EdgeSim3();
en->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex *>(optimizer.vertex(pKFn->mnId)));
en->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex *>(optimizer.vertex(nIDi)));
en->setMeasurement(Sni);
en->information() = matLambda;
optimizer.addEdge(en);
num_connections++;
}
}
}
}
// Optimize!
// 5. 开始优化
optimizer.initializeOptimization();
optimizer.optimize(20);
unique_lock<mutex> lock(pMap->mMutexMapUpdate);
// SE3 Pose Recovering. Sim3:[sR t;0 1] -> SE3:[R t/s;0 1]
// 6. 取出结果
for (KeyFrame *pKFi : vpNonFixedKFs)
{
if (pKFi->isBad())
continue;
const int nIDi = pKFi->mnId;
g2o::VertexSim3Expmap *VSim3 = static_cast<g2o::VertexSim3Expmap *>(optimizer.vertex(nIDi));
g2o::Sim3 CorrectedSiw = VSim3->estimate();
vCorrectedSwc[nIDi] = CorrectedSiw.inverse();
Eigen::Matrix3d eigR = CorrectedSiw.rotation().toRotationMatrix();
Eigen::Vector3d eigt = CorrectedSiw.translation();
double s = CorrectedSiw.scale();
eigt *= (1. / s); //[R t/s;0 1]
cv::Mat Tiw = Converter::toCvSE3(eigR, eigt);
pKFi->mTcwBefMerge = pKFi->GetPose();
pKFi->mTwcBefMerge = pKFi->GetPoseInverse();
pKFi->SetPose(Tiw);
}
// Correct points. Transform to "non-optimized" reference keyframe pose and transform back with optimized pose
for (MapPoint *pMPi : vpNonCorrectedMPs)
{
if (pMPi->isBad())
continue;
KeyFrame *pRefKF = pMPi->GetReferenceKeyFrame();
g2o::Sim3 Srw;
g2o::Sim3 correctedSwr;
while (pRefKF->isBad())
{
if (!pRefKF)
{
Verbose::PrintMess("MP " + to_string(pMPi->mnId) + " without a valid reference KF", Verbose::VERBOSITY_DEBUG);
break;
}
pMPi->EraseObservation(pRefKF);
pRefKF = pMPi->GetReferenceKeyFrame();
}
// 流程就是转到相机坐标系,在用新的rt转到世界
cv::Mat TNonCorrectedwr = pRefKF->mTwcBefMerge;
Eigen::Matrix<double, 3, 3> RNonCorrectedwr = Converter::toMatrix3d(TNonCorrectedwr.rowRange(0, 3).colRange(0, 3));
Eigen::Matrix<double, 3, 1> tNonCorrectedwr = Converter::toVector3d(TNonCorrectedwr.rowRange(0, 3).col(3));
Srw = g2o::Sim3(RNonCorrectedwr, tNonCorrectedwr, 1.0).inverse();
cv::Mat Twr = pRefKF->GetPoseInverse();
Eigen::Matrix<double, 3, 3> Rwr = Converter::toMatrix3d(Twr.rowRange(0, 3).colRange(0, 3));
Eigen::Matrix<double, 3, 1> twr = Converter::toVector3d(Twr.rowRange(0, 3).col(3));
correctedSwr = g2o::Sim3(Rwr, twr, 1.0);
cv::Mat P3Dw = pMPi->GetWorldPos();
Eigen::Matrix<double, 3, 1> eigP3Dw = Converter::toVector3d(P3Dw);
Eigen::Matrix<double, 3, 1> eigCorrectedP3Dw = correctedSwr.map(Srw.map(eigP3Dw));
cv::Mat cvCorrectedP3Dw = Converter::toCvMat(eigCorrectedP3Dw);
pMPi->SetWorldPos(cvCorrectedP3Dw);
pMPi->UpdateNormalAndDepth();
}
}
这里面进行visual inertial ba
LoopClosing::MergeLocal2 中用到
优化目标:
- 相关帧的位姿,速度,偏置,还有涉及点的坐标,可以理解为跨地图的局部窗口优化
void Optimizer::MergeInertialBA(KeyFrame *pCurrKF, KeyFrame *pMergeKF, bool *pbStopFlag, Map *pMap, LoopClosing::KeyFrameAndPose &corrPoses)
{
const int Nd = 6;
const unsigned long maxKFid = pCurrKF->mnId;
// 1. 准备所有被优化的关键帧, 完全固定的帧
// 被优化的帧, 当前帧和融合匹配帧加起来一共12个
vector<KeyFrame *> vpOptimizableKFs;
vpOptimizableKFs.reserve(2 * Nd);
// For cov KFS, inertial parameters are not optimized
// 共视帧, 只优化位姿,不优化速度与偏置
const int maxCovKF = 15;
vector<KeyFrame *> vpOptimizableCovKFs;
vpOptimizableCovKFs.reserve(2 * maxCovKF);
// Add sliding window for current KF
// 当前关键帧先加到容器中
vpOptimizableKFs.push_back(pCurrKF);
// 后面用这个变量避免重复
pCurrKF->mnBALocalForKF = pCurrKF->mnId;
// 添加5个与当前关键帧相连的时序上相连的关键帧进容器
// 1.1 一直放一直放,一直放到没有上一个关键帧,这里面包含了pCurrKF最近的6帧,从晚到早排列,不过有可能存不满
for (int i = 1; i < Nd; i++)
{
if (vpOptimizableKFs.back()->mPrevKF)
{
// 用mPrevKF访问时序上的上一个关键帧
vpOptimizableKFs.push_back(vpOptimizableKFs.back()->mPrevKF);
vpOptimizableKFs.back()->mnBALocalForKF = pCurrKF->mnId;
}
else
break;
}
// 1.2 如果 vpOptimizableKFs 中最早的一帧前面还有,往不更新惯导参数的序列中添加
// 否则把最后一个取出来放到不更新惯导参数的序列中
if (vpOptimizableKFs.back()->mPrevKF)
{
vpOptimizableCovKFs.push_back(vpOptimizableKFs.back()->mPrevKF);
vpOptimizableKFs.back()->mPrevKF->mnBALocalForKF = pCurrKF->mnId;
}
else
{
// 如果没找到时序相连的帧,把被优化的窗口缩减一个给到固定的变量
vpOptimizableCovKFs.push_back(vpOptimizableKFs.back());
vpOptimizableKFs.pop_back();
}
// 没用到
KeyFrame *pKF0 = vpOptimizableCovKFs.back();
cv::Mat Twc0 = pKF0->GetPoseInverse();
// Add temporal neighbours to merge KF (previous and next KFs)
// 2. 同样, 对于融合帧也把它和时序上的几个邻居加到可优化的容器里
// 2.1 把匹配的融合关键帧也放进来准备一起优化
vpOptimizableKFs.push_back(pMergeKF);
pMergeKF->mnBALocalForKF = pCurrKF->mnId;
// Previous KFs
// 把融合帧时序上的邻居添加到可优化的容器里, 因为融合帧左右都有时序上的邻居,所以这里先取一半 Nd/2
// 2.2 再放进来3个pMergeKF之前的关键帧,有可能放不满
for (int i = 1; i < (Nd / 2); i++)
{
if (vpOptimizableKFs.back()->mPrevKF)
{
vpOptimizableKFs.push_back(vpOptimizableKFs.back()->mPrevKF);
vpOptimizableKFs.back()->mnBALocalForKF = pCurrKF->mnId;
}
else
break;
}
// We fix just once the old map
// 记录与融合帧窗口时序上紧邻的帧为完全固定的帧
// 2.3 类似于上面的做法如果有前一个关键帧放入lFixedKeyFrames,否则从vpOptimizableKFs取出,注意这里防止重复添加的标识又多了一个变量
// 记录完全固定的帧
list<KeyFrame *> lFixedKeyFrames;
if (vpOptimizableKFs.back()->mPrevKF)
{
lFixedKeyFrames.push_back(vpOptimizableKFs.back()->mPrevKF);
vpOptimizableKFs.back()->mPrevKF->mnBAFixedForKF = pCurrKF->mnId;
}
// 如果没找到,则从窗口内拿出一帧给到完全固定的帧
else
{
vpOptimizableKFs.back()->mnBALocalForKF = 0;
vpOptimizableKFs.back()->mnBAFixedForKF = pCurrKF->mnId;
lFixedKeyFrames.push_back(vpOptimizableKFs.back());
vpOptimizableKFs.pop_back();
}
// Next KFs
// 2.4 再添加一个pMergeKF的下一个关键帧
if (pMergeKF->mNextKF)
{
vpOptimizableKFs.push_back(pMergeKF->mNextKF);
vpOptimizableKFs.back()->mnBALocalForKF = pCurrKF->mnId;
}
// 2.5 数量不够时,添加最后一个的下一帧。继续添加直到达到2Nd个可优化关键帧,或没有新的可以添加了
while (vpOptimizableKFs.size() < (2 * Nd))
{
if (vpOptimizableKFs.back()->mNextKF)
{
vpOptimizableKFs.push_back(vpOptimizableKFs.back()->mNextKF);
vpOptimizableKFs.back()->mnBALocalForKF = pCurrKF->mnId;
}
else
break;
}
int N = vpOptimizableKFs.size();
// Optimizable points seen by optimizable keyframes
// 3. 帧弄完了该弄点了,将优化的帧的点存入lLocalMapPoints (所有被可优化关键帧看到的点)
// 添加用来优化的地图点
list<MapPoint *> lLocalMapPoints;
// 统计了在这些帧中点被观测的次数
map<MapPoint *, int> mLocalObs;
// 遍历所有可优化的关键帧
for (int i = 0; i < N; i++)
{
vector<MapPoint *> vpMPs = vpOptimizableKFs[i]->GetMapPointMatches();
// 遍历每个关键帧所有的地图点
for (vector<MapPoint *>::iterator vit = vpMPs.begin(), vend = vpMPs.end(); vit != vend; vit++)
{
// Using mnBALocalForKF we avoid redundance here, one MP can not be added several times to lLocalMapPoints
MapPoint *pMP = *vit;
if (pMP)
if (!pMP->isBad())
// 用这个变量记录是否重复添加
if (pMP->mnBALocalForKF != pCurrKF->mnId)
{
mLocalObs[pMP] = 1;
lLocalMapPoints.push_back(pMP);
pMP->mnBALocalForKF = pCurrKF->mnId; // 防止重复添加
}
else
mLocalObs[pMP]++;
}
}
// 4. 把pCurrKF的共视帧加进来 只优化位姿,不优化速度与偏置
// 固定所有观察到地图点,但没有被加到优化变量中的关键帧
int i = 0;
const int min_obs = 10;
vector<KeyFrame *> vNeighCurr = pCurrKF->GetCovisiblesByWeight(min_obs);
// 遍历所有的pair<地图点指针,观测次数>
for (vector<KeyFrame *>::iterator lit = vNeighCurr.begin(), lend = vNeighCurr.end(); lit != lend; lit++)
{
// 拿到所有的观测
// 上限,添加15个
if (i >= maxCovKF)
break;
KeyFrame *pKFi = *lit;
if (pKFi->mnBALocalForKF != pCurrKF->mnId && pKFi->mnBAFixedForKF != pCurrKF->mnId) // If optimizable or already included...
{
pKFi->mnBALocalForKF = pCurrKF->mnId;
if (!pKFi->isBad())
{
i++;
vpOptimizableCovKFs.push_back(pKFi);
}
}
}
i = 0;
// 把pMergeKF的共视帧加进来 只优化位姿,不优化速度与偏置
// BUG !!!!!这里不知道是bug还是啥vNeighCurr是不是应该改成vNeighMerge
// vpOptimizableCovKFs 改成 lFixedKeyFrames
vector<KeyFrame *> vNeighMerge = pMergeKF->GetCovisiblesByWeight(min_obs);
for (vector<KeyFrame *>::iterator lit = vNeighCurr.begin(), lend = vNeighCurr.end(); lit != lend; lit++, i++)
{
// 上限,添加15个
if (i >= maxCovKF)
break;
KeyFrame *pKFi = *lit;
// 如果还没有被添加到共视帧容器里
if (pKFi->mnBALocalForKF != pCurrKF->mnId && pKFi->mnBAFixedForKF != pCurrKF->mnId) // If optimizable or already included...
{
pKFi->mnBALocalForKF = pCurrKF->mnId;
if (!pKFi->isBad())
{
i++;
vpOptimizableCovKFs.push_back(pKFi);
}
}
}
// 5. 构建优化器
g2o::SparseOptimizer optimizer;
g2o::BlockSolverX::LinearSolverType *linearSolver;
linearSolver = new g2o::LinearSolverEigen<g2o::BlockSolverX::PoseMatrixType>();
g2o::BlockSolverX *solver_ptr = new g2o::BlockSolverX(linearSolver);
g2o::OptimizationAlgorithmLevenberg *solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr);
solver->setUserLambdaInit(1e3);
optimizer.setAlgorithm(solver);
optimizer.setVerbose(false);
// Set Local KeyFrame vertices
N = vpOptimizableKFs.size();
// 5.1 设置所有的可优化关键帧顶点
for (int i = 0; i < N; i++)
{
KeyFrame *pKFi = vpOptimizableKFs[i];
VertexPose *VP = new VertexPose(pKFi);
VP->setId(pKFi->mnId);
VP->setFixed(false);
// 位姿
optimizer.addVertex(VP);
if (pKFi->bImu)
{
VertexVelocity *VV = new VertexVelocity(pKFi);
VV->setId(maxKFid + 3 * (pKFi->mnId) + 1);
VV->setFixed(false);
optimizer.addVertex(VV);
VertexGyroBias *VG = new VertexGyroBias(pKFi);
VG->setId(maxKFid + 3 * (pKFi->mnId) + 2);
VG->setFixed(false);
optimizer.addVertex(VG);
VertexAccBias *VA = new VertexAccBias(pKFi);
VA->setId(maxKFid + 3 * (pKFi->mnId) + 3);
VA->setFixed(false);
optimizer.addVertex(VA);
}
}
// Set Local cov keyframes vertices
// 5.2 只优化位姿,不优化速度与偏置
int Ncov = vpOptimizableCovKFs.size();
for (int i = 0; i < Ncov; i++)
{
KeyFrame *pKFi = vpOptimizableCovKFs[i];
VertexPose *VP = new VertexPose(pKFi);
VP->setId(pKFi->mnId);
VP->setFixed(false);
optimizer.addVertex(VP);
if (pKFi->bImu)
{
VertexVelocity *VV = new VertexVelocity(pKFi);
VV->setId(maxKFid + 3 * (pKFi->mnId) + 1);
VV->setFixed(true);
optimizer.addVertex(VV);
VertexGyroBias *VG = new VertexGyroBias(pKFi);
VG->setId(maxKFid + 3 * (pKFi->mnId) + 2);
VG->setFixed(true);
optimizer.addVertex(VG);
VertexAccBias *VA = new VertexAccBias(pKFi);
VA->setId(maxKFid + 3 * (pKFi->mnId) + 3);
VA->setFixed(true);
optimizer.addVertex(VA);
}
}
// Set Fixed KeyFrame vertices
// 5.3 设置所有完全固定的关键帧顶点
for (list<KeyFrame *>::iterator lit = lFixedKeyFrames.begin(), lend = lFixedKeyFrames.end(); lit != lend; lit++)
{
KeyFrame *pKFi = *lit;
VertexPose *VP = new VertexPose(pKFi);
VP->setId(pKFi->mnId);
VP->setFixed(true);
optimizer.addVertex(VP);
if (pKFi->bImu)
{
VertexVelocity *VV = new VertexVelocity(pKFi);
VV->setId(maxKFid + 3 * (pKFi->mnId) + 1);
VV->setFixed(true);
optimizer.addVertex(VV);
VertexGyroBias *VG = new VertexGyroBias(pKFi);
VG->setId(maxKFid + 3 * (pKFi->mnId) + 2);
VG->setFixed(true);
optimizer.addVertex(VG);
VertexAccBias *VA = new VertexAccBias(pKFi);
VA->setId(maxKFid + 3 * (pKFi->mnId) + 3);
VA->setFixed(true);
optimizer.addVertex(VA);
}
}
// 6 设置所有的inertial的边
// Create intertial constraints
// 预积分边
vector<EdgeInertial *> vei(N, (EdgeInertial *)NULL);
// 角速度bias边
vector<EdgeGyroRW *> vegr(N, (EdgeGyroRW *)NULL);
// 加速地bias边
vector<EdgeAccRW *> vear(N, (EdgeAccRW *)NULL);
// 6.1 第一阶段优化vpOptimizableKFs里面的帧,遍历所有可优化的关键帧
for (int i = 0; i < N; i++)
{
// cout << "inserting inertial edge " << i << endl;
KeyFrame *pKFi = vpOptimizableKFs[i];
if (!pKFi->mPrevKF)
{
Verbose::PrintMess("NOT INERTIAL LINK TO PREVIOUS FRAME!!!!", Verbose::VERBOSITY_NORMAL);
continue;
}
if (pKFi->bImu && pKFi->mPrevKF->bImu && pKFi->mpImuPreintegrated)
{
pKFi->mpImuPreintegrated->SetNewBias(pKFi->mPrevKF->GetImuBias());
// 位姿
g2o::HyperGraph::Vertex *VP1 = optimizer.vertex(pKFi->mPrevKF->mnId);
// 速度
g2o::HyperGraph::Vertex *VV1 = optimizer.vertex(maxKFid + 3 * (pKFi->mPrevKF->mnId) + 1);
// 角速度bias
g2o::HyperGraph::Vertex *VG1 = optimizer.vertex(maxKFid + 3 * (pKFi->mPrevKF->mnId) + 2);
// 加速度bias
g2o::HyperGraph::Vertex *VA1 = optimizer.vertex(maxKFid + 3 * (pKFi->mPrevKF->mnId) + 3);
// 同上
g2o::HyperGraph::Vertex *VP2 = optimizer.vertex(pKFi->mnId);
g2o::HyperGraph::Vertex *VV2 = optimizer.vertex(maxKFid + 3 * (pKFi->mnId) + 1);
g2o::HyperGraph::Vertex *VG2 = optimizer.vertex(maxKFid + 3 * (pKFi->mnId) + 2);
g2o::HyperGraph::Vertex *VA2 = optimizer.vertex(maxKFid + 3 * (pKFi->mnId) + 3);
if (!VP1 || !VV1 || !VG1 || !VA1 || !VP2 || !VV2 || !VG2 || !VA2)
{
cerr << "Error " << VP1 << ", " << VV1 << ", " << VG1 << ", " << VA1 << ", " << VP2 << ", " << VV2 << ", " << VG2 << ", " << VA2 << endl;
continue;
}
vei[i] = new EdgeInertial(pKFi->mpImuPreintegrated);
// 设置顶点 2*Pose + 2*Velocity + 1 角速度bias + 1 加速度bias
vei[i]->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex *>(VP1));
vei[i]->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex *>(VV1));
vei[i]->setVertex(2, dynamic_cast<g2o::OptimizableGraph::Vertex *>(VG1));
vei[i]->setVertex(3, dynamic_cast<g2o::OptimizableGraph::Vertex *>(VA1));
vei[i]->setVertex(4, dynamic_cast<g2o::OptimizableGraph::Vertex *>(VP2));
vei[i]->setVertex(5, dynamic_cast<g2o::OptimizableGraph::Vertex *>(VV2));
// TODO Uncomment
// 设置优化参数
g2o::RobustKernelHuber *rki = new g2o::RobustKernelHuber;
vei[i]->setRobustKernel(rki);
rki->setDelta(sqrt(16.92));
// 添加边
optimizer.addEdge(vei[i]);
// 角速度bias的边
vegr[i] = new EdgeGyroRW();
// 设置顶点两个角速度bias
vegr[i]->setVertex(0, VG1);
vegr[i]->setVertex(1, VG2);
// 设置infomation matrix
cv::Mat cvInfoG = pKFi->mpImuPreintegrated->C.rowRange(9, 12).colRange(9, 12).inv(cv::DECOMP_SVD);
Eigen::Matrix3d InfoG;
for (int r = 0; r < 3; r++)
for (int c = 0; c < 3; c++)
InfoG(r, c) = cvInfoG.at<float>(r, c);
vegr[i]->setInformation(InfoG);
optimizer.addEdge(vegr[i]);
// 设置加速度的边
vear[i] = new EdgeAccRW();
vear[i]->setVertex(0, VA1);
vear[i]->setVertex(1, VA2);
// 设置information matrix
cv::Mat cvInfoA = pKFi->mpImuPreintegrated->C.rowRange(12, 15).colRange(12, 15).inv(cv::DECOMP_SVD);
Eigen::Matrix3d InfoA;
for (int r = 0; r < 3; r++)
for (int c = 0; c < 3; c++)
InfoA(r, c) = cvInfoA.at<float>(r, c);
vear[i]->setInformation(InfoA);
optimizer.addEdge(vear[i]);
}
else
Verbose::PrintMess("ERROR building inertial edge", Verbose::VERBOSITY_NORMAL);
}
Verbose::PrintMess("end inserting inertial edges", Verbose::VERBOSITY_DEBUG);
// Set MapPoint vertices
// 6.2 设置所有地图的顶点
// 设置地图点顶点
const int nExpectedSize = (N + Ncov + lFixedKeyFrames.size()) * lLocalMapPoints.size();
// 对于双目单目设置不同
// Mono
vector<EdgeMono *> vpEdgesMono;
vpEdgesMono.reserve(nExpectedSize);
vector<KeyFrame *> vpEdgeKFMono;
vpEdgeKFMono.reserve(nExpectedSize);
vector<MapPoint *> vpMapPointEdgeMono;
vpMapPointEdgeMono.reserve(nExpectedSize);
// Stereo
vector<EdgeStereo *> vpEdgesStereo;
vpEdgesStereo.reserve(nExpectedSize);
vector<KeyFrame *> vpEdgeKFStereo;
vpEdgeKFStereo.reserve(nExpectedSize);
vector<MapPoint *> vpMapPointEdgeStereo;
vpMapPointEdgeStereo.reserve(nExpectedSize);
const float thHuberMono = sqrt(5.991);
const float chi2Mono2 = 5.991;
const float thHuberStereo = sqrt(7.815);
const float chi2Stereo2 = 7.815;
const unsigned long iniMPid = maxKFid * 5;
// 遍历所有被可优化关键帧观测到的的地图点
// 7. 添加重投影的边
for (list<MapPoint *>::iterator lit = lLocalMapPoints.begin(), lend = lLocalMapPoints.end(); lit != lend; lit++)
{
MapPoint *pMP = *lit;
if (!pMP)
continue;
g2o::VertexSBAPointXYZ *vPoint = new g2o::VertexSBAPointXYZ();
vPoint->setEstimate(Converter::toVector3d(pMP->GetWorldPos()));
unsigned long id = pMP->mnId + iniMPid + 1;
vPoint->setId(id);
vPoint->setMarginalized(true);
// 添加顶点
optimizer.addVertex(vPoint);
const map<KeyFrame *, tuple<int, int>> observations = pMP->GetObservations();
// Create visual constraints
// 添加重投影边
for (map<KeyFrame *, tuple<int, int>>::const_iterator mit = observations.begin(), mend = observations.end(); mit != mend; mit++)
{
KeyFrame *pKFi = mit->first;
if (!pKFi)
continue;
if ((pKFi->mnBALocalForKF != pCurrKF->mnId) && (pKFi->mnBAFixedForKF != pCurrKF->mnId))
continue;
if (pKFi->mnId > maxKFid)
{
Verbose::PrintMess("ID greater than current KF is", Verbose::VERBOSITY_NORMAL);
continue;
}
if (optimizer.vertex(id) == NULL || optimizer.vertex(pKFi->mnId) == NULL)
continue;
if (!pKFi->isBad())
{
// 3D点的观测
const cv::KeyPoint &kpUn = pKFi->mvKeysUn[get<0>(mit->second)];
// 如果是单目观测
if (pKFi->mvuRight[get<0>(mit->second)] < 0) // Monocular observation
{
// 投影
Eigen::Matrix<double, 2, 1> obs;
obs << kpUn.pt.x, kpUn.pt.y;
EdgeMono *e = new EdgeMono();
// 设置边的顶点
// 3D点
e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex *>(optimizer.vertex(id)));
// 关键帧位姿
e->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex *>(optimizer.vertex(pKFi->mnId)));
e->setMeasurement(obs);
const float &invSigma2 = pKFi->mvInvLevelSigma2[kpUn.octave];
// 设置信息矩阵
e->setInformation(Eigen::Matrix2d::Identity() * invSigma2);
g2o::RobustKernelHuber *rk = new g2o::RobustKernelHuber;
e->setRobustKernel(rk);
rk->setDelta(thHuberMono);
// 添加边
optimizer.addEdge(e);
vpEdgesMono.push_back(e);
vpEdgeKFMono.push_back(pKFi);
vpMapPointEdgeMono.push_back(pMP);
}
//双目
else // stereo observation
{
const float kp_ur = pKFi->mvuRight[get<0>(mit->second)];
Eigen::Matrix<double, 3, 1> obs;
obs << kpUn.pt.x, kpUn.pt.y, kp_ur;
EdgeStereo *e = new EdgeStereo();
e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex *>(optimizer.vertex(id)));
e->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex *>(optimizer.vertex(pKFi->mnId)));
e->setMeasurement(obs);
const float &invSigma2 = pKFi->mvInvLevelSigma2[kpUn.octave];
e->setInformation(Eigen::Matrix3d::Identity() * invSigma2);
g2o::RobustKernelHuber *rk = new g2o::RobustKernelHuber;
e->setRobustKernel(rk);
rk->setDelta(thHuberStereo);
optimizer.addEdge(e);
vpEdgesStereo.push_back(e);
vpEdgeKFStereo.push_back(pKFi);
vpMapPointEdgeStereo.push_back(pMP);
}
}
}
}
// 如果要停止就直接返回
if (pbStopFlag)
if (*pbStopFlag)
return;
// 8. 开始优化
optimizer.initializeOptimization();
optimizer.optimize(3);
if (pbStopFlag)
if (!*pbStopFlag)
optimizer.optimize(5);
optimizer.setForceStopFlag(pbStopFlag);
// 更具优化结果挑选删除外点
vector<pair<KeyFrame *, MapPoint *>> vToErase;
vToErase.reserve(vpEdgesMono.size() + vpEdgesStereo.size());
// Check inlier observations
// Mono
// 9. 处理外点
// 更具卡方检测来记录要删除的外点
for (size_t i = 0, iend = vpEdgesMono.size(); i < iend; i++)
{
EdgeMono *e = vpEdgesMono[i];
MapPoint *pMP = vpMapPointEdgeMono[i];
if (pMP->isBad())
continue;
if (e->chi2() > chi2Mono2)
{
KeyFrame *pKFi = vpEdgeKFMono[i];
vToErase.push_back(make_pair(pKFi, pMP));
}
}
// Stereo
for (size_t i = 0, iend = vpEdgesStereo.size(); i < iend; i++)
{
EdgeStereo *e = vpEdgesStereo[i];
MapPoint *pMP = vpMapPointEdgeStereo[i];
if (pMP->isBad())
continue;
if (e->chi2() > chi2Stereo2)
{
KeyFrame *pKFi = vpEdgeKFStereo[i];
vToErase.push_back(make_pair(pKFi, pMP));
}
}
// Get Map Mutex and erase outliers
// 移除外点
unique_lock<mutex> lock(pMap->mMutexMapUpdate);
if (!vToErase.empty())
{
for (size_t i = 0; i < vToErase.size(); i++)
{
KeyFrame *pKFi = vToErase[i].first;
MapPoint *pMPi = vToErase[i].second;
pKFi->EraseMapPointMatch(pMPi);
pMPi->EraseObservation(pKFi);
}
}
// Recover optimized data
// 10. 取数
// 根据优化的结果,修改每个被优化的变量
// Keyframes
// 对于每个可优化关键帧
for (int i = 0; i < N; i++)
{
KeyFrame *pKFi = vpOptimizableKFs[i];
// 修改位姿
VertexPose *VP = static_cast<VertexPose *>(optimizer.vertex(pKFi->mnId));
cv::Mat Tcw = Converter::toCvSE3(VP->estimate().Rcw[0], VP->estimate().tcw[0]);
pKFi->SetPose(Tcw);
// 在corrPoses记录每个关键帧融合后的位姿
cv::Mat Tiw = pKFi->GetPose();
cv::Mat Riw = Tiw.rowRange(0, 3).colRange(0, 3);
cv::Mat tiw = Tiw.rowRange(0, 3).col(3);
g2o::Sim3 g2oSiw(Converter::toMatrix3d(Riw), Converter::toVector3d(tiw), 1.0);
corrPoses[pKFi] = g2oSiw;
if (pKFi->bImu)
{
// 速度
VertexVelocity *VV = static_cast<VertexVelocity *>(optimizer.vertex(maxKFid + 3 * (pKFi->mnId) + 1));
// 修改速度
pKFi->SetVelocity(Converter::toCvMat(VV->estimate()));
// 角速度bias
VertexGyroBias *VG = static_cast<VertexGyroBias *>(optimizer.vertex(maxKFid + 3 * (pKFi->mnId) + 2));
// 加速度bias
VertexAccBias *VA = static_cast<VertexAccBias *>(optimizer.vertex(maxKFid + 3 * (pKFi->mnId) + 3));
Vector6d b;
// 修改bias
b << VG->estimate(), VA->estimate();
pKFi->SetNewBias(IMU::Bias(b[3], b[4], b[5], b[0], b[1], b[2]));
}
}
// 遍历所有的共视帧
for (int i = 0; i < Ncov; i++)
{
KeyFrame *pKFi = vpOptimizableCovKFs[i];
VertexPose *VP = static_cast<VertexPose *>(optimizer.vertex(pKFi->mnId));
cv::Mat Tcw = Converter::toCvSE3(VP->estimate().Rcw[0], VP->estimate().tcw[0]);
// 修改位姿
pKFi->SetPose(Tcw);
// 记录融合后的位姿
cv::Mat Tiw = pKFi->GetPose();
cv::Mat Riw = Tiw.rowRange(0, 3).colRange(0, 3);
cv::Mat tiw = Tiw.rowRange(0, 3).col(3);
g2o::Sim3 g2oSiw(Converter::toMatrix3d(Riw), Converter::toVector3d(tiw), 1.0);
corrPoses[pKFi] = g2oSiw;
// 这部分固定了,跟没做一样~
if (pKFi->bImu)
{
VertexVelocity *VV = static_cast<VertexVelocity *>(optimizer.vertex(maxKFid + 3 * (pKFi->mnId) + 1));
pKFi->SetVelocity(Converter::toCvMat(VV->estimate()));
VertexGyroBias *VG = static_cast<VertexGyroBias *>(optimizer.vertex(maxKFid + 3 * (pKFi->mnId) + 2));
VertexAccBias *VA = static_cast<VertexAccBias *>(optimizer.vertex(maxKFid + 3 * (pKFi->mnId) + 3));
Vector6d b;
b << VG->estimate(), VA->estimate();
pKFi->SetNewBias(IMU::Bias(b[3], b[4], b[5], b[0], b[1], b[2]));
}
}
// Points
// 对于所有的地图点
for (list<MapPoint *>::iterator lit = lLocalMapPoints.begin(), lend = lLocalMapPoints.end(); lit != lend; lit++)
{
// 跟新位置和normal等信息
MapPoint *pMP = *lit;
g2o::VertexSBAPointXYZ *vPoint = static_cast<g2o::VertexSBAPointXYZ *>(optimizer.vertex(pMP->mnId + iniMPid + 1));
pMP->SetWorldPos(Converter::toCvMat(vPoint->estimate()));
pMP->UpdateNormalAndDepth();
}
pMap->IncreaseChangeIndex();
}