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

Added image size to all camera calibration inputs. Memory::computeTransform() more fine tuning to know if data should be loaded from database.

Browse Source
This commit is contained in:
matlabbe
2016-03-10 09:07:44 -05:00
parent 609cc3dc59
commit cee9c65b06
10 changed files with 312 additions and 261 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
corelib/include/rtabmap/core/Memory.h
View File

@@ -111,7 +111,7 @@ public:
int maxGraphDepth) const;
void deleteLocation(int locationId, std::list<int> * deletedWords = 0);
void removeLink(int idA, int idB);
void removeRawData(int id);
void removeRawData(int id, bool image = true, bool scan = true, bool userData = true);
//getters
const std::map<int, double> & getWorkingMem() const {return _workingMem;}

3
corelib/include/rtabmap/core/StereoCameraModel.h
View File

@@ -90,6 +90,9 @@ public:
void setName(const std::string & name);
const std::string & name() const {return name_;}
// backward compatibility
void setImageSize(const cv::Size & size) {left_.setImageSize(size); right_.setImageSize(size);}
bool load(const std::string & directory, const std::string & cameraName, bool ignoreStereoTransform = true);
bool save(const std::string & directory, bool ignoreStereoTransform = true) const;

10
corelib/src/CameraRGB.cpp
View File

@@ -672,6 +672,11 @@ SensorData CameraImages::captureImage()
UWARN("Directory is not set, camera must be initialized.");
}
if(_model.imageHeight() == 0 || _model.imageWidth() == 0)
{
_model.setImageSize(img.size());
}
SensorData data(scan, scan.empty()?0:_scanMaxPts, 0, _isDepth?cv::Mat():img, _isDepth?img:depthFromScan, _model, this->getNextSeqID(), stamp);
data.setGroundTruth(groundTruthPose);
return data;
@@ -792,6 +797,11 @@ SensorData CameraVideo::captureImage()
{
if(_capture.read(img))
{
if(_model.imageHeight() == 0 || _model.imageWidth() == 0)
{
_model.setImageSize(img.size());
}
if(_model.isValidForRectification() && (_src != kVideoFile || _rectifyImages))
{
img = _model.rectifyImage(img);

20
corelib/src/CameraRGBD.cpp
View File

@@ -223,7 +223,9 @@ SensorData CameraOpenni::captureImage()
1.0f/depthConstant_, //fy
float(rgb_.cols/2) - 0.5f, //cx
float(rgb_.rows/2) - 0.5f, //cy
this->getLocalTransform());
this->getLocalTransform(),
0,
rgb_.size());
data = SensorData(rgb_, depth_, model, this->getNextSeqID(), UTimer::now());
}
@@ -336,7 +338,9 @@ SensorData CameraOpenNICV::captureImage()
_depthFocal, //fy
float(rgb.cols/2) - 0.5f, //cx
float(rgb.rows/2) - 0.5f, //cy
this->getLocalTransform());
this->getLocalTransform(),
0,
rgb.size());
data = SensorData(rgb, depth, model, this->getNextSeqID(), UTimer::now());
}
}
@@ -710,7 +714,9 @@ SensorData CameraOpenNI2::captureImage()
_depthFy, //fy
float(rgb.cols/2) - 0.5f, //cx
float(rgb.rows/2) - 0.5f, //cy
this->getLocalTransform());
this->getLocalTransform(),
0,
rgb.size());
if(_openNI2StampsAndIDsUsed)
{
data = SensorData(rgb, depth, model, depthFrame.getFrameIndex(), double(depthFrame.getTimestamp()) / 1000000.0);
@@ -1047,7 +1053,9 @@ SensorData CameraFreenect::captureImage()
freenectDevice_->getDepthFocal(), //fy
float(rgb.cols/2) - 0.5f, //cx
float(rgb.rows/2) - 0.5f, //cy
this->getLocalTransform());
this->getLocalTransform(),
0,
rgb.size());
data = SensorData(rgb, depth, model, this->getNextSeqID(), UTimer::now());
}
}
@@ -1643,7 +1651,9 @@ SensorData CameraFreenect2::captureImage()
fy, //fy
cx, //cx
cy, // cy
this->getLocalTransform());
this->getLocalTransform(),
0,
rgb.size());
}
data = SensorData(rgb, depth, model, this->getNextSeqID(), stamp);

17
corelib/src/CameraStereo.cpp
View File

@@ -440,7 +440,8 @@ SensorData CameraStereoDC1394::captureImage()
stereoModel_.left().cx(), //cx
stereoModel_.left().cy(), //cy
stereoModel_.baseline(),
this->getLocalTransform());
this->getLocalTransform(),
left.size());
}
data = SensorData(left, right, model, this->getNextSeqID(), UTimer::now());
}
@@ -715,7 +716,8 @@ SensorData CameraStereoFlyCapture2::captureImage()
cx,
cy,
baseline,
this->getLocalTransform());
this->getLocalTransform(),
left.size());
data = SensorData(left, right, model, this->getNextSeqID(), UTimer::now());
}
}
@@ -892,6 +894,11 @@ SensorData CameraStereoImages::captureImage()
rightImage = stereoModel_.right().rectifyImage(rightImage);
}
if(stereoModel_.left().imageHeight() == 0 || stereoModel_.left().imageWidth() == 0)
{
stereoModel_.setImageSize(leftImage.size());
}
data = SensorData(left.laserScanRaw(), left.laserScanMaxPts(), 0, leftImage, rightImage, stereoModel_, left.id()/(camera2_?1:2), left.stamp());
data.setGroundTruth(left.groundTruth());
}
@@ -1013,6 +1020,12 @@ SensorData CameraStereoVideo::captureImage()
rightImage = rightImage.clone();
}
}
if(stereoModel_.left().imageHeight() == 0 || stereoModel_.left().imageWidth() == 0)
{
stereoModel_.setImageSize(leftImage.size());
}
data = SensorData(leftImage, rightImage, stereoModel_, this->getNextSeqID(), UTimer::now());
}
}

62
corelib/src/Memory.cpp
View File

@@ -2031,15 +2031,24 @@ void Memory::removeLink(int oldId, int newId)
}
}
void Memory::removeRawData(int id)
void Memory::removeRawData(int id, bool image, bool scan, bool userData)
{
Signature * s = this->_getSignature(id);
if(s)
{
s->sensorData().setImageRaw(cv::Mat());
s->sensorData().setDepthOrRightRaw(cv::Mat());
s->sensorData().setLaserScanRaw(cv::Mat(), s->sensorData().laserScanMaxPts(), s->sensorData().laserScanMaxRange());
s->sensorData().setUserDataRaw(cv::Mat());
if(image && (!_reextractLoopClosureFeatures || !_registrationPipeline->isImageRequired()))
{
s->sensorData().setImageRaw(cv::Mat());
s->sensorData().setDepthOrRightRaw(cv::Mat());
}
if(scan && !_registrationPipeline->isScanRequired())
{
s->sensorData().setLaserScanRaw(cv::Mat(), s->sensorData().laserScanMaxPts(), s->sensorData().laserScanMaxRange());
}
if(userData && !_registrationPipeline->isUserDataRequired())
{
s->sensorData().setUserDataRaw(cv::Mat());
}
}
}
@@ -2050,21 +2059,40 @@ Transform Memory::computeTransform(
Transform guess,
RegistrationInfo * info)
{
const Signature * fromS = this->getSignature(fromId);
const Signature * toS = this->getSignature(toId);
Signature * fromS = this->_getSignature(fromId);
Signature * toS = this->_getSignature(toId);
Transform transform;
if(fromS && toS)
{
// make sure we have all data needed
if((_reextractLoopClosureFeatures && _registrationPipeline->isImageRequired()) ||
(_registrationPipeline->isScanRequired()) ||
_registrationPipeline->isUserDataRequired())
// load binary data from database if not in RAM (if image is already here, scan and userData should be or they are null)
if((_reextractLoopClosureFeatures && _registrationPipeline->isImageRequired() && fromS->sensorData().imageCompressed().empty()) ||
(_registrationPipeline->isScanRequired() && fromS->sensorData().imageCompressed().empty() && fromS->sensorData().laserScanCompressed().empty()) ||
(_registrationPipeline->isUserDataRequired() && fromS->sensorData().imageCompressed().empty() && fromS->sensorData().userDataCompressed().empty()))
{
getNodeData(fromS->id(), true);
getNodeData(toS->id(), true);
getNodeData(fromS->id());
}
if((_reextractLoopClosureFeatures && _registrationPipeline->isImageRequired() && toS->sensorData().imageCompressed().empty()) ||
(_registrationPipeline->isScanRequired() && toS->sensorData().imageCompressed().empty() && toS->sensorData().laserScanCompressed().empty()) ||
(_registrationPipeline->isUserDataRequired() && toS->sensorData().imageCompressed().empty() && toS->sensorData().userDataCompressed().empty()))
{
getNodeData(toS->id());
}
// uncompress only what we need
cv::Mat imgBuf, depthBuf, laserBuf, userBuf;
fromS->sensorData().uncompressData(
(_reextractLoopClosureFeatures && _registrationPipeline->isImageRequired())?&imgBuf:0,
(_reextractLoopClosureFeatures && _registrationPipeline->isImageRequired())?&depthBuf:0,
_registrationPipeline->isScanRequired()?&laserBuf:0,
_registrationPipeline->isUserDataRequired()?&userBuf:0);
toS->sensorData().uncompressData(
(_reextractLoopClosureFeatures && _registrationPipeline->isImageRequired())?&imgBuf:0,
(_reextractLoopClosureFeatures && _registrationPipeline->isImageRequired())?&depthBuf:0,
_registrationPipeline->isScanRequired()?&laserBuf:0,
_registrationPipeline->isUserDataRequired()?&userBuf:0);
// compute transform fromId -> toId
std::vector<int> inliersV;
@@ -2171,13 +2199,13 @@ Transform Memory::computeIcpTransform(
if(fromS && toS && _dbDriver)
{
std::list<Signature*> depthsToLoad;
//Depth required, if not in RAM, load it from LTM
if(fromS->sensorData().depthOrRightCompressed().empty() &&
//if image is already here, scan should be or it is null
if(fromS->sensorData().imageCompressed().empty() &&
fromS->sensorData().laserScanCompressed().empty())
{
depthsToLoad.push_back(fromS);
}
if(toS->sensorData().depthOrRightCompressed().empty() &&
if(toS->sensorData().imageCompressed().empty() &&
toS->sensorData().laserScanCompressed().empty())
{
depthsToLoad.push_back(toS);
@@ -2231,7 +2259,9 @@ Transform Memory::computeIcpTransformMulti(
{
Signature * s = _getSignature(iter->first);
UASSERT(s != 0);
if(s->sensorData().laserScanCompressed().empty())
//if image is already here, scan should be or it is null
if(s->sensorData().imageCompressed().empty() &&
s->sensorData().laserScanCompressed().empty())
{
depthToLoad.push_back(s);
}

2
corelib/src/Rtabmap.cpp
View File

@@ -2315,7 +2315,7 @@ bool Rtabmap::process(
}
if(!_rawDataKept)
{
_memory->removeRawData(signature->id());
_memory->removeRawData(signature->id(), true, !_neighborLinkRefining && !_proximityBySpace, true);
}
// remove last signature if the memory is not incremental or is a bad signature (if bad signatures are ignored)