i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-02 09:30:25 +08:00
Code Issues Packages Projects Releases Wiki Activity

Added multi-camera feature

Browse Source
This commit is contained in:
Mathieu Labbe
2015-05-29 14:46:48 -04:00
parent e6923daf1c
commit c6d0d47b1c
51 changed files with 2833 additions and 2297 deletions
Download Patch File Download Diff File Expand all files Collapse all files

152
corelib/src/Signature.cpp
View File

@@ -44,12 +44,7 @@ Signature::Signature() :
_saved(false),
_modified(true),
_linksModified(true),
_enabled(false),
_fx(0.0f),
_fy(0.0f),
_cx(0.0f),
_cy(0.0f),
_laserScanMaxPts(0)
_enabled(false)
{
}
@@ -63,15 +58,7 @@ Signature::Signature(
const std::multimap<int, pcl::PointXYZ> & words3, // in base_link frame (localTransform applied)
const Transform & pose,
const std::vector<unsigned char> & userData,
const cv::Mat & laserScanCompressed, // in base_link frame
const cv::Mat & imageCompressed, // in camera_link frame
const cv::Mat & depthCompressed, // in camera_link frame
float fx,
float fy,
float cx,
float cy,
const Transform & localTransform,
int laserScanMaxPts) :
const SensorData & sensorData) :
_id(id),
_mapId(mapId),
_stamp(stamp),
@@ -82,18 +69,10 @@ Signature::Signature(
_modified(true),
_linksModified(true),
_words(words),
_enabled(false),
_imageCompressed(imageCompressed),
_depthCompressed(depthCompressed),
_laserScanCompressed(laserScanCompressed),
_fx(fx),
_fy(fy),
_cx(cx),
_cy(cy),
_pose(pose),
_localTransform(localTransform),
_words3(words3),
_laserScanMaxPts(laserScanMaxPts)
_enabled(false),
_pose(pose),
_sensorData(sensorData)
{
}
@@ -239,25 +218,9 @@ void Signature::removeWord(int wordId)
_words3.erase(wordId);
}
void Signature::setDepthCompressed(const cv::Mat & bytes, float fx, float fy, float cx, float cy)
cv::Mat Signature::getPoseCovariance() const
{
UASSERT_MSG(bytes.empty() || (!bytes.empty() && fx > 0.0f && fy > 0.0f && cx >= 0.0f && cy >= 0.0f), uFormat("fx=%f fy=%f cx=%f cy=%f",fx,fy,cx,cy).c_str());
_depthCompressed = bytes;
_fx=fx;
_fy=fy;
_cx=cx;
_cy=cy;
}
float Signature::getDepthFx() const {return getFx();}
float Signature::getDepthFy() const {return getFy();}
float Signature::getDepthCx() const {return getCx();}
float Signature::getDepthCy() const {return getCy();}
void Signature::getPoseVariance(float & rotVariance, float & transVariance) const
{
rotVariance = 1.0f;
transVariance = 1.0f;
cv::Mat covariance = cv::Mat::eye(6,6,CV_64FC1);
if(_links.size())
{
for(std::map<int, Link>::const_iterator iter = _links.begin(); iter!=_links.end(); ++iter)
@@ -267,110 +230,13 @@ void Signature::getPoseVariance(float & rotVariance, float & transVariance) cons
//Assume the first neighbor to be the backward neighbor link
if(iter->second.to() < iter->second.from())
{
rotVariance = iter->second.rotVariance();
transVariance = iter->second.transVariance();
covariance = iter->second.infMatrix().inv();
break;
}
}
}
}
}
SensorData Signature::toSensorData()
{
this->uncompressData();
float rotVariance = 1.0f;
float transVariance = 1.0f;
this->getPoseVariance(rotVariance, transVariance);
return SensorData(_laserScanRaw,
_laserScanMaxPts,
_imageRaw,
_depthRaw,
_fx,
_fy,
_cx,
_cy,
_localTransform,
_pose,
rotVariance,
transVariance,
_id,
_stamp,
_userData);
}
void Signature::uncompressData()
{
uncompressData(&_imageRaw, &_depthRaw, &_laserScanRaw);
}
void Signature::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() && _depthRaw.empty())
{
_depthRaw = *depthRaw;
}
if(laserScanRaw && !laserScanRaw->empty() && _laserScanRaw.empty())
{
_laserScanRaw = *laserScanRaw;
}
}
void Signature::uncompressDataConst(cv::Mat * imageRaw, cv::Mat * depthRaw, cv::Mat * laserScanRaw) const
{
if(imageRaw)
{
*imageRaw = _imageRaw;
}
if(depthRaw)
{
*depthRaw = _depthRaw;
}
if(laserScanRaw)
{
*laserScanRaw = _laserScanRaw;
}
if( (imageRaw && imageRaw->empty()) ||
(depthRaw && depthRaw->empty()) ||
(laserScanRaw && laserScanRaw->empty()))
{
rtabmap::CompressionThread ctImage(_imageCompressed, true);
rtabmap::CompressionThread ctDepth(_depthCompressed, 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();
}
}
return covariance;
}
} //namespace rtabmap