OpenVINS config file support (#1719)

This commit is contained in:
matlabbe
2026-06-13 16:20:37 -07:00
committed by GitHub
parent 1dddb350b9
commit 9f464db7c9
8 changed files with 293 additions and 137 deletions
+57 -56
View File
@@ -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.");
+4 -4
View File
@@ -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
+79
View File
@@ -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<ov_core::YamlParser>(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
}