fangcywangermu
8/16/2019 - 1:57 AM
ORBSLAM2
/**
* 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/>.
*/
#ifndef SYSTEM_H
#define SYSTEM_H
#include<string>
#include<thread>
#include<opencv2/core/core.hpp>
#include "Tracking.h"
#include "FrameDrawer.h"
#include "MapDrawer.h"
#include "Map.h"
#include "LocalMapping.h"
#include "LoopClosing.h"
#include "KeyFrameDatabase.h"
#include "ORBVocabulary.h"
#include "Viewer.h"
namespace ORB_SLAM2
{
class Viewer;
class FrameDrawer;
class Map;
class Tracking;
class LocalMapping;
class LoopClosing;
class System
{
public:
// Input sensor
//输入设备：单目、立体视觉、RGBD
enum eSensor{
MONOCULAR=0,
STEREO=1,
RGBD=2
};
public:
// Initialize the SLAM system. It launches the Local Mapping, Loop Closing and Viewer threads.
//对slam系统的初始化，包含局部地图、闭环检测、视图3个线程
System(const string &strVocFile, const string &strSettingsFile, const eSensor sensor, const bool bUseViewer = true);
// Proccess the given stereo frame. Images must be synchronized and rectified.
// Input images: RGB (CV_8UC3) or grayscale (CV_8U). RGB is converted to grayscale.
// Returns the camera pose (empty if tracking fails).
//处理被给的双目帧，两张图片必须是被同步且被纠正后的
//输入图片是RGB三通道的unsigned char型数据或者 灰度图像unsigned int型数据，RGB被转化为灰度图像
//返回的是相机位姿，如果线程失败则返回空
cv::Mat TrackStereo(const cv::Mat &imLeft, const cv::Mat &imRight, const double ×tamp);
// Process the given rgbd frame. Depthmap must be registered to the RGB frame.
// Input image: RGB (CV_8UC3) or grayscale (CV_8U). RGB is converted to grayscale.
// Input depthmap: Float (CV_32F).
// Returns the camera pose (empty if tracking fails).
//处理的是RGBD帧，深度图必须是和RGB图进行标定后的
//输入图片是RGB三通道的unsigned char型数据或者灰度图unsigned int型数据，RGB被转化为灰度图像
//输入深度地图为float型
//返回的是相机位姿，如果线程失败则返回空
cv::Mat TrackRGBD(const cv::Mat &im, const cv::Mat &depthmap, const double ×tamp);
// Proccess the given monocular frame
// Input images: RGB (CV_8UC3) or grayscale (CV_8U). RGB is converted to grayscale.
// Returns the camera pose (empty if tracking fails).
//处理被给的单目帧
//输入图片是RGB三通道的unsigned char型数据或者 灰度图像unsigned int型数据，RGB被转化为灰度图像
//返回的是相机位姿，如果线程失败则返回空
cv::Mat TrackMonocular(const cv::Mat &im, const double ×tamp);
// This stops local mapping thread (map building) and performs only camera tracking.
//这个函数停止局部建图线程并且仅仅执行相机捕捉线程
void ActivateLocalizationMode();
// This resumes local mapping thread and performs SLAM again.
// 这个函数重新开始局部建图线程并执行SLAM
void DeactivateLocalizationMode();
// Returns true if there have been a big map change (loop closure, global BA)
// since last call to this function
//与上次调用该函数时相比较地图是否发生了巨大的改变（回环检测或者全局BA），如果发生了巨大的改变返回真，否则为假
bool MapChanged();
// Reset the system (clear map)
//复位重启系统。。。清空地图
void Reset();
// All threads will be requested to finish.
// It waits until all threads have finished.
// This function must be called before saving the trajectory.
//在所有线程结束任务的时候关闭每个线程
//在保存轨迹之前必须调用此函数
void Shutdown();
// Save camera trajectory in the TUM RGB-D dataset format.
// Only for stereo and RGB-D. This method does not work for monocular.
// Call first Shutdown()
// See format details at: http://vision.in.tum.de/data/datasets/rgbd-dataset
// 保存相机轨迹到TUM RGBD数据集中
// 仅仅是RGBD和双目中使用，这个方法在单目中无效
// 调用之前首先调用关闭程序
void SaveTrajectoryTUM(const string &filename);
// Save keyframe poses in the TUM RGB-D dataset format.
// This method works for all sensor input.
// Call first Shutdown()
// See format details at: http://vision.in.tum.de/data/datasets/rgbd-dataset
// 保存关键帧的位姿到TUM RGBD数据集
// 这种方法在所有传感器输入下都有效
// 调用之前首先调用关闭程序
void SaveKeyFrameTrajectoryTUM(const string &filename);
// Save camera trajectory in the KITTI dataset format.
// Only for stereo and RGB-D. This method does not work for monocular.
// Call first Shutdown()
// See format details at: http://www.cvlibs.net/datasets/kitti/eval_odometry.php
// 保存相机轨迹到KITTI数据集中
// 仅仅是RGBD和双目中使用，这个方法在单目中无效
// 调用之前首先调用关闭程序
void SaveTrajectoryKITTI(const string &filename);
// TODO: Save/Load functions
// SaveMap(const string &filename);
// LoadMap(const string &filename);
// Information from most recent processed frame
// You can call this right after TrackMonocular (or stereo or RGBD)
int GetTrackingState(); // 得到最近进程帧的信息
// 调用这个函数在TrackMonocular，TrackRGBD，TrackStereo函数之后
std::vector<MapPoint*> GetTrackedMapPoints(); //得到追踪线程中的地图点
std::vector<cv::KeyPoint> GetTrackedKeyPointsUn(); //得到追踪线程中的关键点
private:
// Input sensor
//输入传感器
eSensor mSensor;
// ORB vocabulary used for place recognition and feature matching.
//ORB字典 被用来识别和特征匹配
ORBVocabulary* mpVocabulary;
// KeyFrame database for place recognition (relocalization and loop detection).
// 关键帧数据集被用来识别（重定位和回环检测）
KeyFrameDatabase* mpKeyFrameDatabase;
// Map structure that stores the pointers to all KeyFrames and MapPoints.
// 地图构建，存储指针指向所有的关键帧和地图点
Map* mpMap;
// Tracker. It receives a frame and computes the associated camera pose.
// It also decides when to insert a new keyframe, create some new MapPoints and
// performs relocalization if tracking fails.
//接受到一帧数据并且计算相机位姿之间的联系
//它也可以插入一个新帧，创建许多新的地图点
//如果数据跑丢会执行重定位
Tracking* mpTracker;
// Local Mapper. It manages the local map and performs local bundle adjustment.
//局部地图，管理局部地图并执行局部优化
LocalMapping* mpLocalMapper;
// Loop Closer. It searches loops with every new keyframe. If there is a loop it performs
// a pose graph optimization and full bundle adjustment (in a new thread) afterwards.
//回环检测，对于每一个新的帧他都会查找是否有回环 如果存在回环他会执行一个位姿图优化并且在新的线程中会进行全局优化
LoopClosing* mpLoopCloser;
// The viewer draws the map and the current camera pose. It uses Pangolin.
// 这个观测器观测了地图和当前相机位姿，他使用了Pangolin库
Viewer* mpViewer;
FrameDrawer* mpFrameDrawer; //每一帧的可视化
MapDrawer* mpMapDrawer; //整个地图的可视化
// System threads: Local Mapping, Loop Closing, Viewer.
// The Tracking thread "lives" in the main execution thread that creates the System object.
std::thread* mptLocalMapping; //局部建图线程
std::thread* mptLoopClosing; //回环检测线程
std::thread* mptViewer; //可视化线程
// Reset flag
// 复位标志位
std::mutex mMutexReset; //复位锁
bool mbReset; //复位标志
// Change mode flags
std::mutex mMutexMode; //改变模式的锁
bool mbActivateLocalizationMode; //激活仅定位模式，暂停局部建图线程
bool mbDeactivateLocalizationMode; //关闭仅定位模式，激活局部建图线程
// Tracking state
int mTrackingState; // 追踪线程运行状态
std::vector<MapPoint*> mTrackedMapPoints; //追踪线程中的地图点
std::vector<cv::KeyPoint> mTrackedKeyPointsUn; //追踪线程中的关键点
std::mutex mMutexState; // 线程状态锁
};
}// namespace ORB_SLAM
#endif // SYSTEM_H
/**
* 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 "System.h"
#include "Converter.h"
#include <thread>
#include <pangolin/pangolin.h>
#include <iomanip>
namespace ORB_SLAM2
{
* strVocFile：指向字典的路径
* strSettingsFile：指向参数配置文件的路径
* sensor：所使用传感器的种类
* bUseViewer：是否打开实时观测标志
* 函数功能： （1）将配置文件读入到内存中，存储到fsSettings变量中
* （2）将字典读入mpVocabulary变量中
* （3）创建关键帧数据集
* （4）创建全局地图
* （5）初始化观测器（关键帧观测器、地图观测器）、图像追踪器、局部地图构建器、回环检测器、
* （6）四个线程初始化（追踪线程（主线程）、局部地图构建线程、回环检测线程、观测器线程）
* （7）四个线程之间建立联系
System::System(const string &strVocFile, const string &strSettingsFile, const eSensor sensor,
const bool bUseViewer):mSensor(sensor), mpViewer(static_cast<Viewer*>(NULL)), mbReset(false),mbActivateLocalizationMode(false),
mbDeactivateLocalizationMode(false)
{
// Output welcome message
cout << endl <<
"ORB-SLAM2 Copyright (C) 2014-2016 Raul Mur-Artal, University of Zaragoza." << endl <<
"This program comes with ABSOLUTELY NO WARRANTY;" << endl <<
"This is free software, and you are welcome to redistribute it" << endl <<
"under certain conditions. See LICENSE.txt." << endl << endl;
cout << "Input sensor was set to: ";
if(mSensor==MONOCULAR)
cout << "Monocular" << endl;
else if(mSensor==STEREO)
cout << "Stereo" << endl;
else if(mSensor==RGBD)
cout << "RGB-D" << endl;
//Check settings file
//将配置文件读入到内存中，存储到fsSettings变量中
//相机内参、帧率、双目深度阈值等等，类似XXX.yaml这种文件类型
cv::FileStorage fsSettings(strSettingsFile.c_str(), cv::FileStorage::READ);
if(!fsSettings.isOpened())
{
cerr << "Failed to open settings file at: " << strSettingsFile << endl;
exit(-1);
}
//Load ORB Vocabulary
cout << endl << "Loading ORB Vocabulary. This could take a while..." << endl;
mpVocabulary = new ORBVocabulary();
bool bVocLoad = mpVocabulary->loadFromTextFile(strVocFile); //将字典加载到mpVocabulary中
if(!bVocLoad)
{
cerr << "Wrong path to vocabulary. " << endl;
cerr << "Falied to open at: " << strVocFile << endl;
exit(-1);
}
cout << "Vocabulary loaded!" << endl << endl;
//Create KeyFrame Database
//创建关键帧数据集
mpKeyFrameDatabase = new KeyFrameDatabase(*mpVocabulary);
//Create the Map
//创建地图
mpMap = new Map();
//Create Drawers. These are used by the Viewer
//创建观测器：帧的观测器和地图观测器
//创建显示窗口，一个是每张图片的显示，一个是地图
mpFrameDrawer = new FrameDrawer(mpMap);
mpMapDrawer = new MapDrawer(mpMap, strSettingsFile);
//Initialize the Tracking thread
//(it will live in the main thread of execution, the one that called this constructor)
//初始化Tracking线程
mpTracker = new Tracking(this, mpVocabulary, mpFrameDrawer, mpMapDrawer,
mpMap, mpKeyFrameDatabase, strSettingsFile, mSensor);
//Initialize the Local Mapping thread and launch
//初始化Local Mapping线程并启动
mpLocalMapper = new LocalMapping(mpMap, mSensor==MONOCULAR);
mptLocalMapping = new thread(&ORB_SLAM2::LocalMapping::Run,mpLocalMapper);
//Initialize the Loop Closing thread and launch
//初始化回环检测线程并启动
mpLoopCloser = new LoopClosing(mpMap, mpKeyFrameDatabase, mpVocabulary, mSensor!=MONOCULAR);
mptLoopClosing = new thread(&ORB_SLAM2::LoopClosing::Run, mpLoopCloser);
//Initialize the Viewer thread and launch
//初始化观测器线程并启动
if(bUseViewer)
{
mpViewer = new Viewer(this, mpFrameDrawer,mpMapDrawer,mpTracker,strSettingsFile);
mptViewer = new thread(&Viewer::Run, mpViewer);
mpTracker->SetViewer(mpViewer);
}
//Set pointers between threads
//设置线程之间的指针，使得Tracker、LoopCloser、LocalMapper三者可以互相访问
mpTracker->SetLocalMapper(mpLocalMapper);
mpTracker->SetLoopClosing(mpLoopCloser);
mpLocalMapper->SetTracker(mpTracker);
mpLocalMapper->SetLoopCloser(mpLoopCloser);
mpLoopCloser->SetTracker(mpTracker);
mpLoopCloser->SetLocalMapper(mpLocalMapper);
}
* 函数功能： （1）检测传感器类型
* （2）核查系统运行模式是否改变，有两种运行模式：仅追踪模式和同步定位与地图构建模式
* （3）检查是否需要复位
* （4）计算相机的变换矩阵，开启追踪线程
* （5）将运行线程的结果进行保存，包括线程状态、地图点、关键点
* im：彩色图像
* depthmap：深度图像
* timestamp：该帧数据获取时间戳
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); //使用mMutexReset锁
if(mbReset)
{
mpTracker->Reset();
mbReset = false;
}
}
//计算相机的变换矩阵T，追踪线程基础
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;
}
cv::Mat System::TrackRGBD(const cv::Mat &im, const cv::Mat &depthmap, const double ×tamp) //运行系统为RGBD
{
if(mSensor!=RGBD)
{
cerr << "ERROR: you called TrackRGBD but input sensor was not set to RGBD." << endl;
exit(-1);
}
// Check mode change
{
unique_lock<mutex> lock(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->GrabImageRGBD(im,depthmap,timestamp);
unique_lock<mutex> lock2(mMutexState);
mTrackingState = mpTracker->mState;
mTrackedMapPoints = mpTracker->mCurrentFrame.mvpMapPoints;
mTrackedKeyPointsUn = mpTracker->mCurrentFrame.mvKeysUn;
return Tcw;
}
cv::Mat System::TrackMonocular(const cv::Mat &im, const double ×tamp) //运行系统为单目
{
if(mSensor!=MONOCULAR)
{
cerr << "ERROR: you called TrackMonocular but input sensor was not set to Monocular." << endl;
exit(-1);
}
// Check mode change
{
unique_lock<mutex> lock(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->GrabImageMonocular(im,timestamp);
unique_lock<mutex> lock2(mMutexState);
mTrackingState = mpTracker->mState;
mTrackedMapPoints = mpTracker->mCurrentFrame.mvpMapPoints;
mTrackedKeyPointsUn = mpTracker->mCurrentFrame.mvKeysUn;
return Tcw;
}
//激活仅定位模式，关闭局部建图线程
void System::ActivateLocalizationMode()
{
unique_lock<mutex> lock(mMutexMode);
mbActivateLocalizationMode = true;
}
//关闭仅定位模式，开启局部建图线程
void System::DeactivateLocalizationMode()
{
unique_lock<mutex> lock(mMutexMode);
mbDeactivateLocalizationMode = true;
}
//地图是否进行修改
bool System::MapChanged()
{
static int n=0;
int curn = mpMap->GetLastBigChangeIdx();
if(n<curn)
{
n=curn;
return true;
}
else
return false;
}
//系统复位
void System::Reset()
{
unique_lock<mutex> lock(mMutexReset); //复位锁
mbReset = true; //复位标志量
}
//系统关闭
void System::Shutdown()
{
mpLocalMapper->RequestFinish();
mpLoopCloser->RequestFinish();
if(mpViewer)
{
mpViewer->RequestFinish();
while(!mpViewer->isFinished())
usleep(5000);
}
// Wait until all thread have effectively stopped
while(!mpLocalMapper->isFinished() || !mpLoopCloser->isFinished() || mpLoopCloser->isRunningGBA())
{
usleep(5000);
}
if(mpViewer)
pangolin::BindToContext("ORB-SLAM2: Map Viewer");
}
//如果运行的是TUM数据集，保存其轨迹
void System::SaveTrajectoryTUM(const string &filename)
{
cout << endl << "Saving camera trajectory to " << filename << " ..." << endl;
if(mSensor==MONOCULAR)
{
cerr << "ERROR: SaveTrajectoryTUM 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).
list<ORB_SLAM2::KeyFrame*>::iterator lRit = mpTracker->mlpReferences.begin();
list<double>::iterator lT = mpTracker->mlFrameTimes.begin();
list<bool>::iterator lbL = mpTracker->mlbLost.begin();
for(list<cv::Mat>::iterator lit=mpTracker->mlRelativeFramePoses.begin(),
lend=mpTracker->mlRelativeFramePoses.end();lit!=lend;lit++, lRit++, lT++, lbL++)
{
if(*lbL)
continue;
KeyFrame* pKF = *lRit;
cv::Mat Trw = cv::Mat::eye(4,4,CV_32F);
// If the reference keyframe was culled, traverse the spanning tree to get a suitable keyframe.
while(pKF->isBad())
{
Trw = Trw*pKF->mTcp;
pKF = pKF->GetParent();
}
Trw = Trw*pKF->GetPose()*Two;
cv::Mat Tcw = (*lit)*Trw;
cv::Mat Rwc = Tcw.rowRange(0,3).colRange(0,3).t();
cv::Mat twc = -Rwc*Tcw.rowRange(0,3).col(3);
vector<float> q = Converter::toQuaternion(Rwc);
f << setprecision(6) << *lT << " " << setprecision(9) << twc.at<float>(0) << " " << twc.at<float>(1) << " " << twc.at<float>(2) << " " << q[0] << " " << q[1] << " " << q[2] << " " << q[3] << endl;
}
f.close();
cout << endl << "trajectory saved!" << endl;
}
//如果运行的是TUM数据集，保存其关键帧轨迹
void System::SaveKeyFrameTrajectoryTUM(const string &filename)
{
cout << endl << "Saving keyframe trajectory to " << filename << " ..." << endl;
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;
for(size_t i=0; i<vpKFs.size(); i++)
{
KeyFrame* pKF = vpKFs[i];
// pKF->SetPose(pKF->GetPose()*Two);
if(pKF->isBad())
continue;
cv::Mat R = pKF->GetRotation().t();
vector<float> q = Converter::toQuaternion(R);
cv::Mat t = pKF->GetCameraCenter();
f << setprecision(6) << pKF->mTimeStamp << setprecision(7) << " " << t.at<float>(0) << " " << t.at<float>(1) << " " << t.at<float>(2)
<< " " << q[0] << " " << q[1] << " " << q[2] << " " << q[3] << endl;
}
f.close();
cout << endl << "trajectory saved!" << endl;
}
//如果运行的是KITTI数据集，保存其轨迹
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).
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;
cv::Mat Rwc = Tcw.rowRange(0,3).colRange(0,3).t();
cv::Mat twc = -Rwc*Tcw.rowRange(0,3).col(3);
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;
}
//返回追踪线程状态
int System::GetTrackingState()
{
unique_lock<mutex> lock(mMutexState);
return mTrackingState;
}
//返回追踪线程地图点
vector<MapPoint*> System::GetTrackedMapPoints()
{
unique_lock<mutex> lock(mMutexState);
return mTrackedMapPoints;
}
//返回追踪线程的关键点
vector<cv::KeyPoint> System::GetTrackedKeyPointsUn()
{
unique_lock<mutex> lock(mMutexState);
return mTrackedKeyPointsUn;
}
} //namespace ORB_SLAM