Skip to content

Commit

Permalink
Source camera: added feature detection option
Browse files Browse the repository at this point in the history
  • Loading branch information
@matlabbe
matlabbe committed Nov 19, 2023
1 parent 188cb42 commit ae21c35
Show file tree
Hide file tree
Showing 7 changed files with 254 additions and 128 deletions.
5 changes: 5 additions & 0 deletions corelib/include/rtabmap/core/CameraThread.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -47,6 +47,7 @@ class CameraInfo;
class SensorData;
class StereoDense;
class IMUFilter;
class Feature2D;

/**
* Class CameraThread
Expand Down Expand Up @@ -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(
Expand Down Expand Up @@ -152,6 +155,8 @@ class RTABMAP_CORE_EXPORT CameraThread :
float _bilateralSigmaR;
IMUFilter * _imuFilter;
bool _imuBaseFrameConversion;
Feature2D * _featureDetector;
bool _depthAsMask;
};

} // namespace rtabmap
83 changes: 80 additions & 3 deletions corelib/src/CameraThread.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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 <opencv2/stitching/detail/exposure_compensate.hpp>
#include <rtabmap/utilite/UTimer.h>
#include <rtabmap/utilite/ULogger.h>
#include <rtabmap/utilite/UStl.h>

#include <pcl/io/io.h>

Expand Down Expand Up @@ -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);
}
Expand Down Expand Up @@ -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());
Expand Down Expand Up @@ -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");
Expand All @@ -166,6 +174,7 @@ CameraThread::~CameraThread()
delete _distortionModel;
delete _stereoDense;
delete _imuFilter;
delete _featureDetector;
}

void CameraThread::setImageRate(float imageRate)
Expand Down Expand Up @@ -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,
Expand Down Expand Up @@ -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<cv::KeyPoint> keypoints = _featureDetector->generateKeypoints(grayScaleImg, depthMask);
cv::Mat descriptors;
std::vector<cv::Point3f> keypoints3D;
if(!keypoints.empty())
{
descriptors = _featureDetector->generateDescriptors(grayScaleImg, keypoints);
if(!keypoints.empty())
{
keypoints3D = _featureDetector->generateKeypoints3D(data, keypoints);
}
}

data.setFeatures(keypoints, keypoints3D, descriptors);
}
}

} // namespace rtabmap
2 changes: 1 addition & 1 deletion corelib/src/RegistrationVis.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -1095,7 +1095,7 @@ Transform RegistrationVis::computeTransformationImpl(
std::vector<std::vector<float> > dists;
float radius = (float)_guessWinSize; // pixels
rtflann::Matrix<float> 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);
Expand Down
1 change: 1 addition & 0 deletions guilib/include/rtabmap/gui/PreferencesDialog.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down
4 changes: 4 additions & 0 deletions guilib/src/MainWindow.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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(
Expand Down
16 changes: 16 additions & 0 deletions guilib/src/PreferencesDialog.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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()));
Expand Down Expand Up @@ -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);
Expand Down Expand Up @@ -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)
{
Expand Down Expand Up @@ -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());
Expand Down Expand Up @@ -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();
Expand Down Expand Up @@ -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(
Expand Down Expand Up @@ -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(
Expand Down
Loading

0 comments on commit ae21c35

Please sign in to comment.