MapPoint.h
/**
* This file is part of ORB-SLAM2.
*
* Copyright (C) 2014-2016 Raúl Mur-Artal (University of Zaragoza)
* For more information see
*
* ORB-SLAM2 is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* ORB-SLAM2 is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with ORB-SLAM2. If not, see .
*/
#ifndef MAPPOINT_H
#define MAPPOINT_H
#include"KeyFrame.h"
#include"Frame.h"
#include"Map.h"
#include
#include
namespace ORB_SLAM2
{
class KeyFrame;
class Map;
class Frame;
/**
* 1.地图点可以通过关键帧来构造,也可以通过普通帧构造,普通帧构造的地图点只是临时被Tracking用来追踪的
* 2.MapPoint中比较有意思的几个概念是mNormalVector,mObservations,mfMinDistance,mfMaxDistance,mnVisible,mnFound
*
*/
class MapPoint
{
public:
MapPoint(const cv::Mat &Pos, KeyFrame* pRefKF, Map* pMap);
MapPoint(const cv::Mat &Pos, Map* pMap, Frame* pFrame, const int &idxF);
void SetWorldPos(const cv::Mat &Pos);
cv::Mat GetWorldPos();
cv::Mat GetNormal();
KeyFrame* GetReferenceKeyFrame();
std::map<KeyFrame*,size_t> GetObservations();
int Observations();
void AddObservation(KeyFrame* pKF,size_t idx);
void EraseObservation(KeyFrame* pKF);
int GetIndexInKeyFrame(KeyFrame* pKF);
bool IsInKeyFrame(KeyFrame* pKF);
void SetBadFlag();
bool isBad();
void Replace(MapPoint* pMP);
MapPoint* GetReplaced();
void IncreaseVisible(int n=1);
void IncreaseFound(int n=1);
float GetFoundRatio();
inline int GetFound(){
return mnFound;
}
/**
* 在所有观测到该3D点的关键帧中选择一个最好的描述子作为该3D点的描述子
* 最好的标准是:获取所有的描述子,然后计算描述子之间的两两之间的距离,最好的描述子
* 就是和其它描述子具有最小的距离中值
* 中值:计算一个描述子距离其它描述子的距离之后,然后进行排列,取中值,中值不收极大值和极小值的影响
* 能够在一定程度上代表全体数据的一般水平.如果中值比较大,那么说明该描述子离其它的描述子比较远,
* 这个描述子很可能不合理,因此选择一个最小中值对应的描述子作为最佳描述子.
*/
void ComputeDistinctiveDescriptors();
cv::Mat GetDescriptor();
void UpdateNormalAndDepth();
float GetMinDistanceInvariance();
float GetMaxDistanceInvariance();
int PredictScale(const float ¤tDist, KeyFrame*pKF);
int PredictScale(const float ¤tDist, Frame* pF);
public:
long unsigned int mnId;
static long unsigned int nNextId;
long int mnFirstKFid;
long int mnFirstFrame;
int nObs;
// Variables used by the tracking
float mTrackProjX;
float mTrackProjY;
float mTrackProjXR;
bool mbTrackInView;
int mnTrackScaleLevel;
float mTrackViewCos;
long unsigned int mnTrackReferenceForFrame;
long unsigned int mnLastFrameSeen;
// Variables used by local mapping
/// use to avoid add to BA repeate
long unsigned int mnBALocalForKF;
long unsigned int mnFuseCandidateForKF;
// Variables used by loop closing
long unsigned int mnLoopPointForKF;
long unsigned int mnCorrectedByKF;
long unsigned int mnCorrectedReference;
cv::Mat mPosGBA;
long unsigned int mnBAGlobalForKF;
static std::mutex mGlobalMutex;
protected:
// Position in absolute coordinates
cv::Mat mWorldPos;
// Keyframes observing the point and associated index in keyframe
// 观测到该MapPoint的KF和该MapPoint在KF中的索引
std::map<KeyFrame*,size_t> mObservations;
// Mean viewing direction
/**
* 计算所有观测到该地图的关键帧的观测该点的视角(即关键帧的光心到3D地图点的连线)的平均值
* 平均观测方向
* */
cv::Mat mNormalVector;
// Best descriptor to fast matching
cv::Mat mDescriptor;
// Reference KeyFrame
KeyFrame* mpRefKF;
// Tracking counters
int mnVisible; ///该地图在帧的视野中的数目
int mnFound; ///实际观测到该地图点的帧的数目
// Bad flag (we do not currently erase MapPoint from memory)
bool mbBad;
MapPoint* mpReplaced;
// Scale invariance distances
float mfMinDistance;
float mfMaxDistance;
Map* mpMap;
std::mutex mMutexPos;
std::mutex mMutexFeatures;
};
} //namespace ORB_SLAM
#endif // MAPPOINT_H
MapPoint.cc
/**
* This file is part of ORB-SLAM2.
*
* Copyright (C) 2014-2016 Raúl Mur-Artal (University of Zaragoza)
* For more information see
*
* ORB-SLAM2 is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* ORB-SLAM2 is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with ORB-SLAM2. If not, see .
*/
#include "MapPoint.h"
#include "ORBmatcher.h"
#include
namespace ORB_SLAM2
{
long unsigned int MapPoint::nNextId=0;
mutex MapPoint::mGlobalMutex;
MapPoint::MapPoint(const cv::Mat &Pos, KeyFrame *pRefKF, Map* pMap):
mnFirstKFid(pRefKF->mnId), mnFirstFrame(pRefKF->mnFrameId), nObs(0), mnTrackReferenceForFrame(0),
mnLastFrameSeen(0), mnBALocalForKF(0), mnFuseCandidateForKF(0), mnLoopPointForKF(0), mnCorrectedByKF(0),
mnCorrectedReference(0), mnBAGlobalForKF(0), mpRefKF(pRefKF), mnVisible(1), mnFound(1), mbBad(false),
mpReplaced(static_cast<MapPoint*>(NULL)), mfMinDistance(0), mfMaxDistance(0), mpMap(pMap)
{
Pos.copyTo(mWorldPos);
mNormalVector = cv::Mat::zeros(3,1,CV_32F);
// MapPoints can be created from Tracking and Local Mapping. This mutex avoid conflicts with id.
unique_lock<mutex> lock(mpMap->mMutexPointCreation);
mnId=nNextId++;
}
MapPoint::MapPoint(const cv::Mat &Pos, Map* pMap, Frame* pFrame, const int &idxF):
mnFirstKFid(-1), mnFirstFrame(pFrame->mnId), nObs(0), mnTrackReferenceForFrame(0), mnLastFrameSeen(0),
mnBALocalForKF(0), mnFuseCandidateForKF(0),mnLoopPointForKF(0), mnCorrectedByKF(0),
mnCorrectedReference(0), mnBAGlobalForKF(0), mpRefKF(static_cast<KeyFrame*>(NULL)), mnVisible(1),
mnFound(1), mbBad(false), mpReplaced(NULL), mpMap(pMap)
{
/// map point pose
Pos.copyTo(mWorldPos);
cv::Mat Ow = pFrame->GetCameraCenter();
/// norm view angle of map point
mNormalVector = mWorldPos - Ow;
mNormalVector = mNormalVector/cv::norm(mNormalVector);
/// distance from camera center to map point
cv::Mat PC = Pos - Ow;
const float dist = cv::norm(PC);
/// keypoint's scale level
const int level = pFrame->mvKeysUn[idxF].octave;
const float levelScaleFactor = pFrame->mvScaleFactors[level];
/// mnScaleLevels number
const int nLevels = pFrame->mnScaleLevels;
/**
* 特征点的尺寸不变依赖的是多层金字塔来实现的,金字塔图像的缩放等效于相机远离或者靠近特征点,
* 在一定的有效距离内,特征点一般只能在多层金字塔的一层上面能够检测到,想要在其它层检测到这个
* 特征点就需要让相机靠近或者远离特征点.
* 这里用到的一个主要的原理就是:图像缩小n倍,等效于相机距离特征点的距离放大n倍;
* 图像放大n倍,等效于相机距离特征点的距离缩小n倍;.回到代码里面,特征点是在金字塔的level
* 提取的,图像相对于原图像的缩小的比例是levelScaleFactor,然后根据缩小图像等效于相机远离
* 特征点的原理,如果要在原图像检测到该特征点,那么相机距离特征点的距离就应该放大levelScaleFactor,即dist*levelScaleFactor;
* 当图像缩小到金字塔最高一层的时候 (此时缩放因子为pFrame->mvScaleFactors[nLevels-1]),要想检测到
* 该特征点,根据缩小相机到特征点等效于放大图像的原理,相机距离特征点的距离应该等效缩小pFrame->mvScaleFactors[nLevels-1]倍
*/
mfMaxDistance = dist*levelScaleFactor;
mfMinDistance = mfMaxDistance/pFrame->mvScaleFactors[nLevels-1];
pFrame->mDescriptors.row(idxF).copyTo(mDescriptor);
// MapPoints can be created from Tracking and Local Mapping. This mutex avoid conflicts with id.
unique_lock<mutex> lock(mpMap->mMutexPointCreation);
mnId=nNextId++;
}
void MapPoint::SetWorldPos(const cv::Mat &Pos)
{
unique_lock<mutex> lock2(mGlobalMutex);
unique_lock<mutex> lock(mMutexPos);
Pos.copyTo(mWorldPos);
}
cv::Mat MapPoint::GetWorldPos()
{
unique_lock<mutex> lock(mMutexPos);
return mWorldPos.clone();
}
cv::Mat MapPoint::GetNormal()
{
unique_lock<mutex> lock(mMutexPos);
return mNormalVector.clone();
}
KeyFrame* MapPoint::GetReferenceKeyFrame()
{
unique_lock<mutex> lock(mMutexFeatures);
return mpRefKF;
}
void MapPoint::AddObservation(KeyFrame* pKF, size_t idx)
{
unique_lock<mutex> lock(mMutexFeatures);
if(mObservations.count(pKF))
return;
mObservations[pKF]=idx;
if(pKF->mvuRight[idx]>=0)
nObs+=2;
else/// negative value for monocular points
nObs++;
}
void MapPoint::EraseObservation(KeyFrame* pKF)
{
bool bBad=false;
{
unique_lock<mutex> lock(mMutexFeatures);
if(mObservations.count(pKF))
{
int idx = mObservations[pKF];
if(pKF->mvuRight[idx]>=0)
nObs-=2;
else
nObs--;
mObservations.erase(pKF);
/// reset mpRefKF frame to first KeyFrame in mObservations
if(mpRefKF==pKF)
mpRefKF=mObservations.begin()->first;
// If only 2 observations or less, discard point
if(nObs<=2)
bBad=true;
}
}
if(bBad)
SetBadFlag();
}
map<KeyFrame*, size_t> MapPoint::GetObservations()
{
unique_lock<mutex> lock(mMutexFeatures);
return mObservations;
}
int MapPoint::Observations()
{
unique_lock<mutex> lock(mMutexFeatures);
return nObs;
}
void MapPoint::SetBadFlag()
{
map<KeyFrame*,size_t> obs;
{
unique_lock<mutex> lock1(mMutexFeatures);
unique_lock<mutex> lock2(mMutexPos);
mbBad=true;
obs = mObservations;
mObservations.clear();
}
/// erase map point matched in keyframe
for(map<KeyFrame*,size_t>::iterator mit=obs.begin(), mend=obs.end(); mit!=mend; mit++)
{
KeyFrame* pKF = mit->first;
pKF->EraseMapPointMatch(mit->second);
}
/// erase map point from map
mpMap->EraseMapPoint(this);
}
MapPoint* MapPoint::GetReplaced()
{
unique_lock<mutex> lock1(mMutexFeatures);
unique_lock<mutex> lock2(mMutexPos);
return mpReplaced;
}
void MapPoint::Replace(MapPoint* pMP)
{
if(pMP->mnId==this->mnId)
return;
int nvisible, nfound;
map<KeyFrame*,size_t> obs;
{
unique_lock<mutex> lock1(mMutexFeatures);
unique_lock<mutex> lock2(mMutexPos);
obs=mObservations;
mObservations.clear();
mbBad=true;
nvisible = mnVisible;
nfound = mnFound;
mpReplaced = pMP;
}
for(map<KeyFrame*,size_t>::iterator mit=obs.begin(), mend=obs.end(); mit!=mend; mit++)
{
// Replace measurement in keyframe
KeyFrame* pKF = mit->first;
if(!pMP->IsInKeyFrame(pKF))
{
pKF->ReplaceMapPointMatch(mit->second, pMP);
pMP->AddObservation(pKF,mit->second);
}
else
{
pKF->EraseMapPointMatch(mit->second);
}
}
pMP->IncreaseFound(nfound);
pMP->IncreaseVisible(nvisible);
pMP->ComputeDistinctiveDescriptors();
mpMap->EraseMapPoint(this);
}
bool MapPoint::isBad()
{
unique_lock<mutex> lock(mMutexFeatures);
unique_lock<mutex> lock2(mMutexPos);
return mbBad;
}
void MapPoint::IncreaseVisible(int n)
{
unique_lock<mutex> lock(mMutexFeatures);
mnVisible+=n;
}
void MapPoint::IncreaseFound(int n)
{
unique_lock<mutex> lock(mMutexFeatures);
mnFound+=n;
}
/// frame realy view the map point count / frame to be predicted to view the map point large than 25%
///实际观测到该MapPoint的帧的数目和在帧的可视范围内 的帧的数目的比值,即发现比值
float MapPoint::GetFoundRatio()
{
unique_lock<mutex> lock(mMutexFeatures);
return static_cast<float>(mnFound)/mnVisible;
}
/// select best descriptor among all observed frame's descriptors
void MapPoint::ComputeDistinctiveDescriptors()
{
// Retrieve all observed descriptors
vector<cv::Mat> vDescriptors;
map<KeyFrame*,size_t> observations;
{
unique_lock<mutex> lock1(mMutexFeatures);
if(mbBad)
return;
observations=mObservations;
}
if(observations.empty())
return;
vDescriptors.reserve(observations.size());
/// get all keyframe's descriptor
for(map<KeyFrame*,size_t>::iterator mit=observations.begin(), mend=observations.end(); mit!=mend; mit++)
{
KeyFrame* pKF = mit->first;
if(!pKF->isBad())
vDescriptors.push_back(pKF->mDescriptors.row(mit->second));
}
if(vDescriptors.empty())
return;
// Compute distances between them
const size_t N = vDescriptors.size();
/// compute each two descriptors
float Distances[N][N];
for(size_t i=0;i<N;i++)
{
Distances[i][i]=0;
for(size_t j=i+1;j<N;j++)
{
int distij = ORBmatcher::DescriptorDistance(vDescriptors[i],vDescriptors[j]);
Distances[i][j]=distij;
Distances[j][i]=distij;
}
}
// Take the descriptor with least median distance to the rest
int BestMedian = INT_MAX;
int BestIdx = 0;
for(size_t i=0;i<N;i++)
{
/// construct vector from array
vector<int> vDists(Distances[i],Distances[i]+N);
sort(vDists.begin(),vDists.end());
/// median value
int median = vDists[0.5*(N-1)];
if(median<BestMedian)
{
BestMedian = median;
BestIdx = i;
}
}
{
unique_lock<mutex> lock(mMutexFeatures);
mDescriptor = vDescriptors[BestIdx].clone();
}
}
cv::Mat MapPoint::GetDescriptor()
{
unique_lock<mutex> lock(mMutexFeatures);
return mDescriptor.clone();
}
int MapPoint::GetIndexInKeyFrame(KeyFrame *pKF)
{
unique_lock<mutex> lock(mMutexFeatures);
if(mObservations.count(pKF))
return mObservations[pKF];
else
return -1;
}
bool MapPoint::IsInKeyFrame(KeyFrame *pKF)
{
unique_lock<mutex> lock(mMutexFeatures);
return (mObservations.count(pKF));
}
void MapPoint::UpdateNormalAndDepth()
{
map<KeyFrame*,size_t> observations;
KeyFrame* pRefKF;
cv::Mat Pos;
{
unique_lock<mutex> lock1(mMutexFeatures);
unique_lock<mutex> lock2(mMutexPos);
if(mbBad)
return;
observations=mObservations;
pRefKF=mpRefKF;
Pos = mWorldPos.clone();
}
if(observations.empty())
return;
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;
cv::Mat Owi = pKF->GetCameraCenter();
cv::Mat normali = mWorldPos - Owi;
normal = normal + normali/cv::norm(normali);
n++;
}
cv::Mat PC = Pos - pRefKF->GetCameraCenter();
const float dist = cv::norm(PC);
const int level = pRefKF->mvKeysUn[observations[pRefKF]].octave;
const float levelScaleFactor = pRefKF->mvScaleFactors[level];
const int nLevels = pRefKF->mnScaleLevels;
{
unique_lock<mutex> lock3(mMutexPos);
///最大距离:特征点在原图像基础上的放大了levelScaleFactor倍,点到光心的距离为dist,
///那么理论上来说在dist*levelScaleFactor的距离远处也可以看到这个点
mfMaxDistance = dist*levelScaleFactor;
mfMinDistance = mfMaxDistance/pRefKF->mvScaleFactors[nLevels-1];
mNormalVector = normal/n;
}
}
float MapPoint::GetMinDistanceInvariance()
{
unique_lock<mutex> lock(mMutexPos);
return 0.8f*mfMinDistance;
}
float MapPoint::GetMaxDistanceInvariance()
{
unique_lock<mutex> lock(mMutexPos);
return 1.2f*mfMaxDistance;
}
/// predict KeyFrame's
int MapPoint::PredictScale(const float ¤tDist, KeyFrame* pKF)
{
float ratio;
{
unique_lock<mutex> lock(mMutexPos);
ratio = mfMaxDistance/currentDist;
}
int nScale = ceil(log(ratio)/pKF->mfLogScaleFactor);
if(nScale<0)
nScale = 0;
else if(nScale>=pKF->mnScaleLevels)
nScale = pKF->mnScaleLevels-1;
return nScale;
}
int MapPoint::PredictScale(const float ¤tDist, Frame* pF)
{
float ratio;
{
unique_lock<mutex> lock(mMutexPos);
ratio = mfMaxDistance/currentDist;
}
int nScale = ceil(log(ratio)/pF->mfLogScaleFactor);
if(nScale<0)
nScale = 0;
else if(nScale>=pF->mnScaleLevels)
nScale = pF->mnScaleLevels-1;
return nScale;
}
} //namespace ORB_SLAM