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;
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();
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<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);
}
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);
map<KeyFrame *, int> mpObsKFs;
map<KeyFrame *, int> mpObsFinalKFs;
map<MapPoint *, int> mpObsMPs;
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;
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->mnBALocalForMerge != pMainKF->mnId || !pKF->GetMapPoint(get<0>(mit->second)))
continue;
nEdges++;
const cv::KeyPoint &kpUn = pKF->mvKeysUn[get<0>(mit->second)];
if (pKF->mvuRight[get<0>(mit->second)] < 0)
{
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
{
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;
optimizer.initializeOptimization();
optimizer.optimize(5);
bool bDoMore = true;
if (pbStopFlag)
if (*pbStopFlag)
bDoMore = false;
map<unsigned long int, int> mWrongObsKF;
if (bDoMore)
{
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);
optimizer.initializeOptimization(0);
optimizer.optimize(10);
}
vector<pair<KeyFrame *, MapPoint *>> vToErase;
vToErase.reserve(vpEdgesMono.size() + vpEdgesStereo.size());
set<MapPoint *> spErasedMPs;
set<KeyFrame *> spErasedKFs;
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);
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)
{
mpObsFinalKFs[pKF]++;
}
else
{
mpObsFinalKFs[pKF]++;
}
}
}
for (KeyFrame *pKFi : vpAdjustKF)
{
if (pKFi->isBad())
continue;
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;
vector<MapPoint *> vpMonoMPsBad, vpStereoMPsBad;
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)
{
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);
vector<g2o::Sim3, Eigen::aligned_allocator<g2o::Sim3>> vCorrectedSwc(nMaxKFid + 1);
vector<g2o::VertexSim3Expmap *> vpVertices(nMaxKFid + 1);
const int minFeat = 100;
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();
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;
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();
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);
for (KeyFrame *pKFi : vpNonFixedKFs)
{
if (pKFi->isBad())
continue;
const int nIDi = pKFi->mnId;
if (sIdKF.count(nIDi))
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();
for (KeyFrame *pKFi : vpKFs)
{
int num_connections = 0;
const int nIDi = pKFi->mnId;
g2o::Sim3 Swi = vScw[nIDi].inverse();
KeyFrame *pParentKFi = pKFi->GetParent();
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++;
}
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++;
}
}
const vector<KeyFrame *> vpConnectedKFs = pKFi->GetCovisiblesByWeight(minFeat);
for (vector<KeyFrame *>::const_iterator vit = vpConnectedKFs.begin(); vit != vpConnectedKFs.end(); vit++)
{
KeyFrame *pKFn = *vit;
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++;
}
}
}
}
optimizer.initializeOptimization();
optimizer.optimize(20);
unique_lock<mutex> lock(pMap->mMutexMapUpdate);
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);
cv::Mat Tiw = Converter::toCvSE3(eigR, eigt);
pKFi->mTcwBefMerge = pKFi->GetPose();
pKFi->mTwcBefMerge = pKFi->GetPoseInverse();
pKFi->SetPose(Tiw);
}
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();
}
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;
vector<KeyFrame *> vpOptimizableKFs;
vpOptimizableKFs.reserve(2 * Nd);
const int maxCovKF = 15;
vector<KeyFrame *> vpOptimizableCovKFs;
vpOptimizableCovKFs.reserve(2 * maxCovKF);
vpOptimizableKFs.push_back(pCurrKF);
pCurrKF->mnBALocalForKF = pCurrKF->mnId;
for (int i = 1; i < Nd; i++)
{
if (vpOptimizableKFs.back()->mPrevKF)
{
vpOptimizableKFs.push_back(vpOptimizableKFs.back()->mPrevKF);
vpOptimizableKFs.back()->mnBALocalForKF = pCurrKF->mnId;
}
else
break;
}
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();
vpOptimizableKFs.push_back(pMergeKF);
pMergeKF->mnBALocalForKF = pCurrKF->mnId;
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;
}
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();
}
if (pMergeKF->mNextKF)
{
vpOptimizableKFs.push_back(pMergeKF->mNextKF);
vpOptimizableKFs.back()->mnBALocalForKF = pCurrKF->mnId;
}
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();
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++)
{
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]++;
}
}
int i = 0;
const int min_obs = 10;
vector<KeyFrame *> vNeighCurr = pCurrKF->GetCovisiblesByWeight(min_obs);
for (vector<KeyFrame *>::iterator lit = vNeighCurr.begin(), lend = vNeighCurr.end(); lit != lend; lit++)
{
if (i >= maxCovKF)
break;
KeyFrame *pKFi = *lit;
if (pKFi->mnBALocalForKF != pCurrKF->mnId && pKFi->mnBAFixedForKF != pCurrKF->mnId)
{
pKFi->mnBALocalForKF = pCurrKF->mnId;
if (!pKFi->isBad())
{
i++;
vpOptimizableCovKFs.push_back(pKFi);
}
}
}
i = 0;
vector<KeyFrame *> vNeighMerge = pMergeKF->GetCovisiblesByWeight(min_obs);
for (vector<KeyFrame *>::iterator lit = vNeighCurr.begin(), lend = vNeighCurr.end(); lit != lend; lit++, i++)
{
if (i >= maxCovKF)
break;
KeyFrame *pKFi = *lit;
if (pKFi->mnBALocalForKF != pCurrKF->mnId && pKFi->mnBAFixedForKF != pCurrKF->mnId)
{
pKFi->mnBALocalForKF = pCurrKF->mnId;
if (!pKFi->isBad())
{
i++;
vpOptimizableCovKFs.push_back(pKFi);
}
}
}
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);
N = vpOptimizableKFs.size();
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);
}
}
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);
}
}
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);
}
}
vector<EdgeInertial *> vei(N, (EdgeInertial *)NULL);
vector<EdgeGyroRW *> vegr(N, (EdgeGyroRW *)NULL);
vector<EdgeAccRW *> vear(N, (EdgeAccRW *)NULL);
for (int i = 0; i < N; i++)
{
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);
g2o::HyperGraph::Vertex *VG1 = optimizer.vertex(maxKFid + 3 * (pKFi->mPrevKF->mnId) + 2);
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);
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));
g2o::RobustKernelHuber *rki = new g2o::RobustKernelHuber;
vei[i]->setRobustKernel(rki);
rki->setDelta(sqrt(16.92));
optimizer.addEdge(vei[i]);
vegr[i] = new EdgeGyroRW();
vegr[i]->setVertex(0, VG1);
vegr[i]->setVertex(1, VG2);
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);
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);
const int nExpectedSize = (N + Ncov + lFixedKeyFrames.size()) * lLocalMapPoints.size();
vector<EdgeMono *> vpEdgesMono;
vpEdgesMono.reserve(nExpectedSize);
vector<KeyFrame *> vpEdgeKFMono;
vpEdgeKFMono.reserve(nExpectedSize);
vector<MapPoint *> vpMapPointEdgeMono;
vpMapPointEdgeMono.reserve(nExpectedSize);
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;
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();
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())
{
const cv::KeyPoint &kpUn = pKFi->mvKeysUn[get<0>(mit->second)];
if (pKFi->mvuRight[get<0>(mit->second)] < 0)
{
Eigen::Matrix<double, 2, 1> obs;
obs << kpUn.pt.x, kpUn.pt.y;
EdgeMono *e = new EdgeMono();
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
{
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;
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());
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));
}
}
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));
}
}
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);
}
}
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);
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]));
}
}
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]));
}
}
for (list<MapPoint *>::iterator lit = lLocalMapPoints.begin(), lend = lLocalMapPoints.end(); lit != lend; lit++)
{
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();
}
本文内容由网友自发贡献,版权归原作者所有,本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容,请联系:hwhale#tublm.com(使用前将#替换为@)