i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-08 12:30:20 +08:00
Code Issues Packages Projects Releases Wiki Activity

CameraImages: moved scan filtering stuff to CameraThread

Browse Source 
Preferences->Source: refactored scan filtering parameters
OdometryF2M and OdometryF2F: moved odom orientation init from IMU to OdometryROS instead
This commit is contained in:
matlabbe
2019-01-15 15:41:07 -05:00
parent 42c8ed0c0f
commit 9da2c1918f
12 changed files with 210 additions and 269 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/include/rtabmap/core/CameraThread.h
+16 -11
View File
@@ -69,20 +69,24 @@ public:
void enableBilateralFiltering(float sigmaS, float sigmaR);
void disableBilateralFiltering() {_bilateralFiltering = false;}
void setScanFromDepth(
bool enabled,
int decimation=4,
float maxDepth=4.0f,
void setScanParameters(
bool fromDepth,
int downsampleStep=1, // decimation of the depth image in case the scan is from depth image
float rangeMin=0.0f,
float rangeMax=0.0f,
float voxelSize = 0.0f,
int normalsK = 0,
int normalsRadius = 0.0f)
int normalsRadius = 0.0f,
bool forceGroundNormalsUp = false)
{
_scanFromDepth = enabled;
_scanDecimation=decimation;
_scanMaxDepth = maxDepth;
_scanFromDepth = fromDepth;
_scanDownsampleStep=downsampleStep;
_scanRangeMin = rangeMin;
_scanRangeMax = rangeMax;
_scanVoxelSize = voxelSize;
_scanNormalsK = normalsK;
_scanNormalsRadius = normalsRadius;
_scanForceGroundNormalsUp = forceGroundNormalsUp;
}
void postUpdate(SensorData * data, CameraInfo * info = 0) const;
@@ -106,12 +110,13 @@ private:
int _imageDecimation;
bool _stereoToDepth;
bool _scanFromDepth;
int _scanDecimation;
float _scanMaxDepth;
float _scanMinDepth;
int _scanDownsampleStep;
float _scanRangeMin;
float _scanRangeMax;
float _scanVoxelSize;
int _scanNormalsK;
float _scanNormalsRadius;
bool _scanForceGroundNormalsUp;
StereoDense * _stereoDense;
clams::DiscreteDepthDistortionModel * _distortionModel;
bool _bilateralFiltering;
corelib/include/rtabmap/core/camera/CameraImages.h
+1 -16
View File
@@ -76,21 +76,11 @@ public:
void setScanPath(
const std::string & dir,
int maxScanPts = 0,
int downsampleStep = 1,
float voxelSize = 0.0f,
int normalsK = 0, // compute normals if > 0
float normalsRadius = 0, // compute normals if > 0
const Transform & localTransform=Transform::getIdentity(),
bool forceGroundNormalsUp = false)
const Transform & localTransform=Transform::getIdentity())
{
_scanPath = dir;
_scanLocalTransform = localTransform;
_scanMaxPts = maxScanPts;
_scanDownsampleStep = downsampleStep;
_scanNormalsK = normalsK;
_scanNormalsRadius = normalsRadius;
_scanVoxelSize = voxelSize;
_scanForceGroundNormalsUp = forceGroundNormalsUp;
}
void setDepthFromScan(bool enabled, int fillHoles = 1, bool fillHolesFromBorder = false)
@@ -152,11 +142,6 @@ private:
std::string _scanPath;
Transform _scanLocalTransform;
int _scanMaxPts;
int _scanDownsampleStep;
float _scanVoxelSize;
int _scanNormalsK;
float _scanNormalsRadius;
bool _scanForceGroundNormalsUp;
bool _depthFromScan;
int _depthFromScanFillHoles; // <0:horizontal 0:disabled >0:vertical
corelib/src/CameraThread.cpp
+16 -9
View File
@@ -54,12 +54,13 @@ CameraThread::CameraThread(Camera * camera, const ParametersMap & parameters) :
_imageDecimation(1),
_stereoToDepth(false),
_scanFromDepth(false),
_scanDecimation(4),
_scanMaxDepth(4.0f),
_scanMinDepth(0.0f),
_scanDownsampleStep(1),
_scanRangeMin(0.0f),
_scanRangeMax(0.0f),
_scanVoxelSize(0.0f),
_scanNormalsK(0),
_scanNormalsRadius(0.0f),
_scanForceGroundNormalsUp(false),
_stereoDense(StereoDense::create(parameters)),
_distortionModel(0),
_bilateralFiltering(false),
@@ -321,16 +322,16 @@ void CameraThread::postUpdate(SensorData * dataPtr, CameraInfo * info) const
UDEBUG("");
if(data.laserScanRaw().isEmpty())
{
UASSERT(_scanDecimation >= 1);
UASSERT(_scanDownsampleStep >= 1);
UTimer timer;
pcl::IndicesPtr validIndices(new std::vector<int>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = util3d::cloudRGBFromSensorData(
data,
_scanDecimation,
_scanMaxDepth,
_scanMinDepth,
_scanDownsampleStep,
_scanRangeMax,
_scanRangeMin,
validIndices.get());
float maxPoints = (data.depthRaw().rows/_scanDecimation)*(data.depthRaw().cols/_scanDecimation);
float maxPoints = (data.depthRaw().rows/_scanDownsampleStep)*(data.depthRaw().cols/_scanDownsampleStep);
cv::Mat scan;
const Transform & baseToScan = data.cameraModels()[0].localTransform();
LaserScan::Format format = LaserScan::kXYZRGB;
@@ -366,7 +367,7 @@ void CameraThread::postUpdate(SensorData * dataPtr, CameraInfo * info) const
}
}
}
data.setLaserScanRaw(LaserScan(scan, (int)maxPoints, _scanMaxDepth, format, baseToScan));
data.setLaserScanRaw(LaserScan(scan, (int)maxPoints, _scanRangeMax, format, baseToScan));
if(info) info->timeScanFromDepth = timer.ticks();
}
else
@@ -376,6 +377,12 @@ void CameraThread::postUpdate(SensorData * dataPtr, CameraInfo * info) const
"depth will not be created.");
}
}
else if(!data.laserScanRaw().isEmpty())
{
UDEBUG("");
// filter the scan after registration
data.setLaserScanRaw(util3d::commonFiltering(data.laserScanRaw(), _scanDownsampleStep, _scanRangeMin, _scanRangeMax, _scanVoxelSize, _scanNormalsK, _scanNormalsRadius, _scanForceGroundNormalsUp));
}
}
} // namespace rtabmap
corelib/src/camera/CameraImages.cpp
-12
View File
@@ -52,11 +52,6 @@ CameraImages::CameraImages() :
_scanDir(0),
_scanLocalTransform(Transform::getIdentity()),
_scanMaxPts(0),
_scanDownsampleStep(1),
_scanVoxelSize(0.0f),
_scanNormalsK(0),
_scanNormalsRadius(0),
_scanForceGroundNormalsUp(false),
_depthFromScan(false),
_depthFromScanFillHoles(1),
_depthFromScanFillHolesFromBorder(false),
@@ -84,11 +79,6 @@ CameraImages::CameraImages(const std::string & path,
_scanDir(0),
_scanLocalTransform(Transform::getIdentity()),
_scanMaxPts(0),
_scanDownsampleStep(1),
_scanVoxelSize(0.0f),
_scanNormalsK(0),
_scanNormalsRadius(0),
_scanForceGroundNormalsUp(false),
_depthFromScan(false),
_depthFromScanFillHoles(1),
_depthFromScanFillHolesFromBorder(false),
@@ -731,8 +721,6 @@ SensorData CameraImages::captureImage(CameraInfo * info)
}
}
}
// filter the scan after registration
scan = util3d::commonFiltering(scan, _scanDownsampleStep, 0, 0, _scanVoxelSize, _scanNormalsK, _scanNormalsRadius, _scanForceGroundNormalsUp);
}
}
else
corelib/src/odometry/OdometryF2F.cpp
-24
View File
@@ -91,30 +91,6 @@ Transform OdometryF2F::computeTransform(
UASSERT(!this->getPose().isNull());
if(lastKeyFramePose_.isNull())
{
if(this->getPose().rotation().isIdentity() &&
data.imu().linearAcceleration()[0]!=0.0 &&
data.imu().linearAcceleration()[1]!=0.0 &&
data.imu().linearAcceleration()[2]!=0.0 &&
!data.imu().localTransform().isNull())
{
// align with gravity
Eigen::Vector3f n(data.imu().linearAcceleration()[0], data.imu().linearAcceleration()[1], data.imu().linearAcceleration()[2]);
n = data.imu().localTransform().toEigen3f() * n;
n.normalize();
n[0]*=-1;
n[1]*=-1;
n[2]*=-1;
Eigen::Vector3f z(0,0,1);
//get rotation from z to n;
Eigen::Matrix3f R;
R = Eigen::Quaternionf().setFromTwoVectors(n,z);
Transform rotation(
R(0,0), R(0,1), R(0,2), 0,
R(1,0), R(1,1), R(1,2), 0,
R(2,0), R(2,1), R(2,2), 0);
this->reset(rotation);
}
lastKeyFramePose_ = this->getPose(); // reset to current pose
}
Transform motionSinceLastKeyFrame = lastKeyFramePose_.inverse()*this->getPose();
corelib/src/odometry/OdometryF2M.cpp
-25
View File
@@ -938,31 +938,6 @@ Transform OdometryF2M::computeTransform(
regInfo.covariance = cv::Mat::eye(6,6,CV_64FC1)*9999.0;
bool frameValid = false;
if(this->getPose().rotation().isIdentity() &&
data.imu().linearAcceleration()[0]!=0.0 &&
data.imu().linearAcceleration()[1]!=0.0 &&
data.imu().linearAcceleration()[2]!=0.0 &&
!data.imu().localTransform().isNull())
{
// align with gravity
Eigen::Vector3f n(data.imu().linearAcceleration()[0], data.imu().linearAcceleration()[1], data.imu().linearAcceleration()[2]);
n = data.imu().localTransform().toEigen3f() * n;
n.normalize();
n[0]*=-1;
n[1]*=-1;
n[2]*=-1;
Eigen::Vector3f z(0,0,1);
//get rotation from z to n;
Eigen::Matrix3f R;
R = Eigen::Quaternionf().setFromTwoVectors(n,z);
Transform rotation(
R(0,0), R(0,1), R(0,2), 0,
R(1,0), R(1,1), R(1,2), 0,
R(2,0), R(2,1), R(2,2), 0);
this->reset(rotation);
}
Transform newFramePose = this->getPose(); // initial pose may be not identity...
if(regPipeline_->isImageRequired())
{
guilib/include/rtabmap/gui/PreferencesDialog.h
+4 -2
View File
@@ -247,11 +247,13 @@ public:
bool isSourceStereoDepthGenerated() const;
bool isSourceStereoExposureCompensation() const;
bool isSourceScanFromDepth() const;
int getSourceScanFromDepthDecimation() const;
double getSourceScanFromDepthMaxDepth() const;
int getSourceScanDownsampleStep() const;
double getSourceScanRangeMin() const;
double getSourceScanRangeMax() const;
double getSourceScanVoxelSize() const;
int getSourceScanNormalsK() const;
double getSourceScanNormalsRadius() const;
bool isSourceScanForceGroundNormalsUp() const;
Transform getSourceLocalTransform() const; //Openni group
Transform getLaserLocalTransform() const; // directory images
Transform getIMULocalTransform() const; // directory images
guilib/src/MainWindow.cpp
+6 -4
View File
@@ -4863,13 +4863,15 @@ void MainWindow::startDetection()
_camera->setImageDecimation(_preferencesDialog->getSourceImageDecimation());
_camera->setStereoToDepth(_preferencesDialog->isSourceStereoDepthGenerated());
_camera->setStereoExposureCompensation(_preferencesDialog->isSourceStereoExposureCompensation());
_camera->setScanFromDepth(
_camera->setScanParameters(
_preferencesDialog->isSourceScanFromDepth(),
_preferencesDialog->getSourceScanFromDepthDecimation(),
_preferencesDialog->getSourceScanFromDepthMaxDepth(),
_preferencesDialog->getSourceScanDownsampleStep(),
_preferencesDialog->getSourceScanRangeMin(),
_preferencesDialog->getSourceScanRangeMax(),
_preferencesDialog->getSourceScanVoxelSize(),
_preferencesDialog->getSourceScanNormalsK(),
_preferencesDialog->getSourceScanNormalsRadius());
_preferencesDialog->getSourceScanNormalsRadius(),
_preferencesDialog->isSourceScanForceGroundNormalsUp());
if(_preferencesDialog->isDepthFilteringAvailable())
{
if(_preferencesDialog->isBilateralFiltering())
guilib/src/PreferencesDialog.cpp
+74 -81
View File
@@ -603,8 +603,6 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
connect(_ui->lineEdit_cameraImages_path_scans, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->lineEdit_cameraImages_laser_transform, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->spinBox_cameraImages_max_scan_pts, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->spinBox_cameraImages_scanDownsampleStep, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->doubleSpinBox_cameraImages_scanVoxelSize, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->lineEdit_cameraImages_odom, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->comboBox_cameraImages_odomFormat, SIGNAL(currentIndexChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->lineEdit_cameraImages_gt, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel()));
@@ -661,13 +659,14 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
connect(_ui->doubleSpinBox_bilateral_sigmaS, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->doubleSpinBox_bilateral_sigmaR, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->groupBox_scanFromDepth, SIGNAL(toggled(bool)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->spinBox_cameraScanFromDepth_decimation, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->doubleSpinBox_cameraSCanFromDepth_maxDepth, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->doubleSpinBox_cameraImages_scanVoxelSize, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->spinBox_cameraImages_scanNormalsK, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->doubleSpinBox_cameraImages_scanNormalsRadius, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->checkBox_cameraImages_scanForceGroundNormalsUp, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->checkBox_source_scanFromDepth, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->spinBox_source_scanDownsampleStep, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->doubleSpinBox_source_scanRangeMin, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->doubleSpinBox_source_scanRangeMax, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->doubleSpinBox_source_scanVoxelSize, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->spinBox_source_scanNormalsK, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->doubleSpinBox_source_scanNormalsRadius, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->checkBox_source_scanForceGroundNormalsUp, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
//Rtabmap basic
@@ -1731,8 +1730,6 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
_ui->lineEdit_cameraImages_path_scans->setText("");
_ui->lineEdit_cameraImages_laser_transform->setText("0 0 0 0 0 0");
_ui->spinBox_cameraImages_max_scan_pts->setValue(0);
_ui->spinBox_cameraImages_scanDownsampleStep->setValue(1);
_ui->doubleSpinBox_cameraImages_scanVoxelSize->setValue(0.0f);
_ui->lineEdit_cameraImages_odom->setText("");
_ui->comboBox_cameraImages_odomFormat->setCurrentIndex(0);
_ui->lineEdit_cameraImages_gt->setText("");
@@ -1742,13 +1739,14 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
_ui->lineEdit_cameraImages_imu_transform->setText("0 0 1 0 -1 0 1 0 0");
_ui->spinBox_cameraImages_max_imu_rate->setValue(0);
_ui->groupBox_scanFromDepth->setChecked(false);
_ui->spinBox_cameraScanFromDepth_decimation->setValue(8);
_ui->doubleSpinBox_cameraSCanFromDepth_maxDepth->setValue(4.0);
_ui->doubleSpinBox_cameraImages_scanVoxelSize->setValue(0.025f);
_ui->spinBox_cameraImages_scanNormalsK->setValue(20);
_ui->doubleSpinBox_cameraImages_scanNormalsRadius->setValue(0.0);
_ui->checkBox_cameraImages_scanForceGroundNormalsUp->setChecked(false);
_ui->checkBox_source_scanFromDepth->setChecked(false);
_ui->spinBox_source_scanDownsampleStep->setValue(1);
_ui->doubleSpinBox_source_scanRangeMin->setValue(0);
_ui->doubleSpinBox_source_scanRangeMax->setValue(0);
_ui->doubleSpinBox_source_scanVoxelSize->setValue(0.0f);
_ui->spinBox_source_scanNormalsK->setValue(0);
_ui->doubleSpinBox_source_scanNormalsRadius->setValue(0.0);
_ui->checkBox_source_scanForceGroundNormalsUp->setChecked(false);
_ui->groupBox_depthFromScan->setChecked(false);
_ui->groupBox_depthFromScan_fillHoles->setChecked(true);
@@ -2145,8 +2143,6 @@ void PreferencesDialog::readCameraSettings(const QString & filePath)
_ui->lineEdit_cameraImages_path_scans->setText(settings.value("path_scans", _ui->lineEdit_cameraImages_path_scans->text()).toString());
_ui->lineEdit_cameraImages_laser_transform->setText(settings.value("scan_transform", _ui->lineEdit_cameraImages_laser_transform->text()).toString());
_ui->spinBox_cameraImages_max_scan_pts->setValue(settings.value("scan_max_pts", _ui->spinBox_cameraImages_max_scan_pts->value()).toInt());
_ui->spinBox_cameraImages_scanDownsampleStep->setValue(settings.value("scan_downsample_step", _ui->spinBox_cameraImages_scanDownsampleStep->value()).toInt());
_ui->doubleSpinBox_cameraImages_scanVoxelSize->setValue(settings.value("scan_voxel_size", _ui->doubleSpinBox_cameraImages_scanVoxelSize->value()).toDouble());
_ui->lineEdit_cameraImages_odom->setText(settings.value("odom_path", _ui->lineEdit_cameraImages_odom->text()).toString());
_ui->comboBox_cameraImages_odomFormat->setCurrentIndex(settings.value("odom_format", _ui->comboBox_cameraImages_odomFormat->currentIndex()).toInt());
_ui->lineEdit_cameraImages_gt->setText(settings.value("gt_path", _ui->lineEdit_cameraImages_gt->text()).toString());
@@ -2162,16 +2158,16 @@ void PreferencesDialog::readCameraSettings(const QString & filePath)
_ui->source_video_lineEdit_path->setText(settings.value("path", _ui->source_video_lineEdit_path->text()).toString());
settings.endGroup(); // video
settings.beginGroup("ScanFromDepth");
_ui->groupBox_scanFromDepth->setChecked(settings.value("enabled", _ui->groupBox_scanFromDepth->isChecked()).toBool());
_ui->spinBox_cameraScanFromDepth_decimation->setValue(settings.value("decimation", _ui->spinBox_cameraScanFromDepth_decimation->value()).toInt());
_ui->doubleSpinBox_cameraSCanFromDepth_maxDepth->setValue(settings.value("maxDepth", _ui->doubleSpinBox_cameraSCanFromDepth_maxDepth->value()).toDouble());
_ui->doubleSpinBox_cameraImages_scanVoxelSize->setValue(settings.value("voxelSize", _ui->doubleSpinBox_cameraImages_scanVoxelSize->value()).toDouble());
_ui->spinBox_cameraImages_scanNormalsK->setValue(settings.value("normalsK", _ui->spinBox_cameraImages_scanNormalsK->value()).toInt());
_ui->doubleSpinBox_cameraImages_scanNormalsRadius->setValue(settings.value("normalsRadius", _ui->doubleSpinBox_cameraImages_scanNormalsRadius->value()).toDouble());
_ui->checkBox_cameraImages_scanForceGroundNormalsUp->setChecked(settings.value("normalsUp", _ui->checkBox_cameraImages_scanForceGroundNormalsUp->isChecked()).toBool());
settings.endGroup();//ScanFromDepth
settings.beginGroup("Scan");
_ui->checkBox_source_scanFromDepth->setChecked(settings.value("fromDepth", _ui->checkBox_source_scanFromDepth->isChecked()).toBool());
_ui->spinBox_source_scanDownsampleStep->setValue(settings.value("downsampleStep", _ui->spinBox_source_scanDownsampleStep->value()).toInt());
_ui->doubleSpinBox_source_scanRangeMin->setValue(settings.value("rangeMin", _ui->doubleSpinBox_source_scanRangeMin->value()).toDouble());
_ui->doubleSpinBox_source_scanRangeMax->setValue(settings.value("rangeMax", _ui->doubleSpinBox_source_scanRangeMax->value()).toDouble());
_ui->doubleSpinBox_source_scanVoxelSize->setValue(settings.value("voxelSize", _ui->doubleSpinBox_source_scanVoxelSize->value()).toDouble());
_ui->spinBox_source_scanNormalsK->setValue(settings.value("normalsK", _ui->spinBox_source_scanNormalsK->value()).toInt());
_ui->doubleSpinBox_source_scanNormalsRadius->setValue(settings.value("normalsRadius", _ui->doubleSpinBox_source_scanNormalsRadius->value()).toDouble());
_ui->checkBox_source_scanForceGroundNormalsUp->setChecked(settings.value("normalsUp", _ui->checkBox_source_scanForceGroundNormalsUp->isChecked()).toBool());
settings.endGroup();//Scan
settings.beginGroup("DepthFromScan");
_ui->groupBox_depthFromScan->setChecked(settings.value("depthFromScan", _ui->groupBox_depthFromScan->isChecked()).toBool());
@@ -2569,8 +2565,6 @@ void PreferencesDialog::writeCameraSettings(const QString & filePath) const
settings.setValue("path_scans", _ui->lineEdit_cameraImages_path_scans->text());
settings.setValue("scan_transform", _ui->lineEdit_cameraImages_laser_transform->text());
settings.setValue("scan_max_pts", _ui->spinBox_cameraImages_max_scan_pts->value());
settings.setValue("scan_downsample_step", _ui->spinBox_cameraImages_scanDownsampleStep->value());
settings.setValue("scan_voxel_size", _ui->doubleSpinBox_cameraImages_scanVoxelSize->value());
settings.setValue("odom_path", _ui->lineEdit_cameraImages_odom->text());
settings.setValue("odom_format", _ui->comboBox_cameraImages_odomFormat->currentIndex());
settings.setValue("gt_path", _ui->lineEdit_cameraImages_gt->text());
@@ -2585,14 +2579,15 @@ void PreferencesDialog::writeCameraSettings(const QString & filePath) const
settings.setValue("path", _ui->source_video_lineEdit_path->text());
settings.endGroup(); // video
settings.beginGroup("ScanFromDepth");
settings.setValue("enabled", _ui->groupBox_scanFromDepth->isChecked());
settings.setValue("decimation", _ui->spinBox_cameraScanFromDepth_decimation->value());
settings.setValue("maxDepth", _ui->doubleSpinBox_cameraSCanFromDepth_maxDepth->value());
settings.setValue("voxelSize", _ui->doubleSpinBox_cameraImages_scanVoxelSize->value());
settings.setValue("normalsK", _ui->spinBox_cameraImages_scanNormalsK->value());
settings.setValue("normalsRadius", _ui->doubleSpinBox_cameraImages_scanNormalsRadius->value());
settings.setValue("normalsUp", _ui->checkBox_cameraImages_scanForceGroundNormalsUp->isChecked());
settings.beginGroup("Scan");
settings.setValue("fromDepth", _ui->checkBox_source_scanFromDepth->isChecked());
settings.setValue("downsampleStep", _ui->spinBox_source_scanDownsampleStep->value());
settings.setValue("rangeMin", _ui->doubleSpinBox_source_scanRangeMin->value());
settings.setValue("rangeMax", _ui->doubleSpinBox_source_scanRangeMax->value());
settings.setValue("voxelSize", _ui->doubleSpinBox_source_scanVoxelSize->value());
settings.setValue("normalsK", _ui->spinBox_source_scanNormalsK->value());
settings.setValue("normalsRadius", _ui->doubleSpinBox_source_scanNormalsRadius->value());
settings.setValue("normalsUp", _ui->checkBox_source_scanForceGroundNormalsUp->isChecked());
settings.endGroup();
settings.beginGroup("DepthFromScan");
@@ -4364,7 +4359,8 @@ void PreferencesDialog::updateSourceGrpVisibility()
_ui->groupBox_sourceRGB->setVisible(_ui->comboBox_sourceType->currentIndex() == 2);
_ui->groupBox_sourceDatabase->setVisible(_ui->comboBox_sourceType->currentIndex() == 3);
_ui->groupBox_scanFromDepth->setVisible(_ui->comboBox_sourceType->currentIndex() <= 1 || _ui->comboBox_sourceType->currentIndex() == 3);
_ui->checkBox_source_scanFromDepth->setVisible(_ui->comboBox_sourceType->currentIndex() <= 1 || _ui->comboBox_sourceType->currentIndex() == 3);
_ui->label_source_scanFromDepth->setVisible(_ui->comboBox_sourceType->currentIndex() <= 1 || _ui->comboBox_sourceType->currentIndex() == 3);
_ui->stackedWidget_rgbd->setVisible(_ui->comboBox_sourceType->currentIndex() == 0 &&
(_ui->comboBox_cameraRGBD->currentIndex() == kSrcOpenNI2-kSrcRGBD ||
@@ -4953,27 +4949,35 @@ bool PreferencesDialog::isSourceStereoExposureCompensation() const
}
bool PreferencesDialog::isSourceScanFromDepth() const
{
return _ui->groupBox_scanFromDepth->isChecked();
return _ui->checkBox_source_scanFromDepth->isChecked();
}
int PreferencesDialog::getSourceScanFromDepthDecimation() const
int PreferencesDialog::getSourceScanDownsampleStep() const
{
return _ui->spinBox_cameraScanFromDepth_decimation->value();
return _ui->spinBox_source_scanDownsampleStep->value();
}
double PreferencesDialog::getSourceScanFromDepthMaxDepth() const
double PreferencesDialog::getSourceScanRangeMin() const
{
return _ui->doubleSpinBox_cameraSCanFromDepth_maxDepth->value();
return _ui->doubleSpinBox_source_scanRangeMin->value();
}
double PreferencesDialog::getSourceScanRangeMax() const
{
return _ui->doubleSpinBox_source_scanRangeMax->value();
}
double PreferencesDialog::getSourceScanVoxelSize() const
{
return _ui->doubleSpinBox_cameraImages_scanVoxelSize->value();
return _ui->doubleSpinBox_source_scanVoxelSize->value();
}
int PreferencesDialog::getSourceScanNormalsK() const
{
return _ui->spinBox_cameraImages_scanNormalsK->value();
return _ui->spinBox_source_scanNormalsK->value();
}
double PreferencesDialog::getSourceScanNormalsRadius() const
{
return _ui->doubleSpinBox_cameraImages_scanNormalsRadius->value();
return _ui->doubleSpinBox_source_scanNormalsRadius->value();
}
bool PreferencesDialog::isSourceScanForceGroundNormalsUp() const
{
return _ui->checkBox_source_scanForceGroundNormalsUp->isChecked();
}
Camera * PreferencesDialog::createCamera(bool useRawImages, bool useColor)
@@ -5111,12 +5115,7 @@ Camera * PreferencesDialog::createCamera(bool useRawImages, bool useColor)
((CameraRGBDImages*)camera)->setScanPath(
_ui->lineEdit_cameraImages_path_scans->text().isEmpty()?"":_ui->lineEdit_cameraImages_path_scans->text().append(QDir::separator()).toStdString(),
_ui->spinBox_cameraImages_max_scan_pts->value(),
_ui->spinBox_cameraImages_scanDownsampleStep->value(),
_ui->doubleSpinBox_cameraImages_scanVoxelSize->value(),
_ui->spinBox_cameraImages_scanNormalsK->value(),
_ui->doubleSpinBox_cameraImages_scanNormalsRadius->value(),
this->getLaserLocalTransform(),
_ui->checkBox_cameraImages_scanForceGroundNormalsUp->isChecked());
this->getLaserLocalTransform());
((CameraRGBDImages*)camera)->setTimestamps(
_ui->checkBox_cameraImages_timestamps->isChecked(),
_ui->lineEdit_cameraImages_timestamps->text().toStdString(),
@@ -5160,12 +5159,7 @@ Camera * PreferencesDialog::createCamera(bool useRawImages, bool useColor)
((CameraStereoImages*)camera)->setScanPath(
_ui->lineEdit_cameraImages_path_scans->text().isEmpty()?"":_ui->lineEdit_cameraImages_path_scans->text().append(QDir::separator()).toStdString(),
_ui->spinBox_cameraImages_max_scan_pts->value(),
_ui->spinBox_cameraImages_scanDownsampleStep->value(),
_ui->doubleSpinBox_cameraImages_scanVoxelSize->value(),
_ui->spinBox_cameraImages_scanNormalsK->value(),
_ui->doubleSpinBox_cameraImages_scanNormalsRadius->value(),
this->getLaserLocalTransform(),
_ui->checkBox_cameraImages_scanForceGroundNormalsUp->isChecked());
this->getLaserLocalTransform());
((CameraStereoImages*)camera)->setTimestamps(
_ui->checkBox_cameraImages_timestamps->isChecked(),
_ui->lineEdit_cameraImages_timestamps->text().toStdString(),
@@ -5292,12 +5286,7 @@ Camera * PreferencesDialog::createCamera(bool useRawImages, bool useColor)
((CameraImages*)camera)->setScanPath(
_ui->lineEdit_cameraImages_path_scans->text().isEmpty()?"":_ui->lineEdit_cameraImages_path_scans->text().append(QDir::separator()).toStdString(),
_ui->spinBox_cameraImages_max_scan_pts->value(),
_ui->spinBox_cameraImages_scanDownsampleStep->value(),
_ui->doubleSpinBox_cameraImages_scanVoxelSize->value(),
_ui->spinBox_cameraImages_scanNormalsK->value(),
_ui->doubleSpinBox_cameraImages_scanNormalsRadius->value(),
this->getLaserLocalTransform(),
_ui->checkBox_cameraImages_scanForceGroundNormalsUp->isChecked());
this->getLaserLocalTransform());
((CameraImages*)camera)->setDepthFromScan(
_ui->groupBox_depthFromScan->isChecked(),
!_ui->groupBox_depthFromScan_fillHoles->isChecked()?0:_ui->radioButton_depthFromScan_vertical->isChecked()?1:-1,
@@ -5574,13 +5563,15 @@ void PreferencesDialog::testOdometry()
cameraThread.setImageDecimation(_ui->spinBox_source_imageDecimation->value());
cameraThread.setStereoToDepth(_ui->checkbox_stereo_depthGenerated->isChecked());
cameraThread.setStereoExposureCompensation(_ui->checkBox_stereo_exposureCompensation->isChecked());
cameraThread.setScanFromDepth(
_ui->groupBox_scanFromDepth->isChecked(),
_ui->spinBox_cameraScanFromDepth_decimation->value(),
_ui->doubleSpinBox_cameraSCanFromDepth_maxDepth->value(),
_ui->doubleSpinBox_cameraImages_scanVoxelSize->value(),
_ui->spinBox_cameraImages_scanNormalsK->value(),
_ui->doubleSpinBox_cameraImages_scanNormalsRadius->value());
cameraThread.setScanParameters(
_ui->checkBox_source_scanFromDepth->isChecked(),
_ui->spinBox_source_scanDownsampleStep->value(),
_ui->doubleSpinBox_source_scanRangeMin->value(),
_ui->doubleSpinBox_source_scanRangeMax->value(),
_ui->doubleSpinBox_source_scanVoxelSize->value(),
_ui->spinBox_source_scanNormalsK->value(),
_ui->doubleSpinBox_source_scanNormalsRadius->value(),
_ui->checkBox_source_scanForceGroundNormalsUp->isChecked());
if(isDepthFilteringAvailable())
{
if(_ui->groupBox_bilateral->isChecked())
@@ -5639,13 +5630,15 @@ void PreferencesDialog::testCamera()
cameraThread.setImageDecimation(_ui->spinBox_source_imageDecimation->value());
cameraThread.setStereoToDepth(_ui->checkbox_stereo_depthGenerated->isChecked());
cameraThread.setStereoExposureCompensation(_ui->checkBox_stereo_exposureCompensation->isChecked());
cameraThread.setScanFromDepth(
_ui->groupBox_scanFromDepth->isChecked(),
_ui->spinBox_cameraScanFromDepth_decimation->value(),
_ui->doubleSpinBox_cameraSCanFromDepth_maxDepth->value(),
_ui->doubleSpinBox_cameraImages_scanVoxelSize->value(),
_ui->spinBox_cameraImages_scanNormalsK->value(),
_ui->doubleSpinBox_cameraImages_scanNormalsRadius->value());
cameraThread.setScanParameters(
_ui->checkBox_source_scanFromDepth->isChecked(),
_ui->spinBox_source_scanDownsampleStep->value(),
_ui->doubleSpinBox_source_scanRangeMin->value(),
_ui->doubleSpinBox_source_scanRangeMax->value(),
_ui->doubleSpinBox_source_scanVoxelSize->value(),
_ui->spinBox_source_scanNormalsK->value(),
_ui->doubleSpinBox_source_scanNormalsRadius->value(),
_ui->checkBox_source_scanForceGroundNormalsUp->isChecked());
if(isDepthFilteringAvailable())
{
if(_ui->groupBox_bilateral->isChecked())
guilib/src/ui/preferencesDialog.ui
+83 -78
View File
@@ -94,7 +94,7 @@
<property name="geometry">
<rect>
<x>0</x>
<y>0</y>
<y>-1939</y>
<width>680</width>
<height>2986</height>
</rect>
@@ -126,7 +126,7 @@
<enum>QFrame::Raised</enum>
</property>
<property name="currentIndex">
<number>14</number>
<number>5</number>
</property>
<widget class="QWidget" name="page_22">
<layout class="QVBoxLayout" name="verticalLayout_29" stretch="0,1">
@@ -5881,71 +5881,20 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
</property>
</widget>
</item>
<item>
<widget class="QGroupBox" name="groupBox_scanFromDepth">
<property name="title">
<string>Generate laser scan from depth image</string>
</property>
<property name="checkable">
<bool>true</bool>
</property>
<property name="checked">
<bool>false</bool>
</property>
<layout class="QGridLayout" name="gridLayout_63" columnstretch="0,1">
<item row="0" column="0">
<widget class="QSpinBox" name="spinBox_cameraScanFromDepth_decimation">
<property name="maximum">
<number>99999</number>
</property>
<property name="value">
<number>1</number>
</property>
</widget>
</item>
<item row="0" column="1">
<widget class="QLabel" name="label_299">
<property name="text">
<string>Decimation (should be a multiple of the image size). Note that it is done after general image decimation above.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="1" column="0">
<widget class="QDoubleSpinBox" name="doubleSpinBox_cameraSCanFromDepth_maxDepth">
<property name="suffix">
<string> m</string>
</property>
<property name="maximum">
<double>9999.000000000000000</double>
</property>
</widget>
</item>
<item row="1" column="1">
<widget class="QLabel" name="label_301">
<property name="text">
<string>Maximum depth.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
</layout>
</widget>
</item>
<item>
<layout class="QGridLayout" name="gridLayout_62" columnstretch="0,1">
<item row="2" column="0">
<widget class="QSpinBox" name="spinBox_cameraImages_scanNormalsK">
<widget class="QDoubleSpinBox" name="doubleSpinBox_source_scanRangeMin">
<property name="suffix">
<string> m</string>
</property>
<property name="maximum">
<double>9999.000000000000000</double>
</property>
</widget>
</item>
<item row="5" column="0">
<widget class="QSpinBox" name="spinBox_source_scanNormalsK">
<property name="toolTip">
<string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;KITTI: 130 000 points&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
</property>
@@ -5960,10 +5909,10 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
</property>
</widget>
</item>
<item row="0" column="1">
<item row="1" column="1">
<widget class="QLabel" name="label_300">
<property name="text">
<string>Downsample step size for laser scans. If your laser scans are created from depth images, use Decimation above instead (faster).</string>
<string>Downsample step size for laser scans. If your laser scans are created from depth images, the step is used as decimation of the depth images.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
@@ -5973,8 +5922,8 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
</property>
</widget>
</item>
<item row="1" column="0">
<widget class="QDoubleSpinBox" name="doubleSpinBox_cameraImages_scanVoxelSize">
<item row="4" column="0">
<widget class="QDoubleSpinBox" name="doubleSpinBox_source_scanVoxelSize">
<property name="toolTip">
<string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;KITTI: 130 000 points&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
</property>
@@ -5989,8 +5938,8 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
</property>
</widget>
</item>
<item row="0" column="0">
<widget class="QSpinBox" name="spinBox_cameraImages_scanDownsampleStep">
<item row="1" column="0">
<widget class="QSpinBox" name="spinBox_source_scanDownsampleStep">
<property name="toolTip">
<string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;KITTI: 130 000 points&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
</property>
@@ -6002,7 +5951,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
</property>
</widget>
</item>
<item row="1" column="1">
<item row="4" column="1">
<widget class="QLabel" name="label_302">
<property name="text">
<string>Voxel size for uniform sampling. Note that it is done after downsampling.</string>
@@ -6015,7 +5964,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
</property>
</widget>
</item>
<item row="2" column="1">
<item row="5" column="1">
<widget class="QLabel" name="label_304">
<property name="text">
<string>K nearest neighbors for normals computation (0=disabled, 20 can be a good default value). Useful if the ICP registration approach is point to plane.</string>
@@ -6028,7 +5977,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
</property>
</widget>
</item>
<item row="3" column="1">
<item row="6" column="1">
<widget class="QLabel" name="label_425">
<property name="text">
<string>Search radius for normals computation (0=disabled). Useful if the ICP registration approach is point to plane.</string>
@@ -6041,8 +5990,8 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
</property>
</widget>
</item>
<item row="3" column="0">
<widget class="QDoubleSpinBox" name="doubleSpinBox_cameraImages_scanNormalsRadius">
<item row="6" column="0">
<widget class="QDoubleSpinBox" name="doubleSpinBox_source_scanNormalsRadius">
<property name="toolTip">
<string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;KITTI: 130 000 points&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
</property>
@@ -6051,7 +6000,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
</property>
</widget>
</item>
<item row="4" column="1">
<item row="7" column="1">
<widget class="QLabel" name="label_453">
<property name="text">
<string>Force ground normals to be all upward. Useful to make sure Velodyne's scans on ground have all normals upward.</string>
@@ -6064,13 +6013,69 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
</property>
</widget>
</item>
<item row="4" column="0">
<widget class="QCheckBox" name="checkBox_cameraImages_scanForceGroundNormalsUp">
<item row="7" column="0">
<widget class="QCheckBox" name="checkBox_source_scanForceGroundNormalsUp">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="3" column="0">
<widget class="QDoubleSpinBox" name="doubleSpinBox_source_scanRangeMax">
<property name="suffix">
<string> m</string>
</property>
<property name="maximum">
<double>9999.000000000000000</double>
</property>
</widget>
</item>
<item row="2" column="1">
<widget class="QLabel" name="label_522">
<property name="text">
<string>Minimum range, filter points below that range. 0 means disabled.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="3" column="1">
<widget class="QLabel" name="label_523">
<property name="text">
<string>Maximum range, filter points above that range. 0 means disabled.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="0" column="0">
<widget class="QCheckBox" name="checkBox_source_scanFromDepth">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="0" column="1">
<widget class="QLabel" name="label_source_scanFromDepth">
<property name="text">
<string>Generate laser scan from depth image.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
</layout>
</item>
</layout>
tools/KittiDataset/main.cpp
+9 -6
View File
@@ -379,12 +379,15 @@ int main(int argc, char * argv[])
((CameraStereoImages*)cameraThread.camera())->setScanPath(
pathScan,
130000,
scanStep,
scanVoxel,
scanNormalK,
scanNormalRadius,
Transform(-0.27f, 0.0f, 0.08+(height?1.67f:0.0f), 0.0f, 0.0f, 0.0f),
true);
Transform(-0.27f, 0.0f, 0.08+(height?1.67f:0.0f), 0.0f, 0.0f, 0.0f));
cameraThread.setScanParameters(
false,
scanStep,
0,
scanVoxel,
scanNormalK,
scanNormalRadius,
true);
}
float detectionRate = Parameters::defaultRtabmapDetectionRate();
tools/OdometryViewer/main.cpp
+1 -1
View File
@@ -311,7 +311,7 @@ int main (int argc, char * argv[])
{
rtabmap::CameraThread cameraThread(camera, parameters);
cameraThread.setScanFromDepth(icp, decimation<1?1:decimation, maxDepth, voxelSize, normalsK, normalsRadius);
cameraThread.setScanParameters(icp, decimation<1?1:decimation, 0, maxDepth, voxelSize, normalsK, normalsRadius);
odomThread.start();
cameraThread.start();