技术标签: 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();
}
}
文章浏览阅读331次。第一部分:准备工作1 安装虚拟机2 安装centos73 安装JDK以上三步是准备工作,至此已经完成一台已安装JDK的主机第二部分:准备3台虚拟机以下所有工作最好都在root权限下操作1 克隆上面已经有一台虚拟机了,现在对master进行克隆,克隆出另外2台子机;1.1 进行克隆21.2 下一步1.3 下一步1.4 下一步1.5 根据子机需要,命名和安装路径1.6 ..._创建一个hadoop项目
文章浏览阅读1.7k次。心脏滴血漏洞HeartBleed CVE-2014-0160 是由heartbeat功能引入的,本文从深入码层面的分析该漏洞产生的原因_heartbleed代码分析
文章浏览阅读1.4k次。前言ofd是国家文档标准,其对标的文档格式是pdf。ofd文档是容器格式文件,ofd其实就是压缩包。将ofd文件后缀改为.zip,解压后可看到文件包含的内容。ofd文件分析工具下载:点我下载。ofd文件解压后,可以看到如下内容: 对于xml文件,可以用文本工具查看。但是对于印章文件(Seal.esl)、签名文件(SignedValue.dat)就无法查看其内容了。本人开发一款ofd内容查看器,..._signedvalue.dat
文章浏览阅读1.8w次,点赞29次,收藏313次。整体系统设计本设计主要是对ADC和DAC的使用,主要实现功能流程为:首先通过串口向FPGA发送控制信号,控制DAC芯片tlv5618进行DA装换,转换的数据存在ROM中,转换开始时读取ROM中数据进行读取转换。其次用按键控制adc128s052进行模数转换100次,模数转换数据存储到FIFO中,再从FIFO中读取数据通过串口输出显示在pc上。其整体系统框图如下:图1:FPGA数据采集系统框图从图中可以看出,该系统主要包括9个模块:串口接收模块、按键消抖模块、按键控制模块、ROM模块、D.._基于fpga的信息采集
文章浏览阅读2.5w次。1.背景错误信息:-- [http-nio-9904-exec-5] o.s.c.n.z.filters.post.SendErrorFilter : Error during filteringcom.netflix.zuul.exception.ZuulException: Forwarding error at org.springframework.cloud..._com.netflix.zuul.exception.zuulexception
文章浏览阅读358次。1.介绍图的相关概念 图是由顶点的有穷非空集和一个描述顶点之间关系-边(或者弧)的集合组成。通常,图中的数据元素被称为顶点,顶点间的关系用边表示,图通常用字母G表示,图的顶点通常用字母V表示,所以图可以定义为: G=(V,E)其中,V(G)是图中顶点的有穷非空集合,E(G)是V(G)中顶点的边的有穷集合1.1 无向图:图中任意两个顶点构成的边是没有方向的1.2 有向图:图中..._给定一个邻接矩阵未必能够造出一个图
文章浏览阅读321次。(十二)、WDS服务器安装通过前面的测试我们会发现,每次安装的时候需要加域光盘映像,这是一个比较麻烦的事情,试想一个上万个的公司,你天天带着一个光盘与光驱去给别人装系统,这将是一个多么痛苦的事情啊,有什么方法可以解决这个问题了?答案是肯定的,下面我们就来简单说一下。WDS服务器,它是Windows自带的一个免费的基于系统本身角色的一个功能,它主要提供一种简单、安全的通过网络快速、远程将Window..._doc server2012上通过wds+mdt无人值守部署win11系统.doc
文章浏览阅读219次。python–xlrd/xlwt/xlutilsxlrd只能读取,不能改,支持 xlsx和xls 格式xlwt只能改,不能读xlwt只能保存为.xls格式xlutils能将xlrd.Book转为xlwt.Workbook,从而得以在现有xls的基础上修改数据,并创建一个新的xls,实现修改xlrd打开文件import xlrdexcel=xlrd.open_workbook('E:/test.xlsx') 返回值为xlrd.book.Book对象,不能修改获取sheett_xlutils模块可以读xlsx吗
文章浏览阅读8.2w次,点赞267次,收藏656次。运行Selenium出现'WebDriver' object has no attribute 'find_element_by_id'或AttributeError: 'WebDriver' object has no attribute 'find_element_by_xpath'等定位元素代码错误,是因为selenium更新到了新的版本,以前的一些语法经过改动。..............._unresolved attribute reference 'find_element_by_id' for class 'webdriver
文章浏览阅读198次。一:模态窗口//父页面JSwindow.showModalDialog(ifrmehref, window, 'dialogWidth:550px;dialogHeight:150px;help:no;resizable:no;status:no');//子页面获取父页面DOM对象//window.showModalDialog的DOM对象var v=parentWin..._jquery获取父window下的dom对象
文章浏览阅读1.7w次,点赞15次,收藏129次。算法(algorithm)是解决一系列问题的清晰指令,也就是,能对一定规范的输入,在有限的时间内获得所要求的输出。 简单来说,算法就是解决一个问题的具体方法和步骤。算法是程序的灵 魂。二、算法的特征1.可行性 算法中执行的任何计算步骤都可以分解为基本可执行的操作步,即每个计算步都可以在有限时间里完成(也称之为有效性) 算法的每一步都要有确切的意义,不能有二义性。例如“增加x的值”,并没有说增加多少,计算机就无法执行明确的运算。 _算法
文章浏览阅读1.5k次,点赞18次,收藏26次。网络安全的标准和规范是网络安全领域的重要组成部分。它们为网络安全提供了技术依据,规定了网络安全的技术要求和操作方式,帮助我们构建安全的网络环境。下面,我们将详细介绍一些主要的网络安全标准和规范,以及它们在实际操作中的应用。_网络安全标准规范