From dc2746a3659167d08e89b5cfe99119570ae37294 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Wed, 13 Jan 2016 18:13:44 -0500 Subject: [PATCH] CameraImages: Added synchronization of captrue time with timestamps --- corelib/include/rtabmap/core/CameraRGB.h | 9 ++- corelib/src/CameraRGB.cpp | 50 ++++++++++++++++- guilib/src/PreferencesDialog.cpp | 17 +++++- guilib/src/ui/preferencesDialog.ui | 70 +++++++++++++++--------- 4 files changed, 114 insertions(+), 32 deletions(-) diff --git a/corelib/include/rtabmap/core/CameraRGB.h b/corelib/include/rtabmap/core/CameraRGB.h index 2015c8b2..2881b843 100644 --- a/corelib/include/rtabmap/core/CameraRGB.h +++ b/corelib/include/rtabmap/core/CameraRGB.h @@ -31,13 +31,13 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include "rtabmap/core/Camera.h" +#include "rtabmap/utilite/UTimer.h" #include #include #include #include class UDirectory; -class UTimer; namespace rtabmap { @@ -70,10 +70,11 @@ public: void setDirRefreshed(bool enabled) {_refreshDir = enabled;} void setImagesRectified(bool enabled) {_rectifyImages = enabled;} - void setTimestamps(bool fileNamesAreStamps, const std::string & filePath = "") + void setTimestamps(bool fileNamesAreStamps, const std::string & filePath = "", bool syncImageRateWithStamps=true) { _filenamesAreTimestamps = fileNamesAreStamps; timestampsPath_=filePath; + syncImageRateWithStamps_ = syncImageRateWithStamps; } void setScanPath( @@ -147,6 +148,7 @@ private: bool _filenamesAreTimestamps; std::string timestampsPath_; + bool syncImageRateWithStamps_; std::string groundTruthPath_; int _groundTruthFormat; @@ -154,6 +156,9 @@ private: std::list stamps_; std::list groundTruth_; CameraModel _model; + + UTimer _captureTimer; + double _captureDelay; }; diff --git a/corelib/src/CameraRGB.cpp b/corelib/src/CameraRGB.cpp index 850e1f42..5c378197 100644 --- a/corelib/src/CameraRGB.cpp +++ b/corelib/src/CameraRGB.cpp @@ -66,8 +66,12 @@ CameraImages::CameraImages() : _scanVoxelSize(0.0f), _scanNormalsK(0), _depthFromScan(false), + _depthFromScanFillHolesVertical(true), + _depthFromScanFillHolesFromBorder(false), _filenamesAreTimestamps(false), - _groundTruthFormat(0) + syncImageRateWithStamps_(true), + _groundTruthFormat(0), + _captureDelay(0.0) {} CameraImages::CameraImages(const std::string & path, float imageRate, @@ -88,8 +92,12 @@ CameraImages::CameraImages(const std::string & path, _scanVoxelSize(0.0f), _scanNormalsK(0), _depthFromScan(false), + _depthFromScanFillHolesVertical(true), + _depthFromScanFillHolesFromBorder(false), _filenamesAreTimestamps(false), - _groundTruthFormat(0) + syncImageRateWithStamps_(true), + _groundTruthFormat(0), + _captureDelay(0.0) { } @@ -113,6 +121,8 @@ bool CameraImages::init(const std::string & calibrationFolder, const std::string _lastScanFileName.clear(); _count = 0; _countScan = 0; + _captureDelay = 0.0; + _captureTimer.restart(); UDEBUG(""); if(_dir) @@ -383,6 +393,38 @@ std::vector CameraImages::filenames() const SensorData CameraImages::captureImage() { + double actualDelay = 0.0; + if(syncImageRateWithStamps_ && _captureDelay>0.0) + { + int sleepTime = (1000*_captureDelay - 1000.0f*_captureTimer.getElapsedTime()); + if(sleepTime > 2) + { + uSleep(sleepTime-2); + } + else if(sleepTime < 0) + { + if(this->getImageRate() > 0.0f) + { + UWARN("CameraImages: Cannot read images as fast as their timestamps (delay=%f s). Disable " + "source image rate or disable synchronization of capture time with timestamps.", _captureDelay); + } + else + { + UWARN("CameraImages: Cannot read images as fast as their timestamps (delay=%f s).", _captureDelay); + } + } + + actualDelay = 1.0/(_captureTimer.getElapsedTime()); + + // Add precision at the cost of a small overhead + while(_captureTimer.getElapsedTime() < _captureDelay-0.000001) + { + // + } + _captureTimer.start(); + } + _captureDelay = 0.0; + cv::Mat img; cv::Mat scan; double stamp = UTimer::now(); @@ -455,6 +497,10 @@ SensorData CameraImages::captureImage() { stamp = stamps_.front(); stamps_.pop_front(); + if(stamps_.size()) + { + _captureDelay = stamps_.front() - stamp; + } if(groundTruth_.size()) { groundTruthPose = groundTruth_.front(); diff --git a/guilib/src/PreferencesDialog.cpp b/guilib/src/PreferencesDialog.cpp index 48a339aa..3812e834 100644 --- a/guilib/src/PreferencesDialog.cpp +++ b/guilib/src/PreferencesDialog.cpp @@ -397,6 +397,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) : connect(_ui->lineEdit_cameraRGBDImages_path_rgb, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel())); connect(_ui->lineEdit_cameraRGBDImages_path_depth, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel())); connect(_ui->checkBox_cameraImages_timestamps, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel())); + connect(_ui->checkBox_cameraImages_syncTimeStamps, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel())); connect(_ui->doubleSpinBox_cameraRGBDImages_scale, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteSourcePanel())); 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())); @@ -1206,6 +1207,7 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox) _ui->checkBox_stereoVideo_rectify->setChecked(false); _ui->checkBox_cameraImages_timestamps->setChecked(false); + _ui->checkBox_cameraImages_syncTimeStamps->setChecked(true); _ui->lineEdit_cameraImages_timestamps->setText(""); _ui->lineEdit_cameraImages_path_scans->setText(""); _ui->lineEdit_cameraImages_laser_transform->setText("0 0 0 0 0 0"); @@ -1522,6 +1524,7 @@ void PreferencesDialog::readCameraSettings(const QString & filePath) _ui->checkBox_rgbImages_rectify->setChecked(settings.value("rectify",_ui->checkBox_rgbImages_rectify->isChecked()).toBool()); _ui->checkBox_cameraImages_timestamps->setChecked(settings.value("filenames_as_stamps",_ui->checkBox_cameraImages_timestamps->isChecked()).toBool()); + _ui->checkBox_cameraImages_syncTimeStamps->setChecked(settings.value("sync_stamps",_ui->checkBox_cameraImages_syncTimeStamps->isChecked()).toBool()); _ui->lineEdit_cameraImages_timestamps->setText(settings.value("stamps", _ui->lineEdit_cameraImages_timestamps->text()).toString()); _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()); @@ -1885,6 +1888,7 @@ void PreferencesDialog::writeCameraSettings(const QString & filePath) const settings.setValue("refreshDir", _ui->source_images_refreshDir->isChecked()); settings.setValue("rectify", _ui->checkBox_rgbImages_rectify->isChecked()); settings.setValue("filenames_as_stamps", _ui->checkBox_cameraImages_timestamps->isChecked()); + settings.setValue("sync_stamps", _ui->checkBox_cameraImages_syncTimeStamps->isChecked()); settings.setValue("stamps", _ui->lineEdit_cameraImages_timestamps->text()); settings.setValue("path_scans", _ui->lineEdit_cameraImages_path_scans->text()); settings.setValue("scan_transform", _ui->lineEdit_cameraImages_laser_transform->text()); @@ -3826,7 +3830,10 @@ Camera * PreferencesDialog::createCamera(bool useRawImages) _ui->doubleSpinBox_cameraImages_scanVoxelSize->value(), _ui->spinBox_cameraImages_scanNormalsK->value(), this->getLaserLocalTransform()); - ((CameraRGBDImages*)camera)->setTimestamps(_ui->checkBox_cameraImages_timestamps->isChecked(), _ui->lineEdit_cameraImages_timestamps->text().toStdString()); + ((CameraRGBDImages*)camera)->setTimestamps( + _ui->checkBox_cameraImages_timestamps->isChecked(), + _ui->lineEdit_cameraImages_timestamps->text().toStdString(), + _ui->checkBox_cameraImages_syncTimeStamps->isChecked()); } else if(driver == kSrcDC1394) { @@ -3866,7 +3873,10 @@ Camera * PreferencesDialog::createCamera(bool useRawImages) _ui->doubleSpinBox_cameraImages_scanVoxelSize->value(), _ui->spinBox_cameraImages_scanNormalsK->value(), this->getLaserLocalTransform()); - ((CameraStereoImages*)camera)->setTimestamps(_ui->checkBox_cameraImages_timestamps->isChecked(), _ui->lineEdit_cameraImages_timestamps->text().toStdString()); + ((CameraStereoImages*)camera)->setTimestamps( + _ui->checkBox_cameraImages_timestamps->isChecked(), + _ui->lineEdit_cameraImages_timestamps->text().toStdString(), + _ui->checkBox_cameraImages_syncTimeStamps->isChecked()); } else if(driver == kSrcStereoVideo) { @@ -3918,7 +3928,8 @@ Camera * PreferencesDialog::createCamera(bool useRawImages) _ui->checkBox_depthFromScan_fillBorders->isChecked()); ((CameraRGBDImages*)camera)->setTimestamps( _ui->checkBox_cameraImages_timestamps->isChecked(), - _ui->lineEdit_cameraImages_timestamps->text().toStdString()); + _ui->lineEdit_cameraImages_timestamps->text().toStdString(), + _ui->checkBox_cameraImages_syncTimeStamps->isChecked()); } else if(driver == kSrcDatabase) { diff --git a/guilib/src/ui/preferencesDialog.ui b/guilib/src/ui/preferencesDialog.ui index 9d2e7a59..8b4f45de 100644 --- a/guilib/src/ui/preferencesDialog.ui +++ b/guilib/src/ui/preferencesDialog.ui @@ -63,9 +63,9 @@ 0 - -139 - 681 - 1888 + -946 + 676 + 1919 @@ -86,7 +86,7 @@ QFrame::Raised - 7 + 3 @@ -3331,6 +3331,13 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki + + + + + + + @@ -3351,21 +3358,21 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki - + ... - + - + Timestamps file (*.txt). The file should contain one column. The number of rows should be the same than the number of images in the folder. Not used if "Use file names as timestamps" above is checked. @@ -3378,21 +3385,21 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki - + ... - + - + Ground truth file. Select the correct format below. @@ -3405,7 +3412,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki - + @@ -3429,7 +3436,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki - + Ground truth format. @@ -3442,21 +3449,14 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki - + ... - - - - - - - - + Path to directory containing optional laser scans (*.pcd, *.ply, *.bin [KITTI format]). The directory should have the same size has the images directory. @@ -3469,7 +3469,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki - + <html><head/><body><p>Format (3 values): x y z<br/>Format (6 values): x y z roll pitch yaw<br/>Format (7 values): x y z qx qy qz qw<br/>Format (9 values, 3x3 rotation): r11 r12 r13 r21 r22 r23 r31 r32 r33<br/>Format (12 values, 3x4 transform): r11 r12 r13 tx r21 r22 r23 ty r31 r32 r33 tz</p><p>KITTI: /base_link to /scan = -0.27 0 0.08 0 0 0</p></body></html> @@ -3479,7 +3479,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki - + Local transform from /base_link to /scan_link. Mouse over the box to show formats. @@ -3492,7 +3492,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki - + <html><head/><body><p>KITTI: 130 000 points</p></body></html> @@ -3502,7 +3502,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki - + Maximum laser scan points. @@ -3515,6 +3515,26 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki + + + + Synchronize capture rate with timestamps. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + +