From e65022c7799644954f18d2281e7969a3da1c1677 Mon Sep 17 00:00:00 2001 From: Borong Yuan Date: Tue, 10 Oct 2023 20:43:39 +0800 Subject: [PATCH 01/10] set the PrintLevel of OpenVINS to be equivalent to that of ULogger --- corelib/src/odometry/OdometryOpenVINS.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/corelib/src/odometry/OdometryOpenVINS.cpp b/corelib/src/odometry/OdometryOpenVINS.cpp index eaa8fc1ff1..92d71dd92b 100644 --- a/corelib/src/odometry/OdometryOpenVINS.cpp +++ b/corelib/src/odometry/OdometryOpenVINS.cpp @@ -49,7 +49,7 @@ OdometryOpenVINS::OdometryOpenVINS(const ParametersMap & parameters) : #endif { #ifdef RTABMAP_OPENVINS - ov_core::Printer::setPrintLevel("WARNING"); + ov_core::Printer::setPrintLevel(ov_core::Printer::PrintLevel(ULogger::level()+1)); int enum_index; params_ = std::make_unique(); Parameters::parse(parameters, Parameters::kOdomOpenVINSUseStereo(), params_->use_stereo); From 05d6bfaaf49cd80a07840cca47ad8bb72a868aa2 Mon Sep 17 00:00:00 2001 From: Borong Yuan Date: Tue, 10 Oct 2023 22:24:33 +0800 Subject: [PATCH 02/10] use global FAST settings --- corelib/include/rtabmap/core/Parameters.h | 5 +- corelib/src/odometry/OdometryOpenVINS.cpp | 6 +- guilib/src/PreferencesDialog.cpp | 3 - guilib/src/ui/preferencesDialog.ui | 80 ++--------------------- 4 files changed, 8 insertions(+), 86 deletions(-) diff --git a/corelib/include/rtabmap/core/Parameters.h b/corelib/include/rtabmap/core/Parameters.h index 8631a9d3a5..ff27001904 100644 --- a/corelib/include/rtabmap/core/Parameters.h +++ b/corelib/include/rtabmap/core/Parameters.h @@ -580,9 +580,6 @@ class RTABMAP_CORE_EXPORT Parameters 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, FastThreshold, int, 30, "Threshold for fast extraction (warning: lower threshs can be expensive)"); - RTABMAP_PARAM(OdomOpenVINS, GridX, int, 5, "Extraction sub-grid count for horizontal direction (uniform tracking)"); - RTABMAP_PARAM(OdomOpenVINS, GridY, int, 5, "Extraction sub-grid count for vertical direction (uniform tracking)"); RTABMAP_PARAM(OdomOpenVINS, MinPxDist, int, 15, "Eistance between features (features near each other provide less information)"); RTABMAP_PARAM(OdomOpenVINS, KNNRatio, double, 0.7, "Descriptor knn threshold for the top two descriptor matches"); @@ -594,7 +591,7 @@ class RTABMAP_CORE_EXPORT Parameters 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, 2.0, "Delay, in seconds, that we should wait from init before we start estimating 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(OdomOpenVINS, InitWindowTime, double, 2.0, "Amount of time we will initialize over (seconds)"); diff --git a/corelib/src/odometry/OdometryOpenVINS.cpp b/corelib/src/odometry/OdometryOpenVINS.cpp index 92d71dd92b..25b435e04d 100644 --- a/corelib/src/odometry/OdometryOpenVINS.cpp +++ b/corelib/src/odometry/OdometryOpenVINS.cpp @@ -55,9 +55,9 @@ OdometryOpenVINS::OdometryOpenVINS(const ParametersMap & parameters) : Parameters::parse(parameters, Parameters::kOdomOpenVINSUseStereo(), params_->use_stereo); Parameters::parse(parameters, Parameters::kOdomOpenVINSUseKLT(), params_->use_klt); Parameters::parse(parameters, Parameters::kOdomOpenVINSNumPts(), params_->num_pts); - Parameters::parse(parameters, Parameters::kOdomOpenVINSFastThreshold(), params_->fast_threshold); - Parameters::parse(parameters, Parameters::kOdomOpenVINSGridX(), params_->grid_x); - Parameters::parse(parameters, Parameters::kOdomOpenVINSGridY(), params_->grid_y); + Parameters::parse(parameters, Parameters::kFASTThreshold(), params_->fast_threshold); + Parameters::parse(parameters, Parameters::kVisGridCols(), params_->grid_x); + Parameters::parse(parameters, Parameters::kVisGridRows(), params_->grid_y); Parameters::parse(parameters, Parameters::kOdomOpenVINSMinPxDist(), params_->min_px_dist); Parameters::parse(parameters, Parameters::kOdomOpenVINSKNNRatio(), params_->knn_ratio); Parameters::parse(parameters, Parameters::kOdomOpenVINSUseFEJ(), params_->state_options.do_fej); diff --git a/guilib/src/PreferencesDialog.cpp b/guilib/src/PreferencesDialog.cpp index e6ce497e1d..c1c05f9387 100644 --- a/guilib/src/PreferencesDialog.cpp +++ b/guilib/src/PreferencesDialog.cpp @@ -1437,9 +1437,6 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) : _ui->checkBox_OdomOpenVINSUseStereo->setObjectName(Parameters::kOdomOpenVINSUseStereo().c_str()); _ui->checkBox_OdomOpenVINSUseKLT->setObjectName(Parameters::kOdomOpenVINSUseKLT().c_str()); _ui->spinBox_OdomOpenVINSNumPts->setObjectName(Parameters::kOdomOpenVINSNumPts().c_str()); - _ui->spinBox_OdomOpenVINSFastThreshold->setObjectName(Parameters::kOdomOpenVINSFastThreshold().c_str()); - _ui->spinBox_OdomOpenVINSGridX->setObjectName(Parameters::kOdomOpenVINSGridX().c_str()); - _ui->spinBox_OdomOpenVINSGridY->setObjectName(Parameters::kOdomOpenVINSGridY().c_str()); _ui->spinBox_OdomOpenVINSMinPxDist->setObjectName(Parameters::kOdomOpenVINSMinPxDist().c_str()); _ui->doubleSpinBox_OdomOpenVINSKNNRatio->setObjectName(Parameters::kOdomOpenVINSKNNRatio().c_str()); diff --git a/guilib/src/ui/preferencesDialog.ui b/guilib/src/ui/preferencesDialog.ui index 624b8b31d7..aa591d23a2 100644 --- a/guilib/src/ui/preferencesDialog.ui +++ b/guilib/src/ui/preferencesDialog.ui @@ -18024,78 +18024,6 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - - 1 - - - 9999 - - - 30 - - - - - - - Fast extraction threshold - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - 1 - - - 5 - - - - - - - Number of grids we should split column-wise to do feature extraction in - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - 1 - - - 5 - - - - - - - Number of grids we should split row-wise to do feature extraction in - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - 1 @@ -18105,7 +18033,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - + Will check after doing KLT track and remove any features closer than this @@ -18118,7 +18046,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - + 0.010000000000000 @@ -18134,7 +18062,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - + KNN ration between top two descriptor matcher which is required to be a good match @@ -18433,7 +18361,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag 0.500000000000000 - 2.000000000000000 + 0.000000000000000 From d27eb3e65bd38aadb1274e8e7342b6a1b35c2b94 Mon Sep 17 00:00:00 2001 From: Borong Yuan Date: Wed, 11 Oct 2023 11:35:46 +0800 Subject: [PATCH 03/10] add online calibration params --- corelib/include/rtabmap/core/Parameters.h | 5 + corelib/src/odometry/OdometryOpenVINS.cpp | 5 + guilib/src/PreferencesDialog.cpp | 5 + guilib/src/ui/preferencesDialog.ui | 130 +++++++++++++++++++--- 4 files changed, 130 insertions(+), 15 deletions(-) diff --git a/corelib/include/rtabmap/core/Parameters.h b/corelib/include/rtabmap/core/Parameters.h index ff27001904..b66a376186 100644 --- a/corelib/include/rtabmap/core/Parameters.h +++ b/corelib/include/rtabmap/core/Parameters.h @@ -585,6 +585,11 @@ class RTABMAP_CORE_EXPORT Parameters 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."); diff --git a/corelib/src/odometry/OdometryOpenVINS.cpp b/corelib/src/odometry/OdometryOpenVINS.cpp index 25b435e04d..62a06a48e3 100644 --- a/corelib/src/odometry/OdometryOpenVINS.cpp +++ b/corelib/src/odometry/OdometryOpenVINS.cpp @@ -63,6 +63,11 @@ OdometryOpenVINS::OdometryOpenVINS(const ParametersMap & parameters) : Parameters::parse(parameters, Parameters::kOdomOpenVINSUseFEJ(), params_->state_options.do_fej); Parameters::parse(parameters, Parameters::kOdomOpenVINSIntegration(), enum_index); params_->state_options.integration_method = ov_msckf::StateOptions::IntegrationMethod(enum_index); + Parameters::parse(parameters, Parameters::kOdomOpenVINSCalibCamExtrinsics(), params_->state_options.do_calib_camera_pose); + Parameters::parse(parameters, Parameters::kOdomOpenVINSCalibCamIntrinsics(), params_->state_options.do_calib_camera_intrinsics); + Parameters::parse(parameters, Parameters::kOdomOpenVINSCalibCamTimeoffset(), params_->state_options.do_calib_camera_timeoffset); + Parameters::parse(parameters, Parameters::kOdomOpenVINSCalibIMUIntrinsics(), params_->state_options.do_calib_imu_intrinsics); + Parameters::parse(parameters, Parameters::kOdomOpenVINSCalibIMUGSensitivity(), params_->state_options.do_calib_imu_g_sensitivity); Parameters::parse(parameters, Parameters::kOdomOpenVINSMaxClones(), params_->state_options.max_clone_size); Parameters::parse(parameters, Parameters::kOdomOpenVINSMaxSLAM(), params_->state_options.max_slam_features); Parameters::parse(parameters, Parameters::kOdomOpenVINSMaxSLAMInUpdate(), params_->state_options.max_slam_in_update); diff --git a/guilib/src/PreferencesDialog.cpp b/guilib/src/PreferencesDialog.cpp index c1c05f9387..da73e8c2c8 100644 --- a/guilib/src/PreferencesDialog.cpp +++ b/guilib/src/PreferencesDialog.cpp @@ -1442,6 +1442,11 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) : _ui->checkBox_OdomOpenVINSUseFEJ->setObjectName(Parameters::kOdomOpenVINSUseFEJ().c_str()); _ui->comboBox_OdomOpenVINSIntegration->setObjectName(Parameters::kOdomOpenVINSIntegration().c_str()); + _ui->checkBox_OdomOpenVINSCalibCamExtrinsics->setObjectName(Parameters::kOdomOpenVINSCalibCamExtrinsics().c_str()); + _ui->checkBox_OdomOpenVINSCalibCamIntrinsics->setObjectName(Parameters::kOdomOpenVINSCalibCamIntrinsics().c_str()); + _ui->checkBox_OdomOpenVINSCalibCamTimeoffset->setObjectName(Parameters::kOdomOpenVINSCalibCamTimeoffset().c_str()); + _ui->checkBox_OdomOpenVINSCalibIMUIntrinsics->setObjectName(Parameters::kOdomOpenVINSCalibIMUIntrinsics().c_str()); + _ui->checkBox_OdomOpenVINSCalibIMUGSensitivity->setObjectName(Parameters::kOdomOpenVINSCalibIMUGSensitivity().c_str()); _ui->spinBox_OdomOpenVINSMaxClones->setObjectName(Parameters::kOdomOpenVINSMaxClones().c_str()); _ui->spinBox_OdomOpenVINSMaxSLAM->setObjectName(Parameters::kOdomOpenVINSMaxSLAM().c_str()); _ui->spinBox_OdomOpenVINSMaxSLAMInUpdate->setObjectName(Parameters::kOdomOpenVINSMaxSLAMInUpdate().c_str()); diff --git a/guilib/src/ui/preferencesDialog.ui b/guilib/src/ui/preferencesDialog.ui index aa591d23a2..f1478aab2d 100644 --- a/guilib/src/ui/preferencesDialog.ui +++ b/guilib/src/ui/preferencesDialog.ui @@ -18143,6 +18143,106 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag + + + + + + + + + + If the transform between camera and IMU should be optimized (R_ItoC, p_CinI) + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + + + + + + If camera intrinsics should be optimized (focal, center, distortion) + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + + + + + + If timeoffset between camera and IMU should be optimized + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + + + + + + If imu intrinsics should be calibrated (rotation and skew-scale matrix) + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + + + + + + If gyroscope gravity sensitivity (Tg) should be calibrated + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + 1 @@ -18152,7 +18252,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - + Max clone size of sliding window @@ -18165,7 +18265,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - + 0 @@ -18178,7 +18278,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - + Max number of estimated SLAM features @@ -18191,7 +18291,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - + 1 @@ -18204,7 +18304,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - + Max number of SLAM features we allow to be included in a single EKF update. @@ -18217,7 +18317,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - + 0 @@ -18230,7 +18330,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - + Max number of MSCKF features we will use at a given image timestep. @@ -18243,7 +18343,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - + 0 @@ -18283,7 +18383,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - + What representation our features are in (msckf features) @@ -18296,7 +18396,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - + 4 @@ -18336,7 +18436,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - + What representation our features are in (slam features) @@ -18349,7 +18449,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - + s @@ -18365,7 +18465,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - + Delay before initializing (helps with stability from bad initialization...) @@ -18378,7 +18478,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - + 5 @@ -18391,7 +18491,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - + Magnitude of gravity in this location From b635c47f31c3591bdfba2ff35051afcd4c230b02 Mon Sep 17 00:00:00 2001 From: Borong Yuan Date: Wed, 11 Oct 2023 14:34:40 +0800 Subject: [PATCH 04/10] add dynamic initialization params --- corelib/include/rtabmap/core/Parameters.h | 12 + corelib/src/odometry/OdometryOpenVINS.cpp | 12 + guilib/src/PreferencesDialog.cpp | 12 + guilib/src/ui/preferencesDialog.ui | 300 ++++++++++++++++++++++ 4 files changed, 336 insertions(+) diff --git a/corelib/include/rtabmap/core/Parameters.h b/corelib/include/rtabmap/core/Parameters.h index b66a376186..5353ba0e2a 100644 --- a/corelib/include/rtabmap/core/Parameters.h +++ b/corelib/include/rtabmap/core/Parameters.h @@ -603,6 +603,18 @@ class RTABMAP_CORE_EXPORT Parameters 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"); diff --git a/corelib/src/odometry/OdometryOpenVINS.cpp b/corelib/src/odometry/OdometryOpenVINS.cpp index 62a06a48e3..927a53bcc5 100644 --- a/corelib/src/odometry/OdometryOpenVINS.cpp +++ b/corelib/src/odometry/OdometryOpenVINS.cpp @@ -82,6 +82,18 @@ OdometryOpenVINS::OdometryOpenVINS(const ParametersMap & parameters) : Parameters::parse(parameters, Parameters::kOdomOpenVINSInitIMUThresh(), params_->init_options.init_imu_thresh); Parameters::parse(parameters, Parameters::kOdomOpenVINSInitMaxDisparity(), params_->init_options.init_max_disparity); Parameters::parse(parameters, Parameters::kOdomOpenVINSInitMaxFeatures(), params_->init_options.init_max_features); + Parameters::parse(parameters, Parameters::kOdomOpenVINSInitDynUse(), params_->init_options.init_dyn_use); + Parameters::parse(parameters, Parameters::kOdomOpenVINSInitDynMLEOptCalib(), params_->init_options.init_dyn_mle_opt_calib); + Parameters::parse(parameters, Parameters::kOdomOpenVINSInitDynMLEMaxIter(), params_->init_options.init_dyn_mle_max_iter); + Parameters::parse(parameters, Parameters::kOdomOpenVINSInitDynMLEMaxTime(), params_->init_options.init_dyn_mle_max_time); + Parameters::parse(parameters, Parameters::kOdomOpenVINSInitDynMLEMaxThreads(), params_->init_options.init_dyn_mle_max_threads); + Parameters::parse(parameters, Parameters::kOdomOpenVINSInitDynNumPose(), params_->init_options.init_dyn_num_pose); + Parameters::parse(parameters, Parameters::kOdomOpenVINSInitDynMinDeg(), params_->init_options.init_dyn_min_deg); + Parameters::parse(parameters, Parameters::kOdomOpenVINSInitDynInflationOri(), params_->init_options.init_dyn_inflation_orientation); + Parameters::parse(parameters, Parameters::kOdomOpenVINSInitDynInflationVel(), params_->init_options.init_dyn_inflation_velocity); + Parameters::parse(parameters, Parameters::kOdomOpenVINSInitDynInflationBg(), params_->init_options.init_dyn_inflation_bias_gyro); + Parameters::parse(parameters, Parameters::kOdomOpenVINSInitDynInflationBa(), params_->init_options.init_dyn_inflation_bias_accel); + Parameters::parse(parameters, Parameters::kOdomOpenVINSInitDynMinRecCond(), params_->init_options.init_dyn_min_rec_cond); Parameters::parse(parameters, Parameters::kOdomOpenVINSTryZUPT(), params_->try_zupt); Parameters::parse(parameters, Parameters::kOdomOpenVINSZUPTChi2Multiplier(), params_->zupt_options.chi2_multipler); Parameters::parse(parameters, Parameters::kOdomOpenVINSZUPTMaxVelodicy(), params_->zupt_max_velocity); diff --git a/guilib/src/PreferencesDialog.cpp b/guilib/src/PreferencesDialog.cpp index da73e8c2c8..afbcb28313 100644 --- a/guilib/src/PreferencesDialog.cpp +++ b/guilib/src/PreferencesDialog.cpp @@ -1460,6 +1460,18 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) : _ui->doubleSpinBox_OdomOpenVINSInitIMUThresh->setObjectName(Parameters::kOdomOpenVINSInitIMUThresh().c_str()); _ui->doubleSpinBox_OdomOpenVINSInitMaxDisparity->setObjectName(Parameters::kOdomOpenVINSInitMaxDisparity().c_str()); _ui->spinBox_OdomOpenVINSInitMaxFeatures->setObjectName(Parameters::kOdomOpenVINSInitMaxFeatures().c_str()); + _ui->checkBox_OdomOpenVINSInitDynUse->setObjectName(Parameters::kOdomOpenVINSInitDynUse().c_str()); + _ui->checkBox_OdomOpenVINSInitDynMLEOptCalib->setObjectName(Parameters::kOdomOpenVINSInitDynMLEOptCalib().c_str()); + _ui->spinBox_OdomOpenVINSInitDynMLEMaxIter->setObjectName(Parameters::kOdomOpenVINSInitDynMLEMaxIter().c_str()); + _ui->doubleSpinBox_OdomOpenVINSInitDynMLEMaxTime->setObjectName(Parameters::kOdomOpenVINSInitDynMLEMaxTime().c_str()); + _ui->spinBox_OdomOpenVINSInitDynMLEMaxThreads->setObjectName(Parameters::kOdomOpenVINSInitDynMLEMaxThreads().c_str()); + _ui->spinBox_OdomOpenVINSInitDynNumPose->setObjectName(Parameters::kOdomOpenVINSInitDynNumPose().c_str()); + _ui->doubleSpinBox_OdomOpenVINSInitDynMinDeg->setObjectName(Parameters::kOdomOpenVINSInitDynMinDeg().c_str()); + _ui->doubleSpinBox_OdomOpenVINSInitDynInflationOri->setObjectName(Parameters::kOdomOpenVINSInitDynInflationOri().c_str()); + _ui->doubleSpinBox_OdomOpenVINSInitDynInflationVel->setObjectName(Parameters::kOdomOpenVINSInitDynInflationVel().c_str()); + _ui->doubleSpinBox_OdomOpenVINSInitDynInflationBg->setObjectName(Parameters::kOdomOpenVINSInitDynInflationBg().c_str()); + _ui->doubleSpinBox_OdomOpenVINSInitDynInflationBa->setObjectName(Parameters::kOdomOpenVINSInitDynInflationBa().c_str()); + _ui->doubleSpinBox_OdomOpenVINSInitDynMinRecCond->setObjectName(Parameters::kOdomOpenVINSInitDynMinRecCond().c_str()); _ui->checkBox_OdomOpenVINSTryZUPT->setObjectName(Parameters::kOdomOpenVINSTryZUPT().c_str()); _ui->doubleSpinBox_OdomOpenVINSZUPTChi2Multiplier->setObjectName(Parameters::kOdomOpenVINSZUPTChi2Multiplier().c_str()); diff --git a/guilib/src/ui/preferencesDialog.ui b/guilib/src/ui/preferencesDialog.ui index f1478aab2d..d55f343e3c 100644 --- a/guilib/src/ui/preferencesDialog.ui +++ b/guilib/src/ui/preferencesDialog.ui @@ -18623,6 +18623,306 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag + + + + + + + + + + + If we should perform dynamic initialization + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + + + + + + If we should optimize and recover the calibration in our MLE + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + 0 + + + 50 + + + + + + + Max number of MLE iterations for dynamic initialization + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + s + + + 0.000000000000000 + + + 0.010000000000000 + + + 0.050000000000000 + + + + + + + Max time for MLE optimization + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + 1 + + + 6 + + + + + + + Max number of MLE threads for dynamic initialization + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + 4 + + + 6 + + + + + + + Number of poses to use during initialization (max should be cam freq * window) + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + deg + + + 0.000000000000000 + + + 45.000000000000000 + + + 10.000000000000000 + + + + + + + Minimum degrees we need to rotate before we try to init (sum of norm) + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + 1.000000000000000 + + + 999.000000000000000 + + + 10.000000000000000 + + + + + + + Magnitude we will inflate initial covariance of orientation + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + 1.000000000000000 + + + 999.000000000000000 + + + 100.000000000000000 + + + + + + + Magnitude we will inflate initial covariance of velocity + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + 1.000000000000000 + + + 999.000000000000000 + + + 10.000000000000000 + + + + + + + Magnitude we will inflate initial covariance of gyroscope bias + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + 1.000000000000000 + + + 999.000000000000000 + + + 100.000000000000000 + + + + + + + Magnitude we will inflate initial covariance of accelerometer bias + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + 0.000000000000000 + + + 1.000000000000000 + + + 0.000000000001000 + + + 0.000000000000001 + + + + + + + Minimum reciprocal condition number acceptable for our covariance recovery + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + From 30c521b3429e7e7b97b5099cc69195eb8c217618 Mon Sep 17 00:00:00 2001 From: Borong Yuan Date: Wed, 11 Oct 2023 22:03:01 +0800 Subject: [PATCH 05/10] add necessary feature initializer options --- corelib/include/rtabmap/core/Parameters.h | 80 ++++++++-------- corelib/src/odometry/OdometryOpenVINS.cpp | 9 +- guilib/src/PreferencesDialog.cpp | 6 +- guilib/src/ui/preferencesDialog.ui | 106 ++++++++++++++++++++-- 4 files changed, 151 insertions(+), 50 deletions(-) diff --git a/corelib/include/rtabmap/core/Parameters.h b/corelib/include/rtabmap/core/Parameters.h index 5353ba0e2a..329d35c113 100644 --- a/corelib/include/rtabmap/core/Parameters.h +++ b/corelib/include/rtabmap/core/Parameters.h @@ -581,7 +581,11 @@ class RTABMAP_CORE_EXPORT Parameters 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, KNNRatio, double, 0.7, "Descriptor knn threshold for the top two descriptor matches"); + 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)"); @@ -642,56 +646,56 @@ class RTABMAP_CORE_EXPORT Parameters RTABMAP_PARAM(Reg, Force3DoF, bool, false, "Force 3 degrees-of-freedom transform (3Dof: x,y and yaw). Parameters z, roll and pitch will be set to 0."); // Visual registration parameters - RTABMAP_PARAM(Vis, EstimationType, int, 1, "Motion estimation approach: 0:3D->3D, 1:3D->2D (PnP), 2:2D->2D (Epipolar Geometry)"); - RTABMAP_PARAM(Vis, ForwardEstOnly, bool, true, "Forward estimation only (A->B). If false, a transformation is also computed in backward direction (B->A), then the two resulting transforms are merged (middle interpolation between the transforms)."); - RTABMAP_PARAM(Vis, InlierDistance, float, 0.1, uFormat("[%s = 0] Maximum distance for feature correspondences. Used by 3D->3D estimation approach.", kVisEstimationType().c_str())); - RTABMAP_PARAM(Vis, RefineIterations, int, 5, uFormat("[%s = 0] Number of iterations used to refine the transformation found by RANSAC. 0 means that the transformation is not refined.", kVisEstimationType().c_str())); - RTABMAP_PARAM(Vis, PnPReprojError, float, 2, uFormat("[%s = 1] PnP reprojection error.", kVisEstimationType().c_str())); - RTABMAP_PARAM(Vis, PnPFlags, int, 0, uFormat("[%s = 1] PnP flags: 0=Iterative, 1=EPNP, 2=P3P", kVisEstimationType().c_str())); + RTABMAP_PARAM(Vis, EstimationType, int, 1, "Motion estimation approach: 0:3D->3D, 1:3D->2D (PnP), 2:2D->2D (Epipolar Geometry)"); + RTABMAP_PARAM(Vis, ForwardEstOnly, bool, true, "Forward estimation only (A->B). If false, a transformation is also computed in backward direction (B->A), then the two resulting transforms are merged (middle interpolation between the transforms)."); + RTABMAP_PARAM(Vis, InlierDistance, float, 0.1, uFormat("[%s = 0] Maximum distance for feature correspondences. Used by 3D->3D estimation approach.", kVisEstimationType().c_str())); + RTABMAP_PARAM(Vis, RefineIterations, int, 5, uFormat("[%s = 0] Number of iterations used to refine the transformation found by RANSAC. 0 means that the transformation is not refined.", kVisEstimationType().c_str())); + RTABMAP_PARAM(Vis, PnPReprojError, float, 2, uFormat("[%s = 1] PnP reprojection error.", kVisEstimationType().c_str())); + RTABMAP_PARAM(Vis, PnPFlags, int, 0, uFormat("[%s = 1] PnP flags: 0=Iterative, 1=EPNP, 2=P3P", kVisEstimationType().c_str())); #if defined(RTABMAP_G2O) || defined(RTABMAP_ORB_SLAM) - RTABMAP_PARAM(Vis, PnPRefineIterations, int, 0, uFormat("[%s = 1] Refine iterations. Set to 0 if \"%s\" is also used.", kVisEstimationType().c_str(), kVisBundleAdjustment().c_str())); + RTABMAP_PARAM(Vis, PnPRefineIterations, int, 0, uFormat("[%s = 1] Refine iterations. Set to 0 if \"%s\" is also used.", kVisEstimationType().c_str(), kVisBundleAdjustment().c_str())); #else - RTABMAP_PARAM(Vis, PnPRefineIterations, int, 1, uFormat("[%s = 1] Refine iterations. Set to 0 if \"%s\" is also used.", kVisEstimationType().c_str(), kVisBundleAdjustment().c_str())); + RTABMAP_PARAM(Vis, PnPRefineIterations, int, 1, uFormat("[%s = 1] Refine iterations. Set to 0 if \"%s\" is also used.", kVisEstimationType().c_str(), kVisBundleAdjustment().c_str())); #endif - RTABMAP_PARAM(Vis, PnPVarianceMedianRatio, int, 4, uFormat("[%s = 1] Ratio used to compute variance of the estimated transformation if 3D correspondences are provided (should be > 1). The higher it is, the smaller the covariance will be. With accurate depth estimation, this could be set to 2. For depth estimated by stereo, 4 or more maybe used to ignore large errors of very far points.", kVisEstimationType().c_str())); - RTABMAP_PARAM(Vis, PnPMaxVariance, float, 0.0, uFormat("[%s = 1] Max linear variance between 3D point correspondences after PnP. 0 means disabled.", kVisEstimationType().c_str())); - RTABMAP_PARAM(Vis, PnPSamplingPolicy, unsigned int, 1, uFormat("[%s = 1] Multi-camera random sampling policy: 0=AUTO, 1=ANY, 2=HOMOGENEOUS. With HOMOGENEOUS policy, RANSAC will be done uniformly against all cameras, so at least 2 matches per camera are required. With ANY policy, RANSAC is not constraint to sample on all cameras at the same time. AUTO policy will use HOMOGENEOUS if there are at least 2 matches per camera, otherwise it will fallback to ANY policy.", kVisEstimationType().c_str()).c_str()); + RTABMAP_PARAM(Vis, PnPVarianceMedianRatio, int, 4, uFormat("[%s = 1] Ratio used to compute variance of the estimated transformation if 3D correspondences are provided (should be > 1). The higher it is, the smaller the covariance will be. With accurate depth estimation, this could be set to 2. For depth estimated by stereo, 4 or more maybe used to ignore large errors of very far points.", kVisEstimationType().c_str())); + RTABMAP_PARAM(Vis, PnPMaxVariance, float, 0.0, uFormat("[%s = 1] Max linear variance between 3D point correspondences after PnP. 0 means disabled.", kVisEstimationType().c_str())); + RTABMAP_PARAM(Vis, PnPSamplingPolicy, unsigned int, 1, uFormat("[%s = 1] Multi-camera random sampling policy: 0=AUTO, 1=ANY, 2=HOMOGENEOUS. With HOMOGENEOUS policy, RANSAC will be done uniformly against all cameras, so at least 2 matches per camera are required. With ANY policy, RANSAC is not constraint to sample on all cameras at the same time. AUTO policy will use HOMOGENEOUS if there are at least 2 matches per camera, otherwise it will fallback to ANY policy.", kVisEstimationType().c_str()).c_str()); - RTABMAP_PARAM(Vis, EpipolarGeometryVar, float, 0.1, uFormat("[%s = 2] Epipolar geometry maximum variance to accept the transformation.", kVisEstimationType().c_str())); - RTABMAP_PARAM(Vis, MinInliers, int, 20, "Minimum feature correspondences to compute/accept the transformation."); - RTABMAP_PARAM(Vis, MeanInliersDistance, float, 0.0, "Maximum distance (m) of the mean distance of inliers from the camera to accept the transformation. 0 means disabled."); - RTABMAP_PARAM(Vis, MinInliersDistribution, float, 0.0, "Minimum distribution value of the inliers in the image to accept the transformation. The distribution is the second eigen value of the PCA (Principal Component Analysis) on the keypoints of the normalized image [-0.5, 0.5]. The value would be between 0 and 0.5. 0 means disabled."); + RTABMAP_PARAM(Vis, EpipolarGeometryVar, float, 0.1, uFormat("[%s = 2] Epipolar geometry maximum variance to accept the transformation.", kVisEstimationType().c_str())); + RTABMAP_PARAM(Vis, MinInliers, int, 20, "Minimum feature correspondences to compute/accept the transformation."); + RTABMAP_PARAM(Vis, MeanInliersDistance, float, 0.0, "Maximum distance (m) of the mean distance of inliers from the camera to accept the transformation. 0 means disabled."); + RTABMAP_PARAM(Vis, MinInliersDistribution, float, 0.0, "Minimum distribution value of the inliers in the image to accept the transformation. The distribution is the second eigen value of the PCA (Principal Component Analysis) on the keypoints of the normalized image [-0.5, 0.5]. The value would be between 0 and 0.5. 0 means disabled."); - RTABMAP_PARAM(Vis, Iterations, int, 300, "Maximum iterations to compute the transform."); + RTABMAP_PARAM(Vis, Iterations, int, 300, "Maximum iterations to compute the transform."); #if CV_MAJOR_VERSION > 2 && !defined(HAVE_OPENCV_XFEATURES2D) // OpenCV>2 without xFeatures2D module doesn't have BRIEF RTABMAP_PARAM(Vis, FeatureType, int, 8, "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB 9=KAZE 10=ORB-OCTREE 11=SuperPoint 12=SURF/FREAK 13=GFTT/DAISY 14=SURF/DAISY 15=PyDetector"); #else RTABMAP_PARAM(Vis, FeatureType, int, 6, "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB 9=KAZE 10=ORB-OCTREE 11=SuperPoint 12=SURF/FREAK 13=GFTT/DAISY 14=SURF/DAISY 15=PyDetector"); #endif - RTABMAP_PARAM(Vis, MaxFeatures, int, 1000, "0 no limits."); - RTABMAP_PARAM(Vis, MaxDepth, float, 0, "Max depth of the features (0 means no limit)."); - RTABMAP_PARAM(Vis, MinDepth, float, 0, "Min depth of the features (0 means no limit)."); - RTABMAP_PARAM(Vis, DepthAsMask, bool, true, "Use depth image as mask when extracting features."); - RTABMAP_PARAM_STR(Vis, RoiRatios, "0.0 0.0 0.0 0.0", "Region of interest ratios [left, right, top, bottom]."); - RTABMAP_PARAM(Vis, SubPixWinSize, int, 3, "See cv::cornerSubPix()."); - RTABMAP_PARAM(Vis, SubPixIterations, int, 0, "See cv::cornerSubPix(). 0 disables sub pixel refining."); - RTABMAP_PARAM(Vis, SubPixEps, float, 0.02, "See cv::cornerSubPix()."); - RTABMAP_PARAM(Vis, GridRows, int, 1, uFormat("Number of rows of the grid used to extract uniformly \"%s / grid cells\" features from each cell.", kVisMaxFeatures().c_str())); - RTABMAP_PARAM(Vis, GridCols, int, 1, uFormat("Number of columns of the grid used to extract uniformly \"%s / grid cells\" features from each cell.", kVisMaxFeatures().c_str())); - RTABMAP_PARAM(Vis, CorType, int, 0, "Correspondences computation approach: 0=Features Matching, 1=Optical Flow"); - RTABMAP_PARAM(Vis, CorNNType, int, 1, uFormat("[%s=0] kNNFlannNaive=0, kNNFlannKdTree=1, kNNFlannLSH=2, kNNBruteForce=3, kNNBruteForceGPU=4, BruteForceCrossCheck=5, SuperGlue=6, GMS=7. Used for features matching approach.", kVisCorType().c_str())); - RTABMAP_PARAM(Vis, CorNNDR, float, 0.8, uFormat("[%s=0] NNDR: nearest neighbor distance ratio. Used for knn features matching approach.", kVisCorType().c_str())); - RTABMAP_PARAM(Vis, CorGuessWinSize, int, 40, uFormat("[%s=0] Matching window size (pixels) around projected points when a guess transform is provided to find correspondences. 0 means disabled.", kVisCorType().c_str())); - RTABMAP_PARAM(Vis, CorGuessMatchToProjection, bool, false, uFormat("[%s=0] Match frame's corners to source's projected points (when guess transform is provided) instead of projected points to frame's corners.", kVisCorType().c_str())); - RTABMAP_PARAM(Vis, CorFlowWinSize, int, 16, uFormat("[%s=1] See cv::calcOpticalFlowPyrLK(). Used for optical flow approach.", kVisCorType().c_str())); - RTABMAP_PARAM(Vis, CorFlowIterations, int, 30, uFormat("[%s=1] See cv::calcOpticalFlowPyrLK(). Used for optical flow approach.", kVisCorType().c_str())); - RTABMAP_PARAM(Vis, CorFlowEps, float, 0.01, uFormat("[%s=1] See cv::calcOpticalFlowPyrLK(). Used for optical flow approach.", kVisCorType().c_str())); - RTABMAP_PARAM(Vis, CorFlowMaxLevel, int, 3, uFormat("[%s=1] See cv::calcOpticalFlowPyrLK(). Used for optical flow approach.", kVisCorType().c_str())); + RTABMAP_PARAM(Vis, MaxFeatures, int, 1000, "0 no limits."); + RTABMAP_PARAM(Vis, MaxDepth, float, 60.0, "Max depth of the features (0 means no limit)."); + RTABMAP_PARAM(Vis, MinDepth, float, 0.1, "Min depth of the features (0 means no limit)."); + RTABMAP_PARAM(Vis, DepthAsMask, bool, true, "Use depth image as mask when extracting features."); + RTABMAP_PARAM_STR(Vis, RoiRatios, "0.0 0.0 0.0 0.0", "Region of interest ratios [left, right, top, bottom]."); + RTABMAP_PARAM(Vis, SubPixWinSize, int, 3, "See cv::cornerSubPix()."); + RTABMAP_PARAM(Vis, SubPixIterations, int, 0, "See cv::cornerSubPix(). 0 disables sub pixel refining."); + RTABMAP_PARAM(Vis, SubPixEps, float, 0.02, "See cv::cornerSubPix()."); + RTABMAP_PARAM(Vis, GridRows, int, 1, uFormat("Number of rows of the grid used to extract uniformly \"%s / grid cells\" features from each cell.", kVisMaxFeatures().c_str())); + RTABMAP_PARAM(Vis, GridCols, int, 1, uFormat("Number of columns of the grid used to extract uniformly \"%s / grid cells\" features from each cell.", kVisMaxFeatures().c_str())); + RTABMAP_PARAM(Vis, CorType, int, 0, "Correspondences computation approach: 0=Features Matching, 1=Optical Flow"); + RTABMAP_PARAM(Vis, CorNNType, int, 1, uFormat("[%s=0] kNNFlannNaive=0, kNNFlannKdTree=1, kNNFlannLSH=2, kNNBruteForce=3, kNNBruteForceGPU=4, BruteForceCrossCheck=5, SuperGlue=6, GMS=7. Used for features matching approach.", kVisCorType().c_str())); + RTABMAP_PARAM(Vis, CorNNDR, float, 0.8, uFormat("[%s=0] NNDR: nearest neighbor distance ratio. Used for knn features matching approach.", kVisCorType().c_str())); + RTABMAP_PARAM(Vis, CorGuessWinSize, int, 40, uFormat("[%s=0] Matching window size (pixels) around projected points when a guess transform is provided to find correspondences. 0 means disabled.", kVisCorType().c_str())); + RTABMAP_PARAM(Vis, CorGuessMatchToProjection, bool, false, uFormat("[%s=0] Match frame's corners to source's projected points (when guess transform is provided) instead of projected points to frame's corners.", kVisCorType().c_str())); + RTABMAP_PARAM(Vis, CorFlowWinSize, int, 16, uFormat("[%s=1] See cv::calcOpticalFlowPyrLK(). Used for optical flow approach.", kVisCorType().c_str())); + RTABMAP_PARAM(Vis, CorFlowIterations, int, 30, uFormat("[%s=1] See cv::calcOpticalFlowPyrLK(). Used for optical flow approach.", kVisCorType().c_str())); + RTABMAP_PARAM(Vis, CorFlowEps, float, 0.01, uFormat("[%s=1] See cv::calcOpticalFlowPyrLK(). Used for optical flow approach.", kVisCorType().c_str())); + RTABMAP_PARAM(Vis, CorFlowMaxLevel, int, 3, uFormat("[%s=1] See cv::calcOpticalFlowPyrLK(). Used for optical flow approach.", kVisCorType().c_str())); #if defined(RTABMAP_G2O) || defined(RTABMAP_ORB_SLAM) - RTABMAP_PARAM(Vis, BundleAdjustment, int, 1, "Optimization with bundle adjustment: 0=disabled, 1=g2o, 2=cvsba, 3=Ceres."); + RTABMAP_PARAM(Vis, BundleAdjustment, int, 1, "Optimization with bundle adjustment: 0=disabled, 1=g2o, 2=cvsba, 3=Ceres."); #else - RTABMAP_PARAM(Vis, BundleAdjustment, int, 0, "Optimization with bundle adjustment: 0=disabled, 1=g2o, 2=cvsba, 3=Ceres."); + RTABMAP_PARAM(Vis, BundleAdjustment, int, 0, "Optimization with bundle adjustment: 0=disabled, 1=g2o, 2=cvsba, 3=Ceres."); #endif // Features matching approaches diff --git a/corelib/src/odometry/OdometryOpenVINS.cpp b/corelib/src/odometry/OdometryOpenVINS.cpp index 927a53bcc5..71477216cc 100644 --- a/corelib/src/odometry/OdometryOpenVINS.cpp +++ b/corelib/src/odometry/OdometryOpenVINS.cpp @@ -59,7 +59,14 @@ OdometryOpenVINS::OdometryOpenVINS(const ParametersMap & parameters) : Parameters::parse(parameters, Parameters::kVisGridCols(), params_->grid_x); Parameters::parse(parameters, Parameters::kVisGridRows(), params_->grid_y); Parameters::parse(parameters, Parameters::kOdomOpenVINSMinPxDist(), params_->min_px_dist); - Parameters::parse(parameters, Parameters::kOdomOpenVINSKNNRatio(), params_->knn_ratio); + Parameters::parse(parameters, Parameters::kVisCorNNDR(), params_->knn_ratio); + Parameters::parse(parameters, Parameters::kOdomOpenVINSFiTriangulate1d(), params_->featinit_options.triangulate_1d); + Parameters::parse(parameters, Parameters::kOdomOpenVINSFiRefineFeatures(), params_->featinit_options.refine_features); + Parameters::parse(parameters, Parameters::kOdomOpenVINSFiMaxRuns(), params_->featinit_options.max_runs); + Parameters::parse(parameters, Parameters::kVisMinDepth(), params_->featinit_options.min_dist); + Parameters::parse(parameters, Parameters::kVisMaxDepth(), params_->featinit_options.max_dist); + Parameters::parse(parameters, Parameters::kOdomOpenVINSFiMaxBaseline(), params_->featinit_options.max_baseline); + Parameters::parse(parameters, Parameters::kOdomOpenVINSFiMaxCondNumber(), params_->featinit_options.max_cond_number); Parameters::parse(parameters, Parameters::kOdomOpenVINSUseFEJ(), params_->state_options.do_fej); Parameters::parse(parameters, Parameters::kOdomOpenVINSIntegration(), enum_index); params_->state_options.integration_method = ov_msckf::StateOptions::IntegrationMethod(enum_index); diff --git a/guilib/src/PreferencesDialog.cpp b/guilib/src/PreferencesDialog.cpp index afbcb28313..24e3a66ae3 100644 --- a/guilib/src/PreferencesDialog.cpp +++ b/guilib/src/PreferencesDialog.cpp @@ -1438,7 +1438,11 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) : _ui->checkBox_OdomOpenVINSUseKLT->setObjectName(Parameters::kOdomOpenVINSUseKLT().c_str()); _ui->spinBox_OdomOpenVINSNumPts->setObjectName(Parameters::kOdomOpenVINSNumPts().c_str()); _ui->spinBox_OdomOpenVINSMinPxDist->setObjectName(Parameters::kOdomOpenVINSMinPxDist().c_str()); - _ui->doubleSpinBox_OdomOpenVINSKNNRatio->setObjectName(Parameters::kOdomOpenVINSKNNRatio().c_str()); + _ui->checkBox_OdomOpenVINSFiTriangulate1d->setObjectName(Parameters::kOdomOpenVINSFiTriangulate1d().c_str()); + _ui->checkBox_OdomOpenVINSFiRefineFeatures->setObjectName(Parameters::kOdomOpenVINSFiRefineFeatures().c_str()); + _ui->spinBox_OdomOpenVINSFiMaxRuns->setObjectName(Parameters::kOdomOpenVINSFiMaxRuns().c_str()); + _ui->doubleSpinBox_OdomOpenVINSFiMaxBaseline->setObjectName(Parameters::kOdomOpenVINSFiMaxBaseline().c_str()); + _ui->doubleSpinBox_OdomOpenVINSFiMaxCondNumber->setObjectName(Parameters::kOdomOpenVINSFiMaxCondNumber().c_str()); _ui->checkBox_OdomOpenVINSUseFEJ->setObjectName(Parameters::kOdomOpenVINSUseFEJ().c_str()); _ui->comboBox_OdomOpenVINSIntegration->setObjectName(Parameters::kOdomOpenVINSIntegration().c_str()); diff --git a/guilib/src/ui/preferencesDialog.ui b/guilib/src/ui/preferencesDialog.ui index d55f343e3c..aafcf24305 100644 --- a/guilib/src/ui/preferencesDialog.ui +++ b/guilib/src/ui/preferencesDialog.ui @@ -18047,25 +18047,111 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - + + + + + + + + + + If we should perform 1d triangulation instead of 3d + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + + + + + + If we should perform Levenberg-Marquardt refinement + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + - 0.010000000000000 + 1 + + 5 + + + + + + + Max runs for Levenberg-Marquardt + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + - 1.000000000000000 + 1000.000000000000000 - 0.100000000000000 + 10.000000000000000 - 0.700000000000000 + 40.000000000000000 - - + + + + Max baseline ratio to accept triangulated features + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + 99999.000000000000000 + + + 1000.000000000000000 + + + 10000.000000000000000 + + + + + - KNN ration between top two descriptor matcher which is required to be a good match + Max condition number of linear triangulation matrix accept triangulated features true @@ -21367,7 +21453,7 @@ Lower the ratio -> higher the precision. 0.000000000000000 - 5.000000000000000 + 60.000000000000000 @@ -21390,7 +21476,7 @@ Lower the ratio -> higher the precision. 0.000000000000000 - 5.000000000000000 + 0.100000000000000 From 81e742553799a59a42ae66ed64c9839c05c71975 Mon Sep 17 00:00:00 2001 From: Borong Yuan Date: Sat, 14 Oct 2023 22:42:02 +0800 Subject: [PATCH 06/10] fix segfault of OdomImageDecimation --- corelib/src/Odometry.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/corelib/src/Odometry.cpp b/corelib/src/Odometry.cpp index ec50218f2c..f6c7762724 100644 --- a/corelib/src/Odometry.cpp +++ b/corelib/src/Odometry.cpp @@ -675,12 +675,12 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet UASSERT(info->newCorners.size() == info->refCorners.size() || info->refCorners.empty()); for(unsigned int i=0; inewCorners.size(); ++i) { - info->refCorners[i].x *= _imageDecimation; - info->refCorners[i].y *= _imageDecimation; + info->newCorners[i].x *= _imageDecimation; + info->newCorners[i].y *= _imageDecimation; if(!info->refCorners.empty()) { - info->newCorners[i].x *= _imageDecimation; - info->newCorners[i].y *= _imageDecimation; + info->refCorners[i].x *= _imageDecimation; + info->refCorners[i].y *= _imageDecimation; } } for(std::multimap::iterator iter=info->words.begin(); iter!=info->words.end(); ++iter) From 5a03ad9d9e7aba36a631855d891d30b7899d2a0f Mon Sep 17 00:00:00 2001 From: Borong Yuan Date: Fri, 20 Oct 2023 13:49:27 +0800 Subject: [PATCH 07/10] add mask settings --- corelib/include/rtabmap/core/Parameters.h | 2 + corelib/src/odometry/OdometryOpenVINS.cpp | 28 +++++++++- .../include/rtabmap/gui/PreferencesDialog.h | 2 + guilib/src/PreferencesDialog.cpp | 38 +++++++++++++ guilib/src/ui/preferencesDialog.ui | 54 +++++++++++++++++++ 5 files changed, 122 insertions(+), 2 deletions(-) diff --git a/corelib/include/rtabmap/core/Parameters.h b/corelib/include/rtabmap/core/Parameters.h index 329d35c113..2453104d33 100644 --- a/corelib/include/rtabmap/core/Parameters.h +++ b/corelib/include/rtabmap/core/Parameters.h @@ -602,6 +602,8 @@ class RTABMAP_CORE_EXPORT Parameters 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"); diff --git a/corelib/src/odometry/OdometryOpenVINS.cpp b/corelib/src/odometry/OdometryOpenVINS.cpp index 71477216cc..15cb2ceb49 100644 --- a/corelib/src/odometry/OdometryOpenVINS.cpp +++ b/corelib/src/odometry/OdometryOpenVINS.cpp @@ -28,6 +28,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "rtabmap/core/odometry/OdometryOpenVINS.h" #include "rtabmap/core/OdometryInfo.h" #include "rtabmap/core/util3d_transforms.h" +#include "rtabmap/utilite/UFile.h" #include "rtabmap/utilite/ULogger.h" #include "rtabmap/utilite/UTimer.h" #include @@ -51,6 +52,7 @@ OdometryOpenVINS::OdometryOpenVINS(const ParametersMap & parameters) : #ifdef RTABMAP_OPENVINS ov_core::Printer::setPrintLevel(ov_core::Printer::PrintLevel(ULogger::level()+1)); int enum_index; + std::string left_mask_path, right_mask_path; params_ = std::make_unique(); Parameters::parse(parameters, Parameters::kOdomOpenVINSUseStereo(), params_->use_stereo); Parameters::parse(parameters, Parameters::kOdomOpenVINSUseKLT(), params_->use_klt); @@ -85,6 +87,22 @@ OdometryOpenVINS::OdometryOpenVINS(const ParametersMap & parameters) : params_->state_options.feat_rep_slam = ov_type::LandmarkRepresentation::Representation(enum_index); Parameters::parse(parameters, Parameters::kOdomOpenVINSDtSLAMDelay(), params_->dt_slam_delay); Parameters::parse(parameters, Parameters::kOdomOpenVINSGravityMag(), params_->gravity_mag); + Parameters::parse(parameters, Parameters::kOdomOpenVINSLeftMaskPath(), left_mask_path); + if(!left_mask_path.empty()) + { + if(!UFile::exists(left_mask_path)) + UWARN("OpenVINS: invalid left mask path: %s", left_mask_path.c_str()); + else + params_->masks.emplace(0, cv::imread(left_mask_path, cv::IMREAD_GRAYSCALE)); + } + Parameters::parse(parameters, Parameters::kOdomOpenVINSRightMaskPath(), right_mask_path); + if(!right_mask_path.empty()) + { + if(!UFile::exists(right_mask_path)) + UWARN("OpenVINS: invalid right mask path: %s", right_mask_path.c_str()); + else + params_->masks.emplace(1, cv::imread(right_mask_path, cv::IMREAD_GRAYSCALE)); + } Parameters::parse(parameters, Parameters::kOdomOpenVINSInitWindowTime(), params_->init_options.init_window_time); Parameters::parse(parameters, Parameters::kOdomOpenVINSInitIMUThresh(), params_->init_options.init_imu_thresh); Parameters::parse(parameters, Parameters::kOdomOpenVINSInitMaxDisparity(), params_->init_options.init_max_disparity); @@ -315,7 +333,10 @@ Transform OdometryOpenVINS::computeTransform( message.timestamp = data.stamp(); message.sensor_ids.emplace_back(0); message.images.emplace_back(image); - message.masks.emplace_back(cv::Mat::zeros(image.size(), CV_8UC1)); + if(params_->masks.find(0) != params_->masks.end()) + message.masks.emplace_back(params_->masks[0]); + else + message.masks.emplace_back(cv::Mat::zeros(image.size(), CV_8UC1)); if(!data.rightRaw().empty()) { if(data.rightRaw().type() == CV_8UC3) @@ -326,7 +347,10 @@ Transform OdometryOpenVINS::computeTransform( UFATAL("Not supported color type!"); message.sensor_ids.emplace_back(1); message.images.emplace_back(image); - message.masks.emplace_back(cv::Mat::zeros(image.size(), CV_8UC1)); + if(params_->masks.find(1) != params_->masks.end()) + message.masks.emplace_back(params_->masks[1]); + else + message.masks.emplace_back(cv::Mat::zeros(image.size(), CV_8UC1)); } vioManager_->feed_measurement_camera(message); diff --git a/guilib/include/rtabmap/gui/PreferencesDialog.h b/guilib/include/rtabmap/gui/PreferencesDialog.h index 75f5156f6b..7f8f6daa3b 100644 --- a/guilib/include/rtabmap/gui/PreferencesDialog.h +++ b/guilib/include/rtabmap/gui/PreferencesDialog.h @@ -349,6 +349,8 @@ private Q_SLOTS: void changeOdometryORBSLAMVocabulary(); void changeOdometryOKVISConfigPath(); void changeOdometryVINSConfigPath(); + void changeOdometryOpenVINSLeftMask(); + void changeOdometryOpenVINSRightMask(); void changeIcpPMConfigPath(); void changeSuperPointModelPath(); void changePyMatcherPath(); diff --git a/guilib/src/PreferencesDialog.cpp b/guilib/src/PreferencesDialog.cpp index 24e3a66ae3..ef2e7e5bdc 100644 --- a/guilib/src/PreferencesDialog.cpp +++ b/guilib/src/PreferencesDialog.cpp @@ -1459,6 +1459,10 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) : _ui->comboBox_OdomOpenVINSFeatRepSLAM->setObjectName(Parameters::kOdomOpenVINSFeatRepSLAM().c_str()); _ui->doubleSpinBox_OdomOpenVINSDtSLAMDelay->setObjectName(Parameters::kOdomOpenVINSDtSLAMDelay().c_str()); _ui->doubleSpinBox_OdomOpenVINSGravityMag->setObjectName(Parameters::kOdomOpenVINSGravityMag().c_str()); + _ui->lineEdit_OdomOpenVINSLeftMaskPath->setObjectName(Parameters::kOdomOpenVINSLeftMaskPath().c_str()); + connect(_ui->toolButton_OdomOpenVINSLeftMaskPath, SIGNAL(clicked()), this, SLOT(changeOdometryOpenVINSLeftMask())); + _ui->lineEdit_OdomOpenVINSRightMaskPath->setObjectName(Parameters::kOdomOpenVINSRightMaskPath().c_str()); + connect(_ui->toolButton_OdomOpenVINSRightMaskPath, SIGNAL(clicked()), this, SLOT(changeOdometryOpenVINSRightMask())); _ui->doubleSpinBox_OdomOpenVINSInitWindowTime->setObjectName(Parameters::kOdomOpenVINSInitWindowTime().c_str()); _ui->doubleSpinBox_OdomOpenVINSInitIMUThresh->setObjectName(Parameters::kOdomOpenVINSInitIMUThresh().c_str()); @@ -5212,6 +5216,40 @@ void PreferencesDialog::changeOdometryVINSConfigPath() } } +void PreferencesDialog::changeOdometryOpenVINSLeftMask() +{ + QString path; + if(_ui->lineEdit_OdomOpenVINSLeftMaskPath->text().isEmpty()) + { + path = QFileDialog::getOpenFileName(this, tr("Select Left Mask"), this->getWorkingDirectory(), tr("Image mask (*.jpg *.png)")); + } + else + { + path = QFileDialog::getOpenFileName(this, tr("Select Left Mask"), _ui->lineEdit_OdomOpenVINSLeftMaskPath->text(), tr("Image mask (*.jpg *.png)")); + } + if(!path.isEmpty()) + { + _ui->lineEdit_OdomOpenVINSLeftMaskPath->setText(path); + } +} + +void PreferencesDialog::changeOdometryOpenVINSRightMask() +{ + QString path; + if(_ui->lineEdit_OdomOpenVINSRightMaskPath->text().isEmpty()) + { + path = QFileDialog::getOpenFileName(this, tr("Select Right Mask"), this->getWorkingDirectory(), tr("Image mask (*.jpg *.png)")); + } + else + { + path = QFileDialog::getOpenFileName(this, tr("Select Right Mask"), _ui->lineEdit_OdomOpenVINSRightMaskPath->text(), tr("Image mask (*.jpg *.png)")); + } + if(!path.isEmpty()) + { + _ui->lineEdit_OdomOpenVINSRightMaskPath->setText(path); + } +} + void PreferencesDialog::changeIcpPMConfigPath() { QString path; diff --git a/guilib/src/ui/preferencesDialog.ui b/guilib/src/ui/preferencesDialog.ui index aafcf24305..b76f02f92c 100644 --- a/guilib/src/ui/preferencesDialog.ui +++ b/guilib/src/ui/preferencesDialog.ui @@ -18590,6 +18590,60 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag + + + + + + ... + + + + + + + + + + + + Mask for left image + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + ... + + + + + + + + + + + + Mask for right image + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + From 8d7a7cffbb926fc5ce9a89c389d267f54f2b93cc Mon Sep 17 00:00:00 2001 From: Borong Yuan Date: Fri, 20 Oct 2023 17:13:48 +0800 Subject: [PATCH 08/10] use depth as mask in OpenVINS --- corelib/src/odometry/OdometryOpenVINS.cpp | 14 ++++++++++++++ 1 file changed, 14 insertions(+) diff --git a/corelib/src/odometry/OdometryOpenVINS.cpp b/corelib/src/odometry/OdometryOpenVINS.cpp index 15cb2ceb49..2a5ad22922 100644 --- a/corelib/src/odometry/OdometryOpenVINS.cpp +++ b/corelib/src/odometry/OdometryOpenVINS.cpp @@ -87,6 +87,7 @@ OdometryOpenVINS::OdometryOpenVINS(const ParametersMap & parameters) : params_->state_options.feat_rep_slam = ov_type::LandmarkRepresentation::Representation(enum_index); Parameters::parse(parameters, Parameters::kOdomOpenVINSDtSLAMDelay(), params_->dt_slam_delay); Parameters::parse(parameters, Parameters::kOdomOpenVINSGravityMag(), params_->gravity_mag); + Parameters::parse(parameters, Parameters::kVisDepthAsMask(), params_->use_mask); Parameters::parse(parameters, Parameters::kOdomOpenVINSLeftMaskPath(), left_mask_path); if(!left_mask_path.empty()) { @@ -334,9 +335,22 @@ Transform OdometryOpenVINS::computeTransform( message.sensor_ids.emplace_back(0); message.images.emplace_back(image); if(params_->masks.find(0) != params_->masks.end()) + { message.masks.emplace_back(params_->masks[0]); + } + else if(!data.depthRaw().empty() && params_->use_mask) + { + cv::Mat mask; + if(data.depthRaw().type() == CV_32FC1) + cv::inRange(data.depthRaw(), params_->featinit_options.min_dist, params_->featinit_options.max_dist, mask); + else if(data.depthRaw().type() == CV_16UC1) + cv::inRange(data.depthRaw(), params_->featinit_options.min_dist*1000, params_->featinit_options.max_dist*1000, mask); + message.masks.emplace_back(255-mask); + } else + { message.masks.emplace_back(cv::Mat::zeros(image.size(), CV_8UC1)); + } if(!data.rightRaw().empty()) { if(data.rightRaw().type() == CV_8UC3) From d0f82d92d1c3985ceaa0c7c0097cb09dbb1957b6 Mon Sep 17 00:00:00 2001 From: Borong Yuan Date: Sat, 21 Oct 2023 14:18:45 +0800 Subject: [PATCH 09/10] set twist covariance --- .../rtabmap/core/odometry/OdometryOpenVINS.h | 1 + corelib/src/odometry/OdometryOpenVINS.cpp | 27 ++++++++++++------- 2 files changed, 18 insertions(+), 10 deletions(-) diff --git a/corelib/include/rtabmap/core/odometry/OdometryOpenVINS.h b/corelib/include/rtabmap/core/odometry/OdometryOpenVINS.h index 993d6cf10a..1d17e74558 100644 --- a/corelib/include/rtabmap/core/odometry/OdometryOpenVINS.h +++ b/corelib/include/rtabmap/core/odometry/OdometryOpenVINS.h @@ -57,6 +57,7 @@ class RTABMAP_CORE_EXPORT OdometryOpenVINS : public Odometry bool initGravity_; Transform previousPoseInv_; Transform imuLocalTransformInv_; + Eigen::Matrix Phi_; #endif }; diff --git a/corelib/src/odometry/OdometryOpenVINS.cpp b/corelib/src/odometry/OdometryOpenVINS.cpp index 2a5ad22922..1bf7402e62 100644 --- a/corelib/src/odometry/OdometryOpenVINS.cpp +++ b/corelib/src/odometry/OdometryOpenVINS.cpp @@ -31,10 +31,12 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "rtabmap/utilite/UFile.h" #include "rtabmap/utilite/ULogger.h" #include "rtabmap/utilite/UTimer.h" +#include #include #ifdef RTABMAP_OPENVINS #include "core/VioManager.h" +#include "state/Propagator.h" #include "state/State.h" #include "state/StateHelper.h" #endif @@ -177,7 +179,12 @@ Transform OdometryOpenVINS::computeTransform( if(!vioManager_) { if(!data.imu().empty()) + { imuLocalTransformInv_ = data.imu().localTransform().inverse(); + Phi_.setZero(); + Phi_.block(0,0,3,3) = data.imu().localTransform().toEigen4d().block(0,0,3,3); + Phi_.block(3,3,3,3) = data.imu().localTransform().toEigen4d().block(0,0,3,3); + } if(!data.imageRaw().empty() && !imuLocalTransformInv_.isNull()) { @@ -323,6 +330,12 @@ Transform OdometryOpenVINS::computeTransform( if(!data.imageRaw().empty()) { + bool covFilled = false; + Eigen::Matrix state_plus = Eigen::Matrix::Zero(); + Eigen::Matrix cov_plus = Eigen::Matrix::Zero(); + if(vioManager_->initialized()) + covFilled = vioManager_->get_propagator()->fast_state_propagate(vioManager_->get_state(), data.stamp(), state_plus, cov_plus); + cv::Mat image; if(data.imageRaw().type() == CV_8UC3) cv::cvtColor(data.imageRaw(), image, CV_BGR2GRAY); @@ -404,17 +417,11 @@ Transform OdometryOpenVINS::computeTransform( info->type = this->getType(); info->localMapSize = feat_posinG.size(); info->features = features_SLAM.size() + good_features_MSCKF.size(); - info->reg.covariance = cv::Mat(6,6,CV_64FC1); - std::vector> statevars; - statevars.emplace_back(state->_imu->pose()->p()); - statevars.emplace_back(state->_imu->pose()->q()); - Eigen::Matrix covariance_posori = ov_msckf::StateHelper::get_marginal_covariance(state, statevars); - for (int r = 0; r < 6; r++) + info->reg.covariance = cv::Mat::eye(6, 6, CV_64FC1); + if(covFilled) { - for (int c = 0; c < 6; c++) - { - ((double *)info->reg.covariance.data)[6*r+c] = covariance_posori(r,c); - } + Eigen::Matrix covariance = Phi_ * cov_plus.block(6,6,6,6) * Phi_.transpose(); + cv::eigen2cv(covariance, info->reg.covariance); } if(this->isInfoDataFilled()) From a0d67bc12255e8fecdb34532478ffc1633aab2a4 Mon Sep 17 00:00:00 2001 From: Borong Yuan Date: Sat, 21 Oct 2023 15:36:31 +0800 Subject: [PATCH 10/10] restore default values of VisMinDepth and VisMaxDepth --- corelib/include/rtabmap/core/Parameters.h | 4 ++-- corelib/src/odometry/OdometryOpenVINS.cpp | 8 ++++++-- 2 files changed, 8 insertions(+), 4 deletions(-) diff --git a/corelib/include/rtabmap/core/Parameters.h b/corelib/include/rtabmap/core/Parameters.h index 2453104d33..7aee191a1b 100644 --- a/corelib/include/rtabmap/core/Parameters.h +++ b/corelib/include/rtabmap/core/Parameters.h @@ -676,8 +676,8 @@ class RTABMAP_CORE_EXPORT Parameters RTABMAP_PARAM(Vis, FeatureType, int, 6, "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB 9=KAZE 10=ORB-OCTREE 11=SuperPoint 12=SURF/FREAK 13=GFTT/DAISY 14=SURF/DAISY 15=PyDetector"); #endif RTABMAP_PARAM(Vis, MaxFeatures, int, 1000, "0 no limits."); - RTABMAP_PARAM(Vis, MaxDepth, float, 60.0, "Max depth of the features (0 means no limit)."); - RTABMAP_PARAM(Vis, MinDepth, float, 0.1, "Min depth of the features (0 means no limit)."); + RTABMAP_PARAM(Vis, MaxDepth, float, 0, "Max depth of the features (0 means no limit)."); + RTABMAP_PARAM(Vis, MinDepth, float, 0, "Min depth of the features (0 means no limit)."); RTABMAP_PARAM(Vis, DepthAsMask, bool, true, "Use depth image as mask when extracting features."); RTABMAP_PARAM_STR(Vis, RoiRatios, "0.0 0.0 0.0 0.0", "Region of interest ratios [left, right, top, bottom]."); RTABMAP_PARAM(Vis, SubPixWinSize, int, 3, "See cv::cornerSubPix()."); diff --git a/corelib/src/odometry/OdometryOpenVINS.cpp b/corelib/src/odometry/OdometryOpenVINS.cpp index 1bf7402e62..ffde226cfa 100644 --- a/corelib/src/odometry/OdometryOpenVINS.cpp +++ b/corelib/src/odometry/OdometryOpenVINS.cpp @@ -69,6 +69,8 @@ OdometryOpenVINS::OdometryOpenVINS(const ParametersMap & parameters) : Parameters::parse(parameters, Parameters::kOdomOpenVINSFiMaxRuns(), params_->featinit_options.max_runs); Parameters::parse(parameters, Parameters::kVisMinDepth(), params_->featinit_options.min_dist); Parameters::parse(parameters, Parameters::kVisMaxDepth(), params_->featinit_options.max_dist); + if(params_->featinit_options.max_dist == 0) + params_->featinit_options.max_dist = std::numeric_limits::infinity(); Parameters::parse(parameters, Parameters::kOdomOpenVINSFiMaxBaseline(), params_->featinit_options.max_baseline); Parameters::parse(parameters, Parameters::kOdomOpenVINSFiMaxCondNumber(), params_->featinit_options.max_cond_number); Parameters::parse(parameters, Parameters::kOdomOpenVINSUseFEJ(), params_->state_options.do_fej); @@ -355,9 +357,11 @@ Transform OdometryOpenVINS::computeTransform( { cv::Mat mask; if(data.depthRaw().type() == CV_32FC1) - cv::inRange(data.depthRaw(), params_->featinit_options.min_dist, params_->featinit_options.max_dist, mask); + cv::inRange(data.depthRaw(), params_->featinit_options.min_dist, + std::isinf(params_->featinit_options.max_dist)?std::numeric_limits::max():params_->featinit_options.max_dist, mask); else if(data.depthRaw().type() == CV_16UC1) - cv::inRange(data.depthRaw(), params_->featinit_options.min_dist*1000, params_->featinit_options.max_dist*1000, mask); + cv::inRange(data.depthRaw(), params_->featinit_options.min_dist*1000, + std::isinf(params_->featinit_options.max_dist)?std::numeric_limits::max():params_->featinit_options.max_dist*1000, mask); message.masks.emplace_back(255-mask); } else