Skip to content

Commit

Permalink
some updates
Browse files Browse the repository at this point in the history
  • Loading branch information
DreamWaterFound committed Aug 21, 2019
1 parent 84083c0 commit a553a7d
Show file tree
Hide file tree
Showing 5 changed files with 33 additions and 9 deletions.
10 changes: 9 additions & 1 deletion .vscode/settings.json
Original file line number Diff line number Diff line change
Expand Up @@ -51,6 +51,14 @@
"tuple": "cpp",
"typeindex": "cpp",
"typeinfo": "cpp",
"utility": "cpp"
"utility": "cpp",
"condition_variable": "cpp",
"algorithm": "cpp",
"iterator": "cpp",
"map": "cpp",
"memory_resource": "cpp",
"random": "cpp",
"set": "cpp",
"string": "cpp"
}
}
13 changes: 12 additions & 1 deletion Examples/ROS/ORB_SLAM2/src/ros_rgbd.cc
Original file line number Diff line number Diff line change
@@ -1,3 +1,15 @@
/**
* @file ros_rgbd.cc
* @author guoqing (1337841346@qq.com)
* @brief ORB RGB-D 输入的ROS节点实现
* @version 0.1
* @date 2019-08-06
*
* @copyright Copyright (c) 2019
*
*/


/**
* This file is part of ORB-SLAM2.
*
Expand All @@ -18,7 +30,6 @@
* along with ORB-SLAM2. If not, see <http://www.gnu.org/licenses/>.
*/


#include<iostream>
#include<algorithm>
#include<fstream>
Expand Down
3 changes: 2 additions & 1 deletion src/LocalMapping.cc
Original file line number Diff line number Diff line change
Expand Up @@ -312,10 +312,11 @@ void LocalMapping::MapPointCulling()
void LocalMapping::CreateNewMapPoints()
{
// Retrieve neighbor keyframes in covisibility graph
// 不同传感器下要求不一样,单目的时候要求更高一些
// 不同传感器下要求不一样,单目的时候需要有更多的具有较好共视关系的关键帧来建立地图
int nn = 10;
if(mbMonocular)
nn=20;

// step 1:在当前关键帧的共视关键帧中找到共视程度最高的nn帧相邻帧vpNeighKFs
const vector<KeyFrame*> vpNeighKFs = mpCurrentKeyFrame->GetBestCovisibilityKeyFrames(nn);

Expand Down
1 change: 1 addition & 0 deletions src/LoopClosing.cc
Original file line number Diff line number Diff line change
Expand Up @@ -777,6 +777,7 @@ void LoopClosing::CorrectLoop()
// Add loop edge
// STEP 7:添加当前帧与闭环匹配帧之间的边(这个连接关系不优化)
// 这两句话应该放在OptimizeEssentialGraph之前,因为OptimizeEssentialGraph的步骤4.2中有优化,(wubo???) -- 师兄..我们不优化这个回环边
// ? but, 我们为什么不优化这个回环边呢?
mpMatchedKF->AddLoopEdge(mpCurrentKF);
mpCurrentKF->AddLoopEdge(mpMatchedKF);

Expand Down
15 changes: 9 additions & 6 deletions src/Tracking.cc
Original file line number Diff line number Diff line change
Expand Up @@ -448,7 +448,7 @@ void Tracking::Track()
//单目初始化
MonocularInitialization();

//这一帧处理完成了,调用帧绘制器来赋值一份当前帧的状态
//这一帧处理完成了,更新帧绘制器中存储的最近的一份状态
mpFrameDrawer->Update(this);

//这个状态量在上面的初始化函数中被更新
Expand Down Expand Up @@ -900,13 +900,15 @@ void Tracking::MonocularInitialization()
for(size_t i=0; i<mCurrentFrame.mvKeysUn.size(); i++)
mvbPrevMatched[i]=mCurrentFrame.mvKeysUn[i].pt;

// 这两句是多余的
// 这句判断其实看上去有点多余,不过也是在析构指针指向的对象之前先检查一下,来避免出现段错误
// --- 不对,就是多余! 因为单目初始化器还没有被构建,我们才来到这个分支的
if(mpInitializer)
delete mpInitializer;

// 由当前帧构造初始器 sigma:1.0 iterations:200
mpInitializer = new Initializer(mCurrentFrame,1.0,200);

// -1 表示没有任何匹配。这里面存储的是匹配的点的id
fill(mvIniMatches.begin(),mvIniMatches.end(),-1);

return;
Expand Down Expand Up @@ -999,8 +1001,8 @@ void Tracking::MonocularInitialization()
void Tracking::CreateInitialMapMonocular()
{
// Create KeyFrames 认为单目初始化时候的参考帧和当前帧都是关键帧
KeyFrame* pKFini = new KeyFrame(mInitialFrame,mpMap,mpKeyFrameDB);
KeyFrame* pKFcur = new KeyFrame(mCurrentFrame,mpMap,mpKeyFrameDB);
KeyFrame* pKFini = new KeyFrame(mInitialFrame,mpMap,mpKeyFrameDB); // 第一帧
KeyFrame* pKFcur = new KeyFrame(mCurrentFrame,mpMap,mpKeyFrameDB); // 第二帧

// step 1:将初始关键帧的描述子转为BoW
pKFini->ComputeBoW();
Expand Down Expand Up @@ -1043,7 +1045,7 @@ void Tracking::CreateInitialMapMonocular()
pMP->AddObservation(pKFini,i);
pMP->AddObservation(pKFcur,mvIniMatches[i]);

// b.从众多观测到该MapPoint的特征点中挑选区分读最高的描述子
// b.从众多观测到该MapPoint的特征点中挑选区分度最高的描述子
pMP->ComputeDistinctiveDescriptors();
// c.更新该MapPoint平均观测方向以及观测距离的范围
pMP->UpdateNormalAndDepth();
Expand Down Expand Up @@ -1148,6 +1150,7 @@ void Tracking::CheckReplacedInLastFrame()
if(pMP)
{
//获取其是否被替换,以及替换后的点
// 这也是程序不选择间这个地图点删除的原因,因为删除了就。。。段错误了
MapPoint* pRep = pMP->GetReplaced();
if(pRep)
{
Expand Down Expand Up @@ -1179,7 +1182,7 @@ bool Tracking::TrackReferenceKeyFrame()
vector<MapPoint*> vpMapPointMatches;

// step 2:通过特征点的BoW加快当前帧与参考帧之间的特征点匹配
//NOYICE 之前师兄说的,通过词袋模型加速匹配就是在这里哇
//NOTICE 之前师兄说的,通过词袋模型加速匹配就是在这里哇
// 特征点的匹配关系由MapPoints进行维护
int nmatches = matcher.SearchByBoW(
mpReferenceKF, //参考关键帧
Expand Down

0 comments on commit a553a7d

Please sign in to comment.