【ORB_SLAM2源码解读】rgbd_tum 生成世界坐标系下的三维点完成双目和RGBD初始化操作 Tracking::StereoInitialization()
文章目录
-
- KeyFrame SetPose
- cv::Mat Frame::UnprojectStereo(const int &i)
- MapPoint::ComputeDistinctiveDescriptors
- ORBmatcher::DescriptorDistance
- void MapPoint::UpdateNormalAndDepth
- C++语言中的作用域与生命周期
- 优化函数的UpdateNormalAndDepth实现代码
接下来是双目和RGBD的初始化，
首先判断上面创建的当前帧的特征点的是否大于500mCurrentFrame.N>500
如果大于500首先给这个帧设置一个位置姿态mCurrentFrame.SetPose(cv::Mat::eye(4,4,CV_32F))
[1, 0, 0, 0;
0, 1, 0, 0;
0, 0, 1, 0;
0, 0, 0, 1]
这个位置是(0,0,0)，姿态的sin(90°)=1，cos(90°)=1，所以变换矩阵的作用相当于自己旋转到自己，没有变换。
这个当前帧就是ORB_SLAM2的第一帧，后面将作为世界坐标系。
// Set the camera pose.
// 设置相机姿态用cv::Mat Tcw (从世界坐标系到当前帧相机位姿的变换矩阵) mTcw = Tcw.clone(); 更新mTcw
void Frame::SetPose(cv::Mat Tcw){
mTcw = Tcw.clone();
UpdatePoseMatrices();
}
设置相机外参也就是位姿，并计算光心也就是相机坐标系原点位置
// Computes rotation, translation and camera center matrices from the camera pose.
// 根据Tcw计算mRcw旋转,mtcw平移,mRwc,mOw
// mTcw：世界坐标系到相机坐标系的变换矩阵
// mRcw：世界坐标系到相机坐标系的旋转矩阵
// mtcw：代表世界坐标系的原点到相机坐标系的原点的三维空间距离 世界坐标系的原点到相机坐标系原点的平移矩阵
// mRwc：相机坐标系到世界坐标系的旋转矩阵
// mOw： 代表相机坐标系的原点到世界坐标系的原点的三维空间距离 相机坐标系的原点到世界坐标系原点的平移矩阵
void Frame::UpdatePoseMatrices()
{
//从变换矩阵中提取出旋转矩阵rowRange取到范围的左边界而不取右边界,所以取值范围是[0, 1, 2] 前三行 前三列
mRcw = mTcw.rowRange(0,3).colRange(0,3);
//从变换矩阵中提取出平移矩阵,代表世界坐标系的原点到相机坐标系的原点的三维空间距离
mtcw = mTcw.rowRange(0,3).col(3);
mRwc = mRcw.t();
// mRcw求逆后是当前相机坐标系变换到世界坐标系下, 对应的原点(光心,中心)变换到世界坐标系下就是-mRcw.t()*mtcw
// 当前帧位姿时,相机坐标系的原点(光心,中心)在世界坐标系下的3D点坐标
mOw = -mRcw.t()*mtcw;
}
不理解上面这些内容可以参下面这几篇文章
【ORB_SLAM2源码解读】OpenCV模拟图像、数字图像、表示图像
【ORB_SLAM2源码解读】像素坐标系、图像坐标系、相机坐标系、相机归一化坐标系、世界坐标系
【ORB_SLAM2源码解读】坐标系之间坐标点的变换
【ORB_SLAM2源码解读】Eigen、Sophus、旋转矩阵、轴角、旋转向量、欧拉角、四元数、位姿变换
KeyFrame SetPose
相应的关键帧也会有位姿这个操作
// 设置关键帧的位姿
void KeyFrame::SetPose(const cv::Mat &Tcw_)
{
unique_lock<mutex> lock(mMutexPose);
Tcw_.copyTo(Tcw);
cv::Mat Rcw = Tcw.rowRange(0,3).colRange(0,3);
cv::Mat tcw = Tcw.rowRange(0,3).col(3);
cv::Mat Rwc = Rcw.t();
Ow = -Rwc*tcw;
Twc = cv::Mat::eye(4,4,Tcw.type());
Rwc.copyTo(Twc.rowRange(0,3).colRange(0,3));
Ow.copyTo(Twc.rowRange(0,3).col(3));
cv::Mat center = (cv::Mat_<float>(4,1) << mHalfBaseline, 0 , 0, 1);
Cw = Twc*center;
}
// 获取位姿
cv::Mat KeyFrame::GetPose()
{
unique_lock<mutex> lock(mMutexPose);
return Tcw.clone();
}
// 获取位姿的逆
cv::Mat KeyFrame::GetPoseInverse()
{
unique_lock<mutex> lock(mMutexPose);
return Twc.clone();
}
// 获取左目相机的中心在世界坐标系下的坐标
cv::Mat KeyFrame::GetCameraCenter()
{
unique_lock<mutex> lock(mMutexPose);
return Ow.clone();
}
// 获取双目相机的中心在世界坐标系下的坐标
cv::Mat KeyFrame::GetStereoCenter()
{
unique_lock<mutex> lock(mMutexPose);
return Cw.clone();
}
// 获取姿态
cv::Mat KeyFrame::GetRotation()
{
unique_lock<mutex> lock(mMutexPose);
return Tcw.rowRange(0,3).colRange(0,3).clone();
}
// 获取位置
cv::Mat KeyFrame::GetTranslation()
{
unique_lock<mutex> lock(mMutexPose);
return Tcw.rowRange(0,3).col(3).clone();
}
然后用当前帧生成关键帧KeyFrame* pKFini = new KeyFrame(mCurrentFrame, mpMap, mpKeyFrameDB);
传入的参数mCurrentFrame, mpMap, mpKeyFrameDB都是前面创建好的。
注意关键帧的KeyFrame::nNextId=0、mnId=nNextId++，也就是普通帧和关键帧的全局ID是各自独立的
但是位置姿态是共用的，这里把当前帧的位姿赋值给关键帧SetPose(F.mTcw)
mpMap->AddKeyFrame(pKFini);将创建好的关键帧插入地图
向std::set<KeyFrame*> mspKeyFrames; mspKeyFrames.insert(pKF);
中插入关键帧 更新地图中最大的关键帧id mnMaxKFid=pKF->mnId
cv::Mat Frame::UnprojectStereo(const int &i)
cv::Mat Frame::UnprojectStereo(const int &i)
{
const float z = mvDepth[i];
if(z>0)
{
const float u = mvKeysUn[i].pt.x;
const float v = mvKeysUn[i].pt.y;
const float x = (u-cx)*z*invfx;
const float y = (v-cy)*z*invfy;
// 这里是生成相机坐标系下的三维点
cv::Mat x3Dc = (cv::Mat_<float>(3,1) << x, y, z);
std::cout << "mRwc\n" << mRwc << endl;
// mRwc
// [1, 0, 0;
// 0, 1, 0;
// 0, 0, 1]
std::cout << "mOw\n" << mOw << endl;
// mOw
// [0;
// 0;
// 0]
std::cout << "x3Dc\n" << x3Dc << endl;
// x3Dc
// [1.5934014;
// -1.9250251;
// 5.4564133]
// x3Dc是相机坐标系下的点变换到世界坐标系下要旋转mRwc和平移mOw
return mRwc*x3Dc+mOw;
}
else
return cv::Mat();
}
Frame::UnprojectStereo函数主要功能是通过针孔相机投影计算u,v对应的x, y；
z已经经过前面的比例运算了，此处对应3D点的空间坐标。
流程是像素点通过内参恢复到相机坐标系下归一化平面为1的3D点坐标，通过Z可以得到真实世界3D点是相机坐标系下的坐标，
再通过相机和世界坐标系之间的外参转换到世界坐标系下面。
对当前的这一张图片的2D特征点通过相机内参和外参数求3D空间点
进而对当前的这一张图片的3D空间点进行坐标变换，变换到世界坐标系下面
总结来说就是操作的单位是一张图片的的特征点，这些的特征点具有相同的位置姿态
所以求位姿是求某一帧上面所有特征点的的位姿，当然这些特征点的位置不同，但是姿态是相同的。
invfx = 1.0f/fx;
invfy = 1.0f/fy;
const float x = (u - cx) * z * invfx;
const float y = (v - cy) * z * invfy;
有了3D点、关键帧、地图这些原料就可以创建地图点MapPoint了
MapPoint* pNewMP = new MapPoint(x3D, pKFini, mpMap);
注意这里有两个构造函数，调用的是MapPoint::MapPoint(const cv::Mat &Pos, KeyFrame *pRefKF, Map* pMap):
接下来执行这个函数MapPoint::AddObservation(KeyFrame* pKF, size_t idx)
将关键帧的指针做为std::map的"key"，特征点的索引"i"，作为std::map的"value"。
可以看到const std::vector初始化mvuRight的所有值为"-1"，
赋值为左右目相机中同一个特征点在右目中的横坐标。
int nObs表示被观测到的相机数目，单目+1，双目或RGB-D则+2。
理解为单目同一时刻只有一个图像可以观测到特征点，双目有左右两帧图像可以观测到同一个特征点。
可以看到这个函数会返回这个int nObs变量供外部使用，至于谁会调用，后面用到的时候再讲。
然后执行这个句话pKFini->AddMapPoint(pNewMP, i)
std::vector
这里是将特征点的索引"idx"作为"mvpMapPoints"的索引，MapPoint* pNewMP = new MapPoint(x3D, pKFini, mpMap);地图点作为vector的值。
MapPoint::ComputeDistinctiveDescriptors
然后执行pNewMP->ComputeDistinctiveDescriptors()这句，这个函数这里没有参数，
首先说一下这个函数的功能是计算特征点最好的的描述子，由于一个MapPoint会被许多帧观测到，这里就包括前面左右目同一时刻的两帧，因此在插入关键帧后，需要判断是否更新当前点的最适合的描述子，先获得当前点的所有描述子，然后计算描述子之间的两两距离，最好的描述子与其他描述子应该具有最小的距离中值。
执行的流程：
先说一下这个变量
bool mbBad; mbBad 代表当前的3D点 是否是可以使用的
mbBad(false),
SetBadFlag()会使得mbBad=true;
void KeyFrame::SetBadFlag()也会使得mbBad=true;
std::map<KeyFrame*,size_t> mObservations;
vector<cv::Mat> vDescriptors;
observations=mObservations;
std::map的值实在下面这个函数中生成的
前面会将关键帧和特征点的索引存储到 map
程序继续执行，采用迭代器遍历map
先要补充一下const cv::Mat mDescriptors描述子的数据结构和生成位置
mDescriptors是cv::Mat，是OpenCV最基本的数据结构，Mat即矩阵（Matrix）的缩写
所以每一行存储的就是一个特征点的描述子，所以这句话vDescriptors.push_back(pKF->mDescriptors.row(mit->second))就是在获取当前关键帧的，当前的这个特征点的描述子，继续循环获取所有描述子。
代码接着向下执行
这里是调用int distij = ORBmatcher::DescriptorDistance(vDescriptors[i], vDescriptors[j]);计算两个特征点的描述子的距离，进而生成距离矩阵。
这个矩阵对角线上全是零，i=j 和 j=i下标的值相等。
ORBmatcher::DescriptorDistance
// Bit set count operation from
int ORBmatcher::DescriptorDistance(const cv::Mat &a, const cv::Mat &b)
{
// ptr(int r)方法获取指向行首的指针r int32_t 是int的别名, 占4个字节
const int *pa = a.ptr<int32_t>();// 1行32列的描述子的指针
const int *pb = b.ptr<int32_t>();// 1行32列的描述子的指针
int dist=0;
// Hamming distance：
// 它表示两个相同长度字符串对应位置的不同字符的数量
// 对两个字符串进行异或运算，并统计结果为1的个数，那么这个数就是汉明距离。
// 异或运算: 0⊕0=0，1⊕0=1，0⊕1=1，1⊕1=0（同为0，异为1）
// http://graphics.stanford.edu/~seander/bithacks.html#CountBitsSetParallel
// B[0] = 0x55555555 = 01010101 01010101 01010101 01010101
// B[1] = 0x33333333 = 00110011 00110011 00110011 00110011
// B[2] = 0x0F0F0F0F = 00001111 00001111 00001111 00001111
// B[3] = 0x00FF00FF = 00000000 11111111 00000000 11111111
// B[4] = 0x0000FFFF = 00000000 00000000 11111111 11111111
// The best method for counting bits in a 32-bit integer v is the following:
// v = v - ((v >> 1) & 0x55555555); // reuse input as temporary
// v = (v & 0x33333333) + ((v >> 2) & 0x33333333); // temp
// c = ((v + (v >> 4) & 0xF0F0F0F) * 0x1010101) >> 24; // count
for(int i=0; i<8; i++, pa++, pb++)
{
unsigned int v = *pa ^ *pb;
v = v - ((v >> 1) & 0x55555555);
v = (v & 0x33333333) + ((v >> 2) & 0x33333333);
dist += (((v + (v >> 4)) & 0xF0F0F0F) * 0x1010101) >> 24;
}
return dist;
}
继续执行
这句vector的意思是一行行取出矩阵的每一行，这一行代表的第i个特征点，
和其他特征点的特征点描述子的距离，首先对这一行的距离进行了排序，最左边的是描述子距离最小的的，最右边是描述子距离最大的，
为了选择最有代表性的描述子，我们不选择最小的，选择中间的那个，int median = vDists[0.5 * (N - 1)];这个就是取中间的那个描述子。
还有一个地方不是很好理解就是int BestMedian = INT_MAX、int BestIdx = 0这两个值是随着for循环而更新的，所以会判断N次，最终找到符合条件的描述子下标。
其实找描述子的过程，就是在帧中寻找一个最好的和当前3D点对应的2D特征点。
下面的代码可以单独运行帮助理解：
#include void MapPoint::UpdateNormalAndDepth
mbBad、mObservations参考上一篇文章
分析一下这句代码
pRefKF = mpRefKF
C++语言中的作用域与生命周期
详解C++作用域与生命周期
程序的运行过程可以理解为算法对数据的处理过程，程序运行的结果就是算法处理数据产生的结果数据。
算法描述的是对数据处理的步骤，算法的好坏决定了出具处理的效率，算法就是程序中的函数。
数据在计算机中的组织结构就是程序中的数据类型。
C++语言谈及作用域与生命周期针对的就是这程序的组成要素：函数、数据。
作用域用“scope”表示，生命周期则用“duration”表示。作用域是一个静态概念，只在编译程序的时候用到。
一个标识符的作用域指在源文件中该标识符能够独立地合法出现的区域。
生命周期则是一个运行时（Runtime）概念，它是指一个变量在整个程序从载入到结束运行的过程中存在的时间周期。
由于函数和数据类型是静态的概念，它们没有生命周期的说法，它们从编译、程序的运行到结束整个过程是一直存在的。
C++中作用域的级别由高到低，主要有文件域（全局作用域）、名字空间域、类域、函数作用域和代码块作用域，其中函数作用域和代码块作用域又统称为局部域。
局部变量范围：在一个函数内部定义的变量，作用范围只限于本函数体内。
生存期：程序执行到本函数才会给局部变量分配内存单元，函数执行完毕局部变量所占的存储单元就被释放。
静态局部变量范围：函数体内，离开函数体就不起作用，但是值仍然被保留，下次被调用的时候使用的是上次的值。
生存期：在编译阶段赋值，从程序运行开始就被分配固定的存储单元（静态存储区），整个程序运行期间不再重新分配，生存周期是整个程序运行期间。
全局变量在函数体外部定义的变量是全局变量，可以被本文件所有其他函数访问使用。
作用域：所有文件。在一个cpp定义的全局变量在另一个cpp中使用，应该在使用它的函数体内部或外部进行extern说明
生存期：全局变量在程序的全部执行过程中占用固定的内存单元，生存周期是整个程序运行期间。
静态全局变量作用域：定义它的cpp文件，而不是程序中的所有cpp文件
生存期：整个程序运行期间不同cpp文件可以使用相同名称的静态全局变量名互不干扰
static函数静态函数只能被所在的cpp文件调用，不同cpp文件可以有相同的静态函数名字。
可被其他文件调用的函数
调用方式有两种
第一引入函数所在头文件，第二在调用函数前进行extern声明
变量pRefKF的声明来自于MapPoint.cc源码文件中的UpdateNormalAndDepth函数KeyFrame pRefKF，也就说明了这个变量的作用域只在函数内部，声明周期也只是函数的执行过程。
变量mpRefKF的声明来自于MapPoint.h头文件KeyFrame mpRefKF是全局变量，作用域是整个文件，生命周期是整个程序运行期间。
再来来看看mpRefKF在哪里赋值了
MapPoint有两种构造函数，也就是说MapPoint地图点有两种生成方式。
MapPoint::MapPoint(KeyFrame *pRefKF):mpRefKF(pRefKF)
MapPoint::MapPoint(Frame *pFrame, const int &idxF) : mpRefKF(static_cast<KeyFrame *>(NULL))
查看整个ORB_SLAM2代码可以发现生成MapPoint地图点有如下几种方式，发现生成地图点都是调用的第一种构造函数，也就是通过关键帧来生成的地图点。
只有需要更新地图点的时候才会调用第二种构造函数生成地图点。x3D都是局部变量形参函数执行完就销毁了，所以名字相同不影响其它函数。
void LocalMapping::CreateNewMapPoints()
MapPoint* pMP = new MapPoint(x3D, mpCurrentKeyFrame, mpMap);
void Tracking::StereoInitialization()
MapPoint* pNewMP = new MapPoint(x3D, pKFini, mpMap);
void Tracking::CreateInitialMapMonocular()
MapPoint* pMP = new MapPoint(worldPos, pKFcur, mpMap);
void Tracking::UpdateLastFrame()
MapPoint* pNewMP = new MapPoint(x3D, mpMap, &mLastFrame, i);
void Tracking::CreateNewKeyFrame()
MapPoint* pNewMP = new MapPoint(x3D, pKF, mpMap);
再回到mpRefKF变量，它通过函数GetReferenceKeyFrame返回，是被谁使用的呢？被谁生成的呢？
KeyFrame *MapPoint::GetReferenceKeyFrame(){
return mpRefKF;
}
被下面两个函数使用，后面再说
Optimizer::OptimizeEssentialGraph
LoopClosing::RunGlobalBundleAdjustment
实际上在生成地图点的同时，也会生成对应mpRefKF，所以mpRefKF除了被上面构造函数调用的地方生成，还被下面函数更新，删除观测的时候先减少nObs，在删除观测的关键帧，日过删除的关键帧，是生成地图点的关键帧，要把观测中的第一个帧作为当前地图点的参考关键帧，如果此时nObs<=2,也就是说，除了这个关键帧没有其它的关键帧可以看到这个地图点了，那么这个地图点就可以标记为坏点，后面不在使用了。
void MapPoint::EraseObservation(KeyFrame *pKF)
{
bool bBad = false;
{
if (mObservations.count(pKF))
{
int idx = mObservations[pKF];
if (pKF->mvuRight[idx] >= 0)
nObs -= 2;
else
nObs--;
mObservations.erase(pKF);
if (mpRefKF == pKF)
mpRefKF = mObservations.begin()->first;
// If only 2 observations or less, discard point
if (nObs <= 2)
bBad = true;
}
}
if (bBad)
SetBadFlag();
}
接下来看看如何计算相机光心和当前地图点之间的空间距离
cv::Mat normal = cv::Mat::zeros(3, 1, CV_32F);
std::cout << "normal\n" << normal << std::endl;// normal [0; 0; 0]
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();
std::cout << "Owi\n" << Owi << std::endl;// Owi [0; 0; 0]
cv::Mat normali = mWorldPos - Owi;
std::cout << "normali\n" << normali << std::endl;// normali [3.0920818; 1.7724948; 4.3270001]
normal = normal + normali / cv::norm(normali);
std::cout << "cv::norm(normali)\n" << cv::norm(normali) << std::endl;// cv::norm(normali) 5.60586 = (3.0920818*3.0920818 + 1.7724948*1.7724948 + 4.3270001*4.3270001)再开根号
std::cout << "normali / cv::norm(normali)\n" << normali / cv::norm(normali) << std::endl;
// normali / cv::norm(normali) = [0.55158049; 0.31618619; 0.77187121]
std::cout << "normal\n" << normal << std::endl;// normal [0.55158049; 0.31618619; 0.77187121]
n++;
}
cv::Mat PC = Pos - pRefKF->GetCameraCenter();// 当前地图点和相机光心之间的向量
std::cout << "Pos\n" << Pos << std::endl;// Pos [8.1681213; 1.7589091; 6.3900003]
std::cout << "pRefKF->GetCameraCenter()\n" << pRefKF->GetCameraCenter() << std::endl;// [0; 0; 0]
std::cout << "PC\n" << PC << std::endl;// [8.1681213; 1.7589091; 6.3900003]
const float dist = cv::norm(PC);// 当前地图点和相机光心之间的空间距离就是向量的长度
std::cout << "dist" << dist << std::endl;// dist 10.5187
const int level = pRefKF->mvKeysUn[observations[pRefKF]].octave;// octacv:从哪一层金字塔得到的此关键点
const float levelScaleFactor = pRefKF->mvScaleFactors[level];
const int nLevels = pRefKF->mnScaleLevels;
ORBextractor.nFeatures: 1000// 参数含义是一帧图像提取1000个特征点
ORBextractor.scaleFactor: 1.2// 参数含义是一帧图像用这个比例缩放图像
ORBextractor.nLevels: 8// 参数含义是一帧图像用上面的比例缩放出８张图像，这就是所谓的图像金字塔
图像金字塔在OpenCV图像提取特征中应该是一个很常用的算法，所以为了区分提取的特征点来自于哪一个金字塔图像层级，所以针对特征点有一个属性就是octacv
然后我们再来看关键帧的的图像金字塔每一层的缩放因子vector<float> mvScaleFactors
mvScaleFactors = mpORBextractorLeft->GetScaleFactors();
std::vector<float> inline GetScaleFactors()
{
return mvScaleFactor;
}
mvScaleFactor.resize(nlevels);
mvScaleFactor[0] = 1.0f;
for (int i = 1; i < nlevels; i++)
{
mvScaleFactor[i] = mvScaleFactor[i - 1] * scaleFactor;// ORBextractor.scaleFactor: 1.2
}
图像金字塔每一层的缩放因子:mvScaleFactors = [1.0, 1.2, 1.44 ...]
mfMinDistance(0),
mfMaxDistance(0),
mfMaxDistance = dist*levelScaleFactor; //当前图层的"深度"
mfMinDistance = mfMaxDistance/pFrame->mvScaleFactors[nLevels-1]; //该特征点上一个图层的"深度""
这里实际上建立一个地图点到相机关心之间的距离和图像金字塔图层之间缩放系数的关系
// Mean viewing direction
cv::Mat mNormalVector;
mNormalVector = normal/n;
由于图像提取描述子是使用金字塔分层提取，所以计算法向量和深度可以知道该MapPoint在对应的关键帧的金字塔哪一层可以提取到。
法向量是相机光心指向地图点的方向，计算这个方向方法很简单，只需要用地图点的三维坐标减去相机光心的三维坐标就可以。
注意这个变量原作者的注释cv::Mat mNormalVectorMean viewing direction->平均观测方向
平均就是这句代码做到的mNormalVector = normal/n
所以normal就是viewing direction观测方向，这里是怎么求的呢？
需要看一下向量的知识了。
可以看一下我画的这个图
优化函数的UpdateNormalAndDepth实现代码
void MapPoint::UpdateNormalAndDepth(){
map<KeyFrame *, size_t> observations;
KeyFrame *pRefKF;
cv::Mat Pos;
{
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);
// std::cout << __FILE__ << " " << __LINE__ << " " << __FUNCTION__ << " " << __TIME__ << std::endl;
mfMaxDistance = dist * levelScaleFactor;
mfMinDistance = mfMaxDistance / pRefKF->mvScaleFactors[nLevels - 1];
mNormalVector = normal / n;
}
}
MapPoint与KeyFrame的相机位置关系
获得观测到该3d点的所有关键帧，对所有关键帧对该点的观测方向归一化为单位向量进行求和，除以所有关键帧数就是获得的平均观测方向。
获得观测到该点的参考关键帧和3d点在世界坐标系中的位置，得到该点到参考关键帧相机的距离，预测其在金字塔中的层数，就可以获得其其距离范围。
void MapPoint::AddObservation(KeyFrame* pKF, size_t idx){
unique_lock<mutex> lock(mMutexFeatures);
if(mObservations.count(pKF))
// mObservations:观测到该MapPoint的关键帧KF和该MapPoint在KF中的索引如果已经添加过观测返回,Map是一种关联容器，它按照特定顺序存储由键值Key和映射值Value组合而成的元素。
// 使用count返回的是被查找元素的个数。如果有，返回1；否则，返回0。注意，map中不存在相同元素，所以返回值只能是1或0。使用find，返回的是被查找元素的位置，没有则返回map.end()。Map通常是基于二叉搜索树实现的。
return;
mObservations[pKF]=idx;// 如果没有添加过观测，记录下能观测到该MapPoint的KF和该MapPoint在KF中的索引
if(pKF->mvuRight[idx]>=0)
nObs+=2;// 双目或者rgbd是同一时刻有两帧可以观测到这个特征点和3D点
else
nObs++;// 单目只有一帧
}
std::map<KeyFrame*,size_t> mObservations;
// Keyframes observing the point and associated index in keyframe 观测到该MapPoint的KF和该MapPoint在KF中的索引
// Reference KeyFrame
KeyFrame* mpRefKF;
KeyFrame *pRefKF;
pRefKF = mpRefKF;
当前关键帧的参考关键帧
cv::Mat Owi = pKF->GetCameraCenter();
// 相机坐标系的光心
cv::Mat normali = mWorldPos - Owi;
相机光心指向地图点的方向，计算这个方向方法很简单，只需要用地图点的三维坐标减去相机光心的三维坐标就可以。
normal = normal + normali/cv::norm(normali);
对其进行归一化后相加
// 深度范围：地图点到参考帧（只有一帧）相机中心距离，乘上参考帧中描述子获取金字塔放大尺度得到最大距离mfMaxDistance;
// 最大距离除以整个金字塔最高层的放大尺度得到最小距离mfMinDistance.
// 通常说来，距离较近的地图点，将在金字塔较高的地方提出，
// 距离较远的地图点，在金字塔层数较低的地方提取出（金字塔层数越低，分辨率越高，才能识别出远点）
// 因此，通过地图点的信息（主要对应描述子），我们可以获得该地图点对应的金字塔层级
// 从而预测该地图点在什么范围内能够被观测到
{
unique_lock<mutex> lock3(mMutexPos);
mfMaxDistance = dist*levelScaleFactor;
mfMinDistance = mfMaxDistance/pRefKF->mvScaleFactors[nLevels-1];
mNormalVector = normal/n;
}
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();// 相机光心(左目)在世界坐标系下的坐标
//std::cout << "mWorldPos " << mWorldPos << std::endl;
//std::cout << "Owi " << Owi << std::endl;
cv::Mat normali = mWorldPos - Owi;// 相机光心和3D点两个向量之间的关系 [-0.16701621; -0.17532404; 1.6193999]
//std::cout << "cv::norm(normali) " << cv::norm(normali) << std::endl;// 1.63
// 向量归一化，并不是向量元素和为1，而是向量长度为1，归一化的方法简单的就是直接除以向量长度的平方根
//std::cout << "normali/cv::norm(normali) " << normali/cv::norm(normali) << std::endl;// [-0.10200066; -0.10707445; 0.98900497]
normal = normal + normali/cv::norm(normali);// [-0.10200066; -0.10707445; 0.98900497]
// std::cout << "normal " << normal << std::endl;
// std::cout << "normal + normali/cv::norm(normali) " << normal + normali/cv::norm(normali) << std::endl;
n++;
}
// 位姿归一化
cv::Mat PC = Pos - pRefKF->GetCameraCenter();
const float dist = cv::norm(PC);
// 金字塔层级
const int level = pRefKF->mvKeysUn[observations[pRefKF]].octave;// 0
// 图像缩放系数
const float levelScaleFactor = pRefKF->mvScaleFactors[level];// 1
// 金字塔层数
const int nLevels = pRefKF->mnScaleLevels;// 8
{
unique_lock<mutex> lock3(mMutexPos);
mfMaxDistance = dist*levelScaleFactor;// 1.6374 * 1= 1.6374
mfMinDistance = mfMaxDistance/pRefKF->mvScaleFactors[nLevels-1];// 1.6374/3.58318 = 0.4569
mNormalVector = normal/n;// [-0.10200066; -0.10707445; 0.98900497]
// std::cout << "mNormalVector " << mNormalVector << std::endl;
}
}
std::set<MapPoint*> mspMapPoints;
mspMapPoints地图点的存储结构是std::set调用.insert函数接口插入MapPoint
mCurrentFrame.mvpMapPoints[i]=pNewMP;
std::vector<MapPoint*> mvpMapPoints;
当前帧也会用一个vector存储地图点
这个函数可以获取地图中的地图点std::set
这里插入关键帧到局部建图对象，并且不进行BA优化。
应用帧的拷贝构造函数，用当前帧生成最新帧mLastFrame，在跟踪丢失的时候会用到。
Frame mLastFrame;
mLastFrame = Frame(mCurrentFrame);
将当前帧的全局ID赋值给最新的关键帧mnLastKeyFrameId
unsigned int mnLastKeyFrameId;
mnLastKeyFrameId=mCurrentFrame.mnId;
当前关键帧生成最新的关键帧mpLastKeyFrame
KeyFrame* mpLastKeyFrame;
mpLastKeyFrame = pKFini;
将当前帧加入局部关键帧mvpLocalKeyFrames数据结构vector中
std::vector<KeyFrame*> mvpLocalKeyFrames;
mvpLocalKeyFrames.push_back(pKFini);
获取当前地图中的地图点，并存储到vector中
std::vector<MapPoint*> mvpLocalMapPoints;
mvpLocalMapPoints=mpMap->GetAllMapPoints();
std::set<MapPoint*> mspMapPoints;
KeyFrame* mpReferenceKF;
mpReferenceKF = pKFini;
mCurrentFrame.mpReferenceKF = pKFini;
当前关键帧率生成参考关键帧
当前关键帧率生成当前帧的参考关键帧
mvpLocalMapPoints=mpMap->GetAllMapPoints();
mpMap->SetReferenceMapPoints(mvpLocalMapPoints);
std::vector<MapPoint*> mvpReferenceMapPoints;
获取当前地图中的地图点，并存储到vector中
设置参考地图点用于绘图显示局部地图点（红色）
vector<KeyFrame*> mvpKeyFrameOrigins;
mpMap->mvpKeyFrameOrigins.push_back(pKFini);
// Correct keyframes starting at map first keyframe
// 从第一个关键帧开始矫正关键帧。刚开始只保存了初始化第一个关键帧
list<KeyFrame*> lpKFtoCheck(mpMap->mvpKeyFrameOrigins.begin(),mpMap->mvpKeyFrameOrigins.end());
用于回环检测
将当前帧的位姿传递给显示对象 mpMapDrawer->SetCurrentCameraPose(mCurrentFrame.mTcw)
最后mState=OK完成双目和RGBD初始化，做的工作还挺多。