diff --git a/CMakeLists.txt b/CMakeLists.txt index d1066632..dea76193 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -12,8 +12,11 @@ endif() option(ENABLE_ROS "Enable or disable building with ROS (if it is found)" OFF) set(ILLIXR_ROOT "" CACHE PATH "Directory of ILLIXR includes") option(ENABLE_TESTS "Enable building tests" OFF) - -if (ILLIXR_INTEGRATION AND NOT ILLIXR_ROOT) +option(USE_ZED "Whether to build for the zed camera" OFF) +if (USE_ZED) + add_compile_definitions(HAVE_ZED) +endif() +if (NOT ILLIXR_ROOT) message(FATAL_ERROR "ILLIXR_ROOT must be specified if ILLIXR_INTEGRATION is enabled.") endif() diff --git a/ov_msckf/src/core/VioManagerOptions.h b/ov_msckf/src/core/VioManagerOptions.h index 00b98829..72844d13 100644 --- a/ov_msckf/src/core/VioManagerOptions.h +++ b/ov_msckf/src/core/VioManagerOptions.h @@ -97,6 +97,11 @@ struct VioManagerOptions { /// If we should record the timing performance to file bool record_timing_information = false; + /// Amount of time we will initialize over (seconds) + double init_window_time = 1.0; + + /// Variance threshold on our acceleration to be classified as moving + double init_imu_thresh = 1.0; /// The path to the file we will record the timing information into std::string record_timing_filepath = "ov_msckf_timing.txt"; @@ -126,6 +131,8 @@ struct VioManagerOptions { PRINT_DEBUG(" - zupt_noise_multiplier: %.2f\n", zupt_noise_multiplier) PRINT_DEBUG(" - zupt_max_disparity: %.4f\n", zupt_max_disparity) PRINT_DEBUG(" - zupt_only_at_beginning?: %d\n", zupt_only_at_beginning) + PRINT_DEBUG("\t- init_window_time: %.2f\n", init_window_time) + PRINT_DEBUG("\t- init_imu_thresh: %.2f\n", init_imu_thresh) PRINT_DEBUG(" - record timing?: %d\n", (int)record_timing_information) PRINT_DEBUG(" - record timing filepath: %s\n", record_timing_filepath.c_str()) } diff --git a/ov_msckf/src/slam2.cpp b/ov_msckf/src/slam2.cpp index 0f1ac111..db1d45aa 100644 --- a/ov_msckf/src/slam2.cpp +++ b/ov_msckf/src/slam2.cpp @@ -8,11 +8,13 @@ #include "core/VioManager.h" #include "state/State.h" +#include "utils/quat_ops.h" #include "illixr/plugin.hpp" #include "illixr/switchboard.hpp" -#include "illixr/data_format.hpp" -#include "illixr/opencv_data_types.hpp" +#include "illixr/data_format/opencv_data_types.hpp" +#include "illixr/data_format/pose.hpp" +#include "illixr/data_format/imu.hpp" #include "illixr/phonebook.hpp" #include "illixr/relative_clock.hpp" @@ -21,7 +23,7 @@ using namespace ov_msckf; // Comment in if using ZED instead of offline_imu_cam // TODO: Pull from config YAML file -// #define ZED +// #define HAVE_ZED VioManagerOptions create_params() { @@ -29,7 +31,7 @@ VioManagerOptions create_params() // Camera #0 Eigen::Matrix intrinsics_0; -#ifdef ZED +#ifdef HAVE_ZED // ZED calibration tool; fx, fy, cx, cy, k1, k2, p1, p2 // https://docs.opencv.org/2.4/doc/tutorials/calib3d/camera_calibration/camera_calibration.html intrinsics_0 << 349.686, 349.686, 332.778, 192.423, -0.175708, 0.0284421, 0, 0; @@ -37,8 +39,10 @@ VioManagerOptions create_params() // EuRoC intrinsics_0 << 458.654, 457.296, 367.215, 248.375, -0.28340811, 0.07395907, 0.00019359, 1.76187114e-05; #endif + std::shared_ptr camera0(new ov_core::CamRadtan(1280, 720)); + camera0->set_value(intrinsics_0); -#ifdef ZED +#ifdef HAVE_ZED // Camera extrinsics from https://github.com/rpng/open_vins/issues/52#issuecomment-619480497 std::vector matrix_TCtoI_0 = {-0.01080233, 0.00183858, 0.99993996, 0.01220425, -0.99993288, -0.00420947, -0.01079452, 0.0146056, @@ -59,30 +63,25 @@ VioManagerOptions create_params() // Load these into our state Eigen::Matrix extrinsics_0; - extrinsics_0.block(0, 0, 4, 1) = rot_2_quat(T_CtoI_0.block(0, 0, 3, 3).transpose()); + extrinsics_0.block(0, 0, 4, 1) = ov_core::rot_2_quat(T_CtoI_0.block(0, 0, 3, 3).transpose()); extrinsics_0.block(4, 0, 3, 1) = -T_CtoI_0.block(0, 0, 3, 3).transpose() * T_CtoI_0.block(0, 3, 3, 1); - params.camera_fisheye.insert({0, false}); - params.camera_intrinsics.insert({0, intrinsics_0}); + params.camera_intrinsics.insert({0, camera0}); params.camera_extrinsics.insert({0, extrinsics_0}); -#ifdef ZED - params.camera_wh.insert({0, {672, 376}}); -#else - params.camera_wh.insert({0, {752, 480}}); -#endif - // Camera #1 Eigen::Matrix intrinsics_1; -#ifdef ZED +#ifdef HAVE_ZED // ZED calibration tool; fx, fy, cx, cy, k1, k2, p1, p2 intrinsics_1 << 350.01, 350.01, 343.729, 185.405, -0.174559, 0.0277521, 0, 0; #else // EuRoC intrinsics_1 << 457.587, 456.134, 379.999, 255.238, -0.28368365, 0.07451284, -0.00010473, -3.55590700e-05; #endif + std::shared_ptr camera1(new ov_core::CamRadtan(1280, 720)); + camera1->set_value(intrinsics_1); -#ifdef ZED +#ifdef HAVE_ZED // Camera extrinsics from https://github.com/rpng/open_vins/issues/52#issuecomment-619480497 std::vector matrix_TCtoI_1 = {-0.01043535, -0.00191061, 0.99994372, 0.01190459, -0.99993668, -0.00419281, -0.01044329, -0.04732387, @@ -103,23 +102,16 @@ VioManagerOptions create_params() // Load these into our state Eigen::Matrix extrinsics_1; - extrinsics_1.block(0, 0, 4, 1) = rot_2_quat(T_CtoI_1.block(0, 0, 3, 3).transpose()); + extrinsics_1.block(0, 0, 4, 1) = ov_core::rot_2_quat(T_CtoI_1.block(0, 0, 3, 3).transpose()); extrinsics_1.block(4, 0, 3, 1) = -T_CtoI_1.block(0, 0, 3, 3).transpose() * T_CtoI_1.block(0, 3, 3, 1); - params.camera_fisheye.insert({1, false}); - params.camera_intrinsics.insert({1, intrinsics_1}); + params.camera_intrinsics.insert({1, camera1}); params.camera_extrinsics.insert({1, extrinsics_1}); -#ifdef ZED - params.camera_wh.insert({1, {672, 376}}); -#else - params.camera_wh.insert({1, {752, 480}}); -#endif - // params.state_options.max_slam_features = 0; params.state_options.num_cameras = 2; params.init_window_time = 0.75; -#ifdef ZED +#ifdef HAVE_ZED // Hand tuned params.init_imu_thresh = 0.5; #else @@ -129,7 +121,7 @@ VioManagerOptions create_params() params.fast_threshold = 15; params.grid_x = 5; params.grid_y = 3; -#ifdef ZED +#ifdef HAVE_ZED // Hand tuned params.num_pts = 200; #else @@ -138,9 +130,8 @@ VioManagerOptions create_params() params.msckf_options.chi2_multipler = 1; params.knn_ratio = .7; - params.state_options.imu_avg = true; params.state_options.do_fej = true; - params.state_options.use_rk4_integration = true; + params.state_options.integration_method = ov_msckf::StateOptions::IntegrationMethod::RK4; params.use_stereo = true; params.state_options.do_calib_camera_pose = true; params.state_options.do_calib_camera_intrinsics = true; @@ -151,7 +142,7 @@ VioManagerOptions create_params() params.state_options.max_slam_in_update = 25; params.state_options.max_msckf_in_update = 999; -#ifdef ZED +#ifdef HAVE_ZED // Pixel noise; ZED works with defaults values but these may better account for rolling shutter params.msckf_options.chi2_multipler = 2; params.msckf_options.sigma_pix = 5; @@ -175,8 +166,8 @@ VioManagerOptions create_params() params.use_aruco = false; - params.state_options.feat_rep_slam = LandmarkRepresentation::from_string("ANCHORED_FULL_INVERSE_DEPTH"); - params.state_options.feat_rep_aruco = LandmarkRepresentation::from_string("ANCHORED_FULL_INVERSE_DEPTH"); + params.state_options.feat_rep_slam = ov_type::LandmarkRepresentation::from_string("ANCHORED_FULL_INVERSE_DEPTH"); + params.state_options.feat_rep_aruco = ov_type::LandmarkRepresentation::from_string("ANCHORED_FULL_INVERSE_DEPTH"); return params; } @@ -192,9 +183,9 @@ class slam2 : public plugin { : plugin{std::move(name_), pb_} , sb{pb->lookup_impl()} , _m_rtc{pb->lookup_impl()} - , _m_pose{sb->get_writer("slow_pose")} - , _m_imu_integrator_input{sb->get_writer("imu_integrator_input")} - , _m_cam{sb->get_buffered_reader("cam")} + , _m_pose{sb->get_writer("slow_pose")} + , _m_imu_integrator_input{sb->get_writer("imu_integrator_input")} + , _m_cam{sb->get_buffered_reader("cam")} , open_vins_estimator{manager_params} { @@ -208,28 +199,27 @@ class slam2 : public plugin { } - void start() override { plugin::start(); - sb->schedule(id, "imu", [&](const switchboard::ptr& datum, std::size_t iteration_no) { + sb->schedule(id, "imu", [&](const switchboard::ptr& datum, std::size_t iteration_no) { this->feed_imu_cam(datum, iteration_no); }); } - void feed_imu_cam(const switchboard::ptr& datum, [[maybe_unused]]std::size_t iteration_no) { + void feed_imu_cam(const switchboard::ptr& datum, [[maybe_unused]]std::size_t iteration_no) { // Ensures that slam doesnt start before valid IMU readings come in if (datum == nullptr) { return; } // Feed the IMU measurement. There should always be IMU data in each call to feed_imu_cam - open_vins_estimator.feed_measurement_imu(duration2double(datum->time.time_since_epoch()), datum->angular_v, datum->linear_a); + open_vins_estimator.feed_measurement_imu({duration2double(datum->time.time_since_epoch()), datum->angular_v, datum->linear_a}); - switchboard::ptr cam; + switchboard::ptr cam; // Camera data can only go downstream when there's at least one IMU sample whose timestamp is larger than the camera data's. cam = (cam_buffer == nullptr && _m_cam.size() > 0) ? _m_cam.dequeue() : nullptr; - // If there is not cam data this func call, break early + // If there is no cam data this func call, break early if (!cam_buffer && !cam) { return; } else if (!cam_buffer && cam) { @@ -239,19 +229,9 @@ class slam2 : public plugin { return; } - -#ifdef CV_HAS_METRICS - cv::metrics::setAccount(new std::string{std::to_string(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 - - cv::Mat img0{cam_buffer->at(image::LEFT)}; - cv::Mat img1{cam_buffer->at(image::RIGHT)}; - open_vins_estimator.feed_measurement_stereo(duration2double(cam_buffer->time.time_since_epoch()), img0, img1, 0, 1); + cv::Mat img0{cam_buffer->at(ILLIXR::data_format::image::LEFT_EYE)}; + cv::Mat img1{cam_buffer->at(ILLIXR::data_format::image::RIGHT_EYE)}; + open_vins_estimator.feed_measurement_camera({duration2double(cam_buffer->time.time_since_epoch()), {0, 1}, img0, img1}); // Get the pose returned from SLAM state = open_vins_estimator.get_state(); @@ -281,7 +261,7 @@ class slam2 : public plugin { _m_imu_integrator_input.put(_m_imu_integrator_input.allocate( cam_buffer->time, from_seconds(state->_calib_dt_CAMtoIMU->value()(0)), - imu_params{ + ILLIXR::data_format::imu_params{ .gyro_noise = manager_params.imu_noises.sigma_w, .acc_noise = manager_params.imu_noises.sigma_a, .gyro_walk = manager_params.imu_noises.sigma_wb, @@ -305,13 +285,13 @@ class slam2 : public plugin { private: const std::shared_ptr sb; - std::shared_ptr _m_rtc; - switchboard::writer _m_pose; - switchboard::writer _m_imu_integrator_input; - State *state{}; + std::shared_ptr _m_rtc; + switchboard::writer _m_pose; + switchboard::writer _m_imu_integrator_input; + std::shared_ptr state; - switchboard::ptr cam_buffer; - switchboard::buffered_reader _m_cam; + switchboard::ptr cam_buffer; + switchboard::buffered_reader _m_cam; VioManagerOptions manager_params = create_params(); VioManager open_vins_estimator; diff --git a/ov_msckf/src/state/Propagator.h b/ov_msckf/src/state/Propagator.h index 0cc9f5d4..887e80cf 100644 --- a/ov_msckf/src/state/Propagator.h +++ b/ov_msckf/src/state/Propagator.h @@ -27,11 +27,6 @@ #include #include "utils/sensor_data.h" -#ifdef ILLIXR_INTEGRATION -#include "illixr/data_format.hpp" -#endif /// ILLIXR_INTEGRATION - - #include "utils/NoiseManager.h" namespace ov_msckf {