diff --git a/CMakeLists.txt b/CMakeLists.txt index 1f7c65bc494..e034aaac8be 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -17,9 +17,16 @@ 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) + # 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(-DCOMPILEDWITHC11) + 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 +198,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 00000000000..2cb2bc1efa1 --- /dev/null +++ b/Examples/Stereo-Inertial/ILLIXR_stereo_inertial_euroc.cc @@ -0,0 +1,254 @@ +/** +* 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::imu_cam_type; +using ILLIXR::imu_integrator_input; +using ILLIXR::phonebook; +using ILLIXR::switchboard; +using ILLIXR::plugin; +using ILLIXR::pose_type; +using ILLIXR::time_type; +using ILLIXR::writer; + +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 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"}); +#endif + 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(); + } +#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 (!dataum->img0.has_value() && !dataum->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_sys->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); diff --git a/include/LoopClosing.h b/include/LoopClosing.h index 215b80aaef5..382c6d3d70c 100644 --- a/include/LoopClosing.h +++ b/include/LoopClosing.h @@ -194,7 +194,7 @@ class LoopClosing bool mbFixScale; - bool mnFullBAIdx; + int mnFullBAIdx;