SensorData DBReader::getNextData()
{
SensorData data;
if(_dbDriver)
{
float frameRate = _frameRate;
if(frameRate>0.0f)
{
int sleepTime = (1000.0f/frameRate - 1000.0f*_timer.getElapsedTime());
if(sleepTime > 2)
{
uSleep(sleepTime-2);
}
// Add precision at the cost of a small overhead
while(_timer.getElapsedTime() < 1.0/double(frameRate)-0.000001)
{
//
}
double slept = _timer.getElapsedTime();
_timer.start();
UDEBUG("slept=%fs vs target=%fs", slept, 1.0/double(frameRate));
}
if(!this->isKilled() && _currentId != _ids.end())
{
cv::Mat imageBytes;
cv::Mat depthBytes;
cv::Mat laserScanBytes;
int mapId;
float fx,fy,cx,cy;
Transform localTransform, pose;
float variance = 1.0f;
_dbDriver->getNodeData(*_currentId, imageBytes, depthBytes, laserScanBytes, fx, fy, cx, cy, localTransform);
if(!_odometryIgnored)
{
_dbDriver->getPose(*_currentId, pose, mapId);
std::map<int, Link> links;
_dbDriver->loadLinks(*_currentId, links, Link::kNeighbor);
if(links.size())
{
// assume the first is the backward neighbor, take its variance
variance = links.begin()->second.variance();
}
}
int seq = *_currentId;
++_currentId;
if(imageBytes.empty())
{
UWARN("No image loaded from the database for id=%d!", *_currentId);
}
util3d::CompressionThread ctImage(imageBytes, true);
util3d::CompressionThread ctDepth(depthBytes, true);
util3d::CompressionThread ctLaserScan(laserScanBytes, false);
ctImage.start();
ctDepth.start();
ctLaserScan.start();
ctImage.join();
ctDepth.join();
ctLaserScan.join();
data = SensorData(
ctLaserScan.getUncompressedData(),
ctImage.getUncompressedData(),
ctDepth.getUncompressedData(),
fx,fy,cx,cy,
localTransform,
pose,
variance,
seq);
}
}
else
{
UERROR("Not initialized...");
}
return data;
}
void SensorData::uncompressDataConst(cv::Mat * imageRaw, cv::Mat * depthRaw, cv::Mat * laserScanRaw, cv::Mat * userDataRaw) const
{
if(imageRaw)
{
*imageRaw = _imageRaw;
}
if(depthRaw)
{
*depthRaw = _depthOrRightRaw;
}
if(laserScanRaw)
{
*laserScanRaw = _laserScanRaw;
}
if(userDataRaw)
{
*userDataRaw = _userDataRaw;
}
if( (imageRaw && imageRaw->empty()) ||
(depthRaw && depthRaw->empty()) ||
(laserScanRaw && laserScanRaw->empty()) ||
(userDataRaw && userDataRaw->empty()))
{
rtabmap::CompressionThread ctImage(_imageCompressed, true);
rtabmap::CompressionThread ctDepth(_depthOrRightCompressed, true);
rtabmap::CompressionThread ctLaserScan(_laserScanCompressed, false);
rtabmap::CompressionThread ctUserData(_userDataCompressed, false);
if(imageRaw && imageRaw->empty())
{
ctImage.start();
}
if(depthRaw && depthRaw->empty())
{
ctDepth.start();
}
if(laserScanRaw && laserScanRaw->empty())
{
ctLaserScan.start();
}
if(userDataRaw && userDataRaw->empty())
{
ctUserData.start();
}
ctImage.join();
ctDepth.join();
ctLaserScan.join();
ctUserData.join();
if(imageRaw && imageRaw->empty())
{
*imageRaw = ctImage.getUncompressedData();
if(imageRaw->empty())
{
UWARN("Requested raw image data, but the sensor data (%d) doesn't have image.", this->id());
}
}
if(depthRaw && depthRaw->empty())
{
*depthRaw = ctDepth.getUncompressedData();
if(depthRaw->empty())
{
UWARN("Requested depth/right image data, but the sensor data (%d) doesn't have depth/right image.", this->id());
}
}
if(laserScanRaw && laserScanRaw->empty())
{
*laserScanRaw = ctLaserScan.getUncompressedData();
if(laserScanRaw->empty())
{
UWARN("Requested laser scan data, but the sensor data (%d) doesn't have laser scan.", this->id());
}
}
if(userDataRaw && userDataRaw->empty())
{
*userDataRaw = ctUserData.getUncompressedData();
if(userDataRaw->empty())
{
UWARN("Requested user data, but the sensor data (%d) doesn't have user data.", this->id());
}
}
}
}
SensorData DBReader::getNextData()
{
SensorData data;
if(_dbDriver)
{
float frameRate = _frameRate;
if(frameRate>0.0f)
{
int sleepTime = (1000.0f/frameRate - 1000.0f*_timer.getElapsedTime());
if(sleepTime > 2)
{
uSleep(sleepTime-2);
}
// Add precision at the cost of a small overhead
while(_timer.getElapsedTime() < 1.0/double(frameRate)-0.000001)
{
//
}
double slept = _timer.getElapsedTime();
_timer.start();
UDEBUG("slept=%fs vs target=%fs", slept, 1.0/double(frameRate));
}
if(!this->isKilled() && _currentId != _ids.end())
{
cv::Mat imageBytes;
cv::Mat depthBytes;
cv::Mat laserScanBytes;
int mapId;
float fx,fy,cx,cy;
Transform localTransform, pose;
float rotVariance = 1.0f;
float transVariance = 1.0f;
std::vector<unsigned char> userData;
_dbDriver->getNodeData(*_currentId, imageBytes, depthBytes, laserScanBytes, fx, fy, cx, cy, localTransform);
// info
int weight;
std::string label;
double stamp;
_dbDriver->getNodeInfo(*_currentId, pose, mapId, weight, label, stamp, userData);
if(!_odometryIgnored)
{
std::map<int, Link> links;
_dbDriver->loadLinks(*_currentId, links, Link::kNeighbor);
if(links.size())
{
// assume the first is the backward neighbor, take its variance
rotVariance = links.begin()->second.rotVariance();
transVariance = links.begin()->second.transVariance();
}
}
else
{
pose.setNull();
}
int seq = *_currentId;
++_currentId;
if(imageBytes.empty())
{
UWARN("No image loaded from the database for id=%d!", *_currentId);
}
rtabmap::CompressionThread ctImage(imageBytes, true);
rtabmap::CompressionThread ctDepth(depthBytes, true);
rtabmap::CompressionThread ctLaserScan(laserScanBytes, false);
ctImage.start();
ctDepth.start();
ctLaserScan.start();
ctImage.join();
ctDepth.join();
ctLaserScan.join();
data = SensorData(
ctLaserScan.getUncompressedData(),
ctImage.getUncompressedData(),
ctDepth.getUncompressedData(),
fx,fy,cx,cy,
localTransform,
pose,
rotVariance,
transVariance,
seq,
stamp,
userData);
UDEBUG("Laser=%d RGB/Left=%d Depth=%d Right=%d",
data.laserScan().empty()?0:1,
data.image().empty()?0:1,
data.depth().empty()?0:1,
data.rightImage().empty()?0:1);
}
}
else
{
UERROR("Not initialized...");
}
return data;
}
