技术标签: SLAM从小白到大黑
ORB-SLAM2代码阅读笔记(七):LocalMapping线程
DreamWaterFound/self_commit_ORB-SLAM2
局部建图线程调用
// Local BA
// 当局部地图中的关键帧大于2个的时候进行局部地图的BA
if(mpMap->KeyFramesInMap()>2)
//当前关键帧,是否禁止BA的标志位,mpMap(局部见图下的地图 ORB_SLAM2::LocalMapping::Map)
Optimizer::LocalBundleAdjustment(mpCurrentKeyFrame,&mbAbortBA, mpMap);
优化函数
/**
* 局部优化函数步骤如下:
* 1.创建局部关键帧列表, 即那些共享CovisbilityMap的Keyframes. 存入lLocalKeyFrames中.
* 2.遍历 lLocalKeyFrames 中关键帧,将它们观测的MapPoints加入到lLocalMapPoints
* 3.再找出能看到上面的地图点, 却不在Local Keyframe范围内的keyframe(为什么??). 存入lFixedCameras.
* 4.把上面的Local Keyframe, Map Point, FixedCamera都设置为图节点.
* 5.对于lLocalMapPoints中的每一个地图点及能看到它的所有keyframes, 建立边:
* 边的两端分别是keyframe的位姿与当前地图点为位姿.
* 边的观测值为该地图点在当前keyframe中的二维位置.
* 信息矩阵(权重)是观测值的偏离程度, 即3D地图点反投影回地图的误差.
* 6.去除掉一些不符合标准的边.
* 7.把优化后地图点和keyframe位姿放回去.
*
*/
//在局部见图函数内部,传入的参数如下:
//当前关键帧,是否禁止BA的标志位,mpMap(局部见图下的地图 ORB_SLAM2::LocalMapping::Map)
void Optimizer::LocalBundleAdjustment(KeyFrame *pKF, bool* pbStopFlag, Map* pMap)
{
// Local KeyFrames: First Breath Search from Current Keyframe
list<KeyFrame*> lLocalKeyFrames;
//step 1:获取局部关键帧列表存放在lLocalKeyFrames中。也就是和当前关键帧有共视关系的关键帧列表
//1-1:将当前关键帧加入lLocalKeyFrames
lLocalKeyFrames.push_back(pKF);
pKF->mnBALocalForKF = pKF->mnId;
//1-2:找到关键帧连接的关键帧(一级相连),加入lLocalKeyFrames中
const vector<KeyFrame*> vNeighKFs = pKF->GetVectorCovisibleKeyFrames();
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);
}
// Local MapPoints seen in Local KeyFrames
// step 2:遍历 lLocalKeyFrames 中关键帧,将它们观测的MapPoints加入到lLocalMapPoints
list<MapPoint*> lLocalMapPoints;
// 遍历 lLocalKeyFrames 中的每一个关键帧
for(list<KeyFrame*>::iterator lit=lLocalKeyFrames.begin() , lend=lLocalKeyFrames.end(); lit!=lend; lit++)
{
vector<MapPoint*> vpMPs = (*lit)->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!=pKF->mnId)// 防止重复添加
{
lLocalMapPoints.push_back(pMP);
pMP->mnBALocalForKF=pKF->mnId;// 防止重复添加
}
} // 判断这个地图点是否靠谱
} // 遍历这个关键帧观测到的每一个地图点
} // 遍历 lLocalKeyFrames 中的每一个关键帧
// Fixed Keyframes. Keyframes that see Local MapPoints but that are not Local Keyframes
// step 3:得到能被局部MapPoints观测到,但不属于局部关键帧的关键帧,这些关键帧在局部BA优化时不优化
//(创建固定关键帧列表lFixedCameras。这些关键帧可以看到局部地图点,但是这些关键帧并不是局部关键帧) //TODO 这些固定帧有何用??
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;
// pKFi->mnBALocalForKF!=pKF->mnId表示局部关键帧,
// 其它的关键帧虽然能观测到,但不属于局部关键帧
if(pKFi->mnBALocalForKF!=pKF->mnId && pKFi->mnBAFixedForKF!=pKF->mnId)
{
pKFi->mnBAFixedForKF=pKF->mnId;// 防止重复添加,pKF是触发局部BA的当前关键帧
if(!pKFi->isBad())
lFixedCameras.push_back(pKFi);
}
}
}
//TODO 下面的是重头戏,g2o开始优化的部分
// Setup optimizer
// step 4:构造g2o优化器
g2o::SparseOptimizer optimizer;
g2o::BlockSolver_6_3::LinearSolverType * linearSolver;
linearSolver = new g2o::LinearSolverEigen<g2o::BlockSolver_6_3::PoseMatrixType>();
g2o::BlockSolver_6_3 * solver_ptr = new g2o::BlockSolver_6_3(linearSolver);
// LM
g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr);
optimizer.setAlgorithm(solver);
// 外界设置的停止标志,是否禁止BA的标志位
if(pbStopFlag)
optimizer.setForceStopFlag(pbStopFlag);
unsigned long maxKFid = 0;
//step 5:添加顶点和边
// Set Local KeyFrame vertices
// 5-1:添加顶点: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); //mnId 为帧的ID号,每一帧都不一样
vSE3->setFixed(pKFi->mnId==0);//第一帧位置固定,确定会存在第一帧率吗??,这里的目的就是防止优化漂移吧
optimizer.addVertex(vSE3);
if(pKFi->mnId>maxKFid)
maxKFid=pKFi->mnId;
}
// Set Fixed KeyFrame vertices
// 5-2 :添加顶点:Pose of Fixed KeyFrame,注意这里调用了vSE3->setFixed(true)。设置固定关键帧顶点,并加入到优化器列表中。
//注意了,这里的固定关键帧,是固定的,本身是不属于局部关键帧的
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);
vSE3->setFixed(true); // 所有的这些节点的未知都固定
optimizer.addVertex(vSE3);
if(pKFi->mnId>maxKFid)
maxKFid=pKFi->mnId;
}
// Set MapPoint vertices
// 5-3 :添加3D顶点(设置MapPoint顶点)
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);
// step 6 .遍历局部地图点列表,设置优化对应的边
for(list<MapPoint*>::iterator lit=lLocalMapPoints.begin(), lend=lLocalMapPoints.end(); lit!=lend; lit++)
{
// 添加顶点:MapPoint
MapPoint* pMP = *lit;
g2o::VertexSBAPointXYZ* vPoint = new g2o::VertexSBAPointXYZ();
vPoint->setEstimate(Converter::toVector3d(pMP->GetWorldPos()));
int id = pMP->mnId+maxKFid+1; //maxKFid 索引值,虽然顶点类型不一样,但是Tcw和Mapoint两种顶点还是不能有相同的ID的~
vPoint->setId(id);
vPoint->setMarginalized(true); //? 一直不明白这个是做什么的,设置可以被边缘化?
optimizer.addVertex(vPoint);
const map<KeyFrame*,size_t> observations = pMP->GetObservations();
// Set edges
// step 7:在添加完了一个地图点之后, 对每一对关联的MapPoint和KeyFrame构建边
// 遍历所有观测到当前地图点的关键帧
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];
// Monocular observation
// 和前面基本上都是一样的,若为-1则表示只有单目,这是代码的一种约定俗成~
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))); //帧的位姿Tcw
e->setMeasurement(obs); //设置观测值
const float &invSigma2 = pKFi->mvInvLevelSigma2[kpUn.octave];
e->setInformation(Eigen::Matrix2d::Identity()*invSigma2); //TODO 设置信息矩阵~,为什么是尺度因子平方的逆??
// 这里也是使用鲁棒核函数
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);
} // 根据单目/双目两种不同的输入构造不同的误差边
} //判断遍历到的这个观测到当前地图点的关键帧是bad的
} // 遍历所有观测到当前地图点的关键帧
} // 遍历所有的局部地图中的地图点
// 检查是否外部请求停止
if(pbStopFlag) //是否禁止BA的标志位
if(*pbStopFlag)
return;
// step 8:开始优化 -- 第一阶段优化(这里进行了5次迭代优化。)
optimizer.initializeOptimization();
optimizer.optimize(5);
bool bDoMore= true;
// 检查是否外部请求停止
if(pbStopFlag)
if(*pbStopFlag)
bDoMore = false;
// 如果有外部请求停止,那么就不在进行第二阶段的优化
// step 9.检查上边5次迭代优化正确点的观测值,并将异常点排除掉。完成后再次进行10次迭代优化。
if(bDoMore)
{
// Check inlier observations
// step 10:检测outlier,并设置下次不优化
// 遍历所有的单目误差边
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()) //TODO 这两个参数都是啥??
{
e->setLevel(1);// 不优化 //TODO 怎么体现出来的??
}
// 第二阶段优化的时候就属于精求解了,所以就不使用核函数
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);
}
e->setRobustKernel(0);
}
// Optimize again without the outliers
//TODO 这里函数的0表示什么??表示的是优化的层级吗?上面有的层级level设置为1,那么是不是说就在这一层级
optimizer.initializeOptimization(0);
optimizer.optimize(10);
}
vector<pair<KeyFrame*,MapPoint*> > vToErase; //存储经过卡方检验判定较大误差的顶点
vToErase.reserve(vpEdgesMono.size()+vpEdgesStereo.size());
// Check inlier observations
// step 11:优化后进行优化结果检查,剔除连接误差比较大的关键帧和MapPoint
// 对于单目误差边
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())
{
// outlier
KeyFrame* pKFi = vpEdgeKFMono[i];
vToErase.push_back(make_pair(pKFi,pMP));
}
}
// 双目误差边
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));
}
}
// Get Map Mutex
unique_lock<mutex> lock(pMap->mMutexMapUpdate);
// 删除点
// 连接偏差比较大,在关键帧中剔除对该MapPoint的观测
// 连接偏差比较大,在MapPoint中剔除对该关键帧的观测
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
// step 12:优化后更新关键帧位姿以及MapPoints的位置、平均观测方向等属性
//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();
}
}
文章浏览阅读3.6k次。npm报错处理_npm安装报错
文章浏览阅读2.5k次。问题本文主要针对的问题是在Unity中对Button类进行Onclick事件绑定的时候出现的函数参数错误进行分析解决,具体例子如下: Button[] button = GetComponentsInChildren<Button>(); int buttonCnt = 3; for (int i = 0; i < buttonCnt; i++) { button[i].SetActive(true); Debug.Log("i: " + i);_unity 按钮onclick参数类型
文章浏览阅读6.5k次,点赞5次,收藏43次。Halcon矩阵(Matrix)详细说明创建(Creation)create_matrixcopy_matrixrepeat_matrix访问(Access)算法(Arithmetic)分解(Decomposition)特征值(Eigenvalues)特性(Features)文件操作(File)新的改变功能快捷键合理的创建标题,有助于目录的生成如何改变文本的样式插入链接与图片如何插入一段漂亮的代码片生成一个适合你的列表创建一个表格设定内容居中、居左、居右SmartyPants创建一个自定义列表如何创建一个注_get_full_matrix
文章浏览阅读2.5k次。欧氏距离(Euclidean Distance)欧式距离是最经典的一种距离算法,适用于求解两点之间直线的距离,适用于各个向量标准统一的情况,如各种药品的使用量、商品的售销量等。 欧氏距离也是最易于理解的一种距离计算方法,源自欧氏空间中两点间的距离公式。 二维空间上两点a(x1,y1)a(x_1,y_1)与b(x2,y2)b(x_2,y_2)之间的欧式距离: d12=(x1−x2)2+(y1−y_两条线之间的欧式距离怎么算
文章浏览阅读3.9w次,点赞78次,收藏436次。我参加过的数学建模比赛很多,除了本校的两次数学建模(二三等)外,全国数学建模(省二),亚太数学建模(s),ICM/MCM(M),五一建模联赛,电工杯(最近正在准备),之前错过mathorcup,有点遗憾。到2019年暑假前,总计自己一年左右参加7次建模比赛,说下自己建模常用的软件使用,本人在队里主要负责编程,但是写作和建模也同样会和队友交流。论文类LaTeX与WordWor..._什么软件可以分析数学建模的问题,以及给出合理的解释和分析
文章浏览阅读3k次。计算机网络原理╭第一章 计算机网络概述|第二章 网络应用|第三章 传输层|第四章 网络层内容大纲<|第五章 数据链路层与局域网|第六章 物理层|第七章 无线与移动网络╰第八章 网络安全基础第一章 计算机网络概述1.计算机网络基本概念(填空选择题)1>计算机网络定义*1.定义:1)计算机网络是 互连的、自治的 计算机的集合;互连: 是指利用通信链路链接相互独立的计算机系统;自治: 是指互连的计算机系统 彼此独立 ,不存在主从或控制与被控制的关系;2)一个计算机网络_throught up速率
文章浏览阅读6.1k次,点赞7次,收藏41次。图数据可视化_R语言ggplot2包和tidybayes包绘制小提琴图进阶概述:绘制小提琴图时按数据分布的密度填充不同透明度的颜色(渐变填充)。使用工具:R语言中的ggplot2和tidybayes工具包本文使用的数据及计算方式与之前的博文一致:数据可视化——R语言ggplot2包绘制精美的小提琴图(并箱线图或误差条图组合)。本文采用tidybayes包中stat_eye()绘制小提琴图,通过设置aes(alpha = stat(f)可实现渐变填充。由于stat_eye()会默认采用中位数及分位数作_分半小提琴图
文章浏览阅读1.4k次。江苏省专转本计算机_计算机硬件系统是执行软件程序的物质基础,其中能执行程序指令的是( )
文章浏览阅读4.4k次。在Robocode中,坦克分为3个部件: 身体(Body)、炮塔(Gun)、雷达(Radar)。 因此,在Robot类(还记得吗,它是任何坦克的父类)中,有对这些部件操作的方法。要查看Robocode提供的API,可以在robocode目录下的javadoc下找到,也可以在Robocode程序的帮助菜单中找到: 对于Body来说,Robot类提供了4个方法:_robocode炮和机身的运动分离
文章浏览阅读77次。Problem DescriptionA number whose only prime factors are 2,3,5 or 7 is called a humble number. The sequence 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 12, 14, 15, 16, 18, 20, 21, 24, 25, 27, ... shows the first...
文章浏览阅读6.6k次,点赞6次,收藏5次。一位大学毕业生第一份工作不太满意,裸辞跳槽的故事_前端转后端
文章浏览阅读3.2k次,点赞3次,收藏21次。背景: 骑士周游问题在棋盘格里,马走日,遍历所有网格点,找到每个网格都走过,且只有一次的路径。算法实现:用于解决骑士周游问题的图搜索算法是深度优先搜索(DFS),该算法是逐层建立搜索树,沿着树的单支尽量深入的向下搜索。连接尽量多的顶点,必要时可以进行分支。深度优先搜索同样要用到顶点的“前驱”属性,来构建树或森林。另外需要设置“发现时间”和“结束时间”属性。发现时间是在第几步访问到了这个顶点(设置灰色);结束时间是在第几步完成了此顶点的探索(设置黑色)。通用的深度优先搜索算法代码:# BFS采_用python代码写深度优先遍历算法的时间复杂度