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

Added CameraStereoImages class to read stereo images from a directory. Added a particle filter to smooth odometry trajectory. Added parameter RGBD/OptimizeEpsilon to limit TORO iterations when error improvement is small. Added Rtabmap/CreateIntermediateNodes parameter: this can be used to keep all odometry poses 'between' nodes used for loop closure detection. Added PnP approach to loop closure constraint estimation. Fixed decimation of stereo images when image size is odd.

Browse Source
This commit is contained in:
matlabbe
2015-06-11 16:57:16 -04:00
parent feba562c1b
commit b8dccc2228
49 changed files with 2651 additions and 729 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
guilib/include/rtabmap/gui/MainWindow.h
View File

@@ -131,10 +131,12 @@ private slots:
void startDetection();
void pauseDetection();
void stopDetection();
void notifyNoMoreImages();
void printLoopClosureIds();
void generateMap();
void generateLocalMap();
void generateTOROMap();
void exportPoses();
void postProcessing();
void deleteMemory();
void openWorkingDirectory();
@@ -308,6 +310,7 @@ private:
QString _graphSavingFileName;
QString _toroSavingFileName;
QString _posesSavingFileName;
bool _autoScreenCaptureOdomSync;
QVector<int> _refIds;

3
guilib/include/rtabmap/gui/OdometryViewer.h
View File

@@ -49,7 +49,7 @@ class RTABMAPGUI_EXP OdometryViewer : public QDialog, public UEventsHandler
Q_OBJECT
public:
OdometryViewer(int maxClouds = 10, int decimation = 2, float voxelSize = 0.0f, int qualityWarningThr=0, QWidget * parent = 0);
OdometryViewer(int maxClouds = 10, int decimation = 2, float voxelSize = 0.0f, float maxDepth = 0, int qualityWarningThr=0, QWidget * parent = 0);
virtual ~OdometryViewer();
public slots:
@@ -76,6 +76,7 @@ private:
QSpinBox * maxCloudsSpin_;
QDoubleSpinBox * voxelSpin_;
QSpinBox * decimationSpin_;
QDoubleSpinBox * maxDepthSpin_;
QLabel * timeLabel_;
int validDecimationValue_;
};

6
guilib/include/rtabmap/gui/PreferencesDialog.h
View File

@@ -90,7 +90,8 @@ public:
kSrcOpenNI2,
kSrcFreenect2,
kSrcStereoDC1394,
kSrcStereoFlyCapture2
kSrcStereoFlyCapture2,
kSrcStereoImages
};
public:
@@ -205,6 +206,7 @@ public:
double getLoopThr() const;
double getVpThr() const;
int getOdomStrategy() const;
int getOdomBufferSize() const;
QString getCameraInfoDir() const; // "workinfDir/camera_info"
//
@@ -248,6 +250,8 @@ private slots:
void setupTreeView();
void updateBasicParameter();
void openDatabaseViewer();
void selectSourceStereoImagesStamps();
void selectSourceStereoImagesPath();
void updateRGBDCameraGroupBoxVisibility();
void testOdometry();
void testRGBDCamera();

8
guilib/src/DatabaseViewer.cpp
View File

@@ -1064,7 +1064,7 @@ void DatabaseViewer::view3DMap()
if(ok)
{
int decimation = item.toInt();
double maxDepth = QInputDialog::getDouble(this, tr("Camera depth?"), tr("Maximum depth (m, 0=no max):"), 4.0, 0, 10, 2, &ok);
double maxDepth = QInputDialog::getDouble(this, tr("Camera depth?"), tr("Maximum depth (m, 0=no max):"), 4.0, 0, 100, 2, &ok);
if(ok)
{
std::map<int, Transform> optimizedPoses = uValueAt(graphes_, ui_->horizontalSlider_iterations->value());
@@ -1188,7 +1188,7 @@ void DatabaseViewer::generate3DMap()
if(ok)
{
int decimation = item.toInt();
double maxDepth = QInputDialog::getDouble(this, tr("Camera depth?"), tr("Maximum depth (m, 0=no max):"), 4.0, 0, 10, 2, &ok);
double maxDepth = QInputDialog::getDouble(this, tr("Camera depth?"), tr("Maximum depth (m, 0=no max):"), 4.0, 0, 100, 2, &ok);
if(ok)
{
QString path = QFileDialog::getExistingDirectory(this, tr("Save directory"), pathDatabase_);
@@ -2957,6 +2957,7 @@ void DatabaseViewer::refineConstraintVisually(int from, int to, bool silent, boo
parameters.insert(ParametersPair(Parameters::kKpNndrRatio(), uNumber2Str(ui_->doubleSpinBox_visual_nndr->value())));
parameters.insert(ParametersPair(Parameters::kLccBowIterations(), uNumber2Str(ui_->spinBox_visual_iteration->value())));
parameters.insert(ParametersPair(Parameters::kLccBowMinInliers(), uNumber2Str(ui_->spinBox_visual_minCorrespondences->value())));
parameters.insert(ParametersPair(Parameters::kLccBowMaxDepth(), uNumber2Str(ui_->doubleSpinBox_visual_maxDepth->value())));
parameters.insert(ParametersPair(Parameters::kMemGenerateIds(), "false"));
parameters.insert(ParametersPair(Parameters::kMemRehearsalSimilarity(), "1.0"));
parameters.insert(ParametersPair(Parameters::kKpWordsPerImage(), "0"));
@@ -3075,6 +3076,8 @@ bool DatabaseViewer::addConstraint(int from, int to, bool silent, bool updateGra
parameters.insert(ParametersPair(Parameters::kKpNndrRatio(), uNumber2Str(ui_->doubleSpinBox_visual_nndr->value())));
parameters.insert(ParametersPair(Parameters::kLccBowIterations(), uNumber2Str(ui_->spinBox_visual_iteration->value())));
parameters.insert(ParametersPair(Parameters::kLccBowMinInliers(), uNumber2Str(ui_->spinBox_visual_minCorrespondences->value())));
parameters.insert(ParametersPair(Parameters::kLccBowMaxDepth(), uNumber2Str(ui_->doubleSpinBox_visual_maxDepth->value())));
parameters.insert(ParametersPair(Parameters::kLccBowPnPEstimation(), uBool2Str(ui_->checkBox_pnp->isChecked())));
parameters.insert(ParametersPair(Parameters::kMemGenerateIds(), "false"));
parameters.insert(ParametersPair(Parameters::kMemRehearsalSimilarity(), "1.0"));
parameters.insert(ParametersPair(Parameters::kKpWordsPerImage(), "0"));
@@ -3112,6 +3115,7 @@ bool DatabaseViewer::addConstraint(int from, int to, bool silent, bool updateGra
parameters.insert(ParametersPair(Parameters::kLccBowMaxDepth(), uNumber2Str(ui_->doubleSpinBox_visual_maxDepth->value())));
parameters.insert(ParametersPair(Parameters::kLccBowIterations(), uNumber2Str(ui_->spinBox_visual_iteration->value())));
parameters.insert(ParametersPair(Parameters::kLccBowMinInliers(), uNumber2Str(ui_->spinBox_visual_minCorrespondences->value())));
parameters.insert(ParametersPair(Parameters::kLccBowPnPEstimation(), uBool2Str(ui_->checkBox_pnp->isChecked())));
memory_->parseParameters(parameters);
t = memory_->computeVisualTransform(to, from, &rejectedMsg, &inliers, &variance);
}

536
guilib/src/MainWindow.cpp
View File

@@ -301,6 +301,7 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) :
connect(_ui->actionGenerate_map, SIGNAL(triggered()), this , SLOT(generateMap()));
connect(_ui->actionGenerate_local_map, SIGNAL(triggered()), this, SLOT(generateLocalMap()));
connect(_ui->actionGenerate_TORO_graph_graph, SIGNAL(triggered()), this , SLOT(generateTOROMap()));
connect(_ui->actionExport_poses_txt, SIGNAL(triggered()), this , SLOT(exportPoses()));
connect(_ui->actionDelete_memory, SIGNAL(triggered()), this , SLOT(deleteMemory()));
connect(_ui->actionDownload_all_clouds, SIGNAL(triggered()), this , SLOT(downloadAllClouds()));
connect(_ui->actionDownload_graph, SIGNAL(triggered()), this , SLOT(downloadPoseGraph()));
@@ -441,7 +442,7 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) :
qRegisterMetaType<rtabmap::OdometryInfo>("rtabmap::OdometryInfo");
connect(this, SIGNAL(odometryReceived(rtabmap::SensorData, rtabmap::OdometryInfo)), this, SLOT(processOdometry(rtabmap::SensorData, rtabmap::OdometryInfo)));
connect(this, SIGNAL(noMoreImagesReceived()), this, SLOT(stopDetection()));
connect(this, SIGNAL(noMoreImagesReceived()), this, SLOT(notifyNoMoreImages()));
// Apply state
this->changeState(kIdle);
@@ -673,6 +674,11 @@ void MainWindow::handleEvent(UEvent* anEvent)
_processingOdometry = true; // if we receive too many odometry events!
emit odometryReceived(odomEvent->data(), odomEvent->info());
}
else
{
// we receive too many odometry events! just send without data
emit odometryReceived(SensorData(cv::Mat(), odomEvent->data().id()), odomEvent->info());
}
}
}
else if(anEvent->getClassName().compare("ULogEvent") == 0)
@@ -699,37 +705,214 @@ void MainWindow::processOdometry(const rtabmap::SensorData & data, const rtabmap
{
_processingOdometry = true;
UTimer time;
Transform pose = data.pose();
bool lost = false;
bool lostStateChanged = false;
// Process Data
if(data.isValid())
{
Transform pose = data.pose();
bool lost = false;
bool lostStateChanged = false;
if(pose.isNull())
{
UDEBUG("odom lost"); // use last pose
lostStateChanged = _ui->widget_cloudViewer->getBackgroundColor() != Qt::darkRed;
_ui->widget_cloudViewer->setBackgroundColor(Qt::darkRed);
_ui->imageView_odometry->setBackgroundColor(Qt::darkRed);
if(pose.isNull())
{
UDEBUG("odom lost"); // use last pose
lostStateChanged = _ui->widget_cloudViewer->getBackgroundColor() != Qt::darkRed;
_ui->widget_cloudViewer->setBackgroundColor(Qt::darkRed);
_ui->imageView_odometry->setBackgroundColor(Qt::darkRed);
pose = _lastOdomPose;
lost = true;
}
else if(info.inliers>0 &&
_preferencesDialog->getOdomQualityWarnThr() &&
info.inliers < _preferencesDialog->getOdomQualityWarnThr())
{
UDEBUG("odom warn, quality(inliers)=%d thr=%d", info.inliers, _preferencesDialog->getOdomQualityWarnThr());
lostStateChanged = _ui->widget_cloudViewer->getBackgroundColor() == Qt::darkRed;
_ui->widget_cloudViewer->setBackgroundColor(Qt::darkYellow);
_ui->imageView_odometry->setBackgroundColor(Qt::darkYellow);
}
else
{
UDEBUG("odom ok");
lostStateChanged = _ui->widget_cloudViewer->getBackgroundColor() == Qt::darkRed;
_ui->widget_cloudViewer->setBackgroundColor(_ui->widget_cloudViewer->getDefaultBackgroundColor());
_ui->imageView_odometry->setBackgroundColor(Qt::black);
pose = _lastOdomPose;
lost = true;
}
else if(info.inliers>0 &&
_preferencesDialog->getOdomQualityWarnThr() &&
info.inliers < _preferencesDialog->getOdomQualityWarnThr())
{
UDEBUG("odom warn, quality(inliers)=%d thr=%d", info.inliers, _preferencesDialog->getOdomQualityWarnThr());
lostStateChanged = _ui->widget_cloudViewer->getBackgroundColor() == Qt::darkRed;
_ui->widget_cloudViewer->setBackgroundColor(Qt::darkYellow);
_ui->imageView_odometry->setBackgroundColor(Qt::darkYellow);
}
else
{
UDEBUG("odom ok");
lostStateChanged = _ui->widget_cloudViewer->getBackgroundColor() == Qt::darkRed;
_ui->widget_cloudViewer->setBackgroundColor(_ui->widget_cloudViewer->getDefaultBackgroundColor());
_ui->imageView_odometry->setBackgroundColor(Qt::black);
}
if(!pose.isNull() && (_ui->dockWidget_cloudViewer->isVisible() || _ui->graphicsView_graphView->isVisible()))
{
_lastOdomPose = pose;
_odometryReceived = true;
}
if(_ui->dockWidget_cloudViewer->isVisible())
{
if(!pose.isNull())
{
// 3d cloud
if(data.depthOrRightImage().cols == data.image().cols &&
data.depthOrRightImage().rows == data.image().rows &&
!data.depthOrRightImage().empty() &&
data.fx() > 0.0f &&
data.fyOrBaseline() > 0.0f &&
_preferencesDialog->isCloudsShown(1))
{
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
cloud = createCloud(0,
data.image(),
data.depthOrRightImage(),
data.fx(),
data.fyOrBaseline(),
data.cx(),
data.cy(),
data.localTransform(),
pose,
_preferencesDialog->getCloudVoxelSize(1),
_preferencesDialog->getCloudDecimation(1),
_preferencesDialog->getCloudMaxDepth(1));
if(!_ui->widget_cloudViewer->addOrUpdateCloud("cloudOdom", cloud, _odometryCorrection))
{
UERROR("Adding cloudOdom to viewer failed!");
}
_ui->widget_cloudViewer->setCloudVisibility("cloudOdom", true);
_ui->widget_cloudViewer->setCloudOpacity("cloudOdom", _preferencesDialog->getCloudOpacity(1));
_ui->widget_cloudViewer->setCloudPointSize("cloudOdom", _preferencesDialog->getCloudPointSize(1));
}
// 2d cloud
if(!data.laserScan().empty() &&
_preferencesDialog->isScansShown(1))
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;
cloud = util3d::laserScanToPointCloud(data.laserScan());
cloud = util3d::transformPointCloud(cloud, pose);
if(!_ui->widget_cloudViewer->addOrUpdateCloud("scanOdom", cloud, _odometryCorrection))
{
UERROR("Adding scanOdom to viewer failed!");
}
_ui->widget_cloudViewer->setCloudVisibility("scanOdom", true);
_ui->widget_cloudViewer->setCloudOpacity("scanOdom", _preferencesDialog->getScanOpacity(1));
_ui->widget_cloudViewer->setCloudPointSize("scanOdom", _preferencesDialog->getScanPointSize(1));
}
if(!data.pose().isNull())
{
// update camera position
_ui->widget_cloudViewer->updateCameraTargetPosition(_odometryCorrection*data.pose());
}
}
_ui->widget_cloudViewer->update();
}
if(_ui->graphicsView_graphView->isVisible())
{
if(!pose.isNull() && !data.pose().isNull())
{
_ui->graphicsView_graphView->updateReferentialPosition(_odometryCorrection*data.pose());
_ui->graphicsView_graphView->update();
}
}
if(_ui->dockWidget_odometry->isVisible() &&
!data.image().empty())
{
if(_ui->imageView_odometry->isFeaturesShown())
{
if(info.type == 0)
{
_ui->imageView_odometry->setFeatures(info.words, data.depth(), Qt::yellow);
}
else if(info.type == 1)
{
std::vector<cv::KeyPoint> kpts;
cv::KeyPoint::convert(info.refCorners, kpts);
_ui->imageView_odometry->setFeatures(kpts, data.depth(), Qt::red);
}
}
_ui->imageView_odometry->clearLines();
if(lost)
{
if(lostStateChanged)
{
// save state
_odomImageShow = _ui->imageView_odometry->isImageShown();
_odomImageDepthShow = _ui->imageView_odometry->isImageDepthShown();
}
_ui->imageView_odometry->setImageDepth(uCvMat2QImage(data.image()));
_ui->imageView_odometry->setImageShown(true);
_ui->imageView_odometry->setImageDepthShown(true);
}
else
{
if(lostStateChanged)
{
// restore state
_ui->imageView_odometry->setImageShown(_odomImageShow);
_ui->imageView_odometry->setImageDepthShown(_odomImageDepthShow);
}
_ui->imageView_odometry->setImage(uCvMat2QImage(data.image()));
if(_ui->imageView_odometry->isImageDepthShown())
{
_ui->imageView_odometry->setImageDepth(uCvMat2QImage(data.depthOrRightImage()));
}
if(info.type == 0)
{
if(_ui->imageView_odometry->isFeaturesShown())
{
for(unsigned int i=0; i<info.wordMatches.size(); ++i)
{
_ui->imageView_odometry->setFeatureColor(info.wordMatches[i], Qt::red); // outliers
}
for(unsigned int i=0; i<info.wordInliers.size(); ++i)
{
_ui->imageView_odometry->setFeatureColor(info.wordInliers[i], Qt::green); // inliers
}
}
}
}
if(info.type == 1 && info.cornerInliers.size())
{
if(_ui->imageView_odometry->isFeaturesShown() || _ui->imageView_odometry->isLinesShown())
{
//draw lines
UASSERT(info.refCorners.size() == info.newCorners.size());
for(unsigned int i=0; i<info.cornerInliers.size(); ++i)
{
if(_ui->imageView_odometry->isFeaturesShown())
{
_ui->imageView_odometry->setFeatureColor(info.cornerInliers[i], Qt::green); // inliers
}
if(_ui->imageView_odometry->isLinesShown())
{
_ui->imageView_odometry->addLine(
info.refCorners[info.cornerInliers[i]].x,
info.refCorners[info.cornerInliers[i]].y,
info.newCorners[info.cornerInliers[i]].x,
info.newCorners[info.cornerInliers[i]].y,
Qt::blue);
}
}
}
}
if(!data.image().empty())
{
_ui->imageView_odometry->setSceneRect(QRectF(0,0,(float)data.image().cols, (float)data.image().rows));
}
_ui->imageView_odometry->update();
}
if(_ui->actionAuto_screen_capture->isChecked() && _autoScreenCaptureOdomSync)
{
this->captureScreen();
}
}
//Process info
if(info.inliers >= 0)
{
_ui->statsToolBox->updateStat("Odometry/Inliers/", (float)data.id(), (float)info.inliers);
@@ -746,9 +929,13 @@ void MainWindow::processOdometry(const rtabmap::SensorData & data, const rtabmap
{
_ui->statsToolBox->updateStat("Odometry/Variance/", (float)data.id(), (float)info.variance);
}
if(info.time > 0)
if(info.timeEstimation > 0)
{
_ui->statsToolBox->updateStat("Odometry/Time/ms", (float)data.id(), (float)info.time*1000.0f);
_ui->statsToolBox->updateStat("Odometry/TimeEstimation/ms", (float)data.id(), (float)info.timeEstimation*1000.0f);
}
if(info.timeParticleFiltering > 0)
{
_ui->statsToolBox->updateStat("Odometry/TimeFiltering/ms", (float)data.id(), (float)info.timeParticleFiltering*1000.0f);
}
if(info.features >=0)
{
@@ -761,188 +948,35 @@ void MainWindow::processOdometry(const rtabmap::SensorData & data, const rtabmap
_ui->statsToolBox->updateStat("Odometry/ID/", (float)data.id(), (float)data.id());
float x,y,z, roll,pitch,yaw;
pose.getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
_ui->statsToolBox->updateStat("Odometry/T_x/m", (float)data.id(), x);
_ui->statsToolBox->updateStat("Odometry/T_y/m", (float)data.id(), y);
_ui->statsToolBox->updateStat("Odometry/T_z/m", (float)data.id(), z);
_ui->statsToolBox->updateStat("Odometry/T_roll/deg", (float)data.id(), roll*180.0/CV_PI);
_ui->statsToolBox->updateStat("Odometry/T_pitch/deg", (float)data.id(), pitch*180.0/CV_PI);
_ui->statsToolBox->updateStat("Odometry/T_yaw/deg", (float)data.id(), yaw*180.0/CV_PI);
if(!pose.isNull() && (_ui->dockWidget_cloudViewer->isVisible() || _ui->graphicsView_graphView->isVisible()))
if(!info.transform.isNull())
{
_lastOdomPose = pose;
_odometryReceived = true;
info.transform.getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
_ui->statsToolBox->updateStat("Odometry/Tx/m", (float)data.id(), x);
_ui->statsToolBox->updateStat("Odometry/Ty/m", (float)data.id(), y);
_ui->statsToolBox->updateStat("Odometry/Tz/m", (float)data.id(), z);
_ui->statsToolBox->updateStat("Odometry/Troll/deg", (float)data.id(), roll*180.0/CV_PI);
_ui->statsToolBox->updateStat("Odometry/Tpitch/deg", (float)data.id(), pitch*180.0/CV_PI);
_ui->statsToolBox->updateStat("Odometry/Tyaw/deg", (float)data.id(), yaw*180.0/CV_PI);
}
if(_ui->dockWidget_cloudViewer->isVisible())
if(!info.transformFiltered.isNull())
{
if(!pose.isNull())
{
// 3d cloud
if(data.depthOrRightImage().cols == data.image().cols &&
data.depthOrRightImage().rows == data.image().rows &&
!data.depthOrRightImage().empty() &&
data.fx() > 0.0f &&
data.fyOrBaseline() > 0.0f &&
_preferencesDialog->isCloudsShown(1))
{
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
cloud = createCloud(0,
data.image(),
data.depthOrRightImage(),
data.fx(),
data.fyOrBaseline(),
data.cx(),
data.cy(),
data.localTransform(),
pose,
_preferencesDialog->getCloudVoxelSize(1),
_preferencesDialog->getCloudDecimation(1),
_preferencesDialog->getCloudMaxDepth(1));
if(!_ui->widget_cloudViewer->addOrUpdateCloud("cloudOdom", cloud, _odometryCorrection))
{
UERROR("Adding cloudOdom to viewer failed!");
}
_ui->widget_cloudViewer->setCloudVisibility("cloudOdom", true);
_ui->widget_cloudViewer->setCloudOpacity("cloudOdom", _preferencesDialog->getCloudOpacity(1));
_ui->widget_cloudViewer->setCloudPointSize("cloudOdom", _preferencesDialog->getCloudPointSize(1));
}
// 2d cloud
if(!data.laserScan().empty() &&
_preferencesDialog->isScansShown(1))
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;
cloud = util3d::laserScanToPointCloud(data.laserScan());
cloud = util3d::transformPointCloud(cloud, pose);
if(!_ui->widget_cloudViewer->addOrUpdateCloud("scanOdom", cloud, _odometryCorrection))
{
UERROR("Adding scanOdom to viewer failed!");
}
_ui->widget_cloudViewer->setCloudVisibility("scanOdom", true);
_ui->widget_cloudViewer->setCloudOpacity("scanOdom", _preferencesDialog->getScanOpacity(1));
_ui->widget_cloudViewer->setCloudPointSize("scanOdom", _preferencesDialog->getScanPointSize(1));
}
if(!data.pose().isNull())
{
// update camera position
_ui->widget_cloudViewer->updateCameraTargetPosition(_odometryCorrection*data.pose());
}
}
_ui->widget_cloudViewer->update();
info.transformFiltered.getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
_ui->statsToolBox->updateStat("Odometry/Fx/m", (float)data.id(), x);
_ui->statsToolBox->updateStat("Odometry/Fy/m", (float)data.id(), y);
_ui->statsToolBox->updateStat("Odometry/Fz/m", (float)data.id(), z);
_ui->statsToolBox->updateStat("Odometry/Froll/deg", (float)data.id(), roll*180.0/CV_PI);
_ui->statsToolBox->updateStat("Odometry/Fpitch/deg", (float)data.id(), pitch*180.0/CV_PI);
_ui->statsToolBox->updateStat("Odometry/Fyaw/deg", (float)data.id(), yaw*180.0/CV_PI);
}
if(_ui->graphicsView_graphView->isVisible())
if(info.interval > 0)
{
if(!pose.isNull() && !data.pose().isNull())
{
_ui->graphicsView_graphView->updateReferentialPosition(_odometryCorrection*data.pose());
_ui->graphicsView_graphView->update();
}
}
if(_ui->dockWidget_odometry->isVisible() &&
!data.image().empty())
{
if(_ui->imageView_odometry->isFeaturesShown())
{
if(info.type == 0)
{
_ui->imageView_odometry->setFeatures(info.words, data.depth(), Qt::yellow);
}
else if(info.type == 1)
{
std::vector<cv::KeyPoint> kpts;
cv::KeyPoint::convert(info.refCorners, kpts);
_ui->imageView_odometry->setFeatures(kpts, data.depth(), Qt::red);
}
}
_ui->imageView_odometry->clearLines();
if(lost)
{
if(lostStateChanged)
{
// save state
_odomImageShow = _ui->imageView_odometry->isImageShown();
_odomImageDepthShow = _ui->imageView_odometry->isImageDepthShown();
}
_ui->imageView_odometry->setImageDepth(uCvMat2QImage(data.image()));
_ui->imageView_odometry->setImageShown(true);
_ui->imageView_odometry->setImageDepthShown(true);
}
else
{
if(lostStateChanged)
{
// restore state
_ui->imageView_odometry->setImageShown(_odomImageShow);
_ui->imageView_odometry->setImageDepthShown(_odomImageDepthShow);
}
_ui->imageView_odometry->setImage(uCvMat2QImage(data.image()));
if(_ui->imageView_odometry->isImageDepthShown())
{
_ui->imageView_odometry->setImageDepth(uCvMat2QImage(data.depthOrRightImage()));
}
if(info.type == 0)
{
if(_ui->imageView_odometry->isFeaturesShown())
{
for(unsigned int i=0; i<info.wordMatches.size(); ++i)
{
_ui->imageView_odometry->setFeatureColor(info.wordMatches[i], Qt::red); // outliers
}
for(unsigned int i=0; i<info.wordInliers.size(); ++i)
{
_ui->imageView_odometry->setFeatureColor(info.wordInliers[i], Qt::green); // inliers
}
}
}
}
if(info.type == 1 && info.cornerInliers.size())
{
if(_ui->imageView_odometry->isFeaturesShown() || _ui->imageView_odometry->isLinesShown())
{
//draw lines
UASSERT(info.refCorners.size() == info.newCorners.size());
for(unsigned int i=0; i<info.cornerInliers.size(); ++i)
{
if(_ui->imageView_odometry->isFeaturesShown())
{
_ui->imageView_odometry->setFeatureColor(info.cornerInliers[i], Qt::green); // inliers
}
if(_ui->imageView_odometry->isLinesShown())
{
_ui->imageView_odometry->addLine(
info.refCorners[info.cornerInliers[i]].x,
info.refCorners[info.cornerInliers[i]].y,
info.newCorners[info.cornerInliers[i]].x,
info.newCorners[info.cornerInliers[i]].y,
Qt::blue);
}
}
}
}
if(!data.image().empty())
{
_ui->imageView_odometry->setSceneRect(QRectF(0,0,(float)data.image().cols, (float)data.image().rows));
}
_ui->imageView_odometry->update();
}
if(_ui->actionAuto_screen_capture->isChecked() && _autoScreenCaptureOdomSync)
{
this->captureScreen();
_ui->statsToolBox->updateStat("Odometry/Interval/ms", (float)data.id(), info.interval*1000.f);
_ui->statsToolBox->updateStat("Odometry/Speed/kph", (float)data.id(), x/info.interval*3.6f);
}
_ui->statsToolBox->updateStat("/Gui refresh odom/ms", (float)data.id(), time.elapsed()*1000.0);
_processingOdometry = false;
}
@@ -973,21 +1007,26 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
int highestHypothesisId = static_cast<float>(uValue(stat.data(), Statistics::kLoopHighest_hypothesis_id(), 0.0f));
bool highestHypothesisIsSaved = (bool)uValue(stat.data(), Statistics::kLoopHypothesis_reactivated(), 0.0f);
// Loop closure info
_ui->imageView_source->clear();
_ui->imageView_loopClosure->clear();
_ui->imageView_source->setBackgroundColor(Qt::black);
_ui->imageView_loopClosure->setBackgroundColor(Qt::black);
// update cache
Signature signature = stat.getSignature();
signature.uncompressData(); // make sure data are uncompressed
_cachedSignatures.insert(stat.getSignature().id(), signature);
// For intermediate empty nodes, keep latest image shown
if(!signature.getImageRaw().empty() || signature.getWords().size())
{
_ui->imageView_source->clear();
_ui->imageView_loopClosure->clear();
_ui->imageView_source->setBackgroundColor(Qt::black);
_ui->imageView_loopClosure->setBackgroundColor(Qt::black);
_ui->label_matchId->clear();
}
int rehearsed = (int)uValue(stat.data(), Statistics::kMemoryRehearsal_merged(), 0.0f);
int localTimeClosures = (int)uValue(stat.data(), Statistics::kLocalLoopTime_closures(), 0.0f);
bool scanMatchingSuccess = (bool)uValue(stat.data(), Statistics::kOdomCorrectionAccepted(), 0.0f);
_ui->label_matchId->clear();
_ui->label_stats_imageNumber->setText(QString("%1 [%2]").arg(stat.refImageId()).arg(refMapId));
if(rehearsed > 0)
@@ -1119,10 +1158,6 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
if(!stat.posterior().empty() && _ui->dockWidget_posterior->isVisible())
{
UDEBUG("");
if(stat.weights().size() != stat.posterior().size())
{
UWARN("%d %d", stat.weights().size(), stat.posterior().size());
}
_posteriorCurve->setData(QMap<int, float>(stat.posterior()), QMap<int, int>(stat.weights()));
ULOGGER_DEBUG("");
@@ -2747,7 +2782,7 @@ void MainWindow::startDetection()
{
odom = new OdometryBOW(parameters);
}
_odomThread = new OdometryThread(odom);
_odomThread = new OdometryThread(odom, _preferencesDialog->getOdomBufferSize());
UEventsManager::addHandler(_odomThread);
UEventsManager::createPipe(_camera, _odomThread, "CameraEvent");
@@ -2988,6 +3023,13 @@ void MainWindow::stopDetection()
emit stateChanged(kInitialized);
}
void MainWindow::notifyNoMoreImages()
{
QMessageBox::information(this,
tr("No more images..."),
tr("The camera has reached the end of the stream."));
}
void MainWindow::printLoopClosureIds()
{
_ui->dockWidget_console->show();
@@ -3126,6 +3168,66 @@ void MainWindow::generateTOROMap()
}
}
void MainWindow::exportPoses()
{
if(_posesSavingFileName.isEmpty())
{
_posesSavingFileName = _preferencesDialog->getWorkingDirectory() + QDir::separator() + "poses.txt";
}
QStringList items;
items.append("Local map optimized");
items.append("Local map not optimized");
items.append("Global map optimized");
items.append("Global map not optimized");
bool ok;
QString item = QInputDialog::getItem(this, tr("Parameters"), tr("Options:"), items, 2, false, &ok);
if(ok)
{
bool optimized=false, global=false;
if(item.compare("Local map optimized") == 0)
{
optimized = true;
}
else if(item.compare("Local map not optimized") == 0)
{
}
else if(item.compare("Global map optimized") == 0)
{
global=true;
optimized=true;
}
else if(item.compare("Global map not optimized") == 0)
{
global=true;
}
else
{
UFATAL("Item \"%s\" not found?!?", item.toStdString().c_str());
}
QString path = QFileDialog::getSaveFileName(this, tr("Save File"), _posesSavingFileName, tr("Text file (*.txt)"));
if(!path.isEmpty())
{
_posesSavingFileName = path;
if(global)
{
this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdExportPosesGlobal, path.toStdString(), optimized?1:0));
}
else
{
this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdExportPosesLocal, path.toStdString(), optimized?1:0));
}
_ui->dockWidget_console->show();
_ui->widget_console->appendMsg(QString("Poses saved (global=%1, optimized=%2)... %3")
.arg(global?"true":"false").arg(optimized?"true":"false").arg(_posesSavingFileName));
}
}
}
void MainWindow::postProcessing()
{
if(_cachedSignatures.size() == 0)
@@ -5109,6 +5211,7 @@ void MainWindow::changeState(MainWindow::State newState)
_ui->actionGenerate_map->setVisible(!monitoring);
_ui->actionGenerate_local_map->setVisible(!monitoring);
_ui->actionGenerate_TORO_graph_graph->setVisible(!monitoring);
_ui->actionExport_poses_txt->setVisible(!monitoring);
_ui->actionOpen_working_directory->setVisible(!monitoring);
_ui->actionData_recorder->setVisible(!monitoring);
_ui->menuSelect_source->menuAction()->setVisible(!monitoring);
@@ -5188,6 +5291,7 @@ void MainWindow::changeState(MainWindow::State newState)
_ui->actionGenerate_map->setEnabled(false);
_ui->actionGenerate_local_map->setEnabled(false);
_ui->actionGenerate_TORO_graph_graph->setEnabled(false);
_ui->actionExport_poses_txt->setEnabled(false);
_ui->actionDownload_all_clouds->setEnabled(false);
_ui->actionDownload_graph->setEnabled(false);
_ui->menuSelect_source->setEnabled(false);
@@ -5235,6 +5339,7 @@ void MainWindow::changeState(MainWindow::State newState)
_ui->actionGenerate_map->setEnabled(true);
_ui->actionGenerate_local_map->setEnabled(true);
_ui->actionGenerate_TORO_graph_graph->setEnabled(true);
_ui->actionExport_poses_txt->setEnabled(true);
_ui->actionDownload_all_clouds->setEnabled(true);
_ui->actionDownload_graph->setEnabled(true);
_ui->menuSelect_source->setEnabled(true);
@@ -5271,6 +5376,7 @@ void MainWindow::changeState(MainWindow::State newState)
_ui->actionGenerate_map->setEnabled(false);
_ui->actionGenerate_local_map->setEnabled(false);
_ui->actionGenerate_TORO_graph_graph->setEnabled(false);
_ui->actionExport_poses_txt->setEnabled(false);
_ui->actionDownload_all_clouds->setEnabled(false);
_ui->actionDownload_graph->setEnabled(false);
_ui->menuSelect_source->setEnabled(false);
@@ -5309,6 +5415,7 @@ void MainWindow::changeState(MainWindow::State newState)
_ui->actionGenerate_map->setEnabled(false);
_ui->actionGenerate_local_map->setEnabled(false);
_ui->actionGenerate_TORO_graph_graph->setEnabled(false);
_ui->actionExport_poses_txt->setEnabled(false);
_ui->actionDownload_all_clouds->setEnabled(false);
_ui->actionDownload_graph->setEnabled(false);
_state = kDetecting;
@@ -5337,6 +5444,7 @@ void MainWindow::changeState(MainWindow::State newState)
_ui->actionGenerate_map->setEnabled(true);
_ui->actionGenerate_local_map->setEnabled(true);
_ui->actionGenerate_TORO_graph_graph->setEnabled(true);
_ui->actionExport_poses_txt->setEnabled(true);
_ui->actionDownload_all_clouds->setEnabled(true);
_ui->actionDownload_graph->setEnabled(true);
_state = kPaused;

47
guilib/src/OdometryViewer.cpp
View File

@@ -48,7 +48,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
namespace rtabmap {
OdometryViewer::OdometryViewer(int maxClouds, int decimation, float voxelSize, int qualityWarningThr, QWidget * parent) :
OdometryViewer::OdometryViewer(int maxClouds, int decimation, float voxelSize, float maxDepth, int qualityWarningThr, QWidget * parent) :
QDialog(parent),
imageView_(new ImageView(this)),
cloudView_(new CloudViewer(this)),
@@ -66,12 +66,14 @@ OdometryViewer::OdometryViewer(int maxClouds, int decimation, float voxelSize, i
imageView_->setImageDepthShown(false);
imageView_->setMinimumSize(320, 240);
imageView_->setAlpha(255);
cloudView_->setCameraFree();
cloudView_->setCameraTargetLocked();
cloudView_->setGridShown(true);
QLabel * maxCloudsLabel = new QLabel("Max clouds", this);
QLabel * voxelLabel = new QLabel("Voxel", this);
QLabel * maxDepthLabel = new QLabel("Max depth", this);
QLabel * decimationLabel = new QLabel("Decimation", this);
maxCloudsSpin_ = new QSpinBox(this);
maxCloudsSpin_->setMinimum(0);
@@ -84,6 +86,13 @@ OdometryViewer::OdometryViewer(int maxClouds, int decimation, float voxelSize, i
voxelSpin_->setSingleStep(0.01);
voxelSpin_->setSuffix(" m");
voxelSpin_->setValue(voxelSize);
maxDepthSpin_ = new QDoubleSpinBox(this);
maxDepthSpin_->setMinimum(0);
maxDepthSpin_->setMaximum(100);
maxDepthSpin_->setDecimals(0);
maxDepthSpin_->setSingleStep(1);
maxDepthSpin_->setSuffix(" m");
maxDepthSpin_->setValue(maxDepth);
decimationSpin_ = new QSpinBox(this);
decimationSpin_->setMinimum(1);
decimationSpin_->setMaximum(16);
@@ -107,6 +116,8 @@ OdometryViewer::OdometryViewer(int maxClouds, int decimation, float voxelSize, i
hlayout2->addWidget(maxCloudsSpin_);
hlayout2->addWidget(voxelLabel);
hlayout2->addWidget(voxelSpin_);
hlayout2->addWidget(maxDepthLabel);
hlayout2->addWidget(maxDepthSpin_);
hlayout2->addWidget(decimationLabel);
hlayout2->addWidget(decimationSpin_);
hlayout2->addWidget(timeLabel_);
@@ -170,25 +181,32 @@ void OdometryViewer::processData(const rtabmap::SensorData & data, const rtabmap
cloudView_->setBackgroundColor(Qt::black);
}
timeLabel_->setText(QString("%1 s").arg(info.time));
timeLabel_->setText(QString("%1 s").arg(info.timeEstimation));
if(!data.image().empty() && !data.depthOrRightImage().empty() && data.fx()>0.0f && data.fyOrBaseline()>0.0f)
{
UDEBUG("New pose = %s, quality=%d", data.pose().prettyPrint().c_str(), quality);
if(data.image().cols % decimationSpin_->value() == 0 &&
data.image().rows % decimationSpin_->value() == 0)
if(!data.depth().empty())
{
validDecimationValue_ = decimationSpin_->value();
if(data.image().cols % decimationSpin_->value() == 0 &&
data.image().rows % decimationSpin_->value() == 0)
{
validDecimationValue_ = decimationSpin_->value();
}
else
{
UWARN("Decimation (%d) must be a denominator of the width and height of "
"the image (%d/%d). Using last valid decimation value (%d).",
decimationSpin_->value(),
data.image().cols,
data.image().rows,
validDecimationValue_);
}
}
else
{
UWARN("Decimation (%d) must be a denominator of the width and height of "
"the image (%d/%d). Using last valid decimation value (%d).",
decimationSpin_->value(),
data.image().cols,
data.image().rows,
validDecimationValue_);
validDecimationValue_ = decimationSpin_->value();
}
@@ -214,6 +232,11 @@ void OdometryViewer::processData(const rtabmap::SensorData & data, const rtabmap
validDecimationValue_);
}
if(maxDepthSpin_->value() > 0.0f && cloud->size())
{
cloud = util3d::passThrough(cloud, "z", 0, maxDepthSpin_->value());
}
if(voxelSpin_->value() > 0.0f && cloud->size())
{
cloud = util3d::voxelize(cloud, voxelSpin_->value());

5
guilib/src/PdfPlot.cpp
View File

@@ -159,10 +159,9 @@ void PdfPlotCurve::setData(const QMap<int, float> & dataMap, const QMap<int, int
// update values
QList<QGraphicsItem*>::iterator iter = _items.begin();
QMap<int, int>::const_iterator j=weightsMap.begin();
for(QMap<int, float>::const_iterator i=dataMap.begin(); i!=dataMap.end(); ++i, ++j)
for(QMap<int, float>::const_iterator i=dataMap.begin(); i!=dataMap.end(); ++i)
{
((PdfPlotItem*)*iter)->setLikelihood(i.key(), i.value(), j!=weightsMap.end()?j.value():-1);
((PdfPlotItem*)*iter)->setLikelihood(i.key(), i.value(), weightsMap.value(i.key(), -1));
//2 times...
++iter;
++iter;

77
guilib/src/PreferencesDialog.cpp
View File

@@ -311,6 +311,10 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
connect(_ui->openni2_gain, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->openni2_mirroring, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->comboBox_freenect2Format, SIGNAL(currentIndexChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->toolButton_cameraStereoImages_timestamps, SIGNAL(clicked()), this, SLOT(selectSourceStereoImagesStamps()));
connect(_ui->lineEdit_cameraStereoImages_timestamps, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->toolButton_cameraStereoImages_path, SIGNAL(clicked()), this, SLOT(selectSourceStereoImagesPath()));
connect(_ui->lineEdit_cameraStereoImages_path, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->checkbox_rgbd_colorOnly, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->lineEdit_openniDevice, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->lineEdit_openniLocalTransform, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel()));
@@ -351,6 +355,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
_ui->general_spinBox_memoryThr->setObjectName(Parameters::kRtabmapMemoryThr().c_str());
_ui->general_doubleSpinBox_detectionRate->setObjectName(Parameters::kRtabmapDetectionRate().c_str());
_ui->general_spinBox_imagesBufferSize->setObjectName(Parameters::kRtabmapImageBufferSize().c_str());
_ui->general_checkBox_createIntermediateNodes->setObjectName(Parameters::kRtabmapCreateIntermediateNodes().c_str());
_ui->general_spinBox_maxRetrieved->setObjectName(Parameters::kRtabmapMaxRetrieved().c_str());
_ui->general_checkBox_startNewMapOnLoopClosure->setObjectName(Parameters::kRtabmapStartNewMapOnLoopClosure().c_str());
_ui->lineEdit_workingDirectory->setObjectName(Parameters::kRtabmapWorkingDirectory().c_str());
@@ -483,6 +488,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
_ui->graphOptimization_iterations->setObjectName(Parameters::kRGBDOptimizeIterations().c_str());
_ui->graphOptimization_covarianceIgnored->setObjectName(Parameters::kRGBDOptimizeVarianceIgnored().c_str());
_ui->graphOptimization_fromGraphEnd->setObjectName(Parameters::kRGBDOptimizeFromGraphEnd().c_str());
_ui->graphOptimization_stopEpsilon->setObjectName(Parameters::kRGBDOptimizeEpsilon().c_str());
_ui->graphPlan_goalReachedRadius->setObjectName(Parameters::kRGBDGoalReachedRadius().c_str());
_ui->graphPlan_planWithNearNodesLinked->setObjectName(Parameters::kRGBDPlanVirtualLinks().c_str());
@@ -503,6 +509,9 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
_ui->loopClosure_bowForce2D->setObjectName(Parameters::kLccBowForce2D().c_str());
_ui->loopClosure_bowEpipolarGeometry->setObjectName(Parameters::kLccBowEpipolarGeometry().c_str());
_ui->loopClosure_bowEpipolarGeometryVar->setObjectName(Parameters::kLccBowEpipolarGeometryVar().c_str());
_ui->loopClosure_pnpEstimation->setObjectName(Parameters::kLccBowPnPEstimation().c_str());
_ui->loopClosure_pnpReprojError->setObjectName(Parameters::kLccBowPnPReprojError().c_str());
_ui->loopClosure_pnpFlags->setObjectName(Parameters::kLccBowPnPFlags().c_str());
_ui->groupBox_reextract->setObjectName(Parameters::kLccReextractActivated().c_str());
_ui->reextract_nn->setObjectName(Parameters::kLccReextractNNType().c_str());
@@ -542,6 +551,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
_ui->odom_refine_iterations->setObjectName(Parameters::kOdomRefineIterations().c_str());
_ui->odom_force2D->setObjectName(Parameters::kOdomForce2D().c_str());
_ui->odom_fillInfoData->setObjectName(Parameters::kOdomFillInfoData().c_str());
_ui->odom_dataBufferSize->setObjectName(Parameters::kOdomImageBufferSize().c_str());
_ui->lineEdit_odom_roi->setObjectName(Parameters::kOdomRoiRatios().c_str());
_ui->odom_pnpEstimation->setObjectName(Parameters::kOdomPnPEstimation().c_str());
_ui->odom_pnpReprojError->setObjectName(Parameters::kOdomPnPReprojError().c_str());
@@ -567,6 +577,14 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
_ui->doubleSpinBox_minTranslation->setObjectName(Parameters::kOdomMonoMinTranslation().c_str());
_ui->doubleSpinBox_maxVariance->setObjectName(Parameters::kOdomMonoMaxVariance().c_str());
//Odometry particle filter
_ui->odom_particleFiltering->setObjectName(Parameters::kOdomParticleFiltering().c_str());
_ui->spinBox_particleSize->setObjectName(Parameters::kOdomParticleSize().c_str());
_ui->doubleSpinBox_particleNoiseT->setObjectName(Parameters::kOdomParticleNoiseT().c_str());
_ui->doubleSpinBox_particleLambdaT->setObjectName(Parameters::kOdomParticleLambdaT().c_str());
_ui->doubleSpinBox_particleNoiseR->setObjectName(Parameters::kOdomParticleNoiseR().c_str());
_ui->doubleSpinBox_particleLambdaR->setObjectName(Parameters::kOdomParticleLambdaR().c_str());
//Stereo
_ui->stereo_flow_winSize->setObjectName(Parameters::kStereoWinSize().c_str());
_ui->stereo_flow_maxLevel->setObjectName(Parameters::kStereoMaxLevel().c_str());
@@ -972,6 +990,8 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
_ui->openni2_gain->setValue(100);
_ui->openni2_mirroring->setChecked(false);
_ui->comboBox_freenect2Format->setCurrentIndex(0);
_ui->lineEdit_cameraStereoImages_timestamps->setText("");
_ui->lineEdit_cameraStereoImages_path->setText("");
_ui->checkbox_rgbd_colorOnly->setChecked(false);
_ui->lineEdit_openniDevice->setText("");
_ui->lineEdit_openniLocalTransform->setText("0 0 0 -PI_2 0 -PI_2");
@@ -1237,6 +1257,8 @@ void PreferencesDialog::readCameraSettings(const QString & filePath)
_ui->openni2_gain->setValue(settings.value("openni2Gain", _ui->openni2_gain->value()).toInt());
_ui->openni2_mirroring->setChecked(settings.value("openni2Mirroring", _ui->openni2_mirroring->isChecked()).toBool());
_ui->comboBox_freenect2Format->setCurrentIndex(settings.value("freenect2Format", _ui->comboBox_freenect2Format->currentIndex()).toInt());
_ui->lineEdit_cameraStereoImages_timestamps->setText(settings.value("stereoImagesStamps", _ui->lineEdit_cameraStereoImages_timestamps->text()).toString());
_ui->lineEdit_cameraStereoImages_path->setText(settings.value("stereoImagesPath", _ui->lineEdit_cameraStereoImages_path->text()).toString());
_ui->checkbox_rgbd_colorOnly->setChecked(settings.value("rgbdColorOnly", _ui->checkbox_rgbd_colorOnly->isChecked()).toBool());
_ui->lineEdit_openniDevice->setText(settings.value("device",_ui->lineEdit_openniDevice->text()).toString());
_ui->lineEdit_openniLocalTransform->setText(settings.value("localTransform",_ui->lineEdit_openniLocalTransform->text()).toString());
@@ -1507,6 +1529,8 @@ void PreferencesDialog::writeCameraSettings(const QString & filePath) const
settings.setValue("openni2Gain", _ui->openni2_gain->value());
settings.setValue("openni2Mirroring", _ui->openni2_mirroring->isChecked());
settings.setValue("freenect2Format", _ui->comboBox_freenect2Format->currentIndex());
settings.setValue("stereoImagesStamps", _ui->lineEdit_cameraStereoImages_timestamps->text());
settings.setValue("stereoImagesPath", _ui->lineEdit_cameraStereoImages_path->text());
settings.setValue("rgbdColorOnly", _ui->checkbox_rgbd_colorOnly->isChecked());
settings.setValue("device", _ui->lineEdit_openniDevice->text());
settings.setValue("localTransform", _ui->lineEdit_openniLocalTransform->text());
@@ -2111,6 +2135,12 @@ void PreferencesDialog::selectSourceRGBD(Src src)
_ui->groupBox_sourceDatabase->setChecked(false);
}
if(src == kSrcStereoImages)
{
_ui->lineEdit_cameraStereoImages_timestamps->setText("");
_ui->lineEdit_cameraStereoImages_path->setText("");
}
if(validateForm())
{
// Even if there is no change, MainWindow should be notified
@@ -2140,6 +2170,34 @@ void PreferencesDialog::openDatabaseViewer()
}
}
void PreferencesDialog::selectSourceStereoImagesStamps()
{
QString dir = _ui->lineEdit_cameraStereoImages_timestamps->text();
if(dir.isEmpty())
{
dir = getWorkingDirectory();
}
QString path = QFileDialog::getOpenFileName(this, tr("Select file"), dir, tr("Timestamps file (*.txt)"));
if(path.size())
{
_ui->lineEdit_cameraStereoImages_timestamps->setText(path);
}
}
void PreferencesDialog::selectSourceStereoImagesPath()
{
QString dir = _ui->lineEdit_cameraStereoImages_path->text();
if(dir.isEmpty())
{
dir = getWorkingDirectory();
}
QString path = QFileDialog::getExistingDirectory(this, tr("Select stereo images directory"), dir);
if(path.size())
{
_ui->lineEdit_cameraStereoImages_path->setText(path);
}
}
void PreferencesDialog::setParameter(const std::string & key, const std::string & value)
{
UDEBUG("%s=%s", key.c_str(), value.c_str());
@@ -2837,6 +2895,7 @@ void PreferencesDialog::updateRGBDCameraGroupBoxVisibility()
{
_ui->groupBox_openni2->setVisible(_ui->comboBox_cameraRGBD->currentIndex() == kSrcOpenNI2-kSrcOpenNI_PCL);
_ui->groupBox_freenect2->setVisible(_ui->comboBox_cameraRGBD->currentIndex() == kSrcFreenect2-kSrcOpenNI_PCL);
_ui->groupBox_cameraStereoImages->setVisible(_ui->comboBox_cameraRGBD->currentIndex() == kSrcStereoImages-kSrcOpenNI_PCL);
}
/*** GETTERS ***/
@@ -3245,6 +3304,15 @@ CameraRGBD * PreferencesDialog::createCameraRGBD(bool forCalibration)
this->getGeneralInputRate(),
this->getSourceOpenniLocalTransform());
}
else if(this->getSourceRGBD() == kSrcStereoImages)
{
return new CameraStereoImages(
_ui->lineEdit_cameraStereoImages_path->text().toStdString(),
this->getSourceOpenniDevice().toStdString(),
_ui->lineEdit_cameraStereoImages_timestamps->text().toStdString(),
this->getGeneralInputRate(),
this->getSourceOpenniLocalTransform());
}
else
{
UFATAL("RGBD Source type undefined!");
@@ -3268,6 +3336,10 @@ int PreferencesDialog::getOdomStrategy() const
{
return _ui->odom_strategy->currentIndex();
}
int PreferencesDialog::getOdomBufferSize() const
{
return _ui->odom_dataBufferSize->value();
}
QString PreferencesDialog::getCameraInfoDir() const
{
@@ -3409,12 +3481,15 @@ void PreferencesDialog::testOdometry(int type)
odometry = new OdometryBOW(parameters);
}
OdometryThread odomThread(odometry); // take ownership of odometry
OdometryThread odomThread(
odometry, // take ownership of odometry
_ui->odom_dataBufferSize->value());
odomThread.registerToEventsManager();
OdometryViewer * odomViewer = new OdometryViewer(10,
_ui->spinBox_decimation_odom->value(),
_ui->doubleSpinBox_voxelSize_odom->value(),
_ui->doubleSpinBox_maxDepth_odom->value(),
this->getOdomQualityWarnThr(),
this);
odomViewer->setWindowTitle(tr("Odometry viewer"));

112
guilib/src/ui/DatabaseViewer.ui
View File

@@ -783,14 +783,14 @@
<item>
<widget class="QToolBox" name="toolBox">
<property name="currentIndex">
<number>2</number>
<number>1</number>
</property>
<widget class="QWidget" name="page">
<property name="geometry">
<rect>
<x>0</x>
<y>0</y>
<width>312</width>
<width>315</width>
<height>314</height>
</rect>
</property>
@@ -1009,9 +1009,9 @@
<property name="geometry">
<rect>
<x>0</x>
<y>0</y>
<y>-12</y>
<width>351</width>
<height>331</height>
<height>355</height>
</rect>
</property>
<attribute name="label">
@@ -1020,7 +1020,7 @@
<layout class="QVBoxLayout" name="verticalLayout_15">
<item>
<layout class="QGridLayout" name="gridLayout_3" columnstretch="0,1">
<item row="4" column="0">
<item row="5" column="0">
<widget class="QSpinBox" name="spinBox_visual_iteration">
<property name="minimum">
<number>1</number>
@@ -1040,7 +1040,7 @@
</property>
</widget>
</item>
<item row="5" column="0">
<item row="6" column="0">
<widget class="QDoubleSpinBox" name="doubleSpinBox_visual_maxDepth">
<property name="suffix">
<string> m</string>
@@ -1056,47 +1056,28 @@
</property>
</widget>
</item>
<item row="1" column="1">
<widget class="QLabel" name="label_26">
<item row="6" column="1">
<widget class="QLabel" name="label_19">
<property name="text">
<string>NNDR</string>
<string>Max feature depth</string>
</property>
</widget>
</item>
<item row="2" column="1">
<widget class="QLabel" name="label_21">
<item row="4" column="1">
<widget class="QLabel" name="label_22">
<property name="text">
<string>Min correspondences</string>
<string>Max correspondence distance</string>
</property>
</widget>
</item>
<item row="0" column="1">
<widget class="QLabel" name="label_28">
<item row="5" column="1">
<widget class="QLabel" name="label_23">
<property name="text">
<string>2D transform (x,y,yaw)</string>
<string>Iteration</string>
</property>
</widget>
</item>
<item row="3" column="0">
<widget class="QDoubleSpinBox" name="doubleSpinBox_visual_maxCorrespDistance">
<property name="suffix">
<string> m</string>
</property>
<property name="decimals">
<number>3</number>
</property>
<property name="minimum">
<double>0.001000000000000</double>
</property>
<property name="maximum">
<double>1.000000000000000</double>
</property>
<property name="value">
<double>0.020000000000000</double>
</property>
</widget>
</item>
<item row="1" column="0">
<item row="2" column="0">
<widget class="QDoubleSpinBox" name="doubleSpinBox_visual_nndr">
<property name="suffix">
<string> m</string>
@@ -1118,14 +1099,21 @@
</property>
</widget>
</item>
<item row="5" column="1">
<widget class="QLabel" name="label_19">
<item row="2" column="1">
<widget class="QLabel" name="label_26">
<property name="text">
<string>Max feature depth</string>
<string>NNDR</string>
</property>
</widget>
</item>
<item row="2" column="0">
<item row="3" column="1">
<widget class="QLabel" name="label_21">
<property name="text">
<string>Min correspondences</string>
</property>
</widget>
</item>
<item row="3" column="0">
<widget class="QSpinBox" name="spinBox_visual_minCorrespondences">
<property name="minimum">
<number>3</number>
@@ -1138,17 +1126,43 @@
</property>
</widget>
</item>
<item row="3" column="1">
<widget class="QLabel" name="label_22">
<item row="0" column="1">
<widget class="QLabel" name="label_28">
<property name="text">
<string>Max correspondence distance</string>
<string>2D transform (x,y,yaw)</string>
</property>
</widget>
</item>
<item row="4" column="1">
<widget class="QLabel" name="label_23">
<item row="4" column="0">
<widget class="QDoubleSpinBox" name="doubleSpinBox_visual_maxCorrespDistance">
<property name="suffix">
<string> m</string>
</property>
<property name="decimals">
<number>3</number>
</property>
<property name="minimum">
<double>0.001000000000000</double>
</property>
<property name="maximum">
<double>1.000000000000000</double>
</property>
<property name="value">
<double>0.020000000000000</double>
</property>
</widget>
</item>
<item row="1" column="1">
<widget class="QLabel" name="label_53">
<property name="text">
<string>Iteration</string>
<string>PnP (2D-&gt;3D estimation)</string>
</property>
</widget>
</item>
<item row="1" column="0">
<widget class="QCheckBox" name="checkBox_pnp">
<property name="text">
<string/>
</property>
</widget>
</item>
@@ -1278,7 +1292,7 @@
<property name="geometry">
<rect>
<x>0</x>
<y>-118</y>
<y>0</y>
<width>330</width>
<height>304</height>
</rect>
@@ -1496,7 +1510,7 @@
<rect>
<x>0</x>
<y>0</y>
<width>248</width>
<width>315</width>
<height>319</height>
</rect>
</property>
@@ -1690,8 +1704,8 @@
<rect>
<x>0</x>
<y>0</y>
<width>201</width>
<height>126</height>
<width>330</width>
<height>186</height>
</rect>
</property>
<attribute name="label">

10
guilib/src/ui/mainWindow.ui
View File

@@ -27,7 +27,7 @@
<x>0</x>
<y>0</y>
<width>1012</width>
<height>25</height>
<height>22</height>
</rect>
</property>
<widget class="QMenu" name="menuFile">
@@ -49,7 +49,7 @@
</widget>
<widget class="QMenu" name="menuEdit">
<property name="title">
<string>Edit</string>
<string> Edit</string>
</property>
<widget class="QMenu" name="menuAdvanced">
<property name="title">
@@ -60,6 +60,7 @@
<addaction name="actionGenerate_map"/>
<addaction name="actionGenerate_local_map"/>
<addaction name="actionGenerate_TORO_graph_graph"/>
<addaction name="actionExport_poses_txt"/>
<addaction name="actionPrint_loop_closure_IDs_to_console"/>
<addaction name="actionSend_goal"/>
</widget>
@@ -1203,6 +1204,11 @@
<string>Send a goal...</string>
</property>
</action>
<action name="actionExport_poses_txt">
<property name="text">
<string>Export poses (*.txt)...</string>
</property>
</action>
</widget>
<customwidgets>
<customwidget>

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

File diff suppressed because it is too large Load Diff

11
guilib/src/utilite/UPlot.cpp
View File

@@ -378,6 +378,7 @@ void UPlotCurve::_addValue(UPlotItem * data)
{
float x = data->data().x();
float y = data->data().y();
if(_minMax.size() != 4)
{
_minMax = QVector<float>(4);
@@ -428,6 +429,16 @@ void UPlotCurve::addValue(UPlotItem * data)
void UPlotCurve::addValue(float x, float y)
{
if(_items.size() &&
dynamic_cast<UPlotItem*>(_items.back()) &&
x < ((UPlotItem*)_items.back())->data().x())
{
UWARN("New value (%f) added to curve \"%s\" is smaller "
"than the last added (%f). Clearing the curve.",
x, this->name().toStdString().c_str(), _items.back()->pos().x());
this->clear();
}
float width = 2; // TODO warn : hard coded value!
this->addValue(new UPlotItem(x,y,width));
}