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 + + +