From 9f464db7c9d87867cb0dad98871a1614db0b62cf Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sat, 13 Jun 2026 16:20:37 -0700 Subject: [PATCH] OpenVINS config file support (#1719) --- CMakeLists.txt | 21 +-- cmake_modules/FindOpenVINS.cmake | 44 ++++++ corelib/include/rtabmap/core/Parameters.h | 113 +++++++------- corelib/src/CMakeLists.txt | 8 +- corelib/src/odometry/OdometryOpenVINS.cpp | 79 ++++++++++ .../include/rtabmap/gui/PreferencesDialog.h | 1 + guilib/src/PreferencesDialog.cpp | 19 +++ guilib/src/ui/preferencesDialog.ui | 145 +++++++++++------- 8 files changed, 293 insertions(+), 137 deletions(-) create mode 100644 cmake_modules/FindOpenVINS.cmake diff --git a/CMakeLists.txt b/CMakeLists.txt index 60986d09..2f2206b9 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -842,22 +842,7 @@ IF(WITH_VINS_FUSION) ENDIF(WITH_VINS_FUSION) IF(WITH_OPENVINS) - FIND_PACKAGE(ov_msckf) - # On ROS2, the indirect includes and libraries - # are not forwarded by ov_msckf target, append them manually - FIND_PACKAGE(ov_core) - FIND_PACKAGE(ov_init) - IF(ov_msckf_FOUND AND ov_core_FOUND AND ov_init_FOUND) - SET(ov_msckf_INCLUDE_DIRS - ${ov_msckf_INCLUDE_DIRS} - ${ov_core_INCLUDE_DIRS} - ${ov_init_INCLUDE_DIRS}) - SET(ov_msckf_LIBRARIES - ${ov_msckf_LIBRARIES} - ${ov_core_LIBRARIES} - ${ov_init_LIBRARIES}) - MESSAGE(STATUS "Found OpenVINS: ${ov_msckf_INCLUDE_DIRS}") - ENDIF() + FIND_PACKAGE(OpenVINS) ENDIF(WITH_OPENVINS) IF(WITH_FASTCV) @@ -1223,7 +1208,7 @@ ENDIF() IF(NOT vins_FOUND) SET(VINSFUSION "//") ENDIF() -IF(NOT ov_msckf_FOUND) +IF(NOT OpenVINS_FOUND) SET(OPENVINS "//") ENDIF() IF(NOT CUVSLAM_FOUND) @@ -1981,7 +1966,7 @@ ELSE() MESSAGE(STATUS " With VINS-Fusion = NO (VINS-Fusion not found)") ENDIF() -IF(ov_msckf_FOUND) +IF(OpenVINS_FOUND) MESSAGE(STATUS " With OpenVINS = YES (License: GPLv3)") ELSEIF(NOT WITH_OPENVINS) MESSAGE(STATUS " With OpenVINS = NO (WITH_OPENVINS=OFF)") diff --git a/cmake_modules/FindOpenVINS.cmake b/cmake_modules/FindOpenVINS.cmake new file mode 100644 index 00000000..66029218 --- /dev/null +++ b/cmake_modules/FindOpenVINS.cmake @@ -0,0 +1,44 @@ +# Find OpenVINS +# +# We search for a vins installation in ROS/ROS2 first, then fallback on +# ros-free library in common install paths + +FIND_PACKAGE(ov_msckf QUIET) +IF(ov_msckf_FOUND) + # On ROS2, the indirect includes and libraries + # are not forwarded by ov_msckf target, append them manually + FIND_PACKAGE(ov_core) + FIND_PACKAGE(ov_init) + IF(ov_msckf_FOUND AND ov_core_FOUND AND ov_init_FOUND) + SET(OpenVINS_FOUND TRUE) + SET(OpenVINS_INCLUDE_DIRS + ${ov_msckf_INCLUDE_DIRS} + ${ov_core_INCLUDE_DIRS} + ${ov_init_INCLUDE_DIRS}) + SET(OpenVINS_LIBRARIES + ${ov_msckf_LIBRARIES} + ${ov_core_LIBRARIES} + ${ov_init_LIBRARIES}) + ENDIF() +ELSE() + find_path(OpenVINS_INCLUDE_DIR NAMES core/VioManager.h PATH_SUFFIXES open_vins) + find_library(OpenVINS_LIBRARY NAMES ov_msckf_lib) + IF (OpenVINS_INCLUDE_DIR AND OpenVINS_LIBRARY) + SET(OpenVINS_FOUND TRUE) + SET(OpenVINS_INCLUDE_DIRS ${OpenVINS_INCLUDE_DIR}) + SET(OpenVINS_LIBRARIES ${OpenVINS_LIBRARY}) + ENDIF() +ENDIF() + +IF (OpenVINS_FOUND) + # show which OpenVINS was found only if not quiet + IF (NOT OpenVINS_FIND_QUIETLY) + MESSAGE(STATUS "Found OpenVINS: ${OpenVINS_LIBRARIES} ${OpenVINS_INCLUDE_DIRS}") + ENDIF (NOT OpenVINS_FIND_QUIETLY) +ELSE (OpenVINS_FOUND) + # fatal error if OpenVINS is required but not found + IF (OpenVINS_FIND_REQUIRED) + MESSAGE(FATAL_ERROR "Could not find OpenVINS") + ENDIF (OpenVINS_FIND_REQUIRED) +ENDIF (OpenVINS_FOUND) + diff --git a/corelib/include/rtabmap/core/Parameters.h b/corelib/include/rtabmap/core/Parameters.h index 3944acf6..e3c8d574 100644 --- a/corelib/include/rtabmap/core/Parameters.h +++ b/corelib/include/rtabmap/core/Parameters.h @@ -620,66 +620,67 @@ class RTABMAP_CORE_EXPORT Parameters RTABMAP_PARAM_STR(OdomVINSFusion, ConfigPath, "", "Path of VINS-Fusion config file."); // Odometry OpenVINS - RTABMAP_PARAM(OdomOpenVINS, UseStereo, bool, true, "If we have more than 1 camera, if we should try to track stereo constraints between pairs"); - RTABMAP_PARAM(OdomOpenVINS, UseKLT, bool, true, "If true we will use KLT, otherwise use a ORB descriptor + robust matching"); - RTABMAP_PARAM(OdomOpenVINS, NumPts, int, 200, "Number of points (per camera) we will extract and try to track"); - RTABMAP_PARAM(OdomOpenVINS, MinPxDist, int, 15, "Eistance between features (features near each other provide less information)"); - RTABMAP_PARAM(OdomOpenVINS, FiTriangulate1d, bool, false, "If we should perform 1d triangulation instead of 3d"); - RTABMAP_PARAM(OdomOpenVINS, FiRefineFeatures, bool, true, "If we should perform Levenberg-Marquardt refinement"); - RTABMAP_PARAM(OdomOpenVINS, FiMaxRuns, int, 5, "Max runs for Levenberg-Marquardt"); - RTABMAP_PARAM(OdomOpenVINS, FiMaxBaseline, double, 40, "Max baseline ratio to accept triangulated features"); - RTABMAP_PARAM(OdomOpenVINS, FiMaxCondNumber, double, 10000, "Max condition number of linear triangulation matrix accept triangulated features"); + RTABMAP_PARAM_STR(OdomOpenVINS, ConfigPath, "", "Path of OpenVINS config file (*.yaml). Same format used than OpenVINS library. Note that any parameter from that config file will overwrite the same parameter in OdomOpenVINS group."); + RTABMAP_PARAM(OdomOpenVINS, UseStereo, bool, true, "If we have more than 1 camera, if we should try to track stereo constraints between pairs."); + RTABMAP_PARAM(OdomOpenVINS, UseKLT, bool, true, "If true we will use KLT, otherwise use a ORB descriptor + robust matching."); + RTABMAP_PARAM(OdomOpenVINS, NumPts, int, 200, "Number of points (per camera) we will extract and try to track."); + RTABMAP_PARAM(OdomOpenVINS, MinPxDist, int, 15, "Eistance between features (features near each other provide less information)."); + RTABMAP_PARAM(OdomOpenVINS, FiTriangulate1d, bool, false, "If we should perform 1d triangulation instead of 3d."); + RTABMAP_PARAM(OdomOpenVINS, FiRefineFeatures, bool, true, "If we should perform Levenberg-Marquardt refinement."); + RTABMAP_PARAM(OdomOpenVINS, FiMaxRuns, int, 5, "Max runs for Levenberg-Marquardt."); + RTABMAP_PARAM(OdomOpenVINS, FiMaxBaseline, double, 40, "Max baseline ratio to accept triangulated features."); + RTABMAP_PARAM(OdomOpenVINS, FiMaxCondNumber, double, 10000, "Max condition number of linear triangulation matrix accept triangulated features."); - RTABMAP_PARAM(OdomOpenVINS, UseFEJ, bool, true, "If first-estimate Jacobians should be used (enable for good consistency)"); - RTABMAP_PARAM(OdomOpenVINS, Integration, int, 1, "0=discrete, 1=rk4, 2=analytical (if rk4 or analytical used then analytical covariance propagation is used)"); - RTABMAP_PARAM(OdomOpenVINS, CalibCamExtrinsics, bool, false, "Bool to determine whether or not to calibrate imu-to-camera pose"); - RTABMAP_PARAM(OdomOpenVINS, CalibCamIntrinsics, bool, false, "Bool to determine whether or not to calibrate camera intrinsics"); - RTABMAP_PARAM(OdomOpenVINS, CalibCamTimeoffset, bool, false, "Bool to determine whether or not to calibrate camera to IMU time offset"); - RTABMAP_PARAM(OdomOpenVINS, CalibIMUIntrinsics, bool, false, "Bool to determine whether or not to calibrate the IMU intrinsics"); - RTABMAP_PARAM(OdomOpenVINS, CalibIMUGSensitivity, bool, false, "Bool to determine whether or not to calibrate the Gravity sensitivity"); - RTABMAP_PARAM(OdomOpenVINS, MaxClones, int, 11, "Max clone size of sliding window"); - RTABMAP_PARAM(OdomOpenVINS, MaxSLAM, int, 50, "Max number of estimated SLAM features"); - RTABMAP_PARAM(OdomOpenVINS, MaxSLAMInUpdate, int, 25, "Max number of SLAM features we allow to be included in a single EKF update."); - RTABMAP_PARAM(OdomOpenVINS, MaxMSCKFInUpdate, int, 50, "Max number of MSCKF features we will use at a given image timestep."); - RTABMAP_PARAM(OdomOpenVINS, FeatRepMSCKF, int, 0, "What representation our features are in (msckf features)"); - RTABMAP_PARAM(OdomOpenVINS, FeatRepSLAM, int, 4, "What representation our features are in (slam features)"); - RTABMAP_PARAM(OdomOpenVINS, DtSLAMDelay, double, 0.0, "Delay, in seconds, that we should wait from init before we start estimating SLAM features"); - RTABMAP_PARAM(OdomOpenVINS, GravityMag, double, 9.81, "Gravity magnitude in the global frame (i.e. should be 9.81 typically)"); - RTABMAP_PARAM_STR(OdomOpenVINS, LeftMaskPath, "", "Mask for left image"); - RTABMAP_PARAM_STR(OdomOpenVINS, RightMaskPath, "", "Mask for right image"); + RTABMAP_PARAM(OdomOpenVINS, UseFEJ, bool, true, "If first-estimate Jacobians should be used (enable for good consistency)."); + RTABMAP_PARAM(OdomOpenVINS, Integration, int, 1, "0=discrete, 1=rk4, 2=analytical (if rk4 or analytical used then analytical covariance propagation is used)."); + RTABMAP_PARAM(OdomOpenVINS, CalibCamExtrinsics, bool, false, "Bool to determine whether or not to calibrate imu-to-camera pose."); + RTABMAP_PARAM(OdomOpenVINS, CalibCamIntrinsics, bool, false, "Bool to determine whether or not to calibrate camera intrinsics."); + RTABMAP_PARAM(OdomOpenVINS, CalibCamTimeoffset, bool, false, "Bool to determine whether or not to calibrate camera to IMU time offset."); + RTABMAP_PARAM(OdomOpenVINS, CalibIMUIntrinsics, bool, false, "Bool to determine whether or not to calibrate the IMU intrinsics."); + RTABMAP_PARAM(OdomOpenVINS, CalibIMUGSensitivity, bool, false, "Bool to determine whether or not to calibrate the Gravity sensitivity."); + RTABMAP_PARAM(OdomOpenVINS, MaxClones, int, 11, "Max clone size of sliding window."); + RTABMAP_PARAM(OdomOpenVINS, MaxSLAM, int, 50, "Max number of estimated SLAM features."); + RTABMAP_PARAM(OdomOpenVINS, MaxSLAMInUpdate, int, 25, "Max number of SLAM features we allow to be included in a single EKF update.."); + RTABMAP_PARAM(OdomOpenVINS, MaxMSCKFInUpdate, int, 50, "Max number of MSCKF features we will use at a given image timestep.."); + RTABMAP_PARAM(OdomOpenVINS, FeatRepMSCKF, int, 0, "What representation our features are in (msckf features)."); + RTABMAP_PARAM(OdomOpenVINS, FeatRepSLAM, int, 4, "What representation our features are in (slam features)."); + RTABMAP_PARAM(OdomOpenVINS, DtSLAMDelay, double, 0.0, "Delay, in seconds, that we should wait from init before we start estimating SLAM features."); + RTABMAP_PARAM(OdomOpenVINS, GravityMag, double, 9.81, "Gravity magnitude in the global frame (i.e. should be 9.81 typically)."); + RTABMAP_PARAM_STR(OdomOpenVINS, LeftMaskPath, "", "Mask for left image."); + RTABMAP_PARAM_STR(OdomOpenVINS, RightMaskPath, "", "Mask for right image."); - RTABMAP_PARAM(OdomOpenVINS, InitWindowTime, double, 2.0, "Amount of time we will initialize over (seconds)"); - RTABMAP_PARAM(OdomOpenVINS, InitIMUThresh, double, 1.0, "Variance threshold on our acceleration to be classified as moving"); - RTABMAP_PARAM(OdomOpenVINS, InitMaxDisparity, double, 10.0, "Max disparity to consider the platform stationary (dependent on resolution)"); - RTABMAP_PARAM(OdomOpenVINS, InitMaxFeatures, int, 50, "How many features to track during initialization (saves on computation)"); - RTABMAP_PARAM(OdomOpenVINS, InitDynUse, bool, false, "If dynamic initialization should be used"); - RTABMAP_PARAM(OdomOpenVINS, InitDynMLEOptCalib, bool, false, "If we should optimize calibration during intialization (not recommended)"); - RTABMAP_PARAM(OdomOpenVINS, InitDynMLEMaxIter, int, 50, "How many iterations the MLE refinement should use (zero to skip the MLE)"); - RTABMAP_PARAM(OdomOpenVINS, InitDynMLEMaxTime, double, 0.05, "How many seconds the MLE should be completed in"); - RTABMAP_PARAM(OdomOpenVINS, InitDynMLEMaxThreads, int, 6, "How many threads the MLE should use"); - RTABMAP_PARAM(OdomOpenVINS, InitDynNumPose, int, 6, "Number of poses to use within our window time (evenly spaced)"); - RTABMAP_PARAM(OdomOpenVINS, InitDynMinDeg, double, 10.0, "Orientation change needed to try to init"); - RTABMAP_PARAM(OdomOpenVINS, InitDynInflationOri, double, 10.0, "What to inflate the recovered q_GtoI covariance by"); - RTABMAP_PARAM(OdomOpenVINS, InitDynInflationVel, double, 100.0, "What to inflate the recovered v_IinG covariance by"); - RTABMAP_PARAM(OdomOpenVINS, InitDynInflationBg, double, 10.0, "What to inflate the recovered bias_g covariance by"); - RTABMAP_PARAM(OdomOpenVINS, InitDynInflationBa, double, 100.0, "What to inflate the recovered bias_a covariance by"); - RTABMAP_PARAM(OdomOpenVINS, InitDynMinRecCond, double, 1e-15, "Reciprocal condition number thresh for info inversion"); + RTABMAP_PARAM(OdomOpenVINS, InitWindowTime, double, 2.0, "Amount of time we will initialize over (seconds)."); + RTABMAP_PARAM(OdomOpenVINS, InitIMUThresh, double, 1.0, "Variance threshold on our acceleration to be classified as moving."); + RTABMAP_PARAM(OdomOpenVINS, InitMaxDisparity, double, 10.0, "Max disparity to consider the platform stationary (dependent on resolution)."); + RTABMAP_PARAM(OdomOpenVINS, InitMaxFeatures, int, 50, "How many features to track during initialization (saves on computation)."); + RTABMAP_PARAM(OdomOpenVINS, InitDynUse, bool, false, "If dynamic initialization should be used."); + RTABMAP_PARAM(OdomOpenVINS, InitDynMLEOptCalib, bool, false, "If we should optimize calibration during intialization (not recommended)."); + RTABMAP_PARAM(OdomOpenVINS, InitDynMLEMaxIter, int, 50, "How many iterations the MLE refinement should use (zero to skip the MLE)."); + RTABMAP_PARAM(OdomOpenVINS, InitDynMLEMaxTime, double, 0.05, "How many seconds the MLE should be completed in."); + RTABMAP_PARAM(OdomOpenVINS, InitDynMLEMaxThreads, int, 6, "How many threads the MLE should use."); + RTABMAP_PARAM(OdomOpenVINS, InitDynNumPose, int, 6, "Number of poses to use within our window time (evenly spaced)."); + RTABMAP_PARAM(OdomOpenVINS, InitDynMinDeg, double, 10.0, "Orientation change needed to try to init."); + RTABMAP_PARAM(OdomOpenVINS, InitDynInflationOri, double, 10.0, "What to inflate the recovered q_GtoI covariance by."); + RTABMAP_PARAM(OdomOpenVINS, InitDynInflationVel, double, 100.0, "What to inflate the recovered v_IinG covariance by."); + RTABMAP_PARAM(OdomOpenVINS, InitDynInflationBg, double, 10.0, "What to inflate the recovered bias_g covariance by."); + RTABMAP_PARAM(OdomOpenVINS, InitDynInflationBa, double, 100.0, "What to inflate the recovered bias_a covariance by."); + RTABMAP_PARAM(OdomOpenVINS, InitDynMinRecCond, double, 1e-15, "Reciprocal condition number thresh for info inversion."); - RTABMAP_PARAM(OdomOpenVINS, TryZUPT, bool, true, "If we should try to use zero velocity update"); - RTABMAP_PARAM(OdomOpenVINS, ZUPTChi2Multiplier, double, 0.0, "Chi2 multiplier for zero velocity"); - RTABMAP_PARAM(OdomOpenVINS, ZUPTMaxVelodicy, double, 0.1, "Max velocity we will consider to try to do a zupt (i.e. if above this, don't do zupt)"); - RTABMAP_PARAM(OdomOpenVINS, ZUPTNoiseMultiplier, double, 10.0, "Multiplier of our zupt measurement IMU noise matrix (default should be 1.0)"); - RTABMAP_PARAM(OdomOpenVINS, ZUPTMaxDisparity, double, 0.5, "Max disparity we will consider to try to do a zupt (i.e. if above this, don't do zupt)"); - RTABMAP_PARAM(OdomOpenVINS, ZUPTOnlyAtBeginning, bool, false, "If we should only use the zupt at the very beginning static initialization phase"); + RTABMAP_PARAM(OdomOpenVINS, TryZUPT, bool, true, "If we should try to use zero velocity update."); + RTABMAP_PARAM(OdomOpenVINS, ZUPTChi2Multiplier, double, 0.0, "Chi2 multiplier for zero velocity."); + RTABMAP_PARAM(OdomOpenVINS, ZUPTMaxVelodicy, double, 0.1, "Max velocity we will consider to try to do a zupt (i.e. if above this, don't do zupt)."); + RTABMAP_PARAM(OdomOpenVINS, ZUPTNoiseMultiplier, double, 10.0, "Multiplier of our zupt measurement IMU noise matrix (default should be 1.0)."); + RTABMAP_PARAM(OdomOpenVINS, ZUPTMaxDisparity, double, 0.5, "Max disparity we will consider to try to do a zupt (i.e. if above this, don't do zupt)."); + RTABMAP_PARAM(OdomOpenVINS, ZUPTOnlyAtBeginning, bool, false, "If we should only use the zupt at the very beginning static initialization phase."); - RTABMAP_PARAM(OdomOpenVINS, AccelerometerNoiseDensity, double, 0.01, "[m/s^2/sqrt(Hz)] (accel \"white noise\")"); - RTABMAP_PARAM(OdomOpenVINS, AccelerometerRandomWalk, double, 0.001, "[m/s^3/sqrt(Hz)] (accel bias diffusion)"); - RTABMAP_PARAM(OdomOpenVINS, GyroscopeNoiseDensity, double, 0.001, "[rad/s/sqrt(Hz)] (gyro \"white noise\")"); - RTABMAP_PARAM(OdomOpenVINS, GyroscopeRandomWalk, double, 0.0001, "[rad/s^2/sqrt(Hz)] (gyro bias diffusion)"); - RTABMAP_PARAM(OdomOpenVINS, UpMSCKFSigmaPx, double, 1.0, "Pixel noise for MSCKF features"); - RTABMAP_PARAM(OdomOpenVINS, UpMSCKFChi2Multiplier, double, 1.0, "Chi2 multiplier for MSCKF features"); - RTABMAP_PARAM(OdomOpenVINS, UpSLAMSigmaPx, double, 1.0, "Pixel noise for SLAM features"); - RTABMAP_PARAM(OdomOpenVINS, UpSLAMChi2Multiplier, double, 1.0, "Chi2 multiplier for SLAM features"); + RTABMAP_PARAM(OdomOpenVINS, AccelerometerNoiseDensity, double, 0.01, "[m/s^2/sqrt(Hz)] (accel \"white noise\")."); + RTABMAP_PARAM(OdomOpenVINS, AccelerometerRandomWalk, double, 0.001, "[m/s^3/sqrt(Hz)] (accel bias diffusion)."); + RTABMAP_PARAM(OdomOpenVINS, GyroscopeNoiseDensity, double, 0.001, "[rad/s/sqrt(Hz)] (gyro \"white noise\")."); + RTABMAP_PARAM(OdomOpenVINS, GyroscopeRandomWalk, double, 0.0001, "[rad/s^2/sqrt(Hz)] (gyro bias diffusion)."); + RTABMAP_PARAM(OdomOpenVINS, UpMSCKFSigmaPx, double, 1.0, "Pixel noise for MSCKF features."); + RTABMAP_PARAM(OdomOpenVINS, UpMSCKFChi2Multiplier, double, 1.0, "Chi2 multiplier for MSCKF features."); + RTABMAP_PARAM(OdomOpenVINS, UpSLAMSigmaPx, double, 1.0, "Pixel noise for SLAM features."); + RTABMAP_PARAM(OdomOpenVINS, UpSLAMChi2Multiplier, double, 1.0, "Chi2 multiplier for SLAM features."); // Odometry Open3D RTABMAP_PARAM(OdomOpen3D, MaxDepth, float, 3.0, "Maximum depth."); diff --git a/corelib/src/CMakeLists.txt b/corelib/src/CMakeLists.txt index 8f82257b..e6bf74c9 100644 --- a/corelib/src/CMakeLists.txt +++ b/corelib/src/CMakeLists.txt @@ -781,16 +781,16 @@ IF(vins_FOUND) ) ENDIF(vins_FOUND) -IF(ov_msckf_FOUND) +IF(OpenVINS_FOUND) SET(INCLUDE_DIRS - ${ov_msckf_INCLUDE_DIRS} + ${OpenVINS_INCLUDE_DIRS} ${INCLUDE_DIRS} ) SET(LIBRARIES - ${ov_msckf_LIBRARIES} + ${OpenVINS_LIBRARIES} ${LIBRARIES} ) -ENDIF(ov_msckf_FOUND) +ENDIF(OpenVINS_FOUND) IF(ORB_SLAM_FOUND) SET(INCLUDE_DIRS diff --git a/corelib/src/odometry/OdometryOpenVINS.cpp b/corelib/src/odometry/OdometryOpenVINS.cpp index da81f7ee..e9539688 100644 --- a/corelib/src/odometry/OdometryOpenVINS.cpp +++ b/corelib/src/odometry/OdometryOpenVINS.cpp @@ -152,6 +152,85 @@ OdometryOpenVINS::OdometryOpenVINS(const ParametersMap & parameters) : params_->init_options.sigma_wb = params_->imu_noises.sigma_wb; params_->init_options.sigma_pix = params_->slam_options.sigma_pix; params_->init_options.gravity_mag = params_->gravity_mag; + + if(parameters.find(Parameters::kOdomOpenVINSConfigPath()) != parameters.end()) + { + // Load the config: will override all parameters above! + std::string configPath = parameters.at(Parameters::kOdomOpenVINSConfigPath()); + if(!configPath.empty()) + { + if(UFile::exists(configPath)) + { + UWARN("OpenVINS config file is provided (%s=\"%s\"), reading it. The parameters from the config file will overwrite OdomOpenVINS/*** parameters.", + Parameters::kOdomOpenVINSConfigPath().c_str(), configPath.c_str()); + auto parser = std::make_shared(configPath); + + // The sequence of loading is based on VioManagerOptions::print_and_load() + // We removed all parts about intrinsics/extrinsics, which will be loaded later + // when we receive the data (which should already include intrinsics and extrinsics). + + params_->state_options.print(parser); + + params_->init_options.print_and_load_initializer(parser); + params_->init_options.print_and_load_noise(parser); + parser->parse_config("gravity_mag", params_->init_options.gravity_mag); + parser->parse_config("max_cameras", params_->init_options.num_cameras); + parser->parse_config("use_stereo", params_->init_options.use_stereo); + parser->parse_config("downsample_cameras", params_->init_options.downsample_cameras); + + parser->parse_config("dt_slam_delay", params_->dt_slam_delay); + parser->parse_config("try_zupt", params_->try_zupt); + parser->parse_config("zupt_max_velocity",params_-> zupt_max_velocity); + parser->parse_config("zupt_noise_multiplier", params_->zupt_noise_multiplier); + parser->parse_config("zupt_max_disparity", params_->zupt_max_disparity); + parser->parse_config("zupt_only_at_beginning", params_->zupt_only_at_beginning); + parser->parse_config("record_timing_information", params_->record_timing_information); + parser->parse_config("record_timing_filepath", params_->record_timing_filepath); + + params_->print_and_load_trackers(parser); + params_->print_and_load_noise(parser); + + if(params_->state_options.num_cameras > 2) + { + UFATAL("OpenVINS integration in RTAB-Map doesn't support more than 2 cameras (num_cameras=%d).", params_->state_options.num_cameras); + } + + parser->parse_config("gravity_mag", params_->gravity_mag); + parser->parse_config("use_mask", params_->use_mask); + params_->masks.clear(); + if (params_->use_mask) { + for (int i = 0; i < params_->state_options.num_cameras; i++) { + std::string mask_path; + std::string mask_node = "mask" + std::to_string(i); + parser->parse_config(mask_node, mask_path); + std::string total_mask_path = parser->get_config_folder() + mask_path; + if (!boost::filesystem::exists(total_mask_path)) { + PRINT_ERROR(RED "VioManager(): invalid mask path:\n" RESET); + PRINT_ERROR(RED "\t- mask%d - %s\n" RESET, i, total_mask_path.c_str()); + std::exit(EXIT_FAILURE); + } + params_->masks.emplace(i, cv::imread(total_mask_path, cv::IMREAD_GRAYSCALE)); + } + } + + if (!parser->successful()) { + UWARN("Not all expected OpenVINS parameters were read successfully " + "from \"%s\". Values from RTAB-Map's OpenOpenVINS/* parameters " + "will be used instead for the missing ones.", + configPath.c_str()); + } + else { + UINFO("OpenVINS config file(%s=\"%s\") read.", + Parameters::kOdomOpenVINSConfigPath().c_str(), configPath.c_str()); + } + } + else + { + UERROR("OpenVINS config file is provided (%s=\"%s\") but it doesn't exist!", + Parameters::kOdomOpenVINSConfigPath().c_str(), configPath.c_str()); + } + } + } #endif } diff --git a/guilib/include/rtabmap/gui/PreferencesDialog.h b/guilib/include/rtabmap/gui/PreferencesDialog.h index 65a105a2..5fef4cba 100644 --- a/guilib/include/rtabmap/gui/PreferencesDialog.h +++ b/guilib/include/rtabmap/gui/PreferencesDialog.h @@ -372,6 +372,7 @@ private Q_SLOTS: void changeOdometryORBSLAMVocabulary(); void changeOdometryOKVISConfigPath(); void changeOdometryVINSFusionConfigPath(); + void changeOdometryOpenVINSConfigPath(); void changeOdometryLIOSAMConfigPath(); void changeOdometryOpenVINSLeftMask(); void changeOdometryOpenVINSRightMask(); diff --git a/guilib/src/PreferencesDialog.cpp b/guilib/src/PreferencesDialog.cpp index e3da3c46..2ccdd09e 100644 --- a/guilib/src/PreferencesDialog.cpp +++ b/guilib/src/PreferencesDialog.cpp @@ -1617,6 +1617,8 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) : connect(_ui->toolButton_OdomVinsFusionPath, SIGNAL(clicked()), this, SLOT(changeOdometryVINSFusionConfigPath())); // Odometry OpenVINS + _ui->lineEdit_openvinsConfigPath->setObjectName(Parameters::kOdomOpenVINSConfigPath().c_str()); + connect(_ui->toolButton_openvinsConfigPath, SIGNAL(clicked()), this, SLOT(changeOdometryOpenVINSConfigPath())); _ui->checkBox_OdomOpenVINSUseStereo->setObjectName(Parameters::kOdomOpenVINSUseStereo().c_str()); _ui->checkBox_OdomOpenVINSUseKLT->setObjectName(Parameters::kOdomOpenVINSUseKLT().c_str()); _ui->spinBox_OdomOpenVINSNumPts->setObjectName(Parameters::kOdomOpenVINSNumPts().c_str()); @@ -5792,6 +5794,23 @@ void PreferencesDialog::changeOdometryVINSFusionConfigPath() } } +void PreferencesDialog::changeOdometryOpenVINSConfigPath() +{ + QString path; + if(_ui->lineEdit_openvinsConfigPath->text().isEmpty()) + { + path = QFileDialog::getOpenFileName(this, tr("OpenVINS Config"), this->getWorkingDirectory(), tr("OpenVINS config (*.yaml)")); + } + else + { + path = QFileDialog::getOpenFileName(this, tr("OpenVINS Config"), _ui->lineEdit_openvinsConfigPath->text(), tr("OpenVINS config (*.yaml)")); + } + if(!path.isEmpty()) + { + _ui->lineEdit_openvinsConfigPath->setText(path); + } +} + void PreferencesDialog::changeOdometryLIOSAMConfigPath() { QString path; diff --git a/guilib/src/ui/preferencesDialog.ui b/guilib/src/ui/preferencesDialog.ui index d03332e4..64eff092 100644 --- a/guilib/src/ui/preferencesDialog.ui +++ b/guilib/src/ui/preferencesDialog.ui @@ -63,7 +63,7 @@ 0 - 0 + -648 684 5218 @@ -95,7 +95,7 @@ QFrame::Raised - 18 + 19 @@ -16763,7 +16763,7 @@ With <0, the length is estimated once for each unique marker, then re-used fo - 1 + 10 @@ -20388,7 +20388,7 @@ With <0, the length is estimated once for each unique marker, then re-used fo OpenVINS - + @@ -20405,6 +20405,33 @@ With <0, the length is estimated once for each unique marker, then re-used fo + + + + + + ... + + + + + + + + + + Configuration file (*.yaml). Same format used than OpenVINS library. Note that any parameter from that config file will overwrite the same parameter below. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + @@ -20421,7 +20448,7 @@ With <0, the length is estimated once for each unique marker, then re-used fo - If we should process two cameras are being stereo or binocular. If binocular, we do monocular feature tracking on each image. + Stereo mode. If we should process two cameras are being stereo or binocular. If binocular, we do monocular feature tracking on each image. Ignored if provided input data is not stereo. true @@ -20441,7 +20468,7 @@ With <0, the length is estimated once for each unique marker, then re-used fo - If we should use KLT tracking, or descriptor matcher + KLT tracking. Uncheck to use descriptor matcher. true @@ -20467,7 +20494,7 @@ With <0, the length is estimated once for each unique marker, then re-used fo - Number of points (per camera) we will extract and try to track + Number of points (per camera) we will extract and try to track. true @@ -20490,7 +20517,7 @@ With <0, the length is estimated once for each unique marker, then re-used fo - Will check after doing KLT track and remove any features closer than this + Minimum pixel distance. Will check after doing KLT track and remove any features closer than this. true @@ -20510,7 +20537,7 @@ With <0, the length is estimated once for each unique marker, then re-used fo - If we should perform 1d triangulation instead of 3d + If we should perform 1d triangulation instead of 3d. true @@ -20530,7 +20557,7 @@ With <0, the length is estimated once for each unique marker, then re-used fo - If we should perform Levenberg-Marquardt refinement + If we should perform Levenberg-Marquardt refinement. true @@ -20553,7 +20580,7 @@ With <0, the length is estimated once for each unique marker, then re-used fo - Max runs for Levenberg-Marquardt + Max runs for Levenberg-Marquardt. true @@ -20579,7 +20606,7 @@ With <0, the length is estimated once for each unique marker, then re-used fo - Max baseline ratio to accept triangulated features + Max baseline ratio to accept triangulated features. true @@ -20605,7 +20632,7 @@ With <0, the length is estimated once for each unique marker, then re-used fo - Max condition number of linear triangulation matrix accept triangulated features + Max condition number of linear triangulation matrix accept triangulated features. true @@ -20634,7 +20661,7 @@ With <0, the length is estimated once for each unique marker, then re-used fo - If first-estimate Jacobians should be used (enable for good consistency) + If first-estimate Jacobians should be used (enable for good consistency). true @@ -20672,7 +20699,7 @@ With <0, the length is estimated once for each unique marker, then re-used fo - Numerical integration methods + Numerical integration methods. true @@ -20692,7 +20719,7 @@ With <0, the length is estimated once for each unique marker, then re-used fo - If the transform between camera and IMU should be optimized (R_ItoC, p_CinI) + If the transform between camera and IMU should be optimized (R_ItoC, p_CinI). true @@ -20712,7 +20739,7 @@ With <0, the length is estimated once for each unique marker, then re-used fo - If camera intrinsics should be optimized (focal, center, distortion) + If camera intrinsics should be optimized (focal, center, distortion). true @@ -20732,7 +20759,7 @@ With <0, the length is estimated once for each unique marker, then re-used fo - If timeoffset between camera and IMU should be optimized + If timeoffset between camera and IMU should be optimized. true @@ -20752,7 +20779,7 @@ With <0, the length is estimated once for each unique marker, then re-used fo - If imu intrinsics should be calibrated (rotation and skew-scale matrix) + If imu intrinsics should be calibrated (rotation and skew-scale matrix). true @@ -20772,7 +20799,7 @@ With <0, the length is estimated once for each unique marker, then re-used fo - If gyroscope gravity sensitivity (Tg) should be calibrated + If gyroscope gravity sensitivity (Tg) should be calibrated. true @@ -20795,7 +20822,7 @@ With <0, the length is estimated once for each unique marker, then re-used fo - Max clone size of sliding window + Max clone size of sliding window. true @@ -20821,7 +20848,7 @@ With <0, the length is estimated once for each unique marker, then re-used fo - Max number of estimated SLAM features + Max number of estimated SLAM features. true @@ -20926,7 +20953,7 @@ With <0, the length is estimated once for each unique marker, then re-used fo - What representation our features are in (msckf features) + What representation our features are in (msckf features). true @@ -20942,7 +20969,7 @@ With <0, the length is estimated once for each unique marker, then re-used fo 4 - QComboBox::AdjustToContents + QComboBox::AdjustToContentsOnFirstShow @@ -20979,7 +21006,7 @@ With <0, the length is estimated once for each unique marker, then re-used fo - What representation our features are in (slam features) + What representation our features are in (slam features). true @@ -21008,7 +21035,7 @@ With <0, the length is estimated once for each unique marker, then re-used fo - Delay before initializing (helps with stability from bad initialization...) + Delay before initializing (helps with stability from bad initialization...). true @@ -21034,7 +21061,7 @@ With <0, the length is estimated once for each unique marker, then re-used fo - Magnitude of gravity in this location + Magnitude of gravity in this location. true @@ -21061,7 +21088,7 @@ With <0, the length is estimated once for each unique marker, then re-used fo - Mask for left image + Mask for left image (stereo mode) or mono image (RGB-D mode). For RGB-D mode, to use depth as mask, enable that option under Visual Registration panel. true @@ -21088,7 +21115,7 @@ With <0, the length is estimated once for each unique marker, then re-used fo - Mask for right image + Mask for right image. true @@ -21126,7 +21153,7 @@ With <0, the length is estimated once for each unique marker, then re-used fo - Amount of time we will initialize over + Amount of time we will initialize over. true @@ -21155,7 +21182,7 @@ With <0, the length is estimated once for each unique marker, then re-used fo - Variance threshold on our acceleration to be classified as moving + Variance threshold on our acceleration to be classified as moving. true @@ -21181,7 +21208,7 @@ With <0, the length is estimated once for each unique marker, then re-used fo - Max disparity to consider the platform stationary (dependent on resolution) + Max disparity to consider the platform stationary (dependent on resolution). true @@ -21207,7 +21234,7 @@ With <0, the length is estimated once for each unique marker, then re-used fo - How many features to track during initialization (saves on computation) + How many features to track during initialization (saves on computation). true @@ -21227,7 +21254,7 @@ With <0, the length is estimated once for each unique marker, then re-used fo - If we should perform dynamic initialization + If we should perform dynamic initialization. true @@ -21247,7 +21274,7 @@ With <0, the length is estimated once for each unique marker, then re-used fo - If we should optimize and recover the calibration in our MLE + If we should optimize and recover the calibration in our MLE. true @@ -21270,7 +21297,7 @@ With <0, the length is estimated once for each unique marker, then re-used fo - Max number of MLE iterations for dynamic initialization + Max number of MLE iterations for dynamic initialization. true @@ -21299,7 +21326,7 @@ With <0, the length is estimated once for each unique marker, then re-used fo - Max time for MLE optimization + Max time for MLE optimization. true @@ -21322,7 +21349,7 @@ With <0, the length is estimated once for each unique marker, then re-used fo - Max number of MLE threads for dynamic initialization + Max number of MLE threads for dynamic initialization. true @@ -21345,7 +21372,7 @@ With <0, the length is estimated once for each unique marker, then re-used fo - Number of poses to use during initialization (max should be cam freq * window) + Number of poses to use during initialization (max should be cam freq * window). true @@ -21374,7 +21401,7 @@ With <0, the length is estimated once for each unique marker, then re-used fo - Minimum degrees we need to rotate before we try to init (sum of norm) + Minimum degrees we need to rotate before we try to init (sum of norm). true @@ -21400,7 +21427,7 @@ With <0, the length is estimated once for each unique marker, then re-used fo - Magnitude we will inflate initial covariance of orientation + Magnitude we will inflate initial covariance of orientation. true @@ -21426,7 +21453,7 @@ With <0, the length is estimated once for each unique marker, then re-used fo - Magnitude we will inflate initial covariance of velocity + Magnitude we will inflate initial covariance of velocity. true @@ -21452,7 +21479,7 @@ With <0, the length is estimated once for each unique marker, then re-used fo - Magnitude we will inflate initial covariance of gyroscope bias + Magnitude we will inflate initial covariance of gyroscope bias. true @@ -21478,7 +21505,7 @@ With <0, the length is estimated once for each unique marker, then re-used fo - Magnitude we will inflate initial covariance of accelerometer bias + Magnitude we will inflate initial covariance of accelerometer bias. true @@ -21507,7 +21534,7 @@ With <0, the length is estimated once for each unique marker, then re-used fo - Minimum reciprocal condition number acceptable for our covariance recovery + Minimum reciprocal condition number acceptable for our covariance recovery. true @@ -21536,7 +21563,7 @@ With <0, the length is estimated once for each unique marker, then re-used fo - If we should try to use zero velocity update + If we should try to use zero velocity update. true @@ -21565,7 +21592,7 @@ With <0, the length is estimated once for each unique marker, then re-used fo - Chi2 multiplier for zero velocity + Chi2 multiplier for zero velocity. true @@ -21594,7 +21621,7 @@ With <0, the length is estimated once for each unique marker, then re-used fo - Max velocity we will consider to try to do a zupt (i.e. if above this, don't do zupt) + Max velocity we will consider to try to do a zupt (i.e. if above this, don't do zupt). true @@ -21623,7 +21650,7 @@ With <0, the length is estimated once for each unique marker, then re-used fo - Multiplier of our zupt measurement IMU noise matrix (default should be 1.0) + Multiplier of our zupt measurement IMU noise matrix (default should be 1.0). true @@ -21652,7 +21679,7 @@ With <0, the length is estimated once for each unique marker, then re-used fo - Max disparity we will consider to try to do a zupt (i.e. if above this, don't do zupt) + Max disparity we will consider to try to do a zupt (i.e. if above this, don't do zupt). true @@ -21672,7 +21699,7 @@ With <0, the length is estimated once for each unique marker, then re-used fo - If we should only use the zupt at the very beginning static initialization phase + If we should only use the zupt at the very beginning static initialization phase. true @@ -21713,7 +21740,7 @@ With <0, the length is estimated once for each unique marker, then re-used fo - Accel "white noise" + Accel "white noise". true @@ -21745,7 +21772,7 @@ With <0, the length is estimated once for each unique marker, then re-used fo - Accel bias diffusion + Accel bias diffusion. true @@ -21777,7 +21804,7 @@ With <0, the length is estimated once for each unique marker, then re-used fo - Gyro "white noise" + Gyro "white noise". true @@ -21809,7 +21836,7 @@ With <0, the length is estimated once for each unique marker, then re-used fo - Gyro bias diffusion + Gyro bias diffusion. true @@ -21838,7 +21865,7 @@ With <0, the length is estimated once for each unique marker, then re-used fo - Pixel noise for MSCKF features + Pixel noise for MSCKF features. true @@ -21867,7 +21894,7 @@ With <0, the length is estimated once for each unique marker, then re-used fo - Chi2 multiplier for MSCKF features + Chi2 multiplier for MSCKF features. true @@ -21896,7 +21923,7 @@ With <0, the length is estimated once for each unique marker, then re-used fo - Pixel noise for SLAM features + Pixel noise for SLAM features. true @@ -21925,7 +21952,7 @@ With <0, the length is estimated once for each unique marker, then re-used fo - Chi2 multiplier for SLAM features + Chi2 multiplier for SLAM features. true