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

Version 0.10.1: user_data is now a cv::Mat to avoid a deep copy when SensorData is copied

Browse Source
This commit is contained in:
matlabbe
2015-06-28 19:22:18 -04:00
parent 7d3a3317b7
commit 4f96fd3530
22 changed files with 516 additions and 640 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/src/SensorData.cpp
+150 -29
View File
@@ -47,11 +47,10 @@ SensorData::SensorData(
const cv::Mat & image,
int id,
double stamp,
const std::vector<unsigned char> & userData) :
const cv::Mat & userData) :
_id(id),
_stamp(stamp),
_laserScanMaxPts(0),
_userData(userData)
_laserScanMaxPts(0)
{
if(image.rows == 1)
{
@@ -64,6 +63,15 @@ SensorData::SensorData(
image.type() == CV_8UC3); // RGB
_imageRaw = image;
}
if(userData.type() == CV_8UC1) // Bytes
{
_userDataCompressed = userData; // assume compressed
}
else
{
_userDataRaw = userData;
}
}
// Mono constructor
@@ -72,12 +80,11 @@ SensorData::SensorData(
const CameraModel & cameraModel,
int id,
double stamp,
const std::vector<unsigned char> & userData) :
const cv::Mat & userData) :
_id(id),
_stamp(stamp),
_laserScanMaxPts(0),
_cameraModels(std::vector<CameraModel>(1, cameraModel)),
_userData(userData)
_cameraModels(std::vector<CameraModel>(1, cameraModel))
{
if(image.rows == 1)
{
@@ -90,6 +97,15 @@ SensorData::SensorData(
image.type() == CV_8UC3); // RGB
_imageRaw = image;
}
if(userData.type() == CV_8UC1) // Bytes
{
_userDataCompressed = userData; // assume compressed
}
else
{
_userDataRaw = userData;
}
}
// RGB-D constructor
@@ -99,12 +115,11 @@ SensorData::SensorData(
const CameraModel & cameraModel,
int id,
double stamp,
const std::vector<unsigned char> & userData) :
const cv::Mat & userData) :
_id(id),
_stamp(stamp),
_laserScanMaxPts(0),
_cameraModels(std::vector<CameraModel>(1, cameraModel)),
_userData(userData)
_cameraModels(std::vector<CameraModel>(1, cameraModel))
{
if(rgb.rows == 1)
{
@@ -129,6 +144,15 @@ SensorData::SensorData(
depth.type() == CV_16UC1); // Depth in millimetre
_depthOrRightRaw = depth;
}
if(userData.type() == CV_8UC1) // Bytes
{
_userDataCompressed = userData; // assume compressed
}
else
{
_userDataRaw = userData;
}
}
// RGB-D constructor + 2d laser scan
@@ -140,12 +164,11 @@ SensorData::SensorData(
const CameraModel & cameraModel,
int id,
double stamp,
const std::vector<unsigned char> & userData) :
const cv::Mat & userData) :
_id(id),
_stamp(stamp),
_laserScanMaxPts(laserScanMaxPts),
_cameraModels(std::vector<CameraModel>(1, cameraModel)),
_userData(userData)
_cameraModels(std::vector<CameraModel>(1, cameraModel))
{
if(rgb.rows == 1)
{
@@ -179,6 +202,15 @@ SensorData::SensorData(
UASSERT(laserScan.type() == CV_8UC1); // Bytes
_laserScanCompressed = laserScan;
}
if(userData.type() == CV_8UC1) // Bytes
{
_userDataCompressed = userData; // assume compressed
}
else
{
_userDataRaw = userData;
}
}
// Multi-cameras RGB-D constructor
@@ -188,12 +220,11 @@ SensorData::SensorData(
const std::vector<CameraModel> & cameraModels,
int id,
double stamp,
const std::vector<unsigned char> & userData) :
const cv::Mat & userData) :
_id(id),
_stamp(stamp),
_laserScanMaxPts(0),
_cameraModels(cameraModels),
_userData(userData)
_cameraModels(cameraModels)
{
if(rgb.rows == 1)
{
@@ -221,6 +252,15 @@ SensorData::SensorData(
{
UASSERT(cameraModels[i].isValid());
}
if(userData.type() == CV_8UC1) // Bytes
{
_userDataCompressed = userData; // assume compressed
}
else
{
_userDataRaw = userData;
}
}
// Multi-cameras RGB-D constructor + 2d laser scan
@@ -232,12 +272,11 @@ SensorData::SensorData(
const std::vector<CameraModel> & cameraModels,
int id,
double stamp,
const std::vector<unsigned char> & userData) :
const cv::Mat & userData) :
_id(id),
_stamp(stamp),
_laserScanMaxPts(laserScanMaxPts),
_cameraModels(cameraModels),
_userData(userData)
_cameraModels(cameraModels)
{
if(rgb.rows == 1)
{
@@ -276,6 +315,15 @@ SensorData::SensorData(
{
UASSERT(cameraModels[i].isValid());
}
if(userData.type() == CV_8UC1) // Bytes
{
_userDataCompressed = userData; // assume compressed
}
else
{
_userDataRaw = userData;
}
}
// Stereo constructor
@@ -285,12 +333,11 @@ SensorData::SensorData(
const StereoCameraModel & cameraModel,
int id,
double stamp,
const std::vector<unsigned char> & userData):
const cv::Mat & userData):
_id(id),
_stamp(stamp),
_laserScanMaxPts(0),
_stereoCameraModel(cameraModel),
_userData(userData)
_stereoCameraModel(cameraModel)
{
if(left.rows == 1)
{
@@ -314,6 +361,15 @@ SensorData::SensorData(
_depthOrRightRaw = right;
}
if(userData.type() == CV_8UC1) // Bytes
{
_userDataCompressed = userData; // assume compressed
}
else
{
_userDataRaw = userData;
}
}
// Stereo constructor + 2d laser scan
@@ -325,12 +381,11 @@ SensorData::SensorData(
const StereoCameraModel & cameraModel,
int id,
double stamp,
const std::vector<unsigned char> & userData) :
const cv::Mat & userData) :
_id(id),
_stamp(stamp),
_laserScanMaxPts(laserScanMaxPts),
_stereoCameraModel(cameraModel),
_userData(userData)
_stereoCameraModel(cameraModel)
{
if(left.rows == 1)
{
@@ -363,18 +418,60 @@ SensorData::SensorData(
UASSERT(laserScan.type() == CV_8UC1); // Bytes
_laserScanCompressed = laserScan;
}
if(userData.type() == CV_8UC1) // Bytes
{
_userDataCompressed = userData; // assume compressed
}
else
{
_userDataRaw = userData;
}
}
void SensorData::setUserDataRaw(const cv::Mat & userDataRaw)
{
if(!_userDataRaw.empty())
{
UWARN("Writing new user data over existing user data. This may result in data loss.");
}
_userDataRaw = userDataRaw;
}
void SensorData::setUserData(const cv::Mat & userData)
{
if(!userData.empty() && (!_userDataCompressed.empty() || !_userDataRaw.empty()))
{
UWARN("Writing new user data over existing user data. This may result in data loss.");
}
_userDataRaw = cv::Mat();
_userDataCompressed = cv::Mat();
if(!userData.empty())
{
if(userData.type() == CV_8UC1) // Bytes
{
_userDataCompressed = userData; // assume compressed
}
else
{
_userDataRaw = userData;
_userDataCompressed = compressData2(userData);
}
}
}
void SensorData::uncompressData()
{
uncompressData(_imageCompressed.empty()?0:&_imageRaw,
_depthOrRightCompressed.empty()?0:&_depthOrRightRaw,
_laserScanCompressed.empty()?0:&_laserScanRaw);
_laserScanCompressed.empty()?0:&_laserScanRaw,
_userDataCompressed.empty()?0:&_userDataRaw);
}
void SensorData::uncompressData(cv::Mat * imageRaw, cv::Mat * depthRaw, cv::Mat * laserScanRaw)
void SensorData::uncompressData(cv::Mat * imageRaw, cv::Mat * depthRaw, cv::Mat * laserScanRaw, cv::Mat * userDataRaw)
{
uncompressDataConst(imageRaw, depthRaw, laserScanRaw);
uncompressDataConst(imageRaw, depthRaw, laserScanRaw, userDataRaw);
if(imageRaw && !imageRaw->empty() && _imageRaw.empty())
{
_imageRaw = *imageRaw;
@@ -387,9 +484,13 @@ void SensorData::uncompressData(cv::Mat * imageRaw, cv::Mat * depthRaw, cv::Mat
{
_laserScanRaw = *laserScanRaw;
}
if(userDataRaw && !userDataRaw->empty() && _userDataRaw.empty())
{
_userDataRaw = *userDataRaw;
}
}
void SensorData::uncompressDataConst(cv::Mat * imageRaw, cv::Mat * depthRaw, cv::Mat * laserScanRaw) const
void SensorData::uncompressDataConst(cv::Mat * imageRaw, cv::Mat * depthRaw, cv::Mat * laserScanRaw, cv::Mat * userDataRaw) const
{
if(imageRaw)
{
@@ -403,13 +504,19 @@ void SensorData::uncompressDataConst(cv::Mat * imageRaw, cv::Mat * depthRaw, cv:
{
*laserScanRaw = _laserScanRaw;
}
if(userDataRaw)
{
*userDataRaw = _userDataRaw;
}
if( (imageRaw && imageRaw->empty()) ||
(depthRaw && depthRaw->empty()) ||
(laserScanRaw && laserScanRaw->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();
@@ -422,9 +529,14 @@ void SensorData::uncompressDataConst(cv::Mat * imageRaw, cv::Mat * depthRaw, cv:
{
ctLaserScan.start();
}
if(userDataRaw && userDataRaw->empty())
{
ctUserData.start();
}
ctImage.join();
ctDepth.join();
ctLaserScan.join();
ctUserData.join();
if(imageRaw && imageRaw->empty())
{
*imageRaw = ctImage.getUncompressedData();
@@ -450,6 +562,15 @@ void SensorData::uncompressDataConst(cv::Mat * imageRaw, cv::Mat * depthRaw, cv:
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());
}
}
}
}