From c9e7619ef0e391881b2624fbbeb8a593f4e4d695 Mon Sep 17 00:00:00 2001 From: Alkaid Date: Mon, 9 Nov 2020 18:56:08 -0500 Subject: [PATCH 1/2] ADA Hackathon 2020, ORB_SLAM3 as an ILLIXR plugin. --- CMakeLists.txt | 11 +- .../ILLIXR_stereo_inertial_euroc.cc | 256 ++++++++++++++++++ 2 files changed, 265 insertions(+), 2 deletions(-) create mode 100644 Examples/Stereo-Inertial/ILLIXR_stereo_inertial_euroc.cc diff --git a/CMakeLists.txt b/CMakeLists.txt index 1f7c65bc49..ece0da91a3 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -17,9 +17,14 @@ set(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS_RELEASE} -march=native") # Check C++11 or C++0x support include(CheckCXXCompilerFlag) +CHECK_CXX_COMPILER_FLAG("-std=c++17" COMPILER_SUPPORTS_CXX17) CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11) CHECK_CXX_COMPILER_FLAG("-std=c++0x" COMPILER_SUPPORTS_CXX0X) -if(COMPILER_SUPPORTS_CXX11) +if (COMPILER_SUPPORTS_CXX17) + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++17") + add_definitions(-DCOMPILEDWITHC17) + message(STATUS "Using flag -std=c++17.") +elseif(COMPILER_SUPPORTS_CXX11) set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") add_definitions(-DCOMPILEDWITHC11) message(STATUS "Using flag -std=c++11.") @@ -191,4 +196,6 @@ add_executable(stereo_inertial_tum_vi Examples/Stereo-Inertial/stereo_inertial_tum_vi.cc) target_link_libraries(stereo_inertial_tum_vi ${PROJECT_NAME}) - +add_library(plugin SHARED Examples/Stereo-Inertial/ILLIXR_stereo_inertial_euroc.cc) +target_include_directories(plugin PRIVATE ./ILLIXR_common) +target_link_libraries(plugin PUBLIC ${PROJECT_NAME}) diff --git a/Examples/Stereo-Inertial/ILLIXR_stereo_inertial_euroc.cc b/Examples/Stereo-Inertial/ILLIXR_stereo_inertial_euroc.cc new file mode 100644 index 0000000000..3af46cf048 --- /dev/null +++ b/Examples/Stereo-Inertial/ILLIXR_stereo_inertial_euroc.cc @@ -0,0 +1,256 @@ +/** +* This file is part of ORB-SLAM3 +* +* Copyright (C) 2017-2020 Carlos Campos, Richard Elvira, Juan J. Gómez Rodríguez, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. +* Copyright (C) 2014-2016 Raúl Mur-Artal, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. +* +* ORB-SLAM3 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-SLAM3 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-SLAM3. +* If not, see . +*/ + +#include +#include +#include +#include +#include +#include +#include +#include + +#include + + +#include +#include "ImuTypes.h" +#include "Optimizer.h" + +/* + * ILLIXR related headers + */ +#include +#include +#include "plugin.hpp" +#include "switchboard.hpp" +#include "data_format.hpp" +#include "phonebook.hpp" + +using namespace std; +using ILLIXR::phonebook; +using ILLIXR::writer; +using ILLIXR::pose_type; +using ILLIXR::imu_integrator_input; +using ILLIXR::time_type; +using ILLIXR::imu_cam_type; + +std::string get_path() { + const char* ORB_SLAM3_ROOT_c_str = std::getenv("ORB_SLAM3_ROOT"); + if (!ORB_SLAM3_ROOT_c_str) { + std::cerr << "Please define ORB_SLAM3_ROOT" << std::endl; + abort(); + } + std::string ORB_SLAM3_ROOT = std::string{ORB_SLAM3_ROOT_c_str}; + return ORB_SLAM3_ROOT; +} + +class ORB_SLAM3_ILLIXR : public plugin { + private: + const std::shared_ptr sb; + std::unique_ptr> _m_pose; + std::unique_ptr> _m_imu_integrator_input; + time_type _m_begin; + const imu_cam_type *imu_cam_buffer; + double previous_timestamp = 0.0; + vector vImuMeas; + std::unique_ptr sys; + + cv::Mat M1l, M2l, M1r, M2r; + + public: + ORB_SLAM3_ILLIXR(std::string name_, phonebook *pb_) + : plugin{name_, pb_}, + sb{pb->lookup_impl()}, + _m_pose{sb->publish("slow_pose")}, + _m_imu_integrator_input{ + sb->publish("imu_integrator_input")} { + _m_begin = std::chrono::system_clock::now(); + imu_cam_buffer = NULL; + _m_pose->put(new pose_type{ + .sensor_time = std::chrono::time_point(), + .position = Eigen::Vector3f{0, 0, 0}, + .orientation = Eigen::Quaternionf{1, 0, 0, 0}}); +#ifdef CV_HAS_METRICS + cv::metrics::setAccount(new std::string{"-1"}); +#endif + std::string root = get_path(); + std::string voc_path = root + "Vocabulary/ORBvoc.txt"; + std::string settings_path = + root + "Examples/Stereo-Intertial/EuRoC.yaml"; + sys = std::make_unique( + voc_path, settings_path, ORB_SLAM3::System::IMU_STEREO, true); + + // Read rectification parameters + cv::FileStorage fsSettings(settings_path, cv::FileStorage::READ); + if (!fsSettings.isOpened()) { + cerr << "ERROR: Wrong path to settings" << endl; + return -1; + } + + cv::Mat K_l, K_r, P_l, P_r, R_l, R_r, D_l, D_r; + fsSettings["LEFT.K"] >> K_l; + fsSettings["RIGHT.K"] >> K_r; + + fsSettings["LEFT.P"] >> P_l; + fsSettings["RIGHT.P"] >> P_r; + + fsSettings["LEFT.R"] >> R_l; + fsSettings["RIGHT.R"] >> R_r; + + fsSettings["LEFT.D"] >> D_l; + fsSettings["RIGHT.D"] >> D_r; + + int rows_l = fsSettings["LEFT.height"]; + int cols_l = fsSettings["LEFT.width"]; + int rows_r = fsSettings["RIGHT.height"]; + int cols_r = fsSettings["RIGHT.width"]; + + if (K_l.empty() || K_r.empty() || P_l.empty() || P_r.empty() || + R_l.empty() || R_r.empty() || D_l.empty() || D_r.empty() || + rows_l == 0 || rows_r == 0 || cols_l == 0 || cols_r == 0) { + cerr << "ERROR: Calibration parameters to rectify stereo are missing!" + << endl; + return -1; + } + + cv::Mat M1l, M2l, M1r, M2r; + cv::initUndistortRectifyMap(K_l, D_l, R_l, + P_l.rowRange(0, 3).colRange(0, 3), + cv::Size(cols_l, rows_l), CV_32F, M1l, M2l); + cv::initUndistortRectifyMap(K_r, D_r, R_r, + P_r.rowRange(0, 3).colRange(0, 3), + cv::Size(cols_r, rows_r), CV_32F, M1r, M2r); + // TODO: init ORB_SLAM3 + } + + virtual void start() override { + plugin::start(); + sb->schedule(id, "imu_cam", [&](const imu_cam_type *datum) { + this->feed_imu_cam(datum); + }); + } + + std::size_t iteration_no = 0; + void feed_imu_cam(const imu_cam_type *dataum) { + // Ensures that slam doesnt start before valid IMU readings come in + if (datum == NULL) { + assert(previous_timestamp == 0); + return; + } + + // This ensures that every data point is coming in chronological order If youre failing this assert, + // make sure that your data folder matches the name in offline_imu_cam/plugin.cc + assert(datum->dataset_time > previous_timestamp); + previous_timestamp = datum->dataset_time; + + imu_cam_buffer = datum; + + VIO::Vector6 imu_raw_vals; + Eigen::Vector3f &acc = datum->linear_a; + Eigen::Vector3f &gyro = datum->angular_v; + ORB_SLAM3::IMU::Point ORB_p(acc(0), acc(1), acc(2), gyro(0), gyro(1), gyro(2), + previous_timestamp); + + // Feed the IMU measurement. There should always be IMU data in each + // call to feed_imu_cam + assert((datum->img0.has_value() && datum->img1.has_value()) || + (!datum->img0.has_value() && !datum->img1.has_value())); + vImuMeas.push_back(ORB_p); + +#ifdef CV_HAS_METRICS + cv::metrics::setAccount(new std::string{std::to_string(iteration_no)}); + iteration_no++; + if (iteration_no % 20 == 0) { + cv::metrics::dump(); + } +#else +#warning "No OpenCV metrics available. Please recompile OpenCV from git clone --branch 3.4.6-instrumented https://github.com/ILLIXR/opencv/. (see install_deps.sh)" +#endif + + std::optional imLeft = imu_cam_buffer->img0; + std::optional imRight = imu_cam_buffer->img1; + cv::Mat imLeftRect, imRightRect; + +#ifdef COMPILEDWITHC11 + std::chrono::steady_clock::time_point t_Start_Rect = + std::chrono::steady_clock::now(); +#else + std::chrono::monotonic_clock::time_point t_Start_Rect = + std::chrono::monotonic_clock::now(); +#endif + cv::remap(*imLeft, imLeftRect, M1l, M2l, cv::INTER_LINEAR); + cv::remap(*imRight, imRightRect, M1r, M2r, cv::INTER_LINEAR); + +#ifdef COMPILEDWITHC11 + std::chrono::steady_clock::time_point t_End_Rect = + std::chrono::steady_clock::now(); +#else + std::chrono::monotonic_clock::time_point t_End_Rect = + std::chrono::monotonic_clock::now(); +#endif + double t_rect = + std::chrono::duration_cast>( + t_End_Rect - t_Start_Rect) + .count(); + + double tframe = dataum->dataset_time; + + // If there is not cam data this func call, break early + if (!datum->img0.has_value() && !datum->img1.has_value()) { + return; + } else { +#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 images to the SLAM system + cv::Mat Tcw = + SLAM.TrackStereo(imLeftRect, imRightRect, tframe, vImuMeas); + +#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 + vImuMeas.clear(); + } + cv::Mat Rwc = Tcw.rowRange(0,3).colRange(0,3).t(); + cv::Mat twc = -Rwc*Tcw.rowRange(0,3).col(3) + Eigen::Vector3f position{twc.at(0), twc.at(1), + twc.at(2)};; + Eigen::Matrix3f emat3f; + cv2eigen(Rwc, emat3f); + Eigen::Quaternionf orientation(emat3f); + _m_pose->put(new pose_type{ + .sensor_time = imu_cam_buffer->time, + .position = position, + .orientation = orientation + }); + + } + +} + +PLUGIN_MAIN(ORB_SLAM3_ILLIXR); From a7054d4526c7d20b84d1705bd8943d95ebccf837 Mon Sep 17 00:00:00 2001 From: Alkaid Date: Mon, 9 Nov 2020 19:18:52 -0500 Subject: [PATCH 2/2] Fix compilation. Did not test correctness. Makefile can compile the target plugin.so but does not conform with ILLIXR interface. --- CMakeLists.txt | 4 +- .../ILLIXR_stereo_inertial_euroc.cc | 336 +++++++++--------- include/LoopClosing.h | 2 +- 3 files changed, 171 insertions(+), 171 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index ece0da91a3..e034aaac8b 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -21,8 +21,10 @@ CHECK_CXX_COMPILER_FLAG("-std=c++17" COMPILER_SUPPORTS_CXX17) CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11) CHECK_CXX_COMPILER_FLAG("-std=c++0x" COMPILER_SUPPORTS_CXX0X) if (COMPILER_SUPPORTS_CXX17) + # c++17 is for code compatibility with ILLIXIR + # and it also supports the c++11 features used in ORB_SLAM3 set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++17") - add_definitions(-DCOMPILEDWITHC17) + add_definitions(-DCOMPILEDWITHC11) message(STATUS "Using flag -std=c++17.") elseif(COMPILER_SUPPORTS_CXX11) set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") diff --git a/Examples/Stereo-Inertial/ILLIXR_stereo_inertial_euroc.cc b/Examples/Stereo-Inertial/ILLIXR_stereo_inertial_euroc.cc index 3af46cf048..2cb2bc1efa 100644 --- a/Examples/Stereo-Inertial/ILLIXR_stereo_inertial_euroc.cc +++ b/Examples/Stereo-Inertial/ILLIXR_stereo_inertial_euroc.cc @@ -43,12 +43,14 @@ #include "phonebook.hpp" using namespace std; +using ILLIXR::imu_cam_type; +using ILLIXR::imu_integrator_input; using ILLIXR::phonebook; -using ILLIXR::writer; +using ILLIXR::switchboard; +using ILLIXR::plugin; using ILLIXR::pose_type; -using ILLIXR::imu_integrator_input; using ILLIXR::time_type; -using ILLIXR::imu_cam_type; +using ILLIXR::writer; std::string get_path() { const char* ORB_SLAM3_ROOT_c_str = std::getenv("ORB_SLAM3_ROOT"); @@ -61,196 +63,192 @@ std::string get_path() { } class ORB_SLAM3_ILLIXR : public plugin { - private: - const std::shared_ptr sb; - std::unique_ptr> _m_pose; - std::unique_ptr> _m_imu_integrator_input; - time_type _m_begin; - const imu_cam_type *imu_cam_buffer; - double previous_timestamp = 0.0; - vector vImuMeas; - std::unique_ptr sys; - - cv::Mat M1l, M2l, M1r, M2r; - - public: - ORB_SLAM3_ILLIXR(std::string name_, phonebook *pb_) - : plugin{name_, pb_}, - sb{pb->lookup_impl()}, - _m_pose{sb->publish("slow_pose")}, - _m_imu_integrator_input{ - sb->publish("imu_integrator_input")} { - _m_begin = std::chrono::system_clock::now(); - imu_cam_buffer = NULL; - _m_pose->put(new pose_type{ - .sensor_time = std::chrono::time_point(), - .position = Eigen::Vector3f{0, 0, 0}, - .orientation = Eigen::Quaternionf{1, 0, 0, 0}}); + private: + const std::shared_ptr sb; + std::unique_ptr> _m_pose; + std::unique_ptr> _m_imu_integrator_input; + time_type _m_begin; + const imu_cam_type *imu_cam_buffer; + double previous_timestamp = 0.0; + vector vImuMeas; + std::unique_ptr SLAM_sys; + + cv::Mat M1l, M2l, M1r, M2r; + + public: + ORB_SLAM3_ILLIXR(std::string name_, phonebook *pb_) + : plugin{name_, pb_}, + sb{pb->lookup_impl()}, + _m_pose{sb->publish("slow_pose")}, + _m_imu_integrator_input{ + sb->publish("imu_integrator_input")} { + _m_begin = std::chrono::system_clock::now(); + imu_cam_buffer = NULL; + _m_pose->put(new pose_type{ + .sensor_time = std::chrono::time_point(), + .position = Eigen::Vector3f{0, 0, 0}, + .orientation = Eigen::Quaternionf{1, 0, 0, 0}}); #ifdef CV_HAS_METRICS - cv::metrics::setAccount(new std::string{"-1"}); + cv::metrics::setAccount(new std::string{"-1"}); #endif - std::string root = get_path(); - std::string voc_path = root + "Vocabulary/ORBvoc.txt"; - std::string settings_path = - root + "Examples/Stereo-Intertial/EuRoC.yaml"; - sys = std::make_unique( - voc_path, settings_path, ORB_SLAM3::System::IMU_STEREO, true); - - // Read rectification parameters - cv::FileStorage fsSettings(settings_path, cv::FileStorage::READ); - if (!fsSettings.isOpened()) { - cerr << "ERROR: Wrong path to settings" << endl; - return -1; - } - - cv::Mat K_l, K_r, P_l, P_r, R_l, R_r, D_l, D_r; - fsSettings["LEFT.K"] >> K_l; - fsSettings["RIGHT.K"] >> K_r; - - fsSettings["LEFT.P"] >> P_l; - fsSettings["RIGHT.P"] >> P_r; - - fsSettings["LEFT.R"] >> R_l; - fsSettings["RIGHT.R"] >> R_r; - - fsSettings["LEFT.D"] >> D_l; - fsSettings["RIGHT.D"] >> D_r; - - int rows_l = fsSettings["LEFT.height"]; - int cols_l = fsSettings["LEFT.width"]; - int rows_r = fsSettings["RIGHT.height"]; - int cols_r = fsSettings["RIGHT.width"]; - - if (K_l.empty() || K_r.empty() || P_l.empty() || P_r.empty() || - R_l.empty() || R_r.empty() || D_l.empty() || D_r.empty() || - rows_l == 0 || rows_r == 0 || cols_l == 0 || cols_r == 0) { - cerr << "ERROR: Calibration parameters to rectify stereo are missing!" - << endl; - return -1; - } - - cv::Mat M1l, M2l, M1r, M2r; - cv::initUndistortRectifyMap(K_l, D_l, R_l, - P_l.rowRange(0, 3).colRange(0, 3), - cv::Size(cols_l, rows_l), CV_32F, M1l, M2l); - cv::initUndistortRectifyMap(K_r, D_r, R_r, - P_r.rowRange(0, 3).colRange(0, 3), - cv::Size(cols_r, rows_r), CV_32F, M1r, M2r); - // TODO: init ORB_SLAM3 - } - - virtual void start() override { - plugin::start(); - sb->schedule(id, "imu_cam", [&](const imu_cam_type *datum) { - this->feed_imu_cam(datum); - }); - } - - std::size_t iteration_no = 0; - void feed_imu_cam(const imu_cam_type *dataum) { - // Ensures that slam doesnt start before valid IMU readings come in - if (datum == NULL) { - assert(previous_timestamp == 0); - return; - } - - // This ensures that every data point is coming in chronological order If youre failing this assert, - // make sure that your data folder matches the name in offline_imu_cam/plugin.cc - assert(datum->dataset_time > previous_timestamp); - previous_timestamp = datum->dataset_time; - - imu_cam_buffer = datum; - - VIO::Vector6 imu_raw_vals; - Eigen::Vector3f &acc = datum->linear_a; - Eigen::Vector3f &gyro = datum->angular_v; - ORB_SLAM3::IMU::Point ORB_p(acc(0), acc(1), acc(2), gyro(0), gyro(1), gyro(2), - previous_timestamp); - - // Feed the IMU measurement. There should always be IMU data in each - // call to feed_imu_cam - assert((datum->img0.has_value() && datum->img1.has_value()) || - (!datum->img0.has_value() && !datum->img1.has_value())); - vImuMeas.push_back(ORB_p); + std::string root = get_path(); + std::string voc_path = root + "Vocabulary/ORBvoc.txt"; + std::string settings_path = root + "Examples/Stereo-Intertial/EuRoC.yaml"; + SLAM_sys = std::make_unique( + voc_path, settings_path, ORB_SLAM3::System::IMU_STEREO, true); + + // Read rectification parameters + cv::FileStorage fsSettings(settings_path, cv::FileStorage::READ); + if (!fsSettings.isOpened()) { + cerr << "ERROR: Wrong path to settings" << endl; + abort(); + } + + cv::Mat K_l, K_r, P_l, P_r, R_l, R_r, D_l, D_r; + fsSettings["LEFT.K"] >> K_l; + fsSettings["RIGHT.K"] >> K_r; + + fsSettings["LEFT.P"] >> P_l; + fsSettings["RIGHT.P"] >> P_r; + + fsSettings["LEFT.R"] >> R_l; + fsSettings["RIGHT.R"] >> R_r; + + fsSettings["LEFT.D"] >> D_l; + fsSettings["RIGHT.D"] >> D_r; + + int rows_l = fsSettings["LEFT.height"]; + int cols_l = fsSettings["LEFT.width"]; + int rows_r = fsSettings["RIGHT.height"]; + int cols_r = fsSettings["RIGHT.width"]; + + if (K_l.empty() || K_r.empty() || P_l.empty() || P_r.empty() || + R_l.empty() || R_r.empty() || D_l.empty() || D_r.empty() || + rows_l == 0 || rows_r == 0 || cols_l == 0 || cols_r == 0) { + cerr << "ERROR: Calibration parameters to rectify stereo are missing!" + << endl; + abort(); + } + + cv::Mat M1l, M2l, M1r, M2r; + cv::initUndistortRectifyMap(K_l, D_l, R_l, + P_l.rowRange(0, 3).colRange(0, 3), + cv::Size(cols_l, rows_l), CV_32F, M1l, M2l); + cv::initUndistortRectifyMap(K_r, D_r, R_r, + P_r.rowRange(0, 3).colRange(0, 3), + cv::Size(cols_r, rows_r), CV_32F, M1r, M2r); + } + virtual ~ORB_SLAM3_ILLIXR() override {}; + + virtual void start() override { + plugin::start(); + sb->schedule( + id, "imu_cam", + [&](const imu_cam_type *dataum) { this->feed_imu_cam(dataum); }); + } + + std::size_t iteration_no = 0; + void feed_imu_cam(const imu_cam_type *dataum) { + // Ensures that slam doesnt start before valid IMU readings come in + if (dataum == NULL) { + assert(previous_timestamp == 0); + return; + } + + // This ensures that every data point is coming in chronological order If + // youre failing this assert, make sure that your data folder matches the + // name in offline_imu_cam/plugin.cc + assert(dataum->dataset_time > previous_timestamp); + previous_timestamp = dataum->dataset_time; + + imu_cam_buffer = dataum; + + const Eigen::Vector3f &acc = dataum->linear_a; + const Eigen::Vector3f &gyro = dataum->angular_v; + ORB_SLAM3::IMU::Point ORB_p(acc(0), acc(1), acc(2), gyro(0), gyro(1), + gyro(2), previous_timestamp); + + // Feed the IMU measurement. There should always be IMU data in each + // call to feed_imu_cam + assert((dataum->img0.has_value() && dataum->img1.has_value()) || + (!dataum->img0.has_value() && !dataum->img1.has_value())); + vImuMeas.push_back(ORB_p); #ifdef CV_HAS_METRICS - cv::metrics::setAccount(new std::string{std::to_string(iteration_no)}); - iteration_no++; - if (iteration_no % 20 == 0) { - cv::metrics::dump(); - } + cv::metrics::setAccount(new std::string{std::to_string(iteration_no)}); + iteration_no++; + if (iteration_no % 20 == 0) { + cv::metrics::dump(); + } #else -#warning "No OpenCV metrics available. Please recompile OpenCV from git clone --branch 3.4.6-instrumented https://github.com/ILLIXR/opencv/. (see install_deps.sh)" +#warning \ + "No OpenCV metrics available. Please recompile OpenCV from git clone --branch 3.4.6-instrumented https://github.com/ILLIXR/opencv/. (see install_deps.sh)" #endif - std::optional imLeft = imu_cam_buffer->img0; - std::optional imRight = imu_cam_buffer->img1; - cv::Mat imLeftRect, imRightRect; + std::optional imLeft = imu_cam_buffer->img0; + std::optional imRight = imu_cam_buffer->img1; + cv::Mat imLeftRect, imRightRect; #ifdef COMPILEDWITHC11 - std::chrono::steady_clock::time_point t_Start_Rect = - std::chrono::steady_clock::now(); + std::chrono::steady_clock::time_point t_Start_Rect = + std::chrono::steady_clock::now(); #else - std::chrono::monotonic_clock::time_point t_Start_Rect = - std::chrono::monotonic_clock::now(); + std::chrono::monotonic_clock::time_point t_Start_Rect = + std::chrono::monotonic_clock::now(); #endif - cv::remap(*imLeft, imLeftRect, M1l, M2l, cv::INTER_LINEAR); - cv::remap(*imRight, imRightRect, M1r, M2r, cv::INTER_LINEAR); + cv::remap(**imLeft, imLeftRect, M1l, M2l, cv::INTER_LINEAR); + cv::remap(**imRight, imRightRect, M1r, M2r, cv::INTER_LINEAR); #ifdef COMPILEDWITHC11 - std::chrono::steady_clock::time_point t_End_Rect = - std::chrono::steady_clock::now(); + std::chrono::steady_clock::time_point t_End_Rect = + std::chrono::steady_clock::now(); #else - std::chrono::monotonic_clock::time_point t_End_Rect = - std::chrono::monotonic_clock::now(); + std::chrono::monotonic_clock::time_point t_End_Rect = + std::chrono::monotonic_clock::now(); #endif - double t_rect = - std::chrono::duration_cast>( - t_End_Rect - t_Start_Rect) - .count(); + double t_rect = std::chrono::duration_cast>( + t_End_Rect - t_Start_Rect) + .count(); - double tframe = dataum->dataset_time; + double tframe = dataum->dataset_time; - // If there is not cam data this func call, break early - if (!datum->img0.has_value() && !datum->img1.has_value()) { - return; - } else { + // If there is not cam data this func call, break early + if (!dataum->img0.has_value() && !dataum->img1.has_value()) { + return; + } else { #ifdef COMPILEDWITHC11 - std::chrono::steady_clock::time_point t1 = - std::chrono::steady_clock::now(); + 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(); + std::chrono::monotonic_clock::time_point t1 = + std::chrono::monotonic_clock::now(); #endif - // Pass the images to the SLAM system - cv::Mat Tcw = - SLAM.TrackStereo(imLeftRect, imRightRect, tframe, vImuMeas); + // Pass the images to the SLAM system + cv::Mat Tcw = + SLAM_sys->TrackStereo(imLeftRect, imRightRect, tframe, vImuMeas); #ifdef COMPILEDWITHC11 - std::chrono::steady_clock::time_point t2 = - std::chrono::steady_clock::now(); + 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(); + std::chrono::monotonic_clock::time_point t2 = + std::chrono::monotonic_clock::now(); #endif - vImuMeas.clear(); - } - cv::Mat Rwc = Tcw.rowRange(0,3).colRange(0,3).t(); - cv::Mat twc = -Rwc*Tcw.rowRange(0,3).col(3) - Eigen::Vector3f position{twc.at(0), twc.at(1), - twc.at(2)};; - Eigen::Matrix3f emat3f; - cv2eigen(Rwc, emat3f); - Eigen::Quaternionf orientation(emat3f); - _m_pose->put(new pose_type{ - .sensor_time = imu_cam_buffer->time, - .position = position, - .orientation = orientation - }); - - } - -} + vImuMeas.clear(); + cv::Mat Rwc = Tcw.rowRange(0, 3).colRange(0, 3).t(); + cv::Mat twc = -Rwc * Tcw.rowRange(0, 3).col(3); + Eigen::Vector3f position{twc.at(0), twc.at(1), + twc.at(2)}; + Eigen::Matrix3f emat3f; + cv2eigen(Rwc, emat3f); + Eigen::Quaternionf orientation(emat3f); + _m_pose->put(new pose_type{.sensor_time = imu_cam_buffer->time, + .position = position, + .orientation = orientation}); + } + } + +}; PLUGIN_MAIN(ORB_SLAM3_ILLIXR); diff --git a/include/LoopClosing.h b/include/LoopClosing.h index 215b80aaef..382c6d3d70 100644 --- a/include/LoopClosing.h +++ b/include/LoopClosing.h @@ -194,7 +194,7 @@ class LoopClosing bool mbFixScale; - bool mnFullBAIdx; + int mnFullBAIdx;