在localMapping 中,我们需要利用局部地图进行BA()
void Optimizer::LocalBundleAdjustmentWithLine(KeyFrame *pKF, bool *pbStopFlag, Map *pMap)
{
double invSigma = 0.01;
// Local KeyFrames: First Breath Search from Current KeyFrame
list<KeyFrame*> lLocalKeyFrames;
// step1: 将当前关键帧加入到lLocalKeyFrames
lLocalKeyFrames.push_back(pKF);
pKF->mnBALocalForKF = pKF->mnId;
// step2:找到关键帧连接的关键帧(一级相连),加入到lLocalKeyFrames中
const vector<KeyFrame*> vNeighKFs = pKF->GetVectorCovisibleKeyFrames();
cout<<"Optimizer: 关键帧相连的帧:"<<vNeighKFs.size()<<endl;
for(int i=0, iend=vNeighKFs.size(); i<iend; i++)
{
KeyFrame* pKFi = vNeighKFs[i];
pKFi->mnBALocalForKF = pKF->mnId;
if(!pKFi->isBad())
lLocalKeyFrames.push_back(pKFi);
}
// step3:将lLocalKeyFrames的MapPoints加入到lLocalMapPoints
list<MapPoint*> lLocalMapPoints;
for(list<KeyFrame*>::iterator lit=lLocalKeyFrames.begin(), lend=lLocalKeyFrames.end(); lit!=lend; lit++)
{
vector<MapPoint*> vpMPs = (*lit)->GetMapPointMatches();
cout<<"Optimizer: get mappointMatches():"<<vpMPs.size()<<endl;
for(vector<MapPoint*>::iterator vit=vpMPs.begin(), vend=vpMPs.end(); vit!=vend; vit++)
{
MapPoint* pMP = *vit;
if(pMP)
{
if(!pMP->isBad())
{
if(pMP->mnBALocalForKF!=pKF->mnId)
{
lLocalMapPoints.push_back(pMP);
pMP->mnBALocalForKF=pKF->mnId;
}
}
}
}
}
cout<<"Optimizer: finish adding points"<<endl;
// step4: 遍历lLocalKeyFrames,将每个关键帧所能观测到的MapLine提取出来,放到lLocalMapLines
list<MapLine*> lLocalMapLines;
vector<long unsigned int> mlID;
for(list<KeyFrame*>::iterator lit=lLocalKeyFrames.begin(), lend=lLocalKeyFrames.end(); lit!=lend; lit++)
{
vector<MapLine*> vpMLs = (*lit)->GetMapLineMatches();
cout<<"Optimizer: get mapLineMatches():"<<vpMLs.size()<<endl;
for(vector<MapLine*>::iterator vit=vpMLs.begin(), vend=vpMLs.end(); vit!=vend; vit++)
{
MapLine* pML = *vit;
if(pML)
{
cout<<"Optimizer: mapline exist"<<endl;
if(!pML->isBad())
{
cout<<"Optimizer: mapline is not bad"<<endl;
if(pML->mnBALocalForKF!=pKF->mnId)
{
lLocalMapLines.push_back(pML);
pML->mnBALocalForKF = pKF->mnId;
mlID.push_back(pML->mnId);
cout<<"Optimizer:maplinesmatches id: "<<pKF->mnId<<endl;
}
}
}
}
}
cout<<"Optimizer: finish adding lines: size:"<< lLocalMapLines.size()<<endl;
#if 0
for(list<MapLine*>::iterator lit=lLocalMapLines.begin(), lend=lLocalMapLines.end(); lit!=lend; lit++)
{
int i=0;
if(count(mlID.begin(), mlID.end(), mlID[i])>1)
{
cout << "exist ===============" << endl;
lit = lLocalMapLines.erase(lit);
}
i++;
}
#endif
// step5: 得到能被局部MapPoints观测到,但不属于局部关键帧的关键帧,这些关键帧在局部BA优化时固定
list<KeyFrame*> lFixedCameras;
for(list<MapPoint*>::iterator lit=lLocalMapPoints.begin(), lend=lLocalMapPoints.end(); lit!=lend; lit++)
{
map<KeyFrame*, size_t > observations = (*lit)->GetObservations();
for(map<KeyFrame*, size_t>::iterator mit=observations.begin(), mend=observations.end(); mit!=mend; mit++)
{
KeyFrame* pKFi = mit->first;
if(pKFi->mnBALocalForKF!=pKF->mnId && pKFi->mnBAFixedForKF!=pKF->mnId)
{
pKFi->mnBAFixedForKF=pKF->mnId;
if(!pKFi->isBad())
lFixedCameras.push_back(pKFi);
}
}
}
// step6:构造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);
if(pbStopFlag)
optimizer.setForceStopFlag(pbStopFlag);
unsigned long maxKFid = 0;
cout<<"Optimizer: adding pose od local Keyframe"<<endl;
// step7:添加顶点,Pose of Local KeyFrame
for(list<KeyFrame*>::iterator lit=lLocalKeyFrames.begin(), lend=lLocalKeyFrames.end(); lit!=lend; lit++)
{
KeyFrame* pKFi = *lit;
g2o::VertexSE3Expmap* vSE3 = new g2o::VertexSE3Expmap();
vSE3->setEstimate(Converter::toSE3Quat(pKFi->GetPose()));
vSE3->setId(pKFi->mnId);
// cout << "KeyFrame Id = " << pKFi->mnId << endl;
vSE3->setFixed(pKFi->mnId==0); //第一个关键帧需要Fix
optimizer.addVertex(vSE3);
if(pKFi->mnId>maxKFid)
maxKFid=pKFi->mnId;
}
cout<<"Optimizer: adding fixed keyframe"<<endl;
// step8:添加固定帧的顶点,Pose of Fixed KeyFrame
for(list<KeyFrame*>::iterator lit=lFixedCameras.begin(), lend=lFixedCameras.end(); lit!=lend; lit++)
{
KeyFrame* pKFi = *lit;
g2o::VertexSE3Expmap* vSE3 = new g2o::VertexSE3Expmap();
vSE3->setEstimate(Converter::toSE3Quat(pKFi->GetPose()));
vSE3->setId(pKFi->mnId);
// cout << "Fixed KeyFrame Id = " << pKFi->mnId << endl;
vSE3->setFixed(true);
optimizer.addVertex(vSE3);
if(pKFi->mnId > maxKFid)
maxKFid = pKFi->mnId;
}
vector<int> MapPointID;
//***********************Set MapPoint Vertices******************************
// step9:添加MapPoint的3D顶点
const int nExpectedSize = (lLocalKeyFrames.size()+lFixedCameras.size())*lLocalMapPoints.size();
vector<g2o::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 thHuberMono = sqrt(5.991);
const float thHuberStereo = sqrt(7.815);
for(list<MapPoint*>::iterator lit=lLocalMapPoints.begin(), lend=lLocalMapPoints.end(); lit!=lend; lit++)
{
MapPoint* pMP = *lit;
g2o::VertexSBAPointXYZ* vPoint = new g2o::VertexSBAPointXYZ();
vPoint->setEstimate(Converter::toVector3d(pMP->GetWorldPos()));
int id = pMP->mnId + maxKFid + 1;
vPoint->setId(id);
// cout << "MapPoint Id = " << id << endl;
vPoint->setMarginalized(true);
optimizer.addVertex(vPoint);
MapPointID.push_back(id);
const map<KeyFrame*, size_t > observations = pMP->GetObservations();
// Set Edges
for(map<KeyFrame*, size_t>::const_iterator mit=observations.begin(), mend=observations.end(); mit!=mend; mit++)
{
KeyFrame* pKFi = mit->first;
if(!pKFi->isBad())
{
const cv::KeyPoint &kpUn = pKFi->mvKeysUn[mit->second];
if(pKFi->mvuRight[mit->second]<0)
{
Eigen::Matrix<double,2,1> obs;
obs << kpUn.pt.x, kpUn.pt.y;
g2o::EdgeSE3ProjectXYZ* e = new g2o::EdgeSE3ProjectXYZ();
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);
e->fx = pKFi->fx;
e->fy = pKFi->fy;
e->cx = pKFi->cx;
e->cy = pKFi->cy;
optimizer.addEdge(e);
vpEdgesMono.push_back(e);
vpEdgeKFMono.push_back(pKFi);
vpMapPointEdgeMono.push_back(pMP);
}
else // Stereo observation
{
Eigen::Matrix<double,3,1> obs;
const float kp_ur = pKFi->mvuRight[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(pKFi->mnId)));
e->setMeasurement(obs);
const float &invSigma2 = pKFi->mvInvLevelSigma2[kpUn.octave];
Eigen::Matrix3d Info = Eigen::Matrix3d::Identity()*invSigma2;
e->setInformation(Info);
g2o::RobustKernelHuber* rk = new g2o::RobustKernelHuber;
e->setRobustKernel(rk);
rk->setDelta(thHuberStereo);
e->fx = pKFi->fx;
e->fy = pKFi->fy;
e->cx = pKFi->cx;
e->cy = pKFi->cy;
e->bf = pKFi->mbf;
optimizer.addEdge(e);
vpEdgesStereo.push_back(e);
vpEdgeKFStereo.push_back(pKFi);
vpMapPointEdgeStereo.push_back(pMP);
}
}
}
}
sort(MapPointID.begin(), MapPointID.end());
int maxMapPointID = MapPointID[MapPointID.size()-1];
cout << "Optimizer:************ maxMapPointID = " << maxMapPointID << " ************" << endl;
// =========设置线段终止点和相机位姿之间的边=========
for(map<KeyFrame*, size_t>::const_iterator mit=observations.begin(), mend=observations.end(); mit!=mend; mit++)
{
KeyFrame* pKFi = mit->first;
if(!pKFi->isBad())
{
Eigen::Vector3d line_obs;
line_obs = pKFi->mvKeyLineFunctions[mit->second];
EdgeLineProjectXYZ* e = new EdgeLineProjectXYZ();
e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(ide)));
e->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(pKFi->mnId)));
e->setMeasurement(line_obs);
e->setInformation(Eigen::Matrix3d::Identity()*invSigma);
g2o::RobustKernelHuber* rk_line_e = new g2o::RobustKernelHuber;
e->setRobustKernel(rk_line_e);
rk_line_e->setDelta(thHuberMono);
e->fx = pKF->fx;
e->fy = pKF->fy;
e->cx = pKF->cx;
e->cy = pKF->cy;
e->Xw = pML->mWorldPos.head(3);
optimizer.addEdge(e);
vpLineEdgesEP.push_back(e);
vpLineEdgeKF.push_back(pKFi);
vpMapLineEdge.push_back(pML);
}
}
}
cout<<"Optimizer: finish 终点of lines"<<endl;
if(pbStopFlag)
if(*pbStopFlag)
return;
optimizer.initializeOptimization();
optimizer.optimize(5);
bool bDoMore = true;
if(pbStopFlag)
if(*pbStopFlag)
bDoMore = false;
if(bDoMore)
{
// Check inlier observations
for(size_t i=0, iend=vpEdgesMono.size(); i<iend; i++)
{
g2o::EdgeSE3ProjectXYZ* e = vpEdgesMono[i];
MapPoint* pMP = vpMapPointEdgeMono[i];
if(pMP->isBad())
continue;
if(e->chi2()>5.991 || !e->isDepthPositive())
{
e->setLevel(1);
}
e->setRobustKernel(0);
}
cout<<"Optimizer: finish checking inlier observations"<<endl;
for(size_t i=0, iend=vpLineEdgesSP.size(); i<iend; i++)
{
EdgeLineProjectXYZ* e = vpLineEdgesSP[i];
MapLine* pML = vpMapLineEdge[i];
if(pML->isBad())
continue;
if(e->chi2()>7.815)
{
e->setLevel(1);
}
e->setRobustKernel(0);
}
cout<<"Optimizer: finish start points of lines"<<endl;
for(size_t i=0, iend=vpLineEdgesEP.size(); i<iend; i++)
{
EdgeLineProjectXYZ* e = vpLineEdgesEP[i];
MapLine* pML = vpMapLineEdge[i];
if(pML->isBad())
continue;
if(e->chi2()>7.815)
{
e->setLevel(1);
}
e->setRobustKernel(0);
}
cout<<"Optimizer: finish end points of lines"<<endl;
// Optimize again without the outliers
optimizer.initializeOptimization(0);
cout<<"Optimizer: finish initialization optimization"<<endl;
optimizer.optimize(10);
}
cout<<"Optimizer: finish bDoMore"<<endl;
vector<pair<KeyFrame*, MapPoint*>> vToErase;
vToErase.reserve(vpEdgesMono.size());
cout<<"Optimizer: check inlier observations"<<endl;
// check inlier observations
for(size_t i=0, iend=vpEdgesMono.size(); i<iend; i++)
{
g2o::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));
}
}
vector<pair<KeyFrame*,MapLine*>> vLineToErase;
vLineToErase.reserve(vpLineEdgesSP.size());
for(size_t i=0, iend=vpLineEdgesSP.size(); i<iend; i++)
{
EdgeLineProjectXYZ* e = vpLineEdgesSP[i];
MapLine* pML = vpMapLineEdge[i];
if(pML->isBad())
continue;
if(e->chi2()>7.815)
{
KeyFrame* pKFi = vpLineEdgeKF[i];
vLineToErase.push_back(make_pair(pKFi, pML));
}
}
// Get Map Mutex
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);
}
}
if(!vLineToErase.empty())
{
for(size_t i=0; i<vLineToErase.size(); i++)
{
KeyFrame* pKFi = vLineToErase[i].first;
MapLine* pMLi = vLineToErase[i].second;
pKFi->EraseMapLineMatch(pMLi);
pMLi->EraseObservation(pKFi);
}
}
cout<<"Optimizer: recover optimized data"<<endl;
// Recover optimized data
//Keyframes
for(list<KeyFrame*>::iterator lit=lLocalKeyFrames.begin(), lend=lLocalKeyFrames.end(); lit!=lend; lit++)
{
KeyFrame* pKF = *lit;
g2o::VertexSE3Expmap* vSE3 = static_cast<g2o::VertexSE3Expmap*>(optimizer.vertex(pKF->mnId));
g2o::SE3Quat SE3quat = vSE3->estimate();
pKF->SetPose(Converter::toCvMat(SE3quat));
}
//Points
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+maxKFid+1));
pMP->SetWorldPos(Converter::toCvMat(vPoint->estimate()));
pMP->UpdateNormalAndDepth();
}
// Lines
for(list<MapLine*>::iterator lit=lLocalMapLines.begin(), lend=lLocalMapLines.end(); lit!=lend; lit++)
{
MapLine* pML = *lit;
g2o::VertexSBAPointXYZ* vStartP = static_cast<g2o::VertexSBAPointXYZ*>(optimizer.vertex(pML->mnId + maxKFid + maxMapPointID + 1));
g2o::VertexSBAPointXYZ* vEndP = static_cast<g2o::VertexSBAPointXYZ *>(optimizer.vertex(pML->mnId + maxKFid + maxMapPointID + maxMapLineSPID + 1));
Vector6d LinePos;
LinePos << Converter::toVector3d(Converter::toCvMat(vStartP->estimate())), Converter::toVector3d(Converter::toCvMat(vEndP->estimate()));
pML->SetWorldPos(LinePos);
pML->UpdateAverageDir();
}
cout<<"Optimizer: finish local BA "<<endl;
}
在localkeyframe中,装的是当前关键帧 相关联的关键帧(一级关键帧)。我们需要从这些关键帧中获得mvpMapPoint。使用GetMapPointMatches.