ORB_SLAM2代码阅读及总结使用
作者：互联网
在学习ORB_SLAM2之前先从OPENCV入手
OpenCV3】cv::Mat类成员函数详解
我们有多种方法可以获得从现实世界的数字图像：数码相机、扫描仪、计算机体层摄影或磁共振成像就是其中的几种。在每种情况下我们（人类）看到了什么是图像。但是，转换图像到我们的数字设备时我们的记录是图像的每个点的数值。
Q：如何获取和存储像素值
Mat本质上是由两个数据部分组成的类： （包含信息有矩阵的大小，用于存储的方法，矩阵存储的地址等） 的矩阵头和一个指针。
cv::Mat A,C;//仅创建头部
A = imread(argv[1],CV_LOAD_IMAGE_COLOR);//为图像读取
//Mat imread(const string& FileName, int flags = 1)
//FileName：图片名
//flags：载入标识，指定一个加载图像的颜色类型，
//默认值为1，表示载入三通道的彩色图像；
// -1，imread按解码得到的方式读入图像；
// 0，imread按单通道的方式读入图像，即灰白图像。
• 赋值运算符和复制构造函数 （构造函数）只复制头。
• 使用clone () 或copyTo () 函数将复制的图像的基础矩阵。
cv::Mat F= A.clone(); //复制矩阵的本身
cv::Mat G;
A.copyTo(G);
如何存储像素值。可以选择颜色空间和使用的数据类型。
最小的数据类型可能是 char 类型，这意味着一个字节或 8 位。这可能是有符号（值-127 到 + 127）或无符号（以便可以存储从 0 到 255 之间的值）。虽然这三个组件的情况下已经给 16 万可能的颜色来表示 （如 RGB 的情况下） 我们可能通过使用浮点数 （4 字节 = 32 位） 或double（8 字节 = 64 位） 数据类型的每个组件获得甚至更精细的控制。然而，请记住增加组件的大小也会增加在内存中的整张图片的大小。
• Mat()构造函数
Mat M(2,2, CV_8UC3, Scalar(0,0,255));
//对于二维的图像，我们首先定义大小： 按行和列计数 2,2
//然后我们需要指定数据类型，用于存储元素和每个矩阵点通道的数量
//CV_ [每一项的位数] [有符号或无符号] [类型前缀] [通道数]
For instance, CV_8UC3 means we use unsigned char types that are 8 bit long and each pixel has three of these to form the three channels.
The cv::Scalar is four element short vector. Specify this and you can initialize all matrix points with a custom value
Mat L(3,3, CV_8UC3, Scalar::all(0))
// With Two dimension
Summery: The Scalar::all(0) or Scalar(0,0,255), the number of element in the bracket is depend on the custom channel we set up before. The value of each matrix point will be filled out to 0 or 0,0,255.
//cv::Mat::create function:
M.create(4,4, CV_8UC(2));
cout << "M = "<< endl << " " << M << endl << endl;
Summery: The difference between cv::Mat::create with cv::Mat(,Scalar(x)) is that you cannot initialize the matrix values with this construction.
Mat_ (3,3)构造显式Mat的一种方法
cv::Mat A=( Mat_<double> (3,3) <<1,2,3,4,5,6,7,8,9);
std::cout<<"A is"<<endl<<A<<endl
Summery: There are some small difference. remember the space behind A and
Mat_ (space)(3,3)(space)<<1,2,3,4,5,6,7,8,9
**Note:**You can fill out a matrix with random values using the cv::randu() function. You need to give the lower and upper value for the random values:
cv::Mat A = Mat(3,3 CV_8UC3)
cv::randu(A, Scalar::all(0), Scalar::alll(255));
ORB-SLAM主要分为三个线程进行，分别是Tracking、LocalMapping和LoopClosing。三个线程分别存放在对应的三个文件中，分别是Tracking.cpp、LocalMapping.cpp和LoopClosing.cpp文件中。
在ORB_SLAM2将整个系统进行了完整的封装并定义了一个System类作为系统的入口。
在System类的构造函数中，主要执行以下操作：
1.判断传感器类型
2.检验配置文件
3.载入词袋
4.实例化相关成员变量：包括局部建图线程、回环检测线程和视图线程的实例化。注:主线程即为追踪线程！
5.各个线程之间交叉设置指针
System类的构造函数将系统运行的所有条件都已经准备就绪，等待图片序列的传入。
SLAM.TrackStereo(imLeft,imRight,tframe)传入图片序列
cv::Mat System::TrackStereo(const cv::Mat &imLeft, const cv::Mat &imRight, const double ×tamp)
{
if(mSensor!=STEREO)
{
cerr << "ERROR: you called TrackStereo but input sensor was not set to STEREO." << endl;
exit(-1);
}
// Check mode change
{
unique_lock<mutex> lock(mMutexMode); //在该声明周期内对 mMutexMode 进行上锁操作，不允许其他线程修改定位模式
if(mbActivateLocalizationMode) //激活定位模式
{
mpLocalMapper->RequestStop();
// Wait until Local Mapping has effectively stopped
while(!mpLocalMapper->isStopped())
{
usleep(1000);
}
mpTracker->InformOnlyTracking(true);
mbActivateLocalizationMode = false;
}
if(mbDeactivateLocalizationMode) //停用定位模式
{
mpTracker->InformOnlyTracking(false);
mpLocalMapper->Release();
mbDeactivateLocalizationMode = false;
}
}
// Check reset
{
unique_lock<mutex> lock(mMutexReset);
if(mbReset)
{
mpTracker->Reset();
mbReset = false;
}
}
cv::Mat Tcw = mpTracker->GrabImageStereo(imLeft,imRight,timestamp);
//为追踪器输入图像数据，返回世界坐标系到相机坐标系的变换矩阵
unique_lock<mutex> lock2(mMutexState);
mTrackingState = mpTracker->mState;//更新追踪状态
mTrackedMapPoints = mpTracker->mCurrentFrame.mvpMapPoints;//更新地图点向量
mTrackedKeyPointsUn = mpTracker->mCurrentFrame.mvKeysUn;//更新关键点向量
return Tcw;//返回相机坐标系到世界坐标系的变换矩阵
}
在TrackStereo()函数中主要执行以下操作：
1.首先检验传感器类型。若传感器类型错误则直接退出。
2.检验定位模式的变化。若定位模式被激活，则关闭局部建图功能，系统只进行定位。若定位模式被关闭，则之前建立的局部地图被释放（清除）。
3.检验系统是否被重启（有UI界面控制），若系统重启，则重启追踪器。
4.为追踪器输入图像数据，返回世界坐标系到相机坐标系的变换矩阵。
5.更新变量
保存轨迹函数
void System::SaveTrajectoryKITTI(const string &filename)
{
cout << endl << "Saving camera trajectory to " << filename << " ..." << endl;
if(mSensor==MONOCULAR)
{
cerr << "ERROR: SaveTrajectoryKITTI cannot be used for monocular." << endl;
return;
}
vector<KeyFrame*> vpKFs = mpMap->GetAllKeyFrames();//获取所有的关键帧
sort(vpKFs.begin(),vpKFs.end(),KeyFrame::lId);//对关键帧进行排序
// Transform all keyframes so that the first keyframe is at the origin.
// After a loop closure the first keyframe might not be at the origin.
cv::Mat Two = vpKFs[0]->GetPoseInverse();
ofstream f;
f.open(filename.c_str());
f << fixed;
// Frame pose is stored relative to its reference keyframe (which is optimized by BA and pose graph).
// We need to get first the keyframe pose and then concatenate the relative transformation.
// Frames not localized (tracking failure) are not saved.
// For each frame we have a reference keyframe (lRit), the timestamp (lT) and a flag
// which is true when tracking failed (lbL).
// 链表mlpReferences保存了所有图像帧的参考关键帧
// 链表mlRelativeFramePoses保存了所有图像帧的相对于参考帧的姿态变换关系
list<ORB_SLAM2::KeyFrame*>::iterator lRit = mpTracker->mlpReferences.begin();
list<double>::iterator lT = mpTracker->mlFrameTimes.begin();
for(list<cv::Mat>::iterator lit=mpTracker->mlRelativeFramePoses.begin(), lend=mpTracker->mlRelativeFramePoses.end();lit!=lend;lit++, lRit++, lT++)
{
ORB_SLAM2::KeyFrame* pKF = *lRit;
cv::Mat Trw = cv::Mat::eye(4,4,CV_32F);
while(pKF->isBad())
{
// cout << "bad parent" << endl;
Trw = Trw*pKF->mTcp;
pKF = pKF->GetParent();
}
Trw = Trw*pKF->GetPose()*Two;//计算全局坐标系到参考关键帧坐标系变换关系
cv::Mat Tcw = (*lit)*Trw; //计算全局坐标系到相机坐标系的变换关系，注：Tcw是全局坐标系到相机坐标系的变换
/************************************************************************
* Tcw是全局坐标系到相机坐标系的变换
* Tcw的逆为相机坐标系到全局坐标系的变换
* 变换矩阵的逆可由下式计算
* T^-1 =[ R^T -R^T*t
* 0^T 1 ]
*
* **********************************************************************/
cv::Mat Rwc = Tcw.rowRange(0,3).colRange(0,3).t();//根据Tcw计算相机坐标系到全局坐标系的旋转矩阵
cv::Mat twc = -Rwc*Tcw.rowRange(0,3).col(3); //根据Tcw计算相机坐标系到全局坐标系的平移向量
//将旋转矩阵和平移向量写入文件
f << setprecision(9) << Rwc.at<float>(0,0) << " " << Rwc.at<float>(0,1) << " " << Rwc.at<float>(0,2) << " " << twc.at<float>(0) << " " <<
Rwc.at<float>(1,0) << " " << Rwc.at<float>(1,1) << " " << Rwc.at<float>(1,2) << " " << twc.at<float>(1) << " " <<
Rwc.at<float>(2,0) << " " << Rwc.at<float>(2,1) << " " << Rwc.at<float>(2,2) << " " << twc.at<float>(2) << endl;
}
f.close();
cout << endl << "trajectory saved!" << endl;
}
追踪器中维护了两个重要的链表——mlpReferences 和 mlRelativeFramePoses。其中mlpReferences保存了所有图像帧的参考关键帧；mlRelativeFramePoses保存了图像帧的相对于参考帧的姿态变换关系，即图像与参考关键帧之间的变换矩阵。这正是由于这两个链表的存在，我们才能将整个运动轨迹保存下来。
计算每一帧图像相对于原点处的姿态的流程如下：
1.从 mpMap 变量处获取所有关键帧并对这些关键帧按ID号进行排序。
2.获取第一个关键帧的位姿信息。
3.根据链表mlpReferences和mlRelativeFramePoses的数据，以遍历的方式计算每一帧的姿态。在该过程中：
1）.首先计算全局坐标系与参考关键帧之间的变换关系Trw。
2）.计算当前帧相对于全局坐标系的变换关系Tcw。注：Tcw是全局坐标系到相机坐标系的变换，而最终要求的结果是相机坐标系到全局坐标系的变换关系。两者互为逆运算。
3）.根据Tcw计算相机坐标系到全局坐标系的旋转矩阵Rwc和平移向量twc。
So, 在前人的文章中有一个思维导图可以很直观的反映出整个代码的结构
包括4个部分：
- **载入图像 ：**根据命令行输入的参数，载入在相关文件夹下图像和时间戳。LoadImages(string(argv[3]), vstrImageLeft, vstrImageRight, vTimestamps);*注 ：该过程只是将图像的名称保存在相应的vector变量中。
2.**创建SLAM系统：**根据命令行输入的参数创建SLAM系统并进行系统初始化。ORB_SLAM2::System SLAM(argv[1],argv[2],ORB_SLAM2::System::STEREO,true);其中：argv[1]表示词袋文件的路径，argv[2]表示配置文件的路径，ORB_SLAM2::System::STEREO表示创建SLAM系统为双目SLAM系统，true表示进行可视化操作。
3.**传入图像进行追踪 ：**通过一个循环过程将图像传入SLAM系统中进行追踪。SLAM.TrackStereo(imLeft,imRight,tframe);
4.**停止SLAM系统并保存轨迹：**通过Shutdown()函数停止SLAM系统中的所有线程，利用SaveTrajectoryKITTI()函数来保存轨迹
在主接口System.cc中
C++知识的补充
Vector是向量类型，可以容纳许多类型的数据，因此也被称为容器（可以理解为封装好了的动态数组）
进行vector操作前应添加头文件#include <vector>
vector初始化有5种方式
- //定义具有10个整型元素的向量（尖括号为元素类型名，它可以是任何合法的数据类型），不具有初值，其值不确定
vectora(10); - //定义具有10个整型元素的向量，且给出的每个元素初值为1
vectora(10,1); - //用向量b给向量a赋值，a的值完全等价于b的值
vectora(b); - vectora(b.begin(),b.begin+3);
- //从数组中获得初值
int b[7]={1,2,3,4,5,6,7};
vector a(b,b+7）;
vector对象的常用内置函数使用
#include<vector>
vector<int> a,b;
//b为向量，将b的0-2个元素赋值给向量a
a.assign(b.begin(),b.begin()+3);
//a含有4个值为2的元素
a.assign(4,2);
//返回a的最后一个元素
a.back();
//返回a的第一个元素
a.front();
//返回a的第i元素,当且仅当a存在
a[i];
//清空a中的元素
a.clear();
//判断a是否为空，空则返回true，非空则返回false
a.empty();
//删除a向量的最后一个元素
a.pop_back();
//删除a中第一个（从第0个算起）到第二个元素，也就是说删除的元素从a.begin()+1算起（包括它）一直到a.begin()+3（不包括它）结束
a.erase(a.begin()+1,a.begin()+3);
//在a的最后一个向量后插入一个元素，其值为5
a.push_back(5);
//在a的第一个元素（从第0个算起）位置插入数值5,
a.insert(a.begin()+1,5);
//在a的第一个元素（从第0个算起）位置插入3个数，其值都为5
a.insert(a.begin()+1,3,5);
//b为数组，在a的第一个元素（从第0个元素算起）的位置插入b的第三个元素到第5个元素（不包括b+6）
a.insert(a.begin()+1,b+3,b+6);
//返回a中元素的个数
a.size();
//返回a在内存中总共可以容纳的元素个数
a.capacity();
//将a的现有元素个数调整至10个，多则删，少则补，其值随机
a.resize(10);
//将a的现有元素个数调整至10个，多则删，少则补，其值为2
a.resize(10,2);
//将a的容量扩充至100，
a.reserve(100);
//b为向量，将a中的元素和b中的元素整体交换
a.swap(b);
//b为向量，向量的比较操作还有 != >= > <= <
a==b;
mono_kitti的示例代码
/**
* This file is part of ORB-SLAM2.
*
* Copyright (C) 2014-2016 Raúl Mur-Artal <raulmur at unizar dot es> (University of Zaragoza)
* For more information see <https://github.com/raulmur/ORB_SLAM2>
*
* 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 <http://www.gnu.org/licenses/>.
*/
#include<iostream>
#include<algorithm>
#include<fstream>
#include<chrono>
#include<iomanip>
#include<opencv2/core/core.hpp>
#include"System.h"
using namespace std;
void LoadImages(const string &strSequence, vector<string> &vstrImageFilenames,
vector<double> &vTimestamps); //图片的读取函数申明
int main(int argc, char **argv)
{
if(argc != 4)
{
cerr << endl << "Usage: ./mono_kitti path_to_vocabulary path_to_settings path_to_sequence" << endl;
return 1;
}
// Retrieve paths to images
vector<string> vstrImageFilenames;
vector<double> vTimestamps;
LoadImages(string(argv[3]), vstrImageFilenames, vTimestamps);//图片的提取，argv[3]所指向的strsequence地址，将图像名传入vector向量类型的vstrImageFilenames（字符串）
int nImages = vstrImageFilenames.size();//读取vstrImagefilenames数组的个数 number of Images
// Create SLAM system. It initializes all system threads and gets ready to process frames.
ORB_SLAM2::System SLAM(argv[1],argv[2],ORB_SLAM2::System::MONOCULAR,true);
//ORB_SLAM2::System SLAM(argv[1],argv[2],ORB_SLAM2::System::STEREO,true);其中：argv[1]表示词袋文件的路径，argv[2]表示配置文件的路径
// Vector for tracking time statistics
vector<float> vTimesTrack;
vTimesTrack.resize(nImages);//将vector向量数组vTimesTrack容量扩充到图片的数量
cout << endl << "-------" << endl;
cout << "Start processing sequence ..." << endl;
cout << "Images in the sequence: " << nImages << endl << endl;
//到此为止图片load完成
// Main loop
cv::Mat im; //构造一个矩阵
for(int ni=0; ni<nImages; ni++)
{
// Read image from file
im = cv::imread(vstrImageFilenames[ni],CV_LOAD_IMAGE_UNCHANGED);//将图片信息存储到矩阵中
double tframe = vTimestamps[ni];
if(im.empty())
{
cerr << endl << "Failed to load image at: " << vstrImageFilenames[ni] << endl;
return 1;
}
#ifdef COMPILEDWITHC11
std::chrono::steady_clock::time_point t1 = std::chrono::steady_clock::now();
#else
std::chrono::monotonic_clock::time_point t1 = std::chrono::monotonic_clock::now();
#endif
// Pass the image to the SLAM system
SLAM.TrackMonocular(im,tframe);
#ifdef COMPILEDWITHC11
std::chrono::steady_clock::time_point t2 = std::chrono::steady_clock::now();
#else
std::chrono::monotonic_clock::time_point t2 = std::chrono::monotonic_clock::now();
#endif
double ttrack= std::chrono::duration_cast<std::chrono::duration<double> >(t2 - t1).count();
vTimesTrack[ni]=ttrack;
// Wait to load the next frame
double T=0;
if(ni<nImages-1)
T = vTimestamps[ni+1]-tframe;
else if(ni>0)
T = tframe-vTimestamps[ni-1];
if(ttrack<T)
usleep((T-ttrack)*1e6);
}
// Stop all threads
SLAM.Shutdown();
// Tracking time statistics
sort(vTimesTrack.begin(),vTimesTrack.end());
float totaltime = 0;
for(int ni=0; ni<nImages; ni++)
{
totaltime+=vTimesTrack[ni];
}
cout << "-------" << endl << endl;
cout << "median tracking time: " << vTimesTrack[nImages/2] << endl;
cout << "mean tracking time: " << totaltime/nImages << endl;
// Save camera trajectory
SLAM.SaveKeyFrameTrajectoryTUM("KeyFrameTrajectory.txt");
return 0;
}
void LoadImages(const string &strPathToSequence, vector<string> &vstrImageFilenames, vector<double> &vTimestamps)
{
ifstream fTimes;
string strPathTimeFile = strPathToSequence + "/times.txt";
fTimes.open(strPathTimeFile.c_str());
while(!fTimes.eof())
{
string s;
getline(fTimes,s);
if(!s.empty())
{
stringstream ss;
ss << s;
double t;
ss >> t;
vTimestamps.push_back(t);
}
}
string strPrefixLeft = strPathToSequence + "/image_0/";
const int nTimes = vTimestamps.size();
vstrImageFilenames.resize(nTimes);
for(int i=0; i<nTimes; i++)
{
stringstream ss;
ss << setfill('0') << setw(6) << i;
vstrImageFilenames[i] = strPrefixLeft + ss.str() + ".png";
}
}
标签：Mat,代码,元素,cv,SLAM2,ORB,坐标系 来源： https://blog.csdn.net/kakiru/article/details/120783337