i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-02 01:20:25 +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

9
corelib/include/rtabmap/core/CameraRGB.h
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;
};

50
corelib/src/CameraRGB.cpp
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();