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

Adding support for compressed input data and right color image. (#1463)

Browse Source
This commit is contained in:
matlabbe
2025-03-08 16:40:04 -08:00
committed by GitHub
parent d88353dc1b
commit a1b602dbe0
26 changed files with 359 additions and 123 deletions
Download Patch File Download Diff File Expand all files Collapse all files

86
guilib/src/CameraViewer.cpp
View File

@@ -132,9 +132,23 @@ void CameraViewer::showImage(const rtabmap::SensorData & data)
{
processingImages_ = true;
QString sizes;
imageView_->setVisible(!data.imageRaw().empty() || !data.imageRaw().empty());
cv::Mat left;
cv::Mat depthOrRight;
LaserScan scan;
if( !data.imageRaw().empty() || !data.imageCompressed().empty() ||
!data.depthOrRightRaw().empty() || !data.depthOrRightCompressed().empty() ||
!data.laserScanRaw().empty() || !data.laserScanCompressed().empty())
{
data.uncompressDataConst(
!data.imageRaw().empty() || !data.imageCompressed().empty()?&left:0,
!data.depthOrRightRaw().empty() || !data.depthOrRightCompressed().empty()?&depthOrRight:0,
!data.laserScanRaw().empty() || !data.laserScanCompressed().empty()?&scan:0);
}
imageView_->setVisible(!left.empty() || !left.empty());
std::map<int, MarkerInfo> detections;
if(!data.imageRaw().empty())
if(!left.empty())
{
std::vector<CameraModel> models;
if(markerCheckbox_->isEnabled() && markerCheckbox_->isChecked())
@@ -152,36 +166,58 @@ void CameraViewer::showImage(const rtabmap::SensorData & data)
if(!models.empty() && models[0].isValidForProjection())
{
cv::Mat imageWithDetections;
detections = markerDetector_->detect(data.imageRaw(), models, data.depthRaw(), std::map<int, float>(), &imageWithDetections);
detections = markerDetector_->detect(left, models, depthOrRight, std::map<int, float>(), &imageWithDetections);
imageView_->setImage(uCvMat2QImage(imageWithDetections));
}
else
{
imageView_->setImage(uCvMat2QImage(data.imageRaw()));
imageView_->setImage(uCvMat2QImage(left));
}
sizes.append(QString("Color=%1x%2").arg(data.imageRaw().cols).arg(data.imageRaw().rows));
sizes.append(QString("Color=%1x%2").arg(left.cols).arg(left.rows));
}
if(!data.depthOrRightRaw().empty())
if(!depthOrRight.empty())
{
imageView_->setImageDepth(data.depthOrRightRaw());
sizes.append(QString(" Depth=%1x%2").arg(data.depthOrRightRaw().cols).arg(data.depthOrRightRaw().rows));
imageView_->setImageDepth(depthOrRight);
sizes.append(QString(" Depth=%1x%2").arg(depthOrRight.cols).arg(depthOrRight.rows));
}
imageSizeLabel_->setText(sizes);
if(!data.depthOrRightRaw().empty() &&
if(!depthOrRight.empty() &&
((data.stereoCameraModels().size() && data.stereoCameraModels()[0].isValidForProjection()) || (data.cameraModels().size() && data.cameraModels().at(0).isValidForProjection())))
{
if(showCloudCheckbox_->isChecked())
{
if(!data.imageRaw().empty() && !data.depthOrRightRaw().empty())
if(!left.empty() && !depthOrRight.empty())
{
showCloudCheckbox_->setEnabled(true);
cloudView_->addCloud("cloud", util3d::cloudRGBFromSensorData(data, decimationSpin_->value()!=0?decimationSpin_->value():1, 0, 0, 0, parameters_));
if(data.imageRaw().empty())
{
if(!data.stereoCameraModels().empty())
{
cloudView_->addCloud("cloud", util3d::cloudRGBFromSensorData(SensorData(left, depthOrRight, data.stereoCameraModels()), decimationSpin_->value()!=0?decimationSpin_->value():1, 0, 0, 0, parameters_));
}
else
{
cloudView_->addCloud("cloud", util3d::cloudRGBFromSensorData(SensorData(left, depthOrRight, data.cameraModels()), decimationSpin_->value()!=0?decimationSpin_->value():1, 0, 0, 0, parameters_));
}
}
else
{
cloudView_->addCloud("cloud", util3d::cloudRGBFromSensorData(data, decimationSpin_->value()!=0?decimationSpin_->value():1, 0, 0, 0, parameters_));
}
}
else if(!data.depthOrRightRaw().empty())
else if(!depthOrRight.empty())
{
showCloudCheckbox_->setEnabled(true);
cloudView_->addCloud("cloud", util3d::cloudFromSensorData(data, decimationSpin_->value()!=0?fabs(decimationSpin_->value()):1, 0, 0, 0, parameters_));
if(data.depthOrRightRaw().empty())
{
cloudView_->addCloud("cloud", util3d::cloudFromSensorData(SensorData(cv::Mat(), depthOrRight, data.cameraModels()), decimationSpin_->value()!=0?fabs(decimationSpin_->value()):1, 0, 0, 0, parameters_));
}
else
{
cloudView_->addCloud("cloud", util3d::cloudFromSensorData(data, decimationSpin_->value()!=0?fabs(decimationSpin_->value()):1, 0, 0, 0, parameters_));
}
}
// Add landmarks to 3D Map view
@@ -208,37 +244,37 @@ void CameraViewer::showImage(const rtabmap::SensorData & data)
}
}
if(!data.laserScanRaw().isEmpty())
if(!scan.isEmpty())
{
showScanCheckbox_->setEnabled(true);
if(showScanCheckbox_->isChecked())
{
if(data.laserScanRaw().hasNormals())
if(scan.hasNormals())
{
if(data.laserScanRaw().hasIntensity())
if(scan.hasIntensity())
{
cloudView_->addCloud("scan", util3d::downsample(util3d::laserScanToPointCloudINormal(data.laserScanRaw()), decimationSpin_->value()!=0?fabs(decimationSpin_->value()):1), data.laserScanRaw().localTransform(), Qt::yellow);
cloudView_->addCloud("scan", util3d::downsample(util3d::laserScanToPointCloudINormal(scan), decimationSpin_->value()!=0?fabs(decimationSpin_->value()):1), scan.localTransform(), Qt::yellow);
}
else if(data.laserScanRaw().hasRGB())
else if(scan.hasRGB())
{
cloudView_->addCloud("scan", util3d::downsample(util3d::laserScanToPointCloudRGBNormal(data.laserScanRaw()), decimationSpin_->value()!=0?fabs(decimationSpin_->value()):1), data.laserScanRaw().localTransform(), Qt::yellow);
cloudView_->addCloud("scan", util3d::downsample(util3d::laserScanToPointCloudRGBNormal(scan), decimationSpin_->value()!=0?fabs(decimationSpin_->value()):1), scan.localTransform(), Qt::yellow);
}
else
{
cloudView_->addCloud("scan", util3d::downsample(util3d::laserScanToPointCloudNormal(data.laserScanRaw()), decimationSpin_->value()!=0?fabs(decimationSpin_->value()):1), data.laserScanRaw().localTransform(), Qt::yellow);
cloudView_->addCloud("scan", util3d::downsample(util3d::laserScanToPointCloudNormal(scan), decimationSpin_->value()!=0?fabs(decimationSpin_->value()):1), scan.localTransform(), Qt::yellow);
}
}
else if(data.laserScanRaw().hasIntensity())
else if(scan.hasIntensity())
{
cloudView_->addCloud("scan", util3d::downsample(util3d::laserScanToPointCloudI(data.laserScanRaw()), decimationSpin_->value()!=0?fabs(decimationSpin_->value()):1), data.laserScanRaw().localTransform(), Qt::yellow);
cloudView_->addCloud("scan", util3d::downsample(util3d::laserScanToPointCloudI(scan), decimationSpin_->value()!=0?fabs(decimationSpin_->value()):1), scan.localTransform(), Qt::yellow);
}
else if(data.laserScanRaw().hasRGB())
else if(scan.hasRGB())
{
cloudView_->addCloud("scan", util3d::downsample(util3d::laserScanToPointCloudRGB(data.laserScanRaw()), decimationSpin_->value()!=0?fabs(decimationSpin_->value()):1), data.laserScanRaw().localTransform(), Qt::yellow);
cloudView_->addCloud("scan", util3d::downsample(util3d::laserScanToPointCloudRGB(scan), decimationSpin_->value()!=0?fabs(decimationSpin_->value()):1), scan.localTransform(), Qt::yellow);
}
else
{
cloudView_->addCloud("scan", util3d::downsample(util3d::laserScanToPointCloud(data.laserScanRaw()), decimationSpin_->value()!=0?fabs(decimationSpin_->value()):1), data.laserScanRaw().localTransform(), Qt::yellow);
cloudView_->addCloud("scan", util3d::downsample(util3d::laserScanToPointCloud(scan), decimationSpin_->value()!=0?fabs(decimationSpin_->value()):1), scan.localTransform(), Qt::yellow);
}
}
}

21
guilib/src/DatabaseViewer.cpp
View File

@@ -4850,7 +4850,7 @@ void DatabaseViewer::update(int value,
{
keypoints.insert(std::make_pair(iter->first, signatures.front()->getWordsKpts()[iter->second]));
}
view->setFeatures(keypoints, data.depthOrRightRaw().type() == CV_8UC1?cv::Mat():data.depthOrRightRaw(), Qt::yellow);
view->setFeatures(keypoints, data.depthOrRightRaw().type() == CV_8UC1||data.depthOrRightRaw().type() == CV_8UC3?cv::Mat():data.depthOrRightRaw(), Qt::yellow);
}
Transform odomPose, g;
@@ -5088,7 +5088,7 @@ void DatabaseViewer::update(int value,
}
//stereo
if(!data.depthOrRightRaw().empty() && data.depthOrRightRaw().type() == CV_8UC1)
if(!data.depthOrRightRaw().empty() && (data.depthOrRightRaw().type() == CV_8UC1 || data.depthOrRightRaw().type() == CV_8UC3))
{
this->updateStereo(&data);
}
@@ -5782,7 +5782,7 @@ void DatabaseViewer::updateStereo(const SensorData * data)
ui_->dockWidget_stereoView->isVisible() &&
!data->imageRaw().empty() &&
!data->depthOrRightRaw().empty() &&
data->depthOrRightRaw().type() == CV_8UC1 &&
(data->depthOrRightRaw().type() == CV_8UC1 || data->depthOrRightRaw().type() == CV_8UC3) &&
data->stereoCameraModels().size()==1 && // Not implemented for multiple stereo cameras
data->stereoCameraModels()[0].isValidForProjection())
{
@@ -5795,6 +5795,15 @@ void DatabaseViewer::updateStereo(const SensorData * data)
{
leftMono = data->imageRaw();
}
cv::Mat rightMono;
if(data->rightRaw().channels() == 3)
{
cv::cvtColor(data->rightRaw(), rightMono, CV_BGR2GRAY);
}
else
{
rightMono = data->rightRaw();
}
UTimer timer;
ParametersMap parameters = ui_->parameters_toolbox->getParameters();
@@ -5826,7 +5835,7 @@ void DatabaseViewer::updateStereo(const SensorData * data)
rightCorners = stereo->computeCorrespondences(
leftMono,
data->rightRaw(),
rightMono,
leftCorners,
status);
delete stereo;
@@ -6435,7 +6444,7 @@ void DatabaseViewer::updateConstraintView(
}
dataFrom.uncompressData();
UASSERT(dataFrom.imageRaw().empty() || dataFrom.imageRaw().type()==CV_8UC3 || dataFrom.imageRaw().type() == CV_8UC1);
UASSERT(dataFrom.depthOrRightRaw().empty() || dataFrom.depthOrRightRaw().type()==CV_8UC1 || dataFrom.depthOrRightRaw().type() == CV_16UC1 || dataFrom.depthOrRightRaw().type() == CV_32FC1);
UASSERT(dataFrom.depthOrRightRaw().empty() || dataFrom.depthOrRightRaw().type()==CV_8UC1 || dataFrom.depthOrRightRaw().type()==CV_8UC3 || dataFrom.depthOrRightRaw().type() == CV_16UC1 || dataFrom.depthOrRightRaw().type() == CV_32FC1);
if(signatureTo.id()>0)
{
@@ -6447,7 +6456,7 @@ void DatabaseViewer::updateConstraintView(
}
dataTo.uncompressData();
UASSERT(dataTo.imageRaw().empty() || dataTo.imageRaw().type()==CV_8UC3 || dataTo.imageRaw().type() == CV_8UC1);
UASSERT(dataTo.depthOrRightRaw().empty() || dataTo.depthOrRightRaw().type()==CV_8UC1 || dataTo.depthOrRightRaw().type() == CV_16UC1 || dataTo.depthOrRightRaw().type() == CV_32FC1);
UASSERT(dataTo.depthOrRightRaw().empty() || dataTo.depthOrRightRaw().type()==CV_8UC1 || dataTo.depthOrRightRaw().type()==CV_8UC3 || dataTo.depthOrRightRaw().type() == CV_16UC1 || dataTo.depthOrRightRaw().type() == CV_32FC1);
// get odom pose
Transform pose = Transform::getIdentity();

20
guilib/src/PreferencesDialog.cpp
View File

@@ -882,6 +882,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
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->checkBox_stereo_rightGrayScale, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->pushButton_calibrate, SIGNAL(clicked()), this, SLOT(calibrate()));
connect(_ui->pushButton_calibrate_simple, SIGNAL(clicked()), this, SLOT(calibrateSimple()));
connect(_ui->toolButton_openniOniPath, SIGNAL(clicked()), this, SLOT(selectSourceOniPath()));
@@ -2176,6 +2177,7 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
_ui->checkbox_source_feature_detection->setChecked(false);
_ui->checkbox_stereo_depthGenerated->setChecked(false);
_ui->checkBox_stereo_exposureCompensation->setChecked(false);
_ui->checkBox_stereo_rightGrayScale->setChecked(true);
_ui->openni2_autoWhiteBalance->setChecked(true);
_ui->openni2_autoExposure->setChecked(true);
_ui->openni2_exposure->setValue(0);
@@ -2254,7 +2256,7 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
_ui->spinBox_stereoMyntEye_contrast->setValue(116);
_ui->spinBox_stereoMyntEye_irControl->setValue(0);
_ui->comboBox_depthai_image_width->setCurrentIndex(1);
_ui->comboBox_depthai_output_mode->setCurrentIndex(0);
_ui->comboBox_depthai_output_mode->setCurrentIndex(1);
_ui->spinBox_depthai_conf_threshold->setValue(200);
_ui->checkBox_depthai_extended_disparity->setChecked(false);
_ui->checkBox_depthai_disparity_companding->setChecked(false);
@@ -2652,6 +2654,7 @@ void PreferencesDialog::readCameraSettings(const QString & filePath)
_ui->comboBox_cameraStereo->setCurrentIndex(settings.value("driver", _ui->comboBox_cameraStereo->currentIndex()).toInt());
_ui->checkbox_stereo_depthGenerated->setChecked(settings.value("depthGenerated", _ui->checkbox_stereo_depthGenerated->isChecked()).toBool());
_ui->checkBox_stereo_exposureCompensation->setChecked(settings.value("exposureCompensation", _ui->checkBox_stereo_exposureCompensation->isChecked()).toBool());
_ui->checkBox_stereo_rightGrayScale->setChecked(settings.value("rightGrayScale", _ui->checkBox_stereo_rightGrayScale->isChecked()).toBool());
settings.endGroup(); // stereo
settings.beginGroup("rgb");
@@ -3256,6 +3259,7 @@ void PreferencesDialog::writeCameraSettings(const QString & filePath) const
settings.setValue("driver", _ui->comboBox_cameraStereo->currentIndex());
settings.setValue("depthGenerated", _ui->checkbox_stereo_depthGenerated->isChecked());
settings.setValue("exposureCompensation", _ui->checkBox_stereo_exposureCompensation->isChecked());
settings.setValue("rightGrayScale", _ui->checkBox_stereo_rightGrayScale->isChecked());
settings.endGroup(); // stereo
settings.beginGroup("rgb");
@@ -4296,7 +4300,7 @@ void PreferencesDialog::selectSourceDriver(Src src, int variant)
{
_ui->checkBox_depthai_imu_published->setChecked(variant >= 1);
_ui->comboBox_depthai_image_width->setCurrentIndex(1);
_ui->comboBox_depthai_output_mode->setCurrentIndex(variant==2?2:0);
_ui->comboBox_depthai_output_mode->setCurrentIndex(variant==2?2:1);
_ui->doubleSpinBox_depthai_dot_intensity->setValue(variant==2?1:0);
}
}
@@ -6436,6 +6440,10 @@ bool PreferencesDialog::isSourceStereoExposureCompensation() const
{
return _ui->checkBox_stereo_exposureCompensation->isChecked();
}
bool PreferencesDialog::isRightGrayScale() const
{
return _ui->checkBox_stereo_rightGrayScale->isChecked();
}
bool PreferencesDialog::isSourceScanFromDepth() const
{
return _ui->checkBox_source_scanFromDepth->isChecked();
@@ -6758,8 +6766,8 @@ Camera * PreferencesDialog::createCamera(
_ui->checkBox_cameraImages_timestamps->isChecked(),
_ui->lineEdit_cameraImages_timestamps->text().toStdString(),
_ui->checkBox_cameraImages_syncTimeStamps->isChecked());
((CameraRGBDImages*)camera)->setConfigForEachFrame(_ui->checkBox_cameraImages_configForEachFrame->isChecked());
((CameraStereoImages*)camera)->setConfigForEachFrame(_ui->checkBox_cameraImages_configForEachFrame->isChecked());
((CameraStereoImages*)camera)->setRightGrayScale(_ui->checkBox_stereo_rightGrayScale->isChecked());
}
else if (driver == kSrcStereoUsb)
{
@@ -6785,6 +6793,7 @@ Camera * PreferencesDialog::createCamera(
{
((CameraStereoVideo*)camera)->setFOURCC(_ui->lineEdit_stereousbcam_fourcc->text().toStdString());
}
((CameraStereoVideo*)camera)->setRightGrayScale(_ui->checkBox_stereo_rightGrayScale->isChecked());
}
else if(driver == kSrcStereoVideo)
{
@@ -6807,6 +6816,7 @@ Camera * PreferencesDialog::createCamera(
this->getGeneralInputRate(),
this->getSourceLocalTransform());
}
((CameraStereoVideo*)camera)->setRightGrayScale(_ui->checkBox_stereo_rightGrayScale->isChecked());
}
else if (driver == kSrcStereoTara)
@@ -6856,6 +6866,7 @@ Camera * PreferencesDialog::createCamera(
_ui->checkbox_publishInterIMU->isChecked(),
_ui->checkbox_publishInterIMU->isChecked() && getIMUFilteringStrategy()>0?
IMUFilter::create((IMUFilter::Type)(getIMUFilteringStrategy()-1), this->getAllParameters()):0);
((CameraStereoZed*)camera)->setRightGrayScale(_ui->checkBox_stereo_rightGrayScale->isChecked());
}
else if (driver == kSrcStereoZedOC)
{
@@ -6865,6 +6876,7 @@ Camera * PreferencesDialog::createCamera(
_ui->comboBox_stereoZedOC_resolution->currentIndex(),
this->getGeneralInputRate(),
this->getSourceLocalTransform());
((CameraStereoZedOC*)camera)->setRightGrayScale(_ui->checkBox_stereo_rightGrayScale->isChecked());
}
else if (driver == kSrcStereoDepthAI)
{

90
guilib/src/ui/preferencesDialog.ui
View File

@@ -63,9 +63,9 @@
<property name="geometry">
<rect>
<x>0</x>
<y>-594</y>
<y>-639</y>
<width>713</width>
<height>4653</height>
<height>4705</height>
</rect>
</property>
<layout class="QVBoxLayout" name="verticalLayout_16">
@@ -3424,7 +3424,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
<item>
<widget class="QStackedWidget" name="stackedWidget_src">
<property name="currentIndex">
<number>3</number>
<number>1</number>
</property>
<widget class="QWidget" name="page_41">
<layout class="QVBoxLayout" name="verticalLayout_64">
@@ -4972,6 +4972,32 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
</item>
<item>
<layout class="QGridLayout" name="gridLayout_58" columnstretch="0,0,1">
<item row="3" column="2">
<widget class="QLabel" name="label_stereo_rectify_2">
<property name="text">
<string>Auto exposure compensation between left and right images.</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="2">
<widget class="QLabel" name="label_240">
<property name="text">
<string>Driver.</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="1">
<widget class="QComboBox" name="comboBox_cameraStereo">
<property name="sizeAdjustPolicy">
@@ -5034,21 +5060,8 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
</item>
</widget>
</item>
<item row="0" column="2">
<widget class="QLabel" name="label_240">
<property name="text">
<string>Driver.</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="1">
<widget class="QCheckBox" name="checkbox_stereo_depthGenerated">
<item row="2" column="1">
<widget class="QCheckBox" name="checkBox_stereo_rectify">
<property name="text">
<string/>
</property>
@@ -5067,6 +5080,26 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
</property>
</widget>
</item>
<item row="3" column="1">
<widget class="QCheckBox" name="checkBox_stereo_exposureCompensation">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="4" column="2">
<widget class="QLabel" name="label_stereo_rectify_3">
<property name="text">
<string>Convert right image to gray scale if not already.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="2" column="2">
<widget class="QLabel" name="label_stereo_rectify">
<property name="text">
@@ -5080,28 +5113,15 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
</property>
</widget>
</item>
<item row="2" column="1">
<widget class="QCheckBox" name="checkBox_stereo_rectify">
<item row="1" column="1">
<widget class="QCheckBox" name="checkbox_stereo_depthGenerated">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="3" column="2">
<widget class="QLabel" name="label_stereo_rectify_2">
<property name="text">
<string>Auto exposure compensation between left and right images.</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="QCheckBox" name="checkBox_stereo_exposureCompensation">
<item row="4" column="1">
<widget class="QCheckBox" name="checkBox_stereo_rightGrayScale">
<property name="text">
<string/>
</property>