/**轉載自https://blog.csdn.net/xiaoxiaowenqiang/article/details/79281122
* This file is part of ORB-SLAM2.
*
* LocalMapping作用是將Tracking中送來的關鍵幀放在mlNewKeyFrame列表中；
* 處理新關鍵幀，地圖點檢查剔除，生成新地圖點，Local BA，關鍵幀剔除。
* 主要工作在于維護局部地圖，也就是SLAM中的Mapping。
*
*
* Tracking線程 只是判斷當前幀是否需要加入關鍵幀，并沒有真的加入地圖，
* 因為Tracking線程的主要功能是局部定位，
*
* 而處理地圖中的關鍵幀，地圖點，包括如何加入，
* 如何刪除的工作是在LocalMapping線程完成的
*
* 建圖
* 處理新的關鍵幀 KeyFrame 完成局部地圖構建
* 插入關鍵幀 ------>? 處理地圖點(篩選生成的地圖點 生成地圖點)? -------->? 局部 BA最小化重投影誤差? -調整-------->? 篩選 新插入的 關鍵幀
*
* mlNewKeyFrames? ? list 列表隊列存儲關鍵幀
* 1】檢查隊列
*? ? ? CheckNewKeyFrames();
*
* 2】處理新關鍵幀 Proces New Key Frames
* ProcessNewKeyFrame(); 更新地圖點MapPoints 和 關鍵幀 KepFrame 的關聯關系? UpdateConnections() 更新關聯關系
*
* 3】剔除 地圖點 MapPoints
*? ? ? 刪除地圖中新添加的但 質量不好的 地圖點
*? ? ? a)? IncreaseFound 共視點? / IncreaseVisible? 投影在圖像上 < 25%
*? ? ? b) 觀測到該 點的關鍵幀太少
*
* 4】生成 地圖點 MapPoints
* 運動過程中和共視程度比較高的 關鍵幀 通過三角化 恢復出的一些地圖點
*
* 5】地圖點融合 MapPoints
*? ? ? 檢測當前關鍵幀和相鄰 關鍵幀(兩級相鄰) 中 重復的 地圖點 留下觀測幀高的 地圖點
*
* 6】局部 BA 最小化重投影誤差
*? ? ? 和當前關鍵幀相鄰的關鍵幀 中相匹配的 地圖點對 局部 BA最小化重投影誤差優化點坐標 和 位姿
*
* 7】關鍵幀剔除
*? ? ? 其90%以上的 地圖點 能夠被其他 共視 關鍵幀(至少3個) 觀測到，認為該關鍵幀多余，可以刪除
*
*/
#include "LocalMapping.h"
#include "LoopClosing.h"
#include "ORBmatcher.h"
#include "Optimizer.h"
#include<mutex>
namespace ORB_SLAM2
{
LocalMapping::LocalMapping(Map *pMap, const float bMonocular):
? ? mbMonocular(bMonocular), mbResetRequested(false), mbFinishRequested(false), mbFinished(true), mpMap(pMap),
? ? mbAbortBA(false), mbStopped(false), mbStopRequested(false), mbNotStop(false), mbAcceptKeyFrames(true)
{
}
void LocalMapping::SetLoopCloser(LoopClosing* pLoopCloser)
{
? ? mpLoopCloser = pLoopCloser;
}
void LocalMapping::SetTracker(Tracking *pTracker)
{
? ? mpTracker=pTracker;
}
void LocalMapping::Run()
{
? ? mbFinished = false;
? ? while(1)
? ? {
// Tracking will see that Local Mapping is busy
// 步驟1：設置進程間的訪問標志 告訴Tracking線程，LocalMapping線程正在處理新的關鍵幀，處于繁忙狀態
? ? ? ? ? ? ? // LocalMapping線程處理的關鍵幀都是Tracking線程發過的
? ? ? ? ? ? ? // 在LocalMapping線程還沒有處理完關鍵幀之前Tracking線程最好不要發送太快
SetAcceptKeyFrames(false);
// Check if there are keyframes in the queue
// 等待處理的關鍵幀列表不為空
if(CheckNewKeyFrames())
{
? ? // BoW conversion and insertion in Map
// 步驟2：計算關鍵幀特征點的詞典單詞向量BoW映射，將關鍵幀插入地圖
? ? ProcessNewKeyFrame();
? ? // Check recent MapPoints
? // 剔除ProcessNewKeyFrame函數中引入的不合格MapPoints
// 步驟3：對新添加的地圖點融合 對于 ProcessNewKeyFrame 和 CreateNewMapPoints 中 最近添加的MapPoints進行檢查剔除 ? ?
? //? MapPointCulling();
? ? // Triangulate new MapPoints
// 步驟4： 創建新的地圖點 相機運動過程中與相鄰關鍵幀通過三角化恢復出一些新的地圖點MapPoints ? ?
? ? CreateNewMapPoints();
? ? MapPointCulling();// 從上面 移到下面
? ? ? // 已經處理完隊列中的最后的一個關鍵幀
? ? if(!CheckNewKeyFrames())
? ? {
// Find more matches in neighbor keyframes and fuse point duplications
// 步驟5：相鄰幀地圖點融合 檢查并融合當前關鍵幀與相鄰幀（兩級相鄰）重復的MapPoints
SearchInNeighbors();
? ? }
? ? mbAbortBA = false;
// 已經處理完隊列中的最后的一個關鍵幀，并且閉環檢測沒有請求停止LocalMapping
? ? if(!CheckNewKeyFrames() && !stopRequested())
? ? {
// 步驟6：局部地圖優化 Local BA
if(mpMap->KeyFramesInMap() > 2)
? ? Optimizer::LocalBundleAdjustment(mpCurrentKeyFrame,&mbAbortBA, mpMap);
// 步驟7： 關鍵幀融合 檢測并剔除當前幀相鄰的關鍵幀中冗余的關鍵幀 Check redundant local Keyframes
? ? ? ? ? ? ? ? // 剔除的標準是：該關鍵幀的90%的MapPoints可以被其它關鍵幀觀測到
// Tracking中先把關鍵幀交給LocalMapping線程
// 并且在Tracking中InsertKeyFrame函數的條件比較松，交給LocalMapping線程的關鍵幀會比較密
? ? ? ? ? ? ? ? ? ? ? ? // 在這里再刪除冗余的關鍵幀
KeyFrameCulling();
? ? }
// 步驟8：將當前幀加入到閉環檢測隊列中
? ? mpLoopCloser->InsertKeyFrame(mpCurrentKeyFrame);
}
// 步驟9：等待線程空閑 完成一幀關鍵幀的插入融合工作
else if(Stop())
{
? ? // Safe area to stop
? ? while(isStopped() && !CheckFinish())
? ? {
usleep(3000);
? ? }
? ? if(CheckFinish())//檢查 是否完成
break;
}
? ? ? ? ? ? ? // 檢查重置
ResetIfRequested();
// Tracking will see that Local Mapping is not busy
// 步驟10：告訴 Tracking 線程? Local Mapping 線程 空閑 可一處理接收 下一個 關鍵幀
SetAcceptKeyFrames(true);
if(CheckFinish())
? ? break;
usleep(3000);
? ? }
? ? SetFinish();
}
/**
* @brief 插入關鍵幀
*
* 將關鍵幀插入到地圖中，以便將來進行局部地圖優化
* 這里僅僅是將關鍵幀插入到列表中進行等待
* @param pKF KeyFrame
*/
void LocalMapping::InsertKeyFrame(KeyFrame *pKF)
{
? ? unique_lock<mutex> lock(mMutexNewKFs);
? ? // 將關鍵幀插入到 等待處理的關鍵幀列表中
? ? mlNewKeyFrames.push_back(pKF);
? ? mbAbortBA=true;// BA優化停止
}
/**
* @brief? ? 查看列表中是否有等待 被處理的關鍵幀
* @return 如果存在，返回true
*/
bool LocalMapping::CheckNewKeyFrames()
{
? ? unique_lock<mutex> lock(mMutexNewKFs);
? ? return(!mlNewKeyFrames.empty());// 等待處理的關鍵幀列表是否為空
}
? /*
? a. 根據詞典 計算當前關鍵幀Bow，便于后面三角化恢復新地圖點；
? b. 將TrackLocalMap中跟蹤局部地圖匹配上的地圖點綁定到當前關鍵幀
? ? ? （在Tracking線程中只是通過匹配進行局部地圖跟蹤，優化當前關鍵幀姿態），
? ? ? 也就是在graph 中加入當前關鍵幀作為node，并更新edge。
? ? ? 而CreateNewMapPoint()中則通過當前關鍵幀，在局部地圖中添加與新的地圖點；
? c. 更新加入當前關鍵幀之后關鍵幀之間的連接關系，包括更新Covisibility圖和Essential圖
? （最小生成樹spanning tree，共視關系好的邊subset of edges from covisibility graph
? ? with high covisibility (θ=100)， 閉環邊）。
? ? */
? /**
* @brief 處理列表中的關鍵幀
*
* - 計算Bow，加速三角化新的MapPoints
* - 關聯當前關鍵幀至MapPoints，并更新MapPoints的平均觀測方向和觀測距離范圍
* - 插入關鍵幀，更新Covisibility圖和Essential圖
* @see VI-A keyframe insertion
*/
void LocalMapping::ProcessNewKeyFrame()
{ ?
// 步驟1：從緩沖隊列中取出一幀待處理的關鍵幀
? ? ? ? ? ? // Tracking線程向LocalMapping中插入關鍵幀存在該隊列中
? ? {
unique_lock<mutex> lock(mMutexNewKFs);
// 從列表中獲得一個等待被插入的關鍵幀
mpCurrentKeyFrame = mlNewKeyFrames.front();
mlNewKeyFrames.pop_front();// 出隊
? ? }
? ? // Compute Bags of Words structures
// 步驟2：計算該關鍵幀特征點的Bow映射關系? ?
? ? //? 根據詞典 計算當前關鍵幀Bow，便于后面三角化恢復新地圖點? ?
? ? mpCurrentKeyFrame->ComputeBoW();// 幀描述子 用字典單詞線性表示的 向量
? ? // Associate MapPoints to the new keyframe and update normal and descriptor
? ? // 當前關鍵幀? TrackLocalMap中跟蹤局部地圖 匹配上的 地圖點
// 步驟3：跟蹤局部地圖過程中新匹配上的MapPoints和當前關鍵幀綁定
? ? ? // 在TrackLocalMap函數中將局部地圖中的MapPoints與當前幀進行了匹配，
? ? ? // 但沒有對這些匹配上的MapPoints與當前幀進行關聯? ?
? ? const vector<MapPoint*> vpMapPointMatches = mpCurrentKeyFrame->GetMapPointMatches();
? ? for(size_t i=0; i<vpMapPointMatches.size(); i++)
? ? {
MapPoint* pMP = vpMapPointMatches[i];// 每一個與當前關鍵幀匹配好的地圖點
if(pMP)//地圖點存在
{
? ? if(!pMP->isBad())
? ? {
? ? ? // 為當前幀在tracking過重跟蹤到的MapPoints更新屬性
if(!pMP->IsInKeyFrame(mpCurrentKeyFrame))//下視野內
{
? ? pMP->AddObservation(mpCurrentKeyFrame, i);// 地圖點添加關鍵幀
? ? pMP->UpdateNormalAndDepth();// 地圖點更新 平均觀測方向 和 觀測距離深度
? ? pMP->ComputeDistinctiveDescriptors();// 加入關鍵幀后，更新地圖點的最佳描述子
}
else // 雙目追蹤時插入的點 可能不在 幀上this can only happen for new stereo points inserted by the Tracking
{
? // 將雙目或RGBD跟蹤過程中新插入的MapPoints放入mlpRecentAddedMapPoints，等待檢查
? ? ? ? ? ? ? ? ? ? ? ? ? // CreateNewMapPoints函數中通過三角化也會生成MapPoints
? ? ? ? ? ? ? ? ? ? ? ? ? // 這些MapPoints都會經過MapPointCulling函數的檢驗
? ? mlpRecentAddedMapPoints.push_back(pMP);
? ? // 候選待檢查地圖點存放在mlpRecentAddedMapPoints
}
? ? }
}
? ? }? ?
? ? // Update links in the Covisibility Graph
// 步驟4：更新關鍵幀間的連接關系，Covisibility圖和Essential圖(tree)
? ? mpCurrentKeyFrame->UpdateConnections();
? ? // Insert Keyframe in Map
// 步驟5：將該關鍵幀插入到地圖中
? ? mpMap->AddKeyFrame(mpCurrentKeyFrame);
}
/**
* @brief 剔除ProcessNewKeyFrame(不在幀上的地圖點 進入待查list)和
*? ? ? ? ? ? CreateNewMapPoints(兩幀三角變換產生的新地圖點進入 待查list)
*? ? ? ? ? ? 函數中引入的質量不好的MapPoints
* @see VI-B recent map points culling
*/
// 對于ProcessNewKeyFrame和CreateNewMapPoints中最近添加的MapPoints進行檢查剔除
void LocalMapping::MapPointCulling()
{
? ? // Check Recent Added MapPoints
? ? list<MapPoint*>::iterator lit = mlpRecentAddedMapPoints.begin();//待檢測的地圖點 迭代器
? ? const unsigned long int nCurrentKFid = mpCurrentKeyFrame->mnId;//當前關鍵幀id
? ? //? 從添加該地圖點的關鍵幀算起的 初始三個關鍵幀，
? ? // 第一幀不算，后面兩幀看到該地圖點的幀數，對于單目<=2，對于雙目和RGBD<=3；
? ? int nThObs;
? ? if(mbMonocular)
nThObs = 2;
? ? else
nThObs = 3;
? ? const int cnThObs = nThObs;
// 遍歷等待檢查的地圖點MapPoints
? ? while(lit !=mlpRecentAddedMapPoints.end())
? ? {
MapPoint* pMP = *lit;//? 新添加的地圖點
if(pMP->isBad())
{
// 步驟1：已經是壞點的MapPoints直接從檢查鏈表中刪除 ?
? ? lit = mlpRecentAddedMapPoints.erase(lit);
}
//? 跟蹤（匹配上）到該地圖點的普通幀幀數（IncreaseFound）< 應該觀測到該地圖點的普通幀數量（25%*IncreaseVisible）：
// 該地圖點雖在視野范圍內，但很少被普通幀檢測到。 剔除
else if(pMP->GetFoundRatio()<0.25f )
{
// 步驟2：將不滿足VI-B條件的MapPoint剔除
// VI-B 條件1：
// 跟蹤到該MapPoint的Frame數相比預計可觀測到該MapPoint的Frame數的比例需大于25%
// IncreaseFound() / IncreaseVisible(該地圖點在視野范圍內) < 25%，注意不一定是關鍵幀。 ?
? ? pMP->SetBadFlag();
? ? lit = mlpRecentAddedMapPoints.erase(lit);//從待查? list中刪除
}
//
// 初始三個關鍵幀 地圖點觀測次數 不能太少
// 而且單目的要求更嚴格，需要三幀都看到
else if(( (int)nCurrentKFid - (int)pMP->mnFirstKFid) >=2 && pMP->Observations() <= cnThObs)
{
? // 步驟3：將不滿足VI-B條件的MapPoint剔除
? ? ? ? ? ? // VI-B 條件2：從該點建立開始，到現在已經過了不小于2幀，
? ? ? ? ? ? // 但是觀測到該點的關鍵幀數卻不超過cnThObs幀，那么該點檢驗不合格
? ? pMP->SetBadFlag();
? ? lit = mlpRecentAddedMapPoints.erase(lit);//從待查? list中刪除
}
else if(((int)nCurrentKFid - (int)pMP->mnFirstKFid ) >= 3)
// 步驟4：從建立該點開始，已經過了3幀(前三幀地圖點比較寶貴需要特別檢查)，放棄對該MapPoint的檢測 ?
? ? lit = mlpRecentAddedMapPoints.erase(lit);//從待查? list中刪除
else
? ? lit++;
? ? }
}
/**
* @brief 相機運動過程中和共視程度比較高的關鍵幀通過三角化恢復出一些MapPoints
*? 根據當前關鍵幀恢復出一些新的地圖點，不包括和當前關鍵幀匹配的局部地圖點（已經在ProcessNewKeyFrame中處理）
*? 先處理新關鍵幀與局部地圖點之間的關系，然后對局部地圖點進行檢查，
*? 最后再通過新關鍵幀恢復 新的局部地圖點：CreateNewMapPoints()
*
* 步驟1：在當前關鍵幀的 共視關鍵幀 中找到 共視程度 最高的前nn幀 相鄰幀vpNeighKFs
* 步驟2：遍歷和當前關鍵幀 相鄰的 每一個關鍵幀vpNeighKFs
* 步驟3：判斷相機運動的基線在（兩針間的相機相對坐標）是不是足夠長
* 步驟4：根據兩個關鍵幀的位姿計算它們之間的基本矩陣 F =? inv(K1 轉置) * t12 叉乘 R12 * inv(K2)
* 步驟5：通過幀間詞典向量加速匹配，極線約束限制匹配時的搜索范圍，進行特征點匹配
* 步驟6：對每對匹配點 2d-2d 通過三角化生成3D點,和 Triangulate函數差不多
*? 步驟6.1：取出匹配特征點
*? 步驟6.2：利用匹配點反投影得到視差角? 用來決定使用三角化恢復(視差角較大) 還是 直接2-d點反投影(視差角較小)
*? 步驟6.3：對于雙目，利用雙目基線 深度 得到視差角
*? 步驟6.4：視差角較大時 使用 三角化恢復3D點
*? 步驟6.4：對于雙目 視差角較小時 二維點 利用深度值 反投影 成 三維點? ? 單目的話直接跳過
*? 步驟6.5：檢測生成的3D點是否在相機前方
*? 步驟6.6：計算3D點在當前關鍵幀下的重投影誤差? 誤差較大跳過
*? 步驟6.7：計算3D點在 鄰接關鍵幀 下的重投影誤差 誤差較大跳過
*? 步驟6.9：三角化生成3D點成功，構造成地圖點 MapPoint
*? 步驟6.9：為該MapPoint添加屬性
*? 步驟6.10：將新產生的點放入檢測隊列 mlpRecentAddedMapPoints? 交給 MapPointCulling() 檢查生成的點是否合適
* @see?
*/
void LocalMapping::CreateNewMapPoints()
{
? ? // Retrieve neighbor keyframes in covisibility graph
? ? int nn = 10;// 雙目/深度 共視幀 數量
? ? if(mbMonocular)
nn=20;//單目
// 步驟1：在當前關鍵幀的 共視關鍵幀 中找到 共視程度 最高的nn幀 相鄰幀vpNeighKFs
? ? const vector<KeyFrame*> vpNeighKFs = mpCurrentKeyFrame->GetBestCovisibilityKeyFrames(nn);
? ? ORBmatcher matcher(0.6,false);// 描述子匹配器
? ? ? ? ? ? // 當前關鍵幀 旋轉平移矩陣向量
? ? cv::Mat Rcw1 = mpCurrentKeyFrame->GetRotation();// 世界---> 當前關鍵幀
? ? cv::Mat Rwc1 = Rcw1.t();// 當前關鍵幀---> 世界
? ? cv::Mat tcw1 = mpCurrentKeyFrame->GetTranslation();
? ? cv::Mat Tcw1(3,4,CV_32F);// 世界---> 當前關鍵幀 變換矩陣
? ? Rcw1.copyTo(Tcw1.colRange(0,3));
? ? tcw1.copyTo(Tcw1.col(3));
? ? // 得到當前當前關鍵幀在世界坐標系中的坐標
? ? cv::Mat Ow1 = mpCurrentKeyFrame->GetCameraCenter();
? ? ? ? ? // 相機內參數
? ? const float &fx1 = mpCurrentKeyFrame->fx;
? ? const float &fy1 = mpCurrentKeyFrame->fy;
? ? const float &cx1 = mpCurrentKeyFrame->cx;
? ? const float &cy1 = mpCurrentKeyFrame->cy;
? ? const float &invfx1 = mpCurrentKeyFrame->invfx;
? ? const float &invfy1 = mpCurrentKeyFrame->invfy;
? ? const float ratioFactor = 1.5f*mpCurrentKeyFrame->mfScaleFactor;//匹配點 查找范圍
? ? int nnew=0;
? ? // Search matches with epipolar restriction and triangulate
// 步驟2：遍歷和當前關鍵幀 相鄰的 每一個關鍵幀vpNeighKFs ? ?
? ? for(size_t i=0; i<vpNeighKFs.size(); i++)
? ? {
if(i>0 && CheckNewKeyFrames())
? ? return;
KeyFrame* pKF2 = vpNeighKFs[i];//關鍵幀
// Check first that baseline is not too short
? // 鄰接的關鍵幀在世界坐標系中的坐標
cv::Mat Ow2 = pKF2->GetCameraCenter();
? // 基線向量，兩個關鍵幀間的相機相對坐標
cv::Mat vBaseline = Ow2-Ow1;
? // 基線長度
const float baseline = cv::norm(vBaseline);
// 步驟3：判斷相機運動的基線是不是足夠長
if(!mbMonocular)
{
? ? if(baseline < pKF2->mb)
? ? continue;// 如果是立體相機，關鍵幀間距太小時不生成3D點
}
else// 單目相機
{
? // 鄰接關鍵幀的場景深度中值
? ? const float medianDepthKF2 = pKF2->ComputeSceneMedianDepth(2);//中值深度
? ? // baseline與景深的比例
? ? const float ratioBaselineDepth = baseline/medianDepthKF2;
? ? ? ? ? ? ? ? ? // 如果特別遠(比例特別小)，那么不考慮當前鄰接的關鍵幀，不生成3D點
? ? if(ratioBaselineDepth < 0.01)
continue;
}
// Compute Fundamental Matrix
// 步驟4：根據兩個關鍵幀的位姿計算它們之間的基本矩陣
? ? ? // 根據兩關鍵幀的姿態計算兩個關鍵幀之間的基本矩陣
? ? ? // F =? inv(K1 轉置)*E*inv(K2) = inv(K1 轉置) * t12 叉乘 R12 * inv(K2)
cv::Mat F12 = ComputeF12(mpCurrentKeyFrame,pKF2);
// Search matches that fullfil epipolar constraint
// 步驟5：通過幀間詞典向量加速匹配，極線約束限制匹配時的搜索范圍，進行特征點匹配
vector<pair<size_t,size_t> > vMatchedIndices;// 特征匹配候選點
matcher.SearchForTriangulation(mpCurrentKeyFrame,pKF2,F12,vMatchedIndices,false);
? ? ? ? // 相鄰關鍵幀 旋轉平移矩陣向量
cv::Mat Rcw2 = pKF2->GetRotation();
cv::Mat Rwc2 = Rcw2.t();
cv::Mat tcw2 = pKF2->GetTranslation();
cv::Mat Tcw2(3,4,CV_32F);// 轉換矩陣
Rcw2.copyTo(Tcw2.colRange(0,3));
tcw2.copyTo(Tcw2.col(3));
// 相機內參
const float &fx2 = pKF2->fx;
const float &fy2 = pKF2->fy;
const float &cx2 = pKF2->cx;
const float &cy2 = pKF2->cy;
const float &invfx2 = pKF2->invfx;
const float &invfy2 = pKF2->invfy;
// Triangulate each match
// 三角化每一個匹配點對
// 步驟6：對每對匹配點 2d-2d 通過三角化生成3D點,和 Triangulate函數差不多
const int nmatches = vMatchedIndices.size();
for(int ikp=0; ikp<nmatches; ikp++)
{
// 步驟6.1：取出匹配特征點 ?
? ? const int &idx1 = vMatchedIndices[ikp].first; // 當前匹配對在當前關鍵幀中的索引
? ? const int &idx2 = vMatchedIndices[ikp].second;// 當前匹配對在鄰接關鍵幀中的索引
? ? //當前關鍵幀 特征點 和 右圖像匹配點橫坐標
? ? const cv::KeyPoint &kp1 = mpCurrentKeyFrame->mvKeysUn[idx1];
? ? const float kp1_ur=mpCurrentKeyFrame->mvuRight[idx1];
? ? bool bStereo1 = kp1_ur >= 0;//右圖像匹配點橫坐標>=0是雙目/深度相機
? ? // 鄰接關鍵幀 特征點 和 右圖像匹配點橫坐標
? ? const cv::KeyPoint &kp2 = pKF2->mvKeysUn[idx2];
? ? const float kp2_ur = pKF2->mvuRight[idx2];
? ? bool bStereo2 = kp2_ur >= 0;
// 步驟6.2：利用匹配點反投影得到視差角? ?
? ? // Check parallax between rays
? ? // 相機歸一化平面上的點坐標
? ? cv::Mat xn1 = (cv::Mat_<float>(3,1) << (kp1.pt.x - cx1)*invfx1, (kp1.pt.y - cy1)*invfy1, 1.0);
? ? cv::Mat xn2 = (cv::Mat_<float>(3,1) << (kp2.pt.x - cx2)*invfx2, (kp2.pt.y - cy2)*invfy2, 1.0);
? ? // 由相機坐標系轉到世界坐標系，得到視差角余弦值
? ? cv::Mat ray1 = Rwc1*xn1;// 相機坐標系 ------> 世界坐標系
? ? cv::Mat ray2 = Rwc2*xn2;
? ? // 向量a × 向量b / （向量a模 × 向量吧模） = 夾角余弦值
? ? const float cosParallaxRays = ray1.dot(ray2) / (cv::norm(ray1)*cv::norm(ray2));
? ? // 加1是為了讓cosParallaxStereo隨便初始化為一個很大的值
? ? float cosParallaxStereo = cosParallaxRays+1;
? ? float cosParallaxStereo1 = cosParallaxStereo;
? ? float cosParallaxStereo2 = cosParallaxStereo;
// 步驟6.3：對于雙目，利用雙目基線 深度 得到視差角
? ? if(bStereo1)
cosParallaxStereo1 = cos(2*atan2(mpCurrentKeyFrame->mb/2,mpCurrentKeyFrame->mvDepth[idx1]));
? ? else if(bStereo2)
cosParallaxStereo2 = cos(2*atan2(pKF2->mb/2,pKF2->mvDepth[idx2]));
? ? // 得到雙目觀測的視差角
? ? cosParallaxStereo = min(cosParallaxStereo1,cosParallaxStereo2);
// 步驟6.4：三角化恢復3D點
? ? cv::Mat x3D;
? // cosParallaxRays>0 && (bStereo1 || bStereo2 || cosParallaxRays<0.9998)表明視差角正常
? ? ? ? ? ? ? ? ? // cosParallaxRays < cosParallaxStereo表明視差角很小
? ? ? ? ? ? ? ? ? // 視差角度小時用三角法恢復3D點，視差角大時用雙目恢復3D點（雙目以及深度有效）
? ? if(cosParallaxRays < cosParallaxStereo && cosParallaxRays>0 && (bStereo1 || bStereo2 || cosParallaxRays<0.9998))
? ? {
// Linear Triangulation Method
? ? ? // p1 = k × [R1 t1] × D? ? ? k逆 × p1 =? [R1 t1] × D? ? x1 = T1 × D? ? x1叉乘x1 = x1叉乘T1 × D = 0
? ? ? // p2 = k × [ R2 t2]? × D? ? k逆 × p2 =? [R2 t2] × D? ? x2 = T2 × D? ? x2叉乘x2 = x2叉乘T2 × D = 0
? ? ? //
? ? ? //p = ( x,y,1)
? ? ? //其叉乘矩陣為
? ? ? //? 叉乘矩陣 = [0? -1? y;? ? ? ? ? ? ? ? T0
? ? ? //? ? ? ? ? ? ? ? ? ? ? 1? 0? -x;? ? ? ? ? *? T1? *D? ===>( y * T2 - T1 ) *D = 0
? ? ? //? ? ? ? ? ? ? ? ? ? ? -y? x? 0 ] ? ? T2? ? ? ? ? ? ? ? ? ( x * T2 - T0 ) *D = 0
? ? ? //一個點兩個方程? 兩個點 四個方程? A × D =0? 求三維點 D? 對 A奇異值分解
cv::Mat A(4,4,CV_32F);
A.row(0) = xn1.at<float>(0)*Tcw1.row(2) - Tcw1.row(0);
A.row(1) = xn1.at<float>(1)*Tcw1.row(2) - Tcw1.row(1);
A.row(2) = xn2.at<float>(0)*Tcw2.row(2) - Tcw2.row(0);
A.row(3) = xn2.at<float>(1)*Tcw2.row(2) - Tcw2.row(1);
cv::Mat w,u,vt;
cv::SVD::compute(A,w,u,vt,cv::SVD::MODIFY_A| cv::SVD::FULL_UV);
x3D = vt.row(3).t();
if(x3D.at<float>(3)==0)
? ? continue;
// Euclidean coordinates
x3D = x3D.rowRange(0,3) / x3D.at<float>(3);//其次點坐標 除去尺度
? ? }
? //? 步驟6.4：對于雙目 視差角較小時 二維點 利用深度值 反投影 成 三維點
? ? else if(bStereo1 && cosParallaxStereo1<cosParallaxStereo2)// 雙目 視差角 小
? ? {
x3D = mpCurrentKeyFrame->UnprojectStereo(idx1);// 二維點 反投影 成 三維點? ? ? ? ? ? ?
? ? }
? ? else if(bStereo2 && cosParallaxStereo2 < cosParallaxStereo1)
? ? {
x3D = pKF2->UnprojectStereo(idx2);
? ? }
? ? ? // 單目 視差角 較小時 生成不了三維點
? ? else
continue; //沒有雙目/深度 且兩針視角差太小? 三角測量也不合適 得不到三維點 No stereo and very low parallax
? ? cv::Mat x3Dt = x3D.t();
// 步驟6.5：檢測生成的3D點是否在相機前方
? ? //Check triangulation in front of cameras
? ? float z1 = Rcw1.row(2).dot(x3Dt) + tcw1.at<float>(2);// 只算z坐標值
? ? if(z1<= 0)
continue;
? ? float z2 = Rcw2.row(2).dot(x3Dt)+tcw2.at<float>(2);
? ? if(z2 <= 0)
continue;
? ? ? ? // 步驟6.6：計算3D點在當前關鍵幀下的重投影誤差
? ? //Check reprojection error in first keyframe
? ? const float &sigmaSquare1 = mpCurrentKeyFrame->mvLevelSigma2[kp1.octave];//誤差 分布參數
? ? const float x1 = Rcw1.row(0).dot(x3Dt) + tcw1.at<float>(0);//相機歸一化坐標
? ? const float y1 = Rcw1.row(1).dot(x3Dt) + tcw1.at<float>(1);
? ? const float invz1 = 1.0/z1;
? ? if(!bStereo1)
? ? {// 單目
float u1 = fx1*x1*invz1 + cx1;//像素坐標
float v1 = fy1*y1*invz1 + cy1;
float errX1 = u1 - kp1.pt.x;
float errY1 = v1 - kp1.pt.y;
if((errX1*errX1+errY1*errY1) > 5.991*sigmaSquare1)
? ? continue;//投影誤差過大 跳過
? ? }
? ? else
? ? {// 雙目 / 深度 相機? 有右圖像匹配點橫坐標差值
float u1 = fx1*x1*invz1+cx1;
float u1_r = u1 - mpCurrentKeyFrame->mbf * invz1;//左圖像坐標值 - 視差 =? 右圖像匹配點橫坐標
float v1 = fy1*y1*invz1+cy1;
float errX1 = u1 - kp1.pt.x;
float errY1 = v1 - kp1.pt.y;
float errX1_r = u1_r - kp1_ur;
// 基于卡方檢驗計算出的閾值（假設測量有一個一個像素的偏差）
if((errX1*errX1 + errY1*errY1 + errX1_r*errX1_r) > 7.8*sigmaSquare1)
? ? continue;//投影誤差過大 跳過
? ? }
? ? ? ? // 步驟6.7：計算3D點在 鄰接關鍵幀 下的重投影誤差
? ? //Check reprojection error in second keyframe
? ? const float sigmaSquare2 = pKF2->mvLevelSigma2[kp2.octave];
? ? const float x2 = Rcw2.row(0).dot(x3Dt)+tcw2.at<float>(0);
? ? const float y2 = Rcw2.row(1).dot(x3Dt)+tcw2.at<float>(1);
? ? const float invz2 = 1.0/z2;
? ? if(!bStereo2)
? ? {// 單目
float u2 = fx2*x2*invz2+cx2;
float v2 = fy2*y2*invz2+cy2;
float errX2 = u2 - kp2.pt.x;
float errY2 = v2 - kp2.pt.y;
if((errX2*errX2+errY2*errY2)>5.991*sigmaSquare2)
? ? continue;//投影誤差過大 跳過
? ? }
? ? else
? ? {// 雙目 / 深度 相機? 有右圖像匹配點橫坐標差值
float u2 = fx2*x2*invz2+cx2;
float u2_r = u2 - mpCurrentKeyFrame->mbf*invz2;//左圖像坐標值 - 視差 =? 右圖像匹配點橫坐標
float v2 = fy2*y2*invz2+cy2;
float errX2 = u2 - kp2.pt.x;
float errY2 = v2 - kp2.pt.y;
float errX2_r = u2_r - kp2_ur;
if((errX2*errX2+errY2*errY2+errX2_r*errX2_r)>7.8*sigmaSquare2)
? ? continue;//投影誤差過大 跳過
? ? }
? ? ? ? ? // 步驟6.8：檢查尺度連續性
? ? //Check scale consistency
? ? cv::Mat normal1 = x3D-Ow1;//? 世界坐標系下，3D點與相機間的向量，方向由相機指向3D點
? ? float dist1 = cv::norm(normal1);// 模長
? ? cv::Mat normal2 = x3D-Ow2;
? ? float dist2 = cv::norm(normal2);
? ? if(dist1==0 || dist2==0)
continue;// 模長為0 跳過
? ? ? ? ? ? ? ? ? // ratioDist是不考慮金字塔尺度下的距離比例
? ? const float ratioDist = dist2/dist1;
? // 金字塔尺度因子的比例
? ? const float ratioOctave = mpCurrentKeyFrame->mvScaleFactors[kp1.octave] / pKF2->mvScaleFactors[kp2.octave];
? ? /*if(fabs(ratioDist-ratioOctave)>ratioFactor)
continue;*/
? ? // 深度比值和 兩幅圖像下的金字塔層級比值應該相差不大
? ? // |ratioDist/ratioOctave |<ratioFactor
? ? if(ratioDist * ratioFactor<ratioOctave || ratioDist > ratioOctave*ratioFactor)
continue;
? // 步驟6.9：三角化生成3D點成功，構造成地圖點 MapPoint
? ? // Triangulation is succesfull
? ? MapPoint* pMP = new MapPoint(x3D,mpCurrentKeyFrame,mpMap);
? ? ? ? ? ? // 步驟6.9：為該MapPoint添加屬性：
? ? ? // a.觀測到該MapPoint的關鍵幀
? ? pMP->AddObservation(mpCurrentKeyFrame,idx1); // 地圖點 添加觀測幀?
? ? pMP->AddObservation(pKF2,idx2);//
? ? mpCurrentKeyFrame->AddMapPoint(pMP,idx1);// 關鍵幀 添加地圖點
? ? pKF2->AddMapPoint(pMP,idx2);
? ? ? ? ? ? ? // b.該MapPoint的描述子
? ? pMP->ComputeDistinctiveDescriptors();
? ? ? ? ? ? ? // c.該MapPoint的平均觀測方向和深度范圍
? ? pMP->UpdateNormalAndDepth();
? ? ? ? ? ? ? // d.地圖添加地圖點
? ? mpMap->AddMapPoint(pMP);
? ? ? ? ? ? // 步驟6.10：將新產生的點放入檢測隊列 mlpRecentAddedMapPoints
? ? ? ? ? ? ? ? ? // 這些MapPoints都會經過MapPointCulling函數的檢驗
? ? mlpRecentAddedMapPoints.push_back(pMP);
? ? nnew++;
}
? ? }
}
/**
* @brief? ? 檢查并融合 當前關鍵幀 與 相鄰幀（一級二級相鄰幀）重復的地圖點 MapPoints
* 步驟1：獲得當前關鍵幀在covisibility幀連接圖中權重排名前nn的一級鄰接關鍵幀(按觀測到當前幀地圖點次數選取)
* 步驟2：獲得當前關鍵幀在 其一級相鄰幀 在 covisibility圖中權重排名前5 的二級鄰接關鍵幀
* 步驟3：將當前幀的 地圖點MapPoints 分別與 其一級二級相鄰幀的 地圖點 MapPoints 進行融合(保留觀測次數最高的)
* 步驟4：找到一級二級相鄰幀所有的地圖點MapPoints 與當前幀 的? 地圖點MapPoints 進行融合
* 步驟5：更新當前幀 地圖點 MapPoints 的描述子，深度，觀測主方向等屬性
* 步驟5：更新當前 與其它幀的連接關系 (? 觀測到互相的地圖點的次數等信息 )
* @return? 無
*/
void LocalMapping::SearchInNeighbors()
{
? ? // Retrieve neighbor keyframes
// 步驟1：獲得當前關鍵幀在covisibility圖中權重排名前nn的一級鄰接關鍵幀
? ? ? ? ? // 找到當前幀一級相鄰與二級相鄰關鍵幀
? ? int nn = 10;
? ? if(mbMonocular)
nn=20;//單目 多找一些
? // 一級相鄰
? ? const vector<KeyFrame*> vpNeighKFs = mpCurrentKeyFrame->GetBestCovisibilityKeyFrames(nn);
? ? vector<KeyFrame*> vpTargetKFs;// 最后合格的一級二級相鄰關鍵幀
? ? // 遍歷每一個 一級相鄰幀
? ? for(vector<KeyFrame*>::const_iterator vit=vpNeighKFs.begin(), vend=vpNeighKFs.end(); vit!=vend; vit++)
? ? {
KeyFrame* pKFi = *vit;// 一級相鄰關鍵幀
if(pKFi->isBad() || pKFi->mnFuseTargetForKF == mpCurrentKeyFrame->mnId)//壞幀? 或者 已經加入過
? ? continue;// 跳過
vpTargetKFs.push_back(pKFi);// 加入 最后合格的相鄰關鍵幀
pKFi->mnFuseTargetForKF = mpCurrentKeyFrame->mnId;// 已經做過相鄰匹配? 標記已經加入
// 步驟2：獲得當前關鍵幀在 其一級相鄰幀的? covisibility圖中權重排名前5的二級鄰接關鍵幀
? ? ? ? // 二級相鄰
// Extend to some second neighbors
const vector<KeyFrame*> vpSecondNeighKFs = pKFi->GetBestCovisibilityKeyFrames(5);
// 遍歷每一個 二級相鄰幀
for(vector<KeyFrame*>::const_iterator vit2=vpSecondNeighKFs.begin(), vend2=vpSecondNeighKFs.end(); vit2!=vend2; vit2++)
{
? ? KeyFrame* pKFi2 = *vit2;// 二級相鄰關鍵幀
? ? if(pKFi2->isBad() || pKFi2->mnFuseTargetForKF==mpCurrentKeyFrame->mnId || pKFi2->mnId==mpCurrentKeyFrame->mnId)
continue;// 二級相鄰關鍵幀是壞幀 在一級時已經加入 或者 又找回來了找到當前幀了 跳過
? ? vpTargetKFs.push_back(pKFi2);
}
? ? }
// 步驟3：將當前幀的 地圖點MapPoints 分別與 其一級二級相鄰幀的 地圖點 MapPoints 進行融合
? ? // Search matches by projection from current KF in target KFs
? ? ORBmatcher matcher;
? ? vector<MapPoint*> vpMapPointMatches = mpCurrentKeyFrame->GetMapPointMatches();//與當前幀 匹配的地圖點
? ? // vector<KeyFrame*>::iterator
? ? for(auto? vit=vpTargetKFs.begin(), vend=vpTargetKFs.end(); vit!=vend; vit++)
? ? {
KeyFrame* pKFi = *vit;//其一級二級相鄰幀
// 投影當前幀的MapPoints到相鄰關鍵幀pKFi中，在附加區域搜索匹配關鍵點，并判斷是否有重復的MapPoints
// 1.如果MapPoint能匹配關鍵幀的特征點，并且該點有對應的MapPoint，那么將兩個MapPoint合并（選擇觀測數多的）
// 2.如果MapPoint能匹配關鍵幀的特征點，并且該點沒有對應的MapPoint，那么為該點添加MapPoint
matcher.Fuse(pKFi,vpMapPointMatches);
? ? }
// 步驟4：將一級二級相鄰幀所有的地圖點MapPoints 與當前幀（的MapPoints）進行融合
? ? ? ? ? ? // 遍歷每一個一級鄰接和二級鄰接關鍵幀 找到所有的地圖點
? ? // Search matches by projection from target KFs in current KF
? ? // 用于存儲一級鄰接和二級鄰接關鍵幀所有MapPoints的集合
? ? vector<MapPoint*> vpFuseCandidates;// 一級二級相鄰幀所有地圖點
? ? vpFuseCandidates.reserve(vpTargetKFs.size() * vpMapPointMatches.size());// 幀數量 × 每一幀地圖點數量
? ? ? ? ? ? // vector<KeyFrame*>::iterator
? ? for(auto vitKF=vpTargetKFs.begin(), vendKF=vpTargetKFs.end(); vitKF!=vendKF; vitKF++)
? ? {
KeyFrame* pKFi = *vitKF;//其一級二級相鄰幀
vector<MapPoint*> vpMapPointsKFi = pKFi->GetMapPointMatches();//地圖點
// vector<MapPoint*>::iterator
for(auto vitMP=vpMapPointsKFi.begin(), vendMP=vpMapPointsKFi.end(); vitMP!=vendMP; vitMP++)
{
? ? MapPoint* pMP = *vitMP;//? 一級二級相鄰幀 的每一個地圖點
? ? if(!pMP)
continue;
? ? if(pMP->isBad() || pMP->mnFuseCandidateForKF == mpCurrentKeyFrame->mnId)
continue;
? ? // 加入集合，并標記 已經加入
? ? pMP->mnFuseCandidateForKF = mpCurrentKeyFrame->mnId; //標記 已經加
? ? vpFuseCandidates.push_back(pMP); // 加入 一級二級相鄰幀 地圖點 集合
}
? ? }
? ? ? ? ? ? //一級二級相鄰幀 所有的 地圖點 與當前幀 融合
? ? ? ? ? ? // 投影 地圖點MapPoints到當前幀上，在附加區域搜索匹配關鍵點，并判斷是否有重復的地圖點
? ? ? ? // 1.如果MapPoint能匹配當前幀的特征點，并且該點有對應的MapPoint，那么將兩個MapPoint合并（選擇觀測數多的）
? ? ? ? // 2.如果MapPoint能匹配當前幀的特征點，并且該點沒有對應的MapPoint，那么為該點添加MapPoint
? ? matcher.Fuse(mpCurrentKeyFrame,vpFuseCandidates);
// 步驟5：更新當前幀MapPoints的描述子，深度，觀測主方向等屬性
? ? // Update points
? ? vpMapPointMatches = mpCurrentKeyFrame->GetMapPointMatches();//當前幀 所有的 匹配地圖點
? ? for(size_t i=0, iend=vpMapPointMatches.size(); i<iend; i++)
? ? {
MapPoint* pMP=vpMapPointMatches[i];//當前幀 每個關鍵點匹配的地圖點
if(pMP)//存在
{
? ? if(!pMP->isBad())//非 壞點
? ? {
pMP->ComputeDistinctiveDescriptors();// 更新 地圖點的描述子(在所有觀測在的描述子中選出最好的描述子)
pMP->UpdateNormalAndDepth();? ? ? ? ? // 更新平均觀測方向和觀測距離
? ? }
}
? ? }
// 步驟5：更新當前幀的MapPoints后 更新與其它幀的連接關系 觀測到互相的地圖點的次數等信息
? ? ? ? ? ? // 更新covisibility圖
? ? // Update connections in covisibility graph
? ? mpCurrentKeyFrame->UpdateConnections();
}
/**
* @brief? ? 關鍵幀剔除
*? 在Covisibility Graph 關鍵幀連接圖 中的關鍵幀，
*? 其90%以上的地圖點MapPoints能被其他關鍵幀（至少3個）觀測到，
*? 則認為該關鍵幀為冗余關鍵幀。
* @param? pKF1 關鍵幀1
* @param? pKF2 關鍵幀2
* @return 兩個關鍵幀之間的基本矩陣 F
*/
void LocalMapping::KeyFrameCulling()
{
? ? // Check redundant keyframes (only local keyframes)
? ? // A keyframe is considered redundant if the 90% of the MapPoints it sees, are seen
? ? // in at least other 3 keyframes (in the same or finer scale)
? ? // We only consider close stereo points
// 步驟1：根據Covisibility Graph 關鍵幀連接 圖提取當前幀的 所有共視關鍵幀(關聯幀) ?
? ? vector<KeyFrame*> vpLocalKeyFrames = mpCurrentKeyFrame->GetVectorCovisibleKeyFrames();
? ? ? ? ? ? // vector<KeyFrame*>::iterator
? ? ? ? ? // 對所有的局部關鍵幀進行遍歷 ? ?
? ? for(auto? vit=vpLocalKeyFrames.begin(), vend=vpLocalKeyFrames.end(); vit!=vend; vit++)
? ? {
KeyFrame* pKF = *vit;// 當前幀的每一個 局部關聯幀
if(pKF->mnId == 0)//第一幀關鍵幀為 初始化世界關鍵幀 跳過
? ? continue;
// 步驟2：提取每個共視關鍵幀的 地圖點 MapPoints
const vector<MapPoint*> vpMapPoints = pKF->GetMapPointMatches();// 局部關聯幀 匹配的 地圖點
int nObs = 3;
const int thObs=nObs; //3
int nRedundantObservations=0;
int nMPs=0;
// 步驟3：遍歷該局部關鍵幀的MapPoints，判斷是否90%以上的MapPoints能被其它關鍵幀（至少3個）觀測到
for(size_t i=0, iend=vpMapPoints.size(); i<iend; i++)
{
? ? MapPoint* pMP = vpMapPoints[i];// 該局部關鍵幀的 地圖點 MapPoints
? ? if(pMP)
? ? {
if(!pMP->isBad())
{
? ? if(!mbMonocular)// 雙目/深度
? ? {? // 對于雙目，僅考慮近處的MapPoints，不超過mbf * 35 / fx
if(pKF->mvDepth[i] > pKF->mThDepth || pKF->mvDepth[i] < 0)
? ? continue;
? ? }
? ? nMPs++;
? ? // 地圖點 MapPoints 至少被三個關鍵幀觀測到
? ? if(pMP->Observations() > thObs)// 觀測幀個數 > 3
? ? {
const int &scaleLevel = pKF->mvKeysUn[i].octave;// 金字塔層數
const map<KeyFrame*, size_t> observations = pMP->GetObservations();// 局部 觀測關鍵幀地圖
int nObs=0;
for(map<KeyFrame*, size_t>::const_iterator mit=observations.begin(), mend=observations.end(); mit!=mend; mit++)
{
? ? KeyFrame* pKFi = mit->first;
? ? if(pKFi==pKF)// 跳過 原地圖點的幀
continue;
? ? const int &scaleLeveli = pKFi->mvKeysUn[mit->second].octave;// 金字塔層數
? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? // 尺度約束，要求MapPoint在該局部關鍵幀的特征尺度大于（或近似于）其它關鍵幀的特征尺度
? ? if(scaleLeveli <= scaleLevel+1)
? ? {
nObs++;
if(nObs >= thObs)
? ? break;
? ? }
}
if(nObs >= thObs)
{// 該MapPoint至少被三個關鍵幀觀測到
? ? nRedundantObservations++;
}
? ? }
}
? ? }
}?
// 步驟4：該局部關鍵幀90%以上的MapPoints能被其它關鍵幀（至少3個）觀測到，則認為是冗余關鍵幀
if(nRedundantObservations > 0.9*nMPs)
? ? pKF->SetBadFlag();
? ? }
}
/**
* @brief? ? 根據兩關鍵幀的姿態計算兩個關鍵幀之間的基本矩陣
*? ? ? ? ? ? ? ? F =? inv(K1 轉置)*E*inv(K2) = inv(K1 轉置)*t叉乘R*inv(K2)
* @param? pKF1 關鍵幀1
* @param? pKF2 關鍵幀2
* @return 兩個關鍵幀之間的基本矩陣 F
*/
cv::Mat LocalMapping::ComputeF12(KeyFrame *&pKF1, KeyFrame *&pKF2)
{
? ? // Essential Matrix: t12叉乘R12
? ? // Fundamental Matrix: inv(K1轉置)*E*inv(K2) ?
? ? cv::Mat R1w = pKF1->GetRotation();? // Rc1w
? ? cv::Mat t1w = pKF1->GetTranslation();
? ? cv::Mat R2w = pKF2->GetRotation();? // Rc2w
? ? cv::Mat t2w = pKF2->GetTranslation();// t c2 w
? ? cv::Mat R12 = R1w*R2w.t();// R12 =Rc1w *? Rwc2 // c2 -->w --->c1
? ? cv::Mat t12 = -R12*t2w + t1w; // tw2? + t1w? ? // c2 -->w --->c1
? ? // t12 的叉乘矩陣
? ? cv::Mat t12x = SkewSymmetricMatrix(t12);
? ? const cv::Mat &K1 = pKF1->mK;
? ? const cv::Mat &K2 = pKF2->mK;
? ? return K1.t().inv()*t12x*R12*K2.inv();
}
/**
* @brief? ? 請求停止局部建圖線程? 設置停止標志
* @return 無
*/
void LocalMapping::RequestStop()
{
? ? unique_lock<mutex> lock(mMutexStop);
? ? mbStopRequested = true;//局部建圖 請求停止
? ? unique_lock<mutex> lock2(mMutexNewKFs);
? ? mbAbortBA = true;//停止BA 優化
}
/**
* @brief? ? 停止局部建圖線程? 設置停止標志
* @return 無
*/
bool LocalMapping::Stop()
{
? ? unique_lock<mutex> lock(mMutexStop);
? ? if(mbStopRequested && !mbNotStop)
? ? {
mbStopped = true;
cout << "局部建圖停止 Local Mapping STOP" << endl;
return true;
? ? }
? ? return false;
}
/**
* @brief? ? 檢查局部建圖線程 是否停止
* @return 是否停止標志
*/
bool LocalMapping::isStopped()
{
? ? unique_lock<mutex> lock(mMutexStop);
? ? return mbStopped;
}
/**
* @brief? 返回 請求停止局部建圖線程? 標志
* @return 返回 請求停止局部建圖線程? 標志
*/
bool LocalMapping::stopRequested()
{
? ? unique_lock<mutex> lock(mMutexStop);
? ? return mbStopRequested;
}
/**
* @brief? ? 釋放局部建圖線程?
* @return? 無
*/
void LocalMapping::Release()
{
? ? unique_lock<mutex> lock(mMutexStop);
? ? unique_lock<mutex> lock2(mMutexFinish);
? ? if(mbFinished)
return;
? ? mbStopped = false;
? ? mbStopRequested = false;
? ? // list<KeyFrame*>::iterator
? ? for(auto lit = mlNewKeyFrames.begin(), lend=mlNewKeyFrames.end(); lit!=lend; lit++)
delete *lit;//刪除關鍵幀
? ? mlNewKeyFrames.clear();
? ? cout << "局部建圖釋放 Local Mapping RELEASE" << endl;
}
/**
* @brief? ? 返回 可以接收新的一個關鍵幀標志
* @return 是否 可以接收新的一個關鍵幀
*/
bool LocalMapping::AcceptKeyFrames()
{
? ? unique_lock<mutex> lock(mMutexAccept);
? ? return mbAcceptKeyFrames;
}
/**
* @brief? ? 設置 可以接收新的一個關鍵幀標志
* @return 無
*/
void LocalMapping::SetAcceptKeyFrames(bool flag)
{
? ? unique_lock<mutex> lock(mMutexAccept);
? ? mbAcceptKeyFrames=flag;
}
/**
* @brief? ? 設置不要停止標志
* @return? 成功與否
*/
bool LocalMapping::SetNotStop(bool flag)
{
? ? unique_lock<mutex> lock(mMutexStop);
? ? if(flag && mbStopped)//? 在已經停止的情況下 設置不要停止? 錯誤
return false;
? ? mbNotStop = flag;
? ? return true;
}
/**
* @brief? ? 停止 全局優化 BA
* @return 是否 可以接收新的一個關鍵幀
*/
void LocalMapping::InterruptBA()
{
? ? mbAbortBA = true;
}
/**
* @brief? 計算向量的 叉乘矩陣? ? 變叉乘為 矩陣乘
* @return 該向量的叉乘矩陣
*/
cv::Mat LocalMapping::SkewSymmetricMatrix(const cv::Mat &v)
{
// 向量 t=(a1 a2 a3) t叉乘A
// 等于 向量t的叉乘矩陣 * A
//? t的叉乘矩陣
//|0? ? -a3? a2 |
//|a3? ? 0? -a1|
//|-a2? a1? ? 0 |
? ? return (cv::Mat_<float>(3,3) <<? ? ? ? ? ? 0, -v.at<float>(2), v.at<float>(1),
? ? v.at<float>(2),? ? ? ? ? ? ? 0,-v.at<float>(0),
? ? -v.at<float>(1),? v.at<float>(0),? ? ? ? ? ? ? 0);
}
/**
* @brief? ? 請求重置
* @return? 無
*/
void LocalMapping::RequestReset()
{
? ? {
unique_lock<mutex> lock(mMutexReset);
mbResetRequested = true;
? ? }
? ? while(1)
? ? {
{
? ? unique_lock<mutex> lock2(mMutexReset);
? ? if(!mbResetRequested)
break;
}
usleep(3000);
? ? }
}
/**
* @brief? ? 重置線程
* @return? 無
*/
void LocalMapping::ResetIfRequested()
{
? ? unique_lock<mutex> lock(mMutexReset);
? ? if(mbResetRequested)
? ? {
mlNewKeyFrames.clear();
mlpRecentAddedMapPoints.clear();
mbResetRequested=false;
? ? }
}
void LocalMapping::RequestFinish()
{
? ? unique_lock<mutex> lock(mMutexFinish);
? ? mbFinishRequested = true;
}
bool LocalMapping::CheckFinish()
{
? ? unique_lock<mutex> lock(mMutexFinish);
? ? return mbFinishRequested;
}
void LocalMapping::SetFinish()
{
? ? unique_lock<mutex> lock(mMutexFinish);
? ? mbFinished = true;? ?
? ? unique_lock<mutex> lock2(mMutexStop);
? ? mbStopped = true;
}
bool LocalMapping::isFinished()
{
? ? unique_lock<mutex> lock(mMutexFinish);
? ? return mbFinished;
}
} //namespace ORB_SLAM
