i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-04 00:37:46 +08:00
Code Issues Packages Projects Releases Wiki Activity

CameraImages: Added synchronization of captrue time with timestamps

Browse Source
This commit is contained in:
matlabbe
2016-01-13 18:13:44 -05:00
parent f68203f529
commit dc2746a365
4 changed files with 114 additions and 32 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/include/rtabmap/core/CameraRGB.h
+7 -2
View File
@@ -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
+48 -2
View File
@@ -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
+14 -3
View File
@@ -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
+45 -25
View File
@@ -63,9 +63,9 @@
<property name="geometry">
<rect>
<x>0</x>
<y>-139</y>
<width>681</width>
<height>1888</height>
<y>-946</y>
<width>676</width>
<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">
<widget class="QLineEdit" name="lineEdit_cameraImages_path_scans">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="4" column="2">
<item row="5" 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>