mirror of
https://github.com/introlab/rtabmap_ros.git
synced 2026-10-05 01:07:49 +08:00
Added multi-camera feature
This commit is contained in:
Mathieu Labbe
+391
-112
@@ -27,138 +27,417 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
|
||||
|
||||
#include "rtabmap/core/SensorData.h"
|
||||
#include "rtabmap/core/Compression.h"
|
||||
#include "rtabmap/utilite/ULogger.h"
|
||||
#include <rtabmap/utilite/UMath.h>
|
||||
|
||||
namespace rtabmap
|
||||
{
|
||||
|
||||
/**
|
||||
* An id is automatically generated if id=0.
|
||||
*/
|
||||
// empty constructor
|
||||
SensorData::SensorData() :
|
||||
_id(0),
|
||||
_stamp(0.0),
|
||||
_fx(0.0f),
|
||||
_fyOrBaseline(0.0f),
|
||||
_cx(0.0f),
|
||||
_cy(0.0f),
|
||||
_localTransform(Transform::getIdentity()),
|
||||
_poseRotVariance(1.0f),
|
||||
_poseTransVariance(1.0f),
|
||||
_laserScanMaxPts(0)
|
||||
_id(0),
|
||||
_stamp(0.0),
|
||||
_laserScanMaxPts(0)
|
||||
{
|
||||
}
|
||||
|
||||
SensorData::SensorData(const cv::Mat & image,
|
||||
int id,
|
||||
double stamp,
|
||||
const std::vector<unsigned char> & userData) :
|
||||
_image(image),
|
||||
_id(id),
|
||||
_stamp(stamp),
|
||||
_fx(0.0f),
|
||||
_fyOrBaseline(0.0f),
|
||||
_cx(0.0f),
|
||||
_cy(0.0f),
|
||||
_localTransform(Transform::getIdentity()),
|
||||
_poseRotVariance(1.0f),
|
||||
_poseTransVariance(1.0f),
|
||||
_laserScanMaxPts(0),
|
||||
_userData(userData)
|
||||
// Appearance-only constructor
|
||||
SensorData::SensorData(
|
||||
const cv::Mat & image,
|
||||
int id,
|
||||
double stamp,
|
||||
const std::vector<unsigned char> & userData) :
|
||||
_id(id),
|
||||
_stamp(stamp),
|
||||
_laserScanMaxPts(0),
|
||||
_userData(userData)
|
||||
{
|
||||
UASSERT(image.empty() ||
|
||||
image.type() == CV_8UC1 || // Mono
|
||||
image.type() == CV_8UC3); // RGB
|
||||
if(image.rows == 1)
|
||||
{
|
||||
UASSERT(image.type() == CV_8UC1); // Bytes
|
||||
_imageCompressed = image;
|
||||
}
|
||||
else if(!image.empty())
|
||||
{
|
||||
UASSERT(image.type() == CV_8UC1 || // Mono
|
||||
image.type() == CV_8UC3); // RGB
|
||||
_imageRaw = image;
|
||||
}
|
||||
}
|
||||
|
||||
// Metric constructor
|
||||
SensorData::SensorData(const cv::Mat & image,
|
||||
const cv::Mat & depthOrRightImage,
|
||||
float fx,
|
||||
float fyOrBaseline,
|
||||
float cx,
|
||||
float cy,
|
||||
const Transform & localTransform,
|
||||
const Transform & pose,
|
||||
float poseRotVariance,
|
||||
float poseTransVariance,
|
||||
int id,
|
||||
double stamp,
|
||||
const std::vector<unsigned char> & userData) :
|
||||
_image(image),
|
||||
_id(id),
|
||||
_stamp(stamp),
|
||||
_depthOrRightImage(depthOrRightImage),
|
||||
_fx(fx),
|
||||
_fyOrBaseline(fyOrBaseline),
|
||||
_cx(cx),
|
||||
_cy(cy),
|
||||
_pose(pose),
|
||||
_localTransform(localTransform),
|
||||
_poseRotVariance(poseRotVariance),
|
||||
_poseTransVariance(poseTransVariance),
|
||||
_laserScanMaxPts(0),
|
||||
_userData(userData)
|
||||
// Mono constructor
|
||||
SensorData::SensorData(
|
||||
const cv::Mat & image,
|
||||
const CameraModel & cameraModel,
|
||||
int id,
|
||||
double stamp,
|
||||
const std::vector<unsigned char> & userData) :
|
||||
_id(id),
|
||||
_stamp(stamp),
|
||||
_laserScanMaxPts(0),
|
||||
_cameraModels(std::vector<CameraModel>(1, cameraModel)),
|
||||
_userData(userData)
|
||||
{
|
||||
UASSERT(image.empty() ||
|
||||
image.type() == CV_8UC1 || // Mono
|
||||
image.type() == CV_8UC3); // RGB
|
||||
UASSERT(depthOrRightImage.empty() ||
|
||||
depthOrRightImage.type() == CV_32FC1 || // Depth in meter
|
||||
depthOrRightImage.type() == CV_16UC1 || // Depth in millimetre
|
||||
depthOrRightImage.type() == CV_8U); // Right stereo image
|
||||
UASSERT(!_localTransform.isNull());
|
||||
UASSERT_MSG(uIsFinite(_poseRotVariance) && _poseRotVariance>0 && uIsFinite(_poseTransVariance) && _poseTransVariance>0, "Rotational and transitional variances should not be null! (set to 1 if unknown)");
|
||||
if(image.rows == 1)
|
||||
{
|
||||
UASSERT(image.type() == CV_8UC1); // Bytes
|
||||
_imageCompressed = image;
|
||||
}
|
||||
else if(!image.empty())
|
||||
{
|
||||
UASSERT(image.type() == CV_8UC1 || // Mono
|
||||
image.type() == CV_8UC3); // RGB
|
||||
_imageRaw = image;
|
||||
}
|
||||
}
|
||||
|
||||
// Metric constructor + 2d depth
|
||||
SensorData::SensorData(const cv::Mat & laserScan,
|
||||
int laserScanMaxPts,
|
||||
const cv::Mat & image,
|
||||
const cv::Mat & depthOrRightImage,
|
||||
float fx,
|
||||
float fyOrBaseline,
|
||||
float cx,
|
||||
float cy,
|
||||
const Transform & localTransform,
|
||||
const Transform & pose,
|
||||
float poseRotVariance,
|
||||
float poseTransVariance,
|
||||
int id,
|
||||
double stamp,
|
||||
const std::vector<unsigned char> & userData) :
|
||||
_image(image),
|
||||
_id(id),
|
||||
_stamp(stamp),
|
||||
_depthOrRightImage(depthOrRightImage),
|
||||
_laserScan(laserScan),
|
||||
_fx(fx),
|
||||
_fyOrBaseline(fyOrBaseline),
|
||||
_cx(cx),
|
||||
_cy(cy),
|
||||
_pose(pose),
|
||||
_localTransform(localTransform),
|
||||
_poseRotVariance(poseRotVariance),
|
||||
_poseTransVariance(poseTransVariance),
|
||||
_laserScanMaxPts(laserScanMaxPts),
|
||||
_userData(userData)
|
||||
// RGB-D constructor
|
||||
SensorData::SensorData(
|
||||
const cv::Mat & rgb,
|
||||
const cv::Mat & depth,
|
||||
const CameraModel & cameraModel,
|
||||
int id,
|
||||
double stamp,
|
||||
const std::vector<unsigned char> & userData) :
|
||||
_id(id),
|
||||
_stamp(stamp),
|
||||
_laserScanMaxPts(0),
|
||||
_cameraModels(std::vector<CameraModel>(1, cameraModel)),
|
||||
_userData(userData)
|
||||
{
|
||||
UASSERT(_laserScan.empty() || _laserScan.type() == CV_32FC2);
|
||||
UASSERT(image.empty() ||
|
||||
image.type() == CV_8UC1 || // Mono
|
||||
image.type() == CV_8UC3); // RGB
|
||||
UASSERT(depthOrRightImage.empty() ||
|
||||
depthOrRightImage.type() == CV_32FC1 || // Depth in meter
|
||||
depthOrRightImage.type() == CV_16UC1 || // Depth in millimetre
|
||||
depthOrRightImage.type() == CV_8U); // Right stereo image
|
||||
UASSERT(!_localTransform.isNull());
|
||||
UASSERT_MSG(uIsFinite(_poseRotVariance) && _poseRotVariance>0 && uIsFinite(_poseTransVariance) && _poseTransVariance>0, "Rotational and transitional variances should not be null! (set to 1 if unknown)");
|
||||
if(rgb.rows == 1)
|
||||
{
|
||||
UASSERT(rgb.type() == CV_8UC1); // Bytes
|
||||
_imageCompressed = rgb;
|
||||
}
|
||||
else if(!rgb.empty())
|
||||
{
|
||||
UASSERT(rgb.type() == CV_8UC1 || // Mono
|
||||
rgb.type() == CV_8UC3); // RGB
|
||||
_imageRaw = rgb;
|
||||
}
|
||||
|
||||
if(depth.rows == 1)
|
||||
{
|
||||
UASSERT(depth.type() == CV_8UC1); // Bytes
|
||||
_depthOrRightCompressed = depth;
|
||||
}
|
||||
else if(!depth.empty())
|
||||
{
|
||||
UASSERT(depth.type() == CV_32FC1 || // Depth in meter
|
||||
depth.type() == CV_16UC1); // Depth in millimetre
|
||||
_depthOrRightRaw = depth;
|
||||
}
|
||||
}
|
||||
|
||||
bool SensorData::empty() const
|
||||
// RGB-D constructor + 2d laser scan
|
||||
SensorData::SensorData(
|
||||
const cv::Mat & laserScan,
|
||||
int laserScanMaxPts,
|
||||
const cv::Mat & rgb,
|
||||
const cv::Mat & depth,
|
||||
const CameraModel & cameraModel,
|
||||
int id,
|
||||
double stamp,
|
||||
const std::vector<unsigned char> & userData) :
|
||||
_id(id),
|
||||
_stamp(stamp),
|
||||
_laserScanMaxPts(laserScanMaxPts),
|
||||
_cameraModels(std::vector<CameraModel>(1, cameraModel)),
|
||||
_userData(userData)
|
||||
{
|
||||
return _image.empty();
|
||||
if(rgb.rows == 1)
|
||||
{
|
||||
UASSERT(rgb.type() == CV_8UC1); // Bytes
|
||||
_imageCompressed = rgb;
|
||||
}
|
||||
else if(!rgb.empty())
|
||||
{
|
||||
UASSERT(rgb.type() == CV_8UC1 || // Mono
|
||||
rgb.type() == CV_8UC3); // RGB
|
||||
_imageRaw = rgb;
|
||||
}
|
||||
if(depth.rows == 1)
|
||||
{
|
||||
UASSERT(depth.type() == CV_8UC1); // Bytes
|
||||
_depthOrRightCompressed = depth;
|
||||
}
|
||||
else if(!depth.empty())
|
||||
{
|
||||
UASSERT(depth.type() == CV_32FC1 || // Depth in meter
|
||||
depth.type() == CV_16UC1); // Depth in millimetre
|
||||
_depthOrRightRaw = depth;
|
||||
}
|
||||
if(laserScan.rows == 1)
|
||||
{
|
||||
UASSERT(laserScan.type() == CV_8UC1); // Bytes
|
||||
_laserScanCompressed = laserScan;
|
||||
}
|
||||
else if(!laserScan.empty())
|
||||
{
|
||||
UASSERT(laserScan.type() == CV_32FC2);
|
||||
_laserScanRaw = laserScan;
|
||||
}
|
||||
}
|
||||
|
||||
// Multi-cameras RGB-D constructor
|
||||
SensorData::SensorData(
|
||||
const cv::Mat & rgb,
|
||||
const cv::Mat & depth,
|
||||
const std::vector<CameraModel> & cameraModels,
|
||||
int id,
|
||||
double stamp,
|
||||
const std::vector<unsigned char> & userData) :
|
||||
_id(id),
|
||||
_stamp(stamp),
|
||||
_laserScanMaxPts(0),
|
||||
_cameraModels(cameraModels),
|
||||
_userData(userData)
|
||||
{
|
||||
if(rgb.rows == 1)
|
||||
{
|
||||
UASSERT(rgb.type() == CV_8UC1); // Bytes
|
||||
_imageCompressed = rgb;
|
||||
}
|
||||
else if(!rgb.empty())
|
||||
{
|
||||
UASSERT(rgb.type() == CV_8UC1 || // Mono
|
||||
rgb.type() == CV_8UC3); // RGB
|
||||
_imageRaw = rgb;
|
||||
}
|
||||
if(depth.rows == 1)
|
||||
{
|
||||
UASSERT(depth.type() == CV_8UC1); // Bytes
|
||||
_depthOrRightCompressed = depth;
|
||||
}
|
||||
else if(!depth.empty())
|
||||
{
|
||||
UASSERT(depth.type() == CV_32FC1 || // Depth in meter
|
||||
depth.type() == CV_16UC1); // Depth in millimetre
|
||||
_depthOrRightRaw = depth;
|
||||
}
|
||||
for(unsigned int i=0; i<cameraModels.size(); ++i)
|
||||
{
|
||||
UASSERT(cameraModels[i].isValid());
|
||||
}
|
||||
}
|
||||
|
||||
// Multi-cameras RGB-D constructor + 2d laser scan
|
||||
SensorData::SensorData(
|
||||
const cv::Mat & laserScan,
|
||||
int laserScanMaxPts,
|
||||
const cv::Mat & rgb,
|
||||
const cv::Mat & depth,
|
||||
const std::vector<CameraModel> & cameraModels,
|
||||
int id,
|
||||
double stamp,
|
||||
const std::vector<unsigned char> & userData) :
|
||||
_id(id),
|
||||
_stamp(stamp),
|
||||
_laserScanMaxPts(laserScanMaxPts),
|
||||
_cameraModels(cameraModels),
|
||||
_userData(userData)
|
||||
{
|
||||
if(rgb.rows == 1)
|
||||
{
|
||||
UASSERT(rgb.type() == CV_8UC1); // Bytes
|
||||
_imageCompressed = rgb;
|
||||
}
|
||||
else if(!rgb.empty())
|
||||
{
|
||||
UASSERT(rgb.type() == CV_8UC1 || // Mono
|
||||
rgb.type() == CV_8UC3); // RGB
|
||||
_imageRaw = rgb;
|
||||
}
|
||||
if(depth.rows == 1)
|
||||
{
|
||||
UASSERT(depth.type() == CV_8UC1); // Bytes
|
||||
_depthOrRightCompressed = depth;
|
||||
}
|
||||
else if(!depth.empty())
|
||||
{
|
||||
UASSERT(depth.type() == CV_32FC1 || // Depth in meter
|
||||
depth.type() == CV_16UC1); // Depth in millimetre
|
||||
_depthOrRightRaw = depth;
|
||||
}
|
||||
|
||||
if(laserScan.rows == 1)
|
||||
{
|
||||
UASSERT(laserScan.type() == CV_8UC1); // Bytes
|
||||
_laserScanCompressed = laserScan;
|
||||
}
|
||||
else if(!laserScan.empty())
|
||||
{
|
||||
UASSERT(laserScan.type() == CV_32FC2);
|
||||
_laserScanRaw = laserScan;
|
||||
}
|
||||
|
||||
for(unsigned int i=0; i<cameraModels.size(); ++i)
|
||||
{
|
||||
UASSERT(cameraModels[i].isValid());
|
||||
}
|
||||
}
|
||||
|
||||
// Stereo constructor
|
||||
SensorData::SensorData(
|
||||
const cv::Mat & left,
|
||||
const cv::Mat & right,
|
||||
const StereoCameraModel & cameraModel,
|
||||
int id,
|
||||
double stamp,
|
||||
const std::vector<unsigned char> & userData):
|
||||
_id(id),
|
||||
_stamp(stamp),
|
||||
_laserScanMaxPts(0),
|
||||
_stereoCameraModel(cameraModel),
|
||||
_userData(userData)
|
||||
{
|
||||
if(left.rows == 1)
|
||||
{
|
||||
UASSERT(left.type() == CV_8UC1); // Bytes
|
||||
_imageCompressed = left;
|
||||
}
|
||||
else if(!left.empty())
|
||||
{
|
||||
UASSERT(left.type() == CV_8UC1 || // Mono
|
||||
left.type() == CV_8UC3); // RGB
|
||||
_imageRaw = left;
|
||||
}
|
||||
if(right.rows == 1)
|
||||
{
|
||||
UASSERT(right.type() == CV_8UC1); // Bytes
|
||||
_depthOrRightCompressed = right;
|
||||
}
|
||||
else if(!right.empty())
|
||||
{
|
||||
UASSERT(right.type() == CV_8UC1); // Mono
|
||||
_depthOrRightRaw = right;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
// Stereo constructor + 2d laser scan
|
||||
SensorData::SensorData(
|
||||
const cv::Mat & laserScan,
|
||||
int laserScanMaxPts,
|
||||
const cv::Mat & left,
|
||||
const cv::Mat & right,
|
||||
const StereoCameraModel & cameraModel,
|
||||
int id,
|
||||
double stamp,
|
||||
const std::vector<unsigned char> & userData) :
|
||||
_id(id),
|
||||
_stamp(stamp),
|
||||
_laserScanMaxPts(laserScanMaxPts),
|
||||
_stereoCameraModel(cameraModel),
|
||||
_userData(userData)
|
||||
{
|
||||
if(left.rows == 1)
|
||||
{
|
||||
UASSERT(left.type() == CV_8UC1); // Bytes
|
||||
_imageCompressed = left;
|
||||
}
|
||||
else if(!left.empty())
|
||||
{
|
||||
UASSERT(left.type() == CV_8UC1 || // Mono
|
||||
left.type() == CV_8UC3); // RGB
|
||||
_imageRaw = left;
|
||||
}
|
||||
if(right.rows == 1)
|
||||
{
|
||||
UASSERT(right.type() == CV_8UC1); // Bytes
|
||||
_depthOrRightCompressed = right;
|
||||
}
|
||||
else if(!right.empty())
|
||||
{
|
||||
UASSERT(right.type() == CV_8UC1); // Mono
|
||||
_depthOrRightRaw = right;
|
||||
}
|
||||
|
||||
if(laserScan.rows == 1)
|
||||
{
|
||||
UASSERT(laserScan.type() == CV_8UC1); // Bytes
|
||||
_laserScanCompressed = laserScan;
|
||||
}
|
||||
else if(!laserScan.empty())
|
||||
{
|
||||
UASSERT(laserScan.type() == CV_32FC2);
|
||||
_laserScanRaw = laserScan;
|
||||
}
|
||||
}
|
||||
|
||||
void SensorData::uncompressData()
|
||||
{
|
||||
uncompressData(&_imageRaw, &_depthOrRightRaw, &_laserScanRaw);
|
||||
}
|
||||
|
||||
void SensorData::uncompressData(cv::Mat * imageRaw, cv::Mat * depthRaw, cv::Mat * laserScanRaw)
|
||||
{
|
||||
uncompressDataConst(imageRaw, depthRaw, laserScanRaw);
|
||||
if(imageRaw && !imageRaw->empty() && _imageRaw.empty())
|
||||
{
|
||||
_imageRaw = *imageRaw;
|
||||
}
|
||||
if(depthRaw && !depthRaw->empty() && _depthOrRightRaw.empty())
|
||||
{
|
||||
_depthOrRightRaw = *depthRaw;
|
||||
}
|
||||
if(laserScanRaw && !laserScanRaw->empty() && _laserScanRaw.empty())
|
||||
{
|
||||
_laserScanRaw = *laserScanRaw;
|
||||
}
|
||||
}
|
||||
|
||||
void SensorData::uncompressDataConst(cv::Mat * imageRaw, cv::Mat * depthRaw, cv::Mat * laserScanRaw) const
|
||||
{
|
||||
if(imageRaw)
|
||||
{
|
||||
*imageRaw = _imageRaw;
|
||||
}
|
||||
if(depthRaw)
|
||||
{
|
||||
*depthRaw = _depthOrRightRaw;
|
||||
}
|
||||
if(laserScanRaw)
|
||||
{
|
||||
*laserScanRaw = _laserScanRaw;
|
||||
}
|
||||
if( (imageRaw && imageRaw->empty()) ||
|
||||
(depthRaw && depthRaw->empty()) ||
|
||||
(laserScanRaw && laserScanRaw->empty()))
|
||||
{
|
||||
rtabmap::CompressionThread ctImage(_imageCompressed, true);
|
||||
rtabmap::CompressionThread ctDepth(_depthOrRightCompressed, true);
|
||||
rtabmap::CompressionThread ctLaserScan(_laserScanCompressed, false);
|
||||
if(imageRaw && imageRaw->empty())
|
||||
{
|
||||
ctImage.start();
|
||||
}
|
||||
if(depthRaw && depthRaw->empty())
|
||||
{
|
||||
ctDepth.start();
|
||||
}
|
||||
if(laserScanRaw && laserScanRaw->empty())
|
||||
{
|
||||
ctLaserScan.start();
|
||||
}
|
||||
ctImage.join();
|
||||
ctDepth.join();
|
||||
ctLaserScan.join();
|
||||
if(imageRaw && imageRaw->empty())
|
||||
{
|
||||
*imageRaw = ctImage.getUncompressedData();
|
||||
}
|
||||
if(depthRaw && depthRaw->empty())
|
||||
{
|
||||
*depthRaw = ctDepth.getUncompressedData();
|
||||
}
|
||||
if(laserScanRaw && laserScanRaw->empty())
|
||||
{
|
||||
*laserScanRaw = ctLaserScan.getUncompressedData();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
} // namespace rtabmap
|
||||
|
||||
Reference in New Issue
Block a user