概要
ORB-SLAM3 は、純粋な視覚 (ビジュアル) データ処理、視覚 + 慣性 (ビジュアル - 慣性) データ処理、およびマルチマップ (マルチマップ) 機能を初めて備え、単眼、双眼、および RGB-D カメラをサポートします。 SLAM方式のピンホールカメラと魚眼カメラモデルをサポート。
主な革新ポイント:
1. IMU の初期化フェーズで MAP を導入します。この初期化方法はリアルタイムで迅速に実行でき、堅牢性が大幅に向上し (屋内または屋外の環境に関係なく、大小のシーンでパフォーマンスが向上)、他の現在の方法よりも 2 ～ 5 倍優れています。 。
2. 再現率が大幅に向上した場所認識手法に基づいたマルチマップ システムを実装しました (つまり、ループバック検出を実行し、履歴データとの接続を確立します)。ORB-SLAM3 は視覚情報がない場合の長期堅牢性が高く、視覚情報が失われた場合、この時点でサブマップが再構築され、ループバック時に以前のサブマップとマージされます。ORB-SLAM3は、史上初の全アルゴリズムモジュールの全情報を再利用できるシステムです。
主な結論:
すべてのセンサー構成において、ORB-SLAM3 の堅牢性は現在公開されている主要なシステムと同等であり、精度は大幅に向上しています。特にステレオ慣性 SLAM を使用すると、EuRoC データセットの平均誤差は 3.6 cm 近くになり、AR/VR シーンに偏った TUM-VI データセットの平均誤差は 9 mm 近くになります。
ORB_SLAM3システムブロック図
ORB_SLAM3 スタートアップ エントリは、単眼、双眼、RGB-D、およびそれらの慣性の組み合わせを含む Examples フォルダーにあります。この記事では、双眼 + 慣性を例として、stereo_inertial_realsense_D435i の起動プロセスを紹介します。
起動
./Examples/Stereo-Inertial/stereo_inertial_realsense_D435i ./Vocabulary/ORBvoc.txt ./Examples/Stereo-Inertial/Realsense_D435i.yaml
起動パラメータからわかるように、パラメータの数は argc = 3、パラメータ ./Examples/Stereo-Inertial/stereo_inertial_realsense_D435i は、stereo_inertial_realsense_D435i main() 関数のエントリ、./Vocabulary/ORBvoc.txt はトレーニング済みの ORBvoc 辞書です。 , パラメーター ./Examples/ Stereo-Inertial/Realsense_D435i.yaml は D435i の設定ファイルです。ご自身の D435i に合わせてカメラの内部パラメーター、外部パラメーター、および imu データを変更できます。詳細については、一連のブログ投稿を参照してください。Realsense d435i ドライバーのインストール、構成、およびキャリブレーション
main関数に入った後
1. librealsense2 rs インターフェイスを使用して d435i ライン上のデバイスを読み取り、読み取りハンドルをセンサーに設定します。
...
string file_name;
if (argc == 4) {
file_name = string(argv[argc - 1]);
}
struct sigaction sigIntHandler;
sigIntHandler.sa_handler = exit_loop_handler;
sigemptyset(&sigIntHandler.sa_mask);
sigIntHandler.sa_flags = 0;
sigaction(SIGINT, &sigIntHandler, NULL);
b_continue_session = true;
double offset = 0; // ms
rs2::context ctx;
rs2::device_list devices = ctx.query_devices();
rs2::device selected_device;
if (devices.size() == 0)
{
std::cerr << "No device connected, please connect a RealSense device" << std::endl;
return 0;
}
else
selected_device = devices[0];
std::vector<rs2::sensor> sensors = selected_device.query_sensors();
int index = 0;
2. センサーのタイプと名前に応じて、自動露出補正の有効化、露出制限、ストラクチャード ライトの終了など、対応する設定を行います。
for (rs2::sensor sensor : sensors)
if (sensor.supports(RS2_CAMERA_INFO_NAME)) {
++index;
if (index == 1) {
sensor.set_option(RS2_OPTION_ENABLE_AUTO_EXPOSURE, 1);
sensor.set_option(RS2_OPTION_AUTO_EXPOSURE_LIMIT,5000);
sensor.set_option(RS2_OPTION_EMITTER_ENABLED, 0); // switch off emitter
}
get_sensor_option(sensor);
if (index == 2){
// RGB camera (not used here...)
sensor.set_option(RS2_OPTION_EXPOSURE,100.f);
}
if (index == 3){
sensor.set_option(RS2_OPTION_ENABLE_MOTION_CORRECTION,0);
}
}
3. この部分では、カメラの左右の画像のサイズが 640*480、フレーム レートが 30hz、アクセル/ジャイロのデータ タイプが float に設定されていることがわかります。
// Declare RealSense pipeline, encapsulating the actual device and sensors
rs2::pipeline pipe;
// Create a configuration for configuring the pipeline with a non default profile
rs2::config cfg;
cfg.enable_stream(RS2_STREAM_INFRARED, 1, 640, 480, RS2_FORMAT_Y8, 30);
cfg.enable_stream(RS2_STREAM_INFRARED, 2, 640, 480, RS2_FORMAT_Y8, 30);
cfg.enable_stream(RS2_STREAM_ACCEL, RS2_FORMAT_MOTION_XYZ32F);
cfg.enable_stream(RS2_STREAM_GYRO, RS2_FORMAT_MOTION_XYZ32F);
4. 主に左右のカメラ データ cam_left/cam_right、imu データ imu_stream の左カメラの外部参照 Rbc/tbc、右の外部参照 Rbc/tbc、左右の内部参照 intrinsics_left/intrinsics_right を含む d435i センサー データを読み取ります。
auto imu_callback = [&](const rs2::frame& frame)
{
std::unique_lock<std::mutex> lock(imu_mutex);
if(rs2::frameset fs = frame.as<rs2::frameset>())
{
count_im_buffer++;
double new_timestamp_image = fs.get_timestamp()*1e-3;
if(abs(timestamp_image-new_timestamp_image)<0.001){
// cout << "Two frames with the same timeStamp!!!\n";
count_im_buffer--;
return;
}
rs2::video_frame ir_frameL = fs.get_infrared_frame(1);
rs2::video_frame ir_frameR = fs.get_infrared_frame(2);
imCV = cv::Mat(cv::Size(width_img, height_img), CV_8U, (void*)(ir_frameL.get_data()), cv::Mat::AUTO_STEP);
imRightCV = cv::Mat(cv::Size(width_img, height_img), CV_8U, (void*)(ir_frameR.get_data()), cv::Mat::AUTO_STEP);
timestamp_image = fs.get_timestamp()*1e-3;
image_ready = true;
while(v_gyro_timestamp.size() > v_accel_timestamp_sync.size())
{
int index = v_accel_timestamp_sync.size();
double target_time = v_gyro_timestamp[index];
v_accel_data_sync.push_back(current_accel_data);
v_accel_timestamp_sync.push_back(target_time);
}
lock.unlock();
cond_image_rec.notify_all();
}
else if (rs2::motion_frame m_frame = frame.as<rs2::motion_frame>())
{
if (m_frame.get_profile().stream_name() == "Gyro")
{
// It runs at 200Hz
v_gyro_data.push_back(m_frame.get_motion_data());
v_gyro_timestamp.push_back((m_frame.get_timestamp()+offset)*1e-3);
//rs2_vector gyro_sample = m_frame.get_motion_data();
//std::cout << "Gyro:" << gyro_sample.x << ", " << gyro_sample.y << ", " << gyro_sample.z << std::endl;
}
else if (m_frame.get_profile().stream_name() == "Accel")
{
// It runs at 60Hz
prev_accel_timestamp = current_accel_timestamp;
prev_accel_data = current_accel_data;
current_accel_data = m_frame.get_motion_data();
current_accel_timestamp = (m_frame.get_timestamp()+offset)*1e-3;
while(v_gyro_timestamp.size() > v_accel_timestamp_sync.size())
{
int index = v_accel_timestamp_sync.size();
double target_time = v_gyro_timestamp[index];
rs2_vector interp_data = interpolateMeasure(target_time, current_accel_data, current_accel_timestamp,
prev_accel_data, prev_accel_timestamp);
v_accel_data_sync.push_back(interp_data);
v_accel_timestamp_sync.push_back(target_time);
}
// std::cout << "Accel:" << current_accel_data.x << ", " << current_accel_data.y << ", " << current_accel_data.z << std::endl;
}
}
};
rs2::pipeline_profile pipe_profile = pipe.start(cfg, imu_callback);
vector<ORB_SLAM3::IMU::Point> vImuMeas;
rs2::stream_profile cam_left = pipe_profile.get_stream(RS2_STREAM_INFRARED, 1);
rs2::stream_profile cam_right = pipe_profile.get_stream(RS2_STREAM_INFRARED, 2);
rs2::stream_profile imu_stream = pipe_profile.get_stream(RS2_STREAM_GYRO);
float* Rbc = cam_left.get_extrinsics_to(imu_stream).rotation;
float* tbc = cam_left.get_extrinsics_to(imu_stream).translation;
std::cout << "Tbc (left) = " << std::endl;
for(int i = 0; i<3; i++){
for(int j = 0; j<3; j++)
std::cout << Rbc[i*3 + j] << ", ";
std::cout << tbc[i] << "\n";
}
float* Rlr = cam_right.get_extrinsics_to(cam_left).rotation;
float* tlr = cam_right.get_extrinsics_to(cam_left).translation;
std::cout << "Tlr = " << std::endl;
for(int i = 0; i<3; i++){
for(int j = 0; j<3; j++)
std::cout << Rlr[i*3 + j] << ", ";
std::cout << tlr[i] << "\n";
}
rs2_intrinsics intrinsics_left = cam_left.as<rs2::video_stream_profile>().get_intrinsics();
width_img = intrinsics_left.width;
height_img = intrinsics_left.height;
cout << "Left camera: \n";
std::cout << " fx = " << intrinsics_left.fx << std::endl;
std::cout << " fy = " << intrinsics_left.fy << std::endl;
std::cout << " cx = " << intrinsics_left.ppx << std::endl;
std::cout << " cy = " << intrinsics_left.ppy << std::endl;
std::cout << " height = " << intrinsics_left.height << std::endl;
std::cout << " width = " << intrinsics_left.width << std::endl;
std::cout << " Coeff = " << intrinsics_left.coeffs[0] << ", " << intrinsics_left.coeffs[1] << ", " <<
intrinsics_left.coeffs[2] << ", " << intrinsics_left.coeffs[3] << ", " << intrinsics_left.coeffs[4] << ", " << std::endl;
std::cout << " Model = " << intrinsics_left.model << std::endl;
rs2_intrinsics intrinsics_right = cam_right.as<rs2::video_stream_profile>().get_intrinsics();
width_img = intrinsics_right.width;
height_img = intrinsics_right.height;
cout << "Right camera: \n";
std::cout << " fx = " << intrinsics_right.fx << std::endl;
std::cout << " fy = " << intrinsics_right.fy << std::endl;
std::cout << " cx = " << intrinsics_right.ppx << std::endl;
std::cout << " cy = " << intrinsics_right.ppy << std::endl;
std::cout << " height = " << intrinsics_right.height << std::endl;
std::cout << " width = " << intrinsics_right.width << std::endl;
std::cout << " Coeff = " << intrinsics_right.coeffs[0] << ", " << intrinsics_right.coeffs[1] << ", " <<
intrinsics_right.coeffs[2] << ", " << intrinsics_right.coeffs[3] << ", " << intrinsics_right.coeffs[4] << ", " << std::endl;
std::cout << " Model = " << intrinsics_right.model << std::endl;
パート 1 からパート 4 を見ると、この部分は主に d435i とのやり取り (主に検出、設定、データの読み取りなど) であることがわかり、この部分は d435i ドライバー部分とみなすことができます。
5. スラム システムに入ります。これは主にシステム スレッドを初期化し、作業の準備をするために使用されます。
ORB_SLAM3::System SLAM(argv[1],argv[2],ORB_SLAM3::System::IMU_STEREO, true, 0, file_name);
/**
* @brief 系统的构造函数，将会启动其他的线程
* @param strVocFile 词袋文件所在路径
* @param strSettingsFile 配置文件所在路径
* @param sensor 传感器类型
* @param bUseViewer 是否使用可视化界面
* @param initFr initFr表示初始化帧的id,开始设置为0
* @param strSequence 序列名,在跟踪线程和局部建图线程用得到
*/
System::System(const string &strVocFile, const string &strSettingsFile, const eSensor sensor,
const bool bUseViewer, const int initFr, const string &strSequence):
1. ウェルカム情報を出力する
// Output welcome message
cout << endl <<
"ORB-SLAM3 Copyright (C) 2017-2020 Carlos Campos, Richard Elvira, Juan J. Gómez, José M.M. Montiel and Juan D. Tardós, University of Zaragoza." << endl <<
"ORB-SLAM2 Copyright (C) 2014-2016 Raúl Mur-Artal, José M.M. Montiel and Juan D. Tardós, 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: ";
2. 設定ファイルを読み込む
構成ファイルの読み取りは、./Examples/Stereo-Inertial/Realsense_D435i.yaml ファイルの開始に相当します。ファイル バージョンが異なると、mStrLoadAtlasFromFile と mStrSaveAtlasToFile も異なります。
//Check settings file
cv::FileStorage fsSettings(strSettingsFile.c_str(), cv::FileStorage::READ);
// 如果打开失败，就输出错误信息
if(!fsSettings.isOpened())
{
cerr << "Failed to open settings file at: " << strSettingsFile << endl;
exit(-1);
}
// 查看配置文件版本，不同版本有不同处理方法
cv::FileNode node = fsSettings["File.version"];
if(!node.empty() && node.isString() && node.string() == "1.0")
{
settings_ = new Settings(strSettingsFile,mSensor);
// 保存及加载地图的名字
mStrLoadAtlasFromFile = settings_->atlasLoadFile();
mStrSaveAtlasToFile = settings_->atlasSaveFile();
cout << (*settings_) << endl;
}
else
{
settings_ = nullptr;
cv::FileNode node = fsSettings["System.LoadAtlasFromFile"];
if(!node.empty() && node.isString())
{
mStrLoadAtlasFromFile = (string)node;
}
node = fsSettings["System.SaveAtlasToFile"];
if(!node.empty() && node.isString())
{
mStrSaveAtlasToFile = (string)node;
}
}
3. ループバックを有効にするかどうか
node = fsSettings["loopClosing"];
bool activeLC = true;
if(!node.empty())
{
activeLC = static_cast<int>(fsSettings["loopClosing"]) != 0;
}
4. 単語バッグ ファイルは、起動パラメータに対応する mStrVocabularyFilePath に割り当てられます。/Vocabulary/ORBvoc.txt
mStrVocabularyFilePath = strVocFile;
5. マルチマップ管理機能
mStrLoadAtlasFromFileファイルにAtlasがあるかどうかに応じて、対応する処理を行います。
(1) ファイルが存在する場合:
新しい ORB ディクショナリを作成し、事前トレーニングされた ORB ディクショナリを読み取り、成功/失敗フラグを返し、キーフレーム データベースを作成し、マルチマップを作成します; ###### ( 2) ファイルが存在しない場合
:
create新しい ORB ディクショナリ、事前トレーニングされた ORB ディクショナリを読み取って成功/失敗フラグを返す、キーフレーム データベースを作成する、キーフレーム データベースを作成する、アトラス マップをインポートする、新しいマップを作成する。
bool loadedAtlas = false;
if(mStrLoadAtlasFromFile.empty())
{
//Load ORB Vocabulary
cout << endl << "Loading ORB Vocabulary. This could take a while..." << endl;
// 建立一个新的ORB字典
mpVocabulary = new ORBVocabulary();
// 读取预训练好的ORB字典并返回成功/失败标志
bool bVocLoad = mpVocabulary->loadFromTextFile(strVocFile);
// 如果加载失败，就输出错误信息
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
// Step 4 创建关键帧数据库
mpKeyFrameDatabase = new KeyFrameDatabase(*mpVocabulary);
//Create the Atlas
// Step 5 创建多地图，参数0表示初始化关键帧id为0
cout << "Initialization of Atlas from scratch " << endl;
mpAtlas = new Atlas(0);
}
else
{
//Load ORB Vocabulary
cout << endl << "Loading ORB Vocabulary. This could take a while..." << endl;
mpVocabulary = new ORBVocabulary();
bool bVocLoad = mpVocabulary->loadFromTextFile(strVocFile);
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);
cout << "Load File" << endl;
// Load the file with an earlier session
//clock_t start = clock();
cout << "Initialization of Atlas from file: " << mStrLoadAtlasFromFile << endl;
bool isRead = LoadAtlas(FileType::BINARY_FILE);
if(!isRead)
{
cout << "Error to load the file, please try with other session file or vocabulary file" << endl;
exit(-1);
}
loadedAtlas = true;
mpAtlas->CreateNewMap();
}
6. この部分は imu データの有無に応じて初期化されます
// 如果是有imu的传感器类型，设置mbIsInertial = true;以后的跟踪和预积分将和这个标志有关
if (mSensor==IMU_STEREO || mSensor==IMU_MONOCULAR || mSensor==IMU_RGBD)
mpAtlas->SetInertialSensor();
7. トラッキング、ローカル マッピング、閉ループを作成し、スレッドを順番に表示します
(1) Viewerから呼び出される、フレームやマップを表示するためのクラスを作成する
//Create Drawers. These are used by the Viewer
mpFrameDrawer = new FrameDrawer(mpAtlas);
mpMapDrawer = new MapDrawer(mpAtlas, strSettingsFile, settings_);
(2) 追跡スレッド (メイン スレッド) を作成します。これはすぐには開かれませんが、画像と imu の前処理後にメインのメイン スレッドで実行されます (メイン)SLAM.TrackStereo()–>mpTracker->GrabImageStereo– >Track( ) でトレースをオンにします。
//Initialize the Tracking thread
//(it will live in the main thread of execution, the one that called this constructor)
cout << "Seq. Name: " << strSequence << endl;
mpTracker = new Tracking(this, mpVocabulary, mpFrameDrawer, mpMapDrawer, mpAtlas, mpKeyFrameDatabase, strSettingsFile, mSensor, settings_, strSequence);
(3) ローカル マッピング スレッド、スレッド エントリ LocalMapping::Run を作成して開始します。
//Initialize the Local Mapping thread and launch
mpLocalMapper = new LocalMapping(this, mpAtlas, mSensor==MONOCULAR || mSensor==IMU_MONOCULAR,
mSensor==IMU_MONOCULAR || mSensor==IMU_STEREO || mSensor==IMU_RGBD, strSequence);
mptLocalMapping = new thread(&ORB_SLAM3::LocalMapping::Run,mpLocalMapper);
mpLocalMapper->mInitFr = initFr;
(4) 最も遠い 3D マップ ポイントの深度値を設定します。閾値を超えた場合、三角測量が成功しない可能性があることを意味し、破棄します。
if(settings_)
mpLocalMapper->mThFarPoints = settings_->thFarPoints();
else
mpLocalMapper->mThFarPoints = fsSettings["thFarPoints"];
if(mpLocalMapper->mThFarPoints!=0)
{
cout << "Discard points further than " << mpLocalMapper->mThFarPoints << " m from current camera" << endl;
mpLocalMapper->mbFarPoints = true;
}
else
mpLocalMapper->mbFarPoints = false;
(5) 閉ループ スレッド、プログラム エントリ LoopCloing::Run を作成して開きます。
//Initialize the Loop Closing thread and launch
// mSensor!=MONOCULAR && mSensor!=IMU_MONOCULAR
mpLoopCloser = new LoopClosing(mpAtlas, mpKeyFrameDatabase, mpVocabulary, mSensor!=MONOCULAR, activeLC); // mSensor!=MONOCULAR);
mptLoopClosing = new thread(&ORB_SLAM3::LoopClosing::Run, mpLoopCloser);
(6) スレッド間ポインタの設定
//Set pointers between threads
mpTracker->SetLocalMapper(mpLocalMapper);
mpTracker->SetLoopClosing(mpLoopCloser);
mpLocalMapper->SetTracker(mpTracker);
mpLocalMapper->SetLoopCloser(mpLoopCloser);
mpLoopCloser->SetTracker(mpTracker);
mpLoopCloser->SetLocalMapper(mpLocalMapper);
(7) 表示スレッド、プログラム エントリ Viewer::Run を作成して開きます。
//Initialize the Viewer thread and launch
if(bUseViewer)
//if(false) // TODO
{
mpViewer = new Viewer(this, mpFrameDrawer,mpMapDrawer,mpTracker,strSettingsFile,settings_);
mptViewer = new thread(&Viewer::Run, mpViewer);
mpTracker->SetViewer(mpViewer);
mpLoopCloser->mpViewer = mpViewer;
mpViewer->both = mpFrameDrawer->both;
}
// Fix verbosity
// 打印输出中间的信息，设置为安静模式
Verbose::SetTh(Verbose::VERBOSITY_QUIET);
この時点で、Systemf は完了です。
6. この部分は imu データ ベクトルをクリアします
// Clear IMU vectors
v_gyro_data.clear();
v_gyro_timestamp.clear();
v_accel_data_sync.clear();
v_accel_timestamp_sync.clear();
7. while(!isShutDown()) メインループを開きます
8. タイムスタンプに基づいて加速度を補間する
while(v_gyro_timestamp.size() > v_accel_timestamp_sync.size())
{
int index = v_accel_timestamp_sync.size();
double target_time = v_gyro_timestamp[index];
rs2_vector interp_data = interpolateMeasure(target_time, current_accel_data, current_accel_timestamp, prev_accel_data, prev_accel_timestamp);
v_accel_data_sync.push_back(interp_data);
// v_accel_data_sync.push_back(current_accel_data); // 0 interpolation
v_accel_timestamp_sync.push_back(target_time);
}
9. データを vImuMeas にプッシュする
for(int i=0; i<vGyro.size(); ++i)
{
ORB_SLAM3::IMU::Point lastPoint(vAccel[i].x, vAccel[i].y, vAccel[i].z,
vGyro[i].x, vGyro[i].y, vGyro[i].z,
vGyro_times[i]);
vImuMeas.push_back(lastPoint);
}
10. 両眼追跡スレッドを正式にオープンする
// Stereo images are already rectified.
SLAM.TrackStereo(im, imRight, timestamp, vImuMeas);
ここまでで、stereo_inertial_realsense_D435i の起動処理がすべて完了しました。そのうち、追跡Track、ローカル マッピングLocalMapping、閉ループLoopCloing、および分子配列の追跡ビュー スレッドViewerについて詳しく説明します。
参考：
1.https：//blog.csdn.net/u010196944/article/details/128972333？spm = 1001.2014.3001.5501
2.https：//blog.csdn.net/u010196944/article/details/details/1127240169？ 1.5501