干货第一时间送达
pKFini->ComputeBoW();
pKFcur->ComputeBoW();
mpAtlas->AddKeyFrame(pKFini);
mpAtlas->AddKeyFrame(pKFcur);
for(size_t i=0; i<mvIniMatches.size();i++)
if(mvIniMatches[i]<0)
continue;
cv::Mat worldPos(mvIniP3D[i]);
MapPoint* pMP = new MapPoint(worldPos,pKFcur,mpAtlas->GetCurrentMap());
pMP->AddObservation(pKFini,i);
pMP->AddObservation(pKFcur,mvIniMatches[i]);
pMP->ComputeDistinctiveDescriptors();
pMP->UpdateNormalAndDepth();
observations=mObservations; // 获得观测到该地图点的所有关键帧
pRefKF=mpRefKF; // 观测到该点的参考关键帧(第一次创建时的关键帧)
Pos = mWorldPos.clone(); // 地图点在世界坐标系中的位置
cv::Mat normal = cv::Mat::zeros(3,1,CV_32F);
int n=0;
for(map<KeyFrame*,size_t>::iterator mit=observations.begin(), mend=observations.end(); mit!=mend; mit++)
{KeyFrame* pKF = mit->first;
tuple<int,int> indexes = mit -> second;
int leftIndex = get<0>(indexes), rightIndex = get<1>(indexes);
if(leftIndex != -1){cv::Mat Owi = pKF->GetCameraCenter();
cv::Mat normali = mWorldPos - Owi;normal = normal + normali/cv::norm(normali);
n++;
}
if(rightIndex != -1){cv::Mat Owi = pKF->GetRightCameraCenter();
cv::Mat normali = mWorldPos - Owi;normal = normal + normali/cv::norm(normali);
n++;
}
}
cv::Mat PC = Pos - pRefKF->GetCameraCenter();
const float dist = cv::norm(PC);
// 观测到该地图点的当前帧的特征点在金字塔的第几层
tuple<int ,int> indexes = observations[pRefKF];
int leftIndex = get<0>(indexes), rightIndex = get<1>(indexes);
int level;
if(pRefKF -> NLeft == -1){
level = pRefKF->mvKeysUn[leftIndex].octave;
}
else if(leftIndex != -1){
level = pRefKF -> mvKeys[leftIndex].octave;
}
else{
level = pRefKF -> mvKeysRight[rightIndex - pRefKF -> NLeft].octave;
}
//const int level = pRefKF->mvKeysUn[observations[pRefKF]].octave;
const float levelScaleFactor = pRefKF->mvScaleFactors[level]; // 当前金字塔层对应的缩放倍数
const int nLevels = pRefKF->mnScaleLevels; // 金字塔层数
{
unique_lock<mutex> lock3(mMutexPos);
// 使用方法见PredictScale函数前的注释
mfMaxDistance = dist*levelScaleFactor; // 观测到该点的距离上限
mfMinDistance = mfMaxDistance/pRefKF->mvScaleFactors[nLevels-1]; // 观测到该点的距离下限
mNormalVector = normal/n; // 获得地图点平均的观测方向
}
mpAtlas->AddMapPoint(pMP);
// 遍历每一个地图点
for(vector<MapPoint*>::iterator vit=vpMP.begin(), vend=vpMP.end(); vit!=vend; vit++)
...
// 统计与当前关键帧存在共视关系的其他帧
map<KeyFrame*,size_t> observations = pMP->GetObservations();
for(map<KeyFrame*,size_t>::iterator mit=observations.begin(), mend=observations.end(); mit!=mend; mit++)
...
// 体现了作者的编程功底,很强
KFcounter[mit->first]++;
for(map<KeyFrame*,int>::iterator mit=KFcounter.begin(), mend=KFcounter.end(); mit!=mend; mit++)
{
...
// 找到共视程度最高的关键帧
if(mit->second>nmax)
{
nmax=mit->second;
pKFmax=mit->first;
}
if(mit->second>=th)
{
// 更新共视关系大于一定阈值的边
vPairs.push_back(make_pair(mit->second,mit->first));
// 更新其它关键帧与当前帧的连接权重
(mit->first)->AddConnection(this,mit->second);
}
}
if(vPairs.empty())
{
vPairs.push_back(make_pair(nmax,pKFmax));
pKFmax->AddConnection(this,nmax);
}
sort(vPairs.begin(),vPairs.end());
// 将排序后的结果分别组织成为两种数据类型
list<KeyFrame*> lKFs;
list<int> lWs;
for(size_t i=0; i<vPairs.size();i++)
{
// push_front 后变成了从大到小顺序
lKFs.push_front(vPairs[i].second);
lWs.push_front(vPairs[i].first);
}
...
mConnectedKeyFrameWeights = KFcounter;
mvpOrderedConnectedKeyFrames = vector<KeyFrame*>(lKFs.begin(),lKFs.end());
mvOrderedWeights = vector<int>(lWs.begin(), lWs.end());
...
if(mbFirstConnection && mnId!=mpMap->GetInitKFid())
{
// 初始化该关键帧的父关键帧为共视程度最高的那个关键帧
mpParent = mvpOrderedConnectedKeyFrames.front();
// 建立双向连接关系,将当前关键帧作为其子关键帧
mpParent->AddChild(this);
mbFirstConnection = false;
}
// 调用
Optimizer::GlobalBundleAdjustemnt(mpAtlas->GetCurrentMap(),20);
// 定义
void Optimizer::GlobalBundleAdjustemnt(Map* pMap, int nIterations, bool* pbStopFlag, const unsigned long nLoopKF, const bool bRobust)
//调用
vector<KeyFrame*> vpKFs = pMap->GetAllKeyFrames();
vector<MapPoint*> vpMP = pMap->GetAllMapPoints();
BundleAdjustment(vpKFs,vpMP,nIterations,pbStopFlag, nLoopKF, bRobust);
// 定义
void Optimizer::BundleAdjustment(const vector<KeyFrame *> &vpKFs, const vector<MapPoint *> &vpMP, int nIterations, bool* pbStopFlag, const unsigned long nLoopKF, const bool bRobust)
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);
typedef BlockSolver< BlockSolverTraits<6, 3> > BlockSolver_6_3;
g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr);
g2o::SparseOptimizer optimizer;
optimizer.setAlgorithm(solver);// 用前面定义好的求解器作为求解方法
optimizer.setVerbose(false);// 设置优化过程输出信息用的
// 对于当前地图中的所有关键帧
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()));
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
// Set MapPoint vertices
// Step 2.2:向优化器添加MapPoints顶点
// 遍历地图中的所有地图点
for(size_t i=0; i<vpMP.size(); i++)
{
MapPoint* pMP = vpMP[i];
// 跳过无效地图点
if(pMP->isBad())
continue;
// 创建顶
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;
//SET EDGES
// 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++;
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();
// 填充数据,构造约束关系
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);
// 信息矩阵,也是协方差,表明了这个约束的观测在各个维度(x,y)上的可信程度,在我们这里对于具体的一个点,两个坐标的可信程度都是相同的,
// 其可信程度受到特征点在图像金字塔中的图层有关,图层越高,可信度越差
// 为了避免出现信息矩阵中元素为负数的情况,这里使用的是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的阈值,如果重投影的误差大约大于1个像素的时候,就认为不太靠谱的点了,
// 核函数是为了避免其误差的平方项出现数值上过大的增长
rk->setDelta(thHuber2D);
}
// 设置相机内参
// ORB-SLAM2的做法
//e->fx = pKF->fx;
//e->fy = pKF->fy;
//e->cx = pKF->cx;
//e->cy = pKF->cy;
// ORB-SLAM3的改变
e->pCamera = pKF->mpCamera;
optimizer.addEdge(e);
}
else
{
// 双目或RGBD相机按照下面操作
......这里忽略,只讲单目
}
} // 向优化器添加投影边,也就是遍历所有观测到当前地图点的关键帧
// 如果因为一些特殊原因,实际上并没有任何关键帧观测到当前的这个地图点,那么就删除掉这个顶点,并且这个地图点也就不参与优化
if(nEdges==0)
{
optimizer.removeVertex(vPoint);
vbNotIncludedMP[i]=true;
}
else
{
vbNotIncludedMP[i]=false;
}
}
optimizer.initializeOptimization();
optimizer.optimize(nIterations);
float medianDepth = pKFini->ComputeSceneMedianDepth(2);
float invMedianDepth = 1.0f/medianDepth;
cv::Mat Tc2w = pKFcur->GetPose();
// x/z y/z 将z归一化到1
Tc2w.col(3).rowRange(0,3) = Tc2w.col(3).rowRange(0,3)*invMedianDepth;
pKFcur->SetPose(Tc2w);
vector<MapPointPtr> vpAllMapPoints = pKFini->GetMapPointMatches();
for (size_t iMP = 0; iMP < vpAllMapPoints.size(); iMP++)
{
if (vpAllMapPoints[iMP])
{
MapPointPtr pMP = vpAllMapPoints[iMP];
if(!pMP->isBad())
pMP->SetWorldPos(pMP->GetWorldPos() * invMedianDepth);
}
}
mpLocalMapper->InsertKeyFrame(pKFini);
mpLocalMapper->InsertKeyFrame(pKFcur);
mCurrentFrame.SetPose(pKFcur->GetPose());
mnLastKeyFrameId = pKFcur->mnId;
mnLastKeyFrameFrameId=mCurrentFrame.mnId;
mpLastKeyFrame = pKFcur;
mvpLocalKeyFrames.push_back(pKFcur);
mvpLocalKeyFrames.push_back(pKFini);
mvpLocalMapPoints = mpMap->GetAllMapPoints();
mpReferenceKF = pKFcur;
mCurrentFrame.mpReferenceKF = pKFcur;
mLastFrame = Frame(mCurrentFrame);
mpMap->SetReferenceMapPoints(mvpLocalMapPoints);
{
unique_lock<mutex> lock(mMutexState);
mState = eTrackingState::OK;
}
mpMap->calculateAvgZ();
// 初始化成功,至此,初始化过程完成
联系客服