From ae21c358e5e622b35977fde3895f562ff380677c Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sat, 18 Nov 2023 17:18:02 -0800 Subject: [PATCH] Source camera: added feature detection option --- corelib/include/rtabmap/core/CameraThread.h | 5 + corelib/src/CameraThread.cpp | 83 +++++- corelib/src/RegistrationVis.cpp | 2 +- .../include/rtabmap/gui/PreferencesDialog.h | 1 + guilib/src/MainWindow.cpp | 4 + guilib/src/PreferencesDialog.cpp | 16 ++ guilib/src/ui/preferencesDialog.ui | 271 ++++++++++-------- 7 files changed, 254 insertions(+), 128 deletions(-) diff --git a/corelib/include/rtabmap/core/CameraThread.h b/corelib/include/rtabmap/core/CameraThread.h index 514fea9f58..b4535fa40f 100644 --- a/corelib/include/rtabmap/core/CameraThread.h +++ b/corelib/include/rtabmap/core/CameraThread.h @@ -47,6 +47,7 @@ class CameraInfo; class SensorData; class StereoDense; class IMUFilter; +class Feature2D; /** * Class CameraThread @@ -88,6 +89,8 @@ class RTABMAP_CORE_EXPORT CameraThread : void disableBilateralFiltering() {_bilateralFiltering = false;} void enableIMUFiltering(int filteringStrategy=1, const ParametersMap & parameters = ParametersMap(), bool baseFrameConversion = false); void disableIMUFiltering(); + void enableFeatureDetection(const ParametersMap & parameters = ParametersMap()); + void disableFeatureDetection(); // Use new version of this function with groundNormalsUp=0.8 for forceGroundNormalsUp=True and groundNormalsUp=0.0 for forceGroundNormalsUp=False. RTABMAP_DEPRECATED void setScanParameters( @@ -152,6 +155,8 @@ class RTABMAP_CORE_EXPORT CameraThread : float _bilateralSigmaR; IMUFilter * _imuFilter; bool _imuBaseFrameConversion; + Feature2D * _featureDetector; + bool _depthAsMask; }; } // namespace rtabmap diff --git a/corelib/src/CameraThread.cpp b/corelib/src/CameraThread.cpp index b2a981d292..a8c099a26e 100644 --- a/corelib/src/CameraThread.cpp +++ b/corelib/src/CameraThread.cpp @@ -36,10 +36,12 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "rtabmap/core/StereoDense.h" #include "rtabmap/core/DBReader.h" #include "rtabmap/core/IMUFilter.h" +#include "rtabmap/core/Features2d.h" #include "rtabmap/core/clams/discrete_depth_distortion_model.h" #include #include #include +#include #include @@ -73,7 +75,9 @@ CameraThread::CameraThread(Camera * camera, const ParametersMap & parameters) : _bilateralSigmaS(10), _bilateralSigmaR(0.1), _imuFilter(0), - _imuBaseFrameConversion(false) + _imuBaseFrameConversion(false), + _featureDetector(0), + _depthAsMask(Parameters::defaultVisDepthAsMask()) { UASSERT(_camera != 0); } @@ -113,7 +117,9 @@ CameraThread::CameraThread( _bilateralSigmaS(10), _bilateralSigmaR(0.1), _imuFilter(0), - _imuBaseFrameConversion(false) + _imuBaseFrameConversion(false), + _featureDetector(0), + _depthAsMask(Parameters::defaultVisDepthAsMask()) { UASSERT(_camera != 0 && _odomSensor != 0 && !_extrinsicsOdomToCamera.isNull()); UDEBUG("_extrinsicsOdomToCamera=%s", _extrinsicsOdomToCamera.prettyPrint().c_str()); @@ -152,7 +158,9 @@ CameraThread::CameraThread( _bilateralSigmaS(10), _bilateralSigmaR(0.1), _imuFilter(0), - _imuBaseFrameConversion(false) + _imuBaseFrameConversion(false), + _featureDetector(0), + _depthAsMask(Parameters::defaultVisDepthAsMask()) { UASSERT(_camera != 0); UDEBUG("_odomAsGt =%s", _odomAsGt?"true":"false"); @@ -166,6 +174,7 @@ CameraThread::~CameraThread() delete _distortionModel; delete _stereoDense; delete _imuFilter; + delete _featureDetector; } void CameraThread::setImageRate(float imageRate) @@ -217,6 +226,30 @@ void CameraThread::disableIMUFiltering() _imuFilter = 0; } +void CameraThread::enableFeatureDetection(const ParametersMap & parameters) +{ + delete _featureDetector; + ParametersMap params = parameters; + ParametersMap defaultParams = Parameters::getDefaultParameters("Vis"); + uInsert(params, ParametersPair(Parameters::kKpDetectorStrategy(), uValue(params, Parameters::kVisFeatureType(), defaultParams.at(Parameters::kVisFeatureType())))); + uInsert(params, ParametersPair(Parameters::kKpMaxFeatures(), uValue(params, Parameters::kVisMaxFeatures(), defaultParams.at(Parameters::kVisMaxFeatures())))); + uInsert(params, ParametersPair(Parameters::kKpMaxDepth(), uValue(params, Parameters::kVisMaxDepth(), defaultParams.at(Parameters::kVisMaxDepth())))); + uInsert(params, ParametersPair(Parameters::kKpMinDepth(), uValue(params, Parameters::kVisMinDepth(), defaultParams.at(Parameters::kVisMinDepth())))); + uInsert(params, ParametersPair(Parameters::kKpRoiRatios(), uValue(params, Parameters::kVisRoiRatios(), defaultParams.at(Parameters::kVisRoiRatios())))); + uInsert(params, ParametersPair(Parameters::kKpSubPixEps(), uValue(params, Parameters::kVisSubPixEps(), defaultParams.at(Parameters::kVisSubPixEps())))); + uInsert(params, ParametersPair(Parameters::kKpSubPixIterations(), uValue(params, Parameters::kVisSubPixIterations(), defaultParams.at(Parameters::kVisSubPixIterations())))); + uInsert(params, ParametersPair(Parameters::kKpSubPixWinSize(), uValue(params, Parameters::kVisSubPixWinSize(), defaultParams.at(Parameters::kVisSubPixWinSize())))); + uInsert(params, ParametersPair(Parameters::kKpGridRows(), uValue(params, Parameters::kVisGridRows(), defaultParams.at(Parameters::kVisGridRows())))); + uInsert(params, ParametersPair(Parameters::kKpGridCols(), uValue(params, Parameters::kVisGridCols(), defaultParams.at(Parameters::kVisGridCols())))); + _featureDetector = Feature2D::create(params); + _depthAsMask = Parameters::parse(params, Parameters::kVisDepthAsMask(), _depthAsMask); +} +void CameraThread::disableFeatureDetection() +{ + delete _featureDetector; + _featureDetector = 0; +} + void CameraThread::setScanParameters( bool fromDepth, int downsampleStep, @@ -732,6 +765,50 @@ void CameraThread::postUpdate(SensorData * dataPtr, CameraInfo * info) const data.stamp()); } } + + if(_featureDetector && !data.imageRaw().empty()) + { + UDEBUG("Detecting features"); + cv::Mat grayScaleImg = data.imageRaw(); + if(data.imageRaw().channels() > 1) + { + cv::Mat tmp; + cv::cvtColor(grayScaleImg, tmp, cv::COLOR_BGR2GRAY); + grayScaleImg = tmp; + } + + cv::Mat depthMask; + if(!data.depthRaw().empty() && _depthAsMask) + { + if( data.imageRaw().rows % data.depthRaw().rows == 0 && + data.imageRaw().cols % data.depthRaw().cols == 0 && + data.imageRaw().rows/data.depthRaw().rows == data.imageRaw().cols/data.depthRaw().cols) + { + depthMask = util2d::interpolate(data.depthRaw(), data.imageRaw().rows/data.depthRaw().rows, 0.1f); + } + else + { + UWARN("%s is true, but RGB size (%dx%d) modulo depth size (%dx%d) is not 0. Ignoring depth mask for feature detection.", + Parameters::kVisDepthAsMask().c_str(), + data.imageRaw().rows, data.imageRaw().cols, + data.depthRaw().rows, data.depthRaw().cols); + } + } + + std::vector keypoints = _featureDetector->generateKeypoints(grayScaleImg, depthMask); + cv::Mat descriptors; + std::vector keypoints3D; + if(!keypoints.empty()) + { + descriptors = _featureDetector->generateDescriptors(grayScaleImg, keypoints); + if(!keypoints.empty()) + { + keypoints3D = _featureDetector->generateKeypoints3D(data, keypoints); + } + } + + data.setFeatures(keypoints, keypoints3D, descriptors); + } } } // namespace rtabmap diff --git a/corelib/src/RegistrationVis.cpp b/corelib/src/RegistrationVis.cpp index a0fce6a635..0bdeb0cd34 100644 --- a/corelib/src/RegistrationVis.cpp +++ b/corelib/src/RegistrationVis.cpp @@ -1095,7 +1095,7 @@ Transform RegistrationVis::computeTransformationImpl( std::vector > dists; float radius = (float)_guessWinSize; // pixels rtflann::Matrix cornersProjectedMat((float*)cornersProjected.data(), cornersProjected.size(), 2); - index.radiusSearch(cornersProjectedMat, indices, dists, radius*radius, rtflann::SearchParams()); + index.radiusSearch(cornersProjectedMat, indices, dists, radius*radius, rtflann::SearchParams(32, 0, false)); UASSERT(indices.size() == cornersProjectedMat.rows); UASSERT(descriptorsFrom.cols == descriptorsTo.cols); diff --git a/guilib/include/rtabmap/gui/PreferencesDialog.h b/guilib/include/rtabmap/gui/PreferencesDialog.h index 7f8f6daa3b..37301a0536 100644 --- a/guilib/include/rtabmap/gui/PreferencesDialog.h +++ b/guilib/include/rtabmap/gui/PreferencesDialog.h @@ -265,6 +265,7 @@ class RTABMAP_GUI_EXPORT PreferencesDialog : public QDialog double getBilateralSigmaR() const; int getSourceImageDecimation() const; int getSourceHistogramMethod() const; + bool isSourceFeatureDetection() const; bool isSourceStereoDepthGenerated() const; bool isSourceStereoExposureCompensation() const; bool isSourceScanFromDepth() const; diff --git a/guilib/src/MainWindow.cpp b/guilib/src/MainWindow.cpp index a63039de49..8a5908497e 100644 --- a/guilib/src/MainWindow.cpp +++ b/guilib/src/MainWindow.cpp @@ -5774,6 +5774,10 @@ void MainWindow::startDetection() _camera->setColorOnly(_preferencesDialog->isSourceRGBDColorOnly()); _camera->setImageDecimation(_preferencesDialog->getSourceImageDecimation()); _camera->setHistogramMethod(_preferencesDialog->getSourceHistogramMethod()); + if(_preferencesDialog->isSourceFeatureDetection()) + { + _camera->enableFeatureDetection(parameters); + } _camera->setStereoToDepth(_preferencesDialog->isSourceStereoDepthGenerated()); _camera->setStereoExposureCompensation(_preferencesDialog->isSourceStereoExposureCompensation()); _camera->setScanParameters( diff --git a/guilib/src/PreferencesDialog.cpp b/guilib/src/PreferencesDialog.cpp index 78bf26ba92..3476d5be02 100644 --- a/guilib/src/PreferencesDialog.cpp +++ b/guilib/src/PreferencesDialog.cpp @@ -817,6 +817,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) : connect(_ui->checkbox_rgbd_colorOnly, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel())); connect(_ui->spinBox_source_imageDecimation, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel())); connect(_ui->comboBox_source_histogramMethod, SIGNAL(currentIndexChanged(int)), this, SLOT(makeObsoleteSourcePanel())); + connect(_ui->checkbox_source_feature_detection, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel())); connect(_ui->checkbox_stereo_depthGenerated, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel())); connect(_ui->checkBox_stereo_exposureCompensation, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel())); connect(_ui->pushButton_calibrate, SIGNAL(clicked()), this, SLOT(calibrate())); @@ -2058,6 +2059,7 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox) _ui->checkbox_rgbd_colorOnly->setChecked(false); _ui->spinBox_source_imageDecimation->setValue(1); _ui->comboBox_source_histogramMethod->setCurrentIndex(0); + _ui->checkbox_source_feature_detection->setChecked(false); _ui->checkbox_stereo_depthGenerated->setChecked(false); _ui->checkBox_stereo_exposureCompensation->setChecked(false); _ui->openni2_autoWhiteBalance->setChecked(true); @@ -2493,6 +2495,7 @@ void PreferencesDialog::readCameraSettings(const QString & filePath) _ui->lineEdit_sourceLocalTransform->setText(settings.value("localTransform",_ui->lineEdit_sourceLocalTransform->text()).toString()); _ui->spinBox_source_imageDecimation->setValue(settings.value("imageDecimation",_ui->spinBox_source_imageDecimation->value()).toInt()); _ui->comboBox_source_histogramMethod->setCurrentIndex(settings.value("histogramMethod", _ui->comboBox_source_histogramMethod->currentIndex()).toInt()); + _ui->checkbox_source_feature_detection->setChecked(settings.value("featureDetection", _ui->checkbox_source_feature_detection->isChecked()).toBool()); // Backward compatibility if(_ui->lineEdit_sourceLocalTransform->text().compare("0 0 1 -1 0 0 0 -1 0") == 0) { @@ -3032,6 +3035,7 @@ void PreferencesDialog::writeCameraSettings(const QString & filePath) const settings.setValue("localTransform", _ui->lineEdit_sourceLocalTransform->text()); settings.setValue("imageDecimation", _ui->spinBox_source_imageDecimation->value()); settings.setValue("histogramMethod", _ui->comboBox_source_histogramMethod->currentIndex()); + settings.setValue("featureDetection", _ui->checkbox_source_feature_detection->isChecked()); settings.beginGroup("rgbd"); settings.setValue("driver", _ui->comboBox_cameraRGBD->currentIndex()); @@ -6034,6 +6038,10 @@ int PreferencesDialog::getSourceHistogramMethod() const { return _ui->comboBox_source_histogramMethod->currentIndex(); } +bool PreferencesDialog::isSourceFeatureDetection() const +{ + return _ui->checkbox_source_feature_detection->isChecked(); +} bool PreferencesDialog::isSourceStereoDepthGenerated() const { return _ui->checkbox_stereo_depthGenerated->isChecked(); @@ -6839,6 +6847,10 @@ void PreferencesDialog::testOdometry() cameraThread.setColorOnly(_ui->checkbox_rgbd_colorOnly->isChecked()); cameraThread.setImageDecimation(_ui->spinBox_source_imageDecimation->value()); cameraThread.setHistogramMethod(_ui->comboBox_source_histogramMethod->currentIndex()); + if(_ui->checkbox_source_feature_detection->isChecked()) + { + cameraThread.enableFeatureDetection(this->getAllParameters()); + } cameraThread.setStereoToDepth(_ui->checkbox_stereo_depthGenerated->isChecked()); cameraThread.setStereoExposureCompensation(_ui->checkBox_stereo_exposureCompensation->isChecked()); cameraThread.setScanParameters( @@ -6911,6 +6923,10 @@ void PreferencesDialog::testCamera() cameraThread.setColorOnly(_ui->checkbox_rgbd_colorOnly->isChecked()); cameraThread.setImageDecimation(_ui->spinBox_source_imageDecimation->value()); cameraThread.setHistogramMethod(_ui->comboBox_source_histogramMethod->currentIndex()); + if(_ui->checkbox_source_feature_detection->isChecked()) + { + cameraThread.enableFeatureDetection(this->getAllParameters()); + } cameraThread.setStereoToDepth(_ui->checkbox_stereo_depthGenerated->isChecked()); cameraThread.setStereoExposureCompensation(_ui->checkBox_stereo_exposureCompensation->isChecked()); cameraThread.setScanParameters( diff --git a/guilib/src/ui/preferencesDialog.ui b/guilib/src/ui/preferencesDialog.ui index 26641bfdd0..643be37779 100644 --- a/guilib/src/ui/preferencesDialog.ui +++ b/guilib/src/ui/preferencesDialog.ui @@ -63,9 +63,9 @@ 0 - -781 + 0 713 - 3853 + 3893 @@ -95,7 +95,7 @@ QFrame::Raised - 19 + 5 @@ -2906,29 +2906,6 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki - - - - Input rate (0 means as fast as possible). - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - Local transform from /base_link to /camera_link. Mouse over the box to show formats. If odometry sensor is enabled, it is the transform to odometry sensor. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - @@ -2942,33 +2919,17 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki - - - - ID of the device, which might be a serial number, bus@address or the index of the device. If empty, the first device found is taken. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + <html><head/><body><p>Format (3 values): x y z<br/>Format (6 values): x y z roll pitch yaw<br/>Format (7 values): x y z qx qy qz qw<br/>Format (9 values, 3x3 rotation): r11 r12 r13 r21 r22 r23 r31 r32 r33<br/>Format (12 values, 3x4 transform): r11 r12 r13 tx r21 r22 r23 ty r31 r32 r33 tz</p><p>KITTI: /base_link to /gray_camera = 0 0 0 0 0 0<br/>KITTI: /base_link to /color_camera = 0 -0.06 0 0 0 0<br/>KITTI: /base_footprint to /gray_camera = 0 0 1.67 0 0 0<br/>KITTI: /base_footprint to /color_camera = 0 -0.06 1.67 0 0 0</p><p>EuRoC MAV: /base_link to /cam0 = T_BS*T_SC0 = 0.999661 0.0257743 -0.00375625 0.00981073 -0.0257154 0.999557 0.0149672 0.064677 0.00414035 -0.0148655 0.999881 -0.0216401</p></body></html> - - - - - Create a calibration file with known intrinsics (fx, fy, cx, cy). Useful if you already know the intrinsics of the source of images. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + 0 0 0 0 0 0 - + @@ -2992,6 +2953,16 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki + + + + + + + false + + + @@ -3011,36 +2982,6 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki - - - - Mirroring mode (flip image horizontally). It has no effect on database source. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - - - - - - - - <html><head/><body><p>Format (3 values): x y z<br/>Format (6 values): x y z roll pitch yaw<br/>Format (7 values): x y z qx qy qz qw<br/>Format (9 values, 3x3 rotation): r11 r12 r13 r21 r22 r23 r31 r32 r33<br/>Format (12 values, 3x4 transform): r11 r12 r13 tx r21 r22 r23 ty r31 r32 r33 tz</p><p>KITTI: /base_link to /gray_camera = 0 0 0 0 0 0<br/>KITTI: /base_link to /color_camera = 0 -0.06 0 0 0 0<br/>KITTI: /base_footprint to /gray_camera = 0 0 1.67 0 0 0<br/>KITTI: /base_footprint to /color_camera = 0 -0.06 1.67 0 0 0</p><p>EuRoC MAV: /base_link to /cam0 = T_BS*T_SC0 = 0.999661 0.0257743 -0.00375625 0.00981073 -0.0257154 0.999557 0.0149672 0.064677 0.00414035 -0.0148655 0.999881 -0.0216401</p></body></html> - - - 0 0 0 0 0 0 - - - @@ -3069,7 +3010,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki - + 0 @@ -3077,11 +3018,24 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki - Test + Create Calibration - + + + + Equalizes the histogram of grayscale images. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + @@ -3094,10 +3048,10 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki - - + + - Calibration files are saved in "camera_info" folder of the working directory. + Create a calibration file with known intrinsics (fx, fy, cx, cy). Useful if you already know the intrinsics of the source of images. true @@ -3107,15 +3061,50 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki - - + + + + 0 + + + QComboBox::AdjustToContents + + + + None + + + + + Histogram + + + + + CLAHE + + + + + + + + Input rate (0 means as fast as possible). + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + - - + + 0 @@ -3123,14 +3112,14 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki - Create Calibration + Test - - + + - Calibration file path (*.yaml). If empty, the GUID of the camera is used (for those having one). OpenNI and Freenect drivers use factory calibration by default (so they ignore this parameter). A calibrated camera is required for RGB-D SLAM mode. + Calibration files are saved in "camera_info" folder of the working directory. true @@ -3140,10 +3129,10 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki - - + + - Image decimation. RGB/Mono and depth images will be resized according to this value (size*1/decimation). Note that if depth images are captured, decimation should be a multiple of the depth image size. If depth images are smaller than RGB images, the decimation is first applied on RGB, if the resulting RGB image is still bigger than depth image, the depth is not decimated. + Only RGB images (for RGB-D cameras) or left images (for stereo cameras) are published. true @@ -3160,10 +3149,10 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki - - + + - Equalizes the histogram of grayscale images. + ID of the device, which might be a serial number, bus@address or the index of the device. If empty, the first device found is taken. true @@ -3173,35 +3162,30 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki - - - - 0 + + + + - - QComboBox::AdjustToContents + + + + + + Mirroring mode (flip image horizontally). It has no effect on database source. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - None - - - - - Histogram - - - - - CLAHE - - - - + + - Only RGB images (for RGB-D cameras) or left images (for stereo cameras) are published. + Image decimation. RGB/Mono and depth images will be resized according to this value (size*1/decimation). Note that if depth images are captured, decimation should be a multiple of the depth image size. If depth images are smaller than RGB images, the decimation is first applied on RGB, if the resulting RGB image is still bigger than depth image, the depth is not decimated. true @@ -3211,8 +3195,47 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki - - + + + + Local transform from /base_link to /camera_link. Mouse over the box to show formats. If odometry sensor is enabled, it is the transform to odometry sensor. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + Calibration file path (*.yaml). If empty, the GUID of the camera is used (for those having one). OpenNI and Freenect drivers use factory calibration by default (so they ignore this parameter). A calibrated camera is required for RGB-D SLAM mode. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + Detect features inside camera thread using parameters set in Visual Registration panel. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + +