CameraImages: Added synchronization of captrue time with timestamps

corelib/include/rtabmap/core/CameraRGB.h

@@ -31,13 +31,13 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include <opencv2/highgui/highgui.hpp>
 #include "rtabmap/core/Camera.h"
+#include "rtabmap/utilite/UTimer.h"
 #include <set>
 #include <stack>
 #include <list>
 #include <vector>
 
 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<double> stamps_;
 	std::list<Transform> groundTruth_;
 	CameraModel _model;
+
+	UTimer _captureTimer;
+	double _captureDelay;
 };
 
 

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<std::string> 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();

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)
 	{

guilib/src/ui/preferencesDialog.ui

@@ -63,9 +63,9 @@
           <property name="geometry">
            <rect>
             <x>0</x>
-            <y>-139</y>
+            <y>-946</y>
-            <width>681</width>
+            <width>676</width>
-            <height>1888</height>
+            <height>1919</height>
            </rect>
           </property>
           <layout class="QVBoxLayout" name="verticalLayout_16">
@@ -86,7 +86,7 @@
                 <enum>QFrame::Raised</enum>
                </property>
                <property name="currentIndex">
-                <number>7</number>
+                <number>3</number>
                </property>
                <widget class="QWidget" name="page_22">
                 <layout class="QVBoxLayout" name="verticalLayout_29" stretch="0,1">
@@ -3331,6 +3331,13 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                        </property>
                        <item>
                         <layout class="QGridLayout" name="gridLayout_68" columnstretch="0,0,1">
+                         <item row="5" column="1">
+                          <widget class="QLineEdit" name="lineEdit_cameraImages_path_scans">
+                           <property name="text">
+                            <string/>
+                           </property>
+                          </widget>
+                         </item>
                          <item row="0" column="1">
                           <widget class="QCheckBox" name="checkBox_cameraImages_timestamps">
                            <property name="text">
@@ -3351,21 +3358,21 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                            </property>
                           </widget>
                          </item>
-                         <item row="1" column="0">
+                         <item row="2" column="0">
                           <widget class="QToolButton" name="toolButton_cameraImages_timestamps">
                            <property name="text">
                             <string>...</string>
                            </property>
                           </widget>
                          </item>
-                         <item row="1" column="1">
+                         <item row="2" column="1">
                           <widget class="QLineEdit" name="lineEdit_cameraImages_timestamps">
                            <property name="text">
                             <string/>
                            </property>
                           </widget>
                          </item>
-                         <item row="1" column="2">
+                         <item row="2" column="2">
                           <widget class="QLabel" name="label_251">
                            <property name="text">
                             <string>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 &quot;Use file names as timestamps&quot; above is checked. </string>
@@ -3378,21 +3385,21 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                            </property>
                           </widget>
                          </item>
-                         <item row="2" column="0">
+                         <item row="3" column="0">
                           <widget class="QToolButton" name="toolButton_cameraImages_gt">
                            <property name="text">
                             <string>...</string>
                            </property>
                           </widget>
                          </item>
-                         <item row="2" column="1">
+                         <item row="3" column="1">
                           <widget class="QLineEdit" name="lineEdit_cameraImages_gt">
                            <property name="text">
                             <string/>
                            </property>
                           </widget>
                          </item>
-                         <item row="2" column="2">
+                         <item row="3" column="2">
                           <widget class="QLabel" name="label_288">
                            <property name="text">
                             <string>Ground truth file. Select the correct format below.</string>
@@ -3405,7 +3412,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="4" column="1">
                           <widget class="QComboBox" name="comboBox_cameraImages_gtFormat">
                            <item>
                             <property name="text">
@@ -3429,7 +3436,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                            </item>
                           </widget>
                          </item>
-                         <item row="3" column="2">
+                         <item row="4" column="2">
                           <widget class="QLabel" name="label_289">
                            <property name="text">
                             <string>Ground truth format.</string>
@@ -3442,21 +3449,14 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                            </property>
                           </widget>
                          </item>
-                         <item row="4" column="0">
+                         <item row="5" column="0">
                           <widget class="QToolButton" name="toolButton_cameraImages_path_scans">
                            <property name="text">
                             <string>...</string>
                            </property>
                           </widget>
                          </item>
-                         <item row="4" column="1">
+                         <item row="5" column="2">
-                          <widget class="QLineEdit" name="lineEdit_cameraImages_path_scans">
-                           <property name="text">
-                            <string/>
-                           </property>
-                          </widget>
-                         </item>
-                         <item row="4" column="2">
                           <widget class="QLabel" name="label_293">
                            <property name="text">
                             <string>Path to directory containing optional laser scans (*.pcd, *.ply, *.bin [KITTI format]). The directory should have the same size has the images directory. </string>
@@ -3469,7 +3469,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                            </property>
                           </widget>
                          </item>
-                         <item row="5" column="1">
+                         <item row="6" column="1">
                           <widget class="QLineEdit" name="lineEdit_cameraImages_laser_transform">
                            <property name="toolTip">
                             <string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;Format (3 values): x y z&lt;br/&gt;Format (6 values): x y z roll pitch yaw&lt;br/&gt;Format (7 values): x y z qx qy qz qw&lt;br/&gt;Format (9 values, 3x3 rotation): r11 r12 r13 r21 r22 r23 r31 r32 r33&lt;br/&gt;Format (12 values, 3x4 transform): r11 r12 r13 tx r21 r22 r23 ty r31 r32 r33 tz&lt;/p&gt;&lt;p&gt;KITTI: /base_link to /scan = -0.27 0 0.08 0 0 0&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
@@ -3479,7 +3479,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                            </property>
                           </widget>
                          </item>
-                         <item row="5" column="2">
+                         <item row="6" column="2">
                           <widget class="QLabel" name="label_294">
                            <property name="text">
                             <string>Local transform from /base_link to /scan_link. Mouse over the box to show formats.</string>
@@ -3492,7 +3492,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                            </property>
                           </widget>
                          </item>
-                         <item row="6" column="1">
+                         <item row="7" column="1">
                           <widget class="QSpinBox" name="spinBox_cameraImages_max_scan_pts">
                            <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>
@@ -3502,7 +3502,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                            </property>
                           </widget>
                          </item>
-                         <item row="6" column="2">
+                         <item row="7" column="2">
                           <widget class="QLabel" name="label_292">
                            <property name="text">
                             <string>Maximum laser scan points.</string>
@@ -3515,6 +3515,26 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                            </property>
                           </widget>
                          </item>
+                         <item row="1" column="2">
+                          <widget class="QLabel" name="label_256">
+                           <property name="text">
+                            <string>Synchronize capture rate with timestamps.</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_cameraImages_syncTimeStamps">
+                           <property name="text">
+                            <string/>
+                           </property>
+                          </widget>
+                         </item>
                         </layout>
                        </item>
                        <item>