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

Backward compatibility (DBViewer, DBReader) with old database format not saving CameraModel's image size, cx and cy

Browse Source
This commit is contained in:
matlabbe
2016-12-02 13:43:44 -05:00
parent a5f4468c54
commit d2287ab9f4
6 changed files with 131 additions and 88 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -107,7 +107,7 @@ public:
void setLocalTransform(const Transform & transform) {localTransform_ = transform;}
const Transform & localTransform() const {return localTransform_;}
void setImageSize(const cv::Size & size) {imageSize_ = size;}
void setImageSize(const cv::Size & size);
const cv::Size & imageSize() const {return imageSize_;}
int imageWidth() const {return imageSize_.width;}
int imageHeight() const {return imageSize_.height;}

39
corelib/src/CameraModel.cpp
View File

@@ -76,15 +76,15 @@ CameraModel::CameraModel(
{
UASSERT_MSG(fx > 0.0, uFormat("fx=%f", fx).c_str());
UASSERT_MSG(fy > 0.0, uFormat("fy=%f", fy).c_str());
UASSERT_MSG(cx > 0.0 || imageSize.width>0, uFormat("cx=%f imageSize.width=%d", cx, imageSize.width).c_str());
UASSERT_MSG(cy > 0.0 || imageSize.height>0, uFormat("cy=%f imageSize.height=%d", cy, imageSize.height).c_str());
UASSERT_MSG(cx >= 0.0 && imageSize.width>=0, uFormat("cx=%f imageSize.width=%d", cx, imageSize.width).c_str());
UASSERT_MSG(cy >= 0.0 && imageSize.height>=0, uFormat("cy=%f imageSize.height=%d", cy, imageSize.height).c_str());
UASSERT(!localTransform.isNull());
if(cx<=0.0)
if(cx==0.0 && imageSize.width > 0)
{
cx = double(imageSize.width)/2.0-0.5;
}
if(cy<=0.0)
if(cy==0.0 && imageSize.height > 0)
{
cy = double(imageSize.height)/2.0-0.5;
}
@@ -121,15 +121,15 @@ CameraModel::CameraModel(
{
UASSERT_MSG(fx > 0.0, uFormat("fx=%f", fx).c_str());
UASSERT_MSG(fy > 0.0, uFormat("fy=%f", fy).c_str());
UASSERT_MSG(cx > 0.0 || imageSize.width>0, uFormat("cx=%f imageSize.width=%d", cx, imageSize.width).c_str());
UASSERT_MSG(cy > 0.0 || imageSize.height>0, uFormat("cy=%f imageSize.height=%d", cy, imageSize.height).c_str());
UASSERT_MSG(cx >= 0.0 && imageSize.width>=0, uFormat("cx=%f imageSize.width=%d", cx, imageSize.width).c_str());
UASSERT_MSG(cy >= 0.0 && imageSize.height>=0, uFormat("cy=%f imageSize.height=%d", cy, imageSize.height).c_str());
UASSERT(!localTransform.isNull());
if(cx<=0.0)
if(cx==0.0 && imageSize.width > 0)
{
cx = double(imageSize.width)/2.0-0.5;
}
if(cy<=0.0)
if(cy==0.0 && imageSize.height > 0)
{
cy = double(imageSize.height)/2.0-0.5;
}
@@ -161,6 +161,29 @@ void CameraModel::initRectificationMap()
cv::initUndistortRectifyMap(K_, D_, R_, P_, imageSize_, CV_32FC1, mapX_, mapY_);
}
void CameraModel::setImageSize(const cv::Size & size)
{
UASSERT((size.height > 0 && size.width > 0) || (size.height == 0 && size.width == 0));
imageSize_ = size;
double ncx = cx();
double ncy = cy();
if(ncx==0.0 && imageSize_.width > 0)
{
ncx = double(imageSize_.width)/2.0-0.5;
}
if(ncy==0.0 && imageSize_.height > 0)
{
ncy = double(imageSize_.height)/2.0-0.5;
}
if(!P_.empty())
{
P_.at<double>(0,2) = ncx;
P_.at<double>(1,2) = ncy;
}
K_.at<double>(0,2) = ncx;
K_.at<double>(1,2) = ncy;
}
bool CameraModel::load(const std::string & directory, const std::string & cameraName)
{
K_ = cv::Mat();

19
corelib/src/DBReader.cpp
View File

@@ -162,9 +162,24 @@ bool DBReader::init(
StereoCameraModel stereoModel;
if(_dbDriver->getCalibration(*_ids.begin(), models, stereoModel))
{
if(models.size() && models.at(0).isValidForProjection())
if(models.size())
{
_calibrated = true;
if(models.at(0).isValidForProjection())
{
_calibrated = true;
}
else if(models.at(0).fx() && models.at(0).fy() && models.at(0).imageWidth() == 0)
{
// backward compatibility for databases not saving cx,cy and imageSize
SensorData data;
_dbDriver->getNodeData(*_ids.begin(), data, true, false, false, false);
cv::Mat rgb;
data.uncompressData(&rgb, 0); // this will update camera models if old format
if(data.cameraModels().size() && data.cameraModels().at(0).isValidForProjection())
{
_calibrated = true;
}
}
}
else if(stereoModel.isValidForProjection())
{

2
corelib/src/SensorData.cpp
View File

@@ -580,7 +580,7 @@ void SensorData::uncompressData(
cv::Size size(_imageRaw.cols/_cameraModels.size(), _imageRaw.rows);
for(unsigned int i=0; i<_cameraModels.size(); ++i)
{
if(_cameraModels[i].isValidForProjection() && _cameraModels[i].imageWidth() == 0)
if(_cameraModels[i].fx() && _cameraModels[i].fy() && _cameraModels[i].imageWidth() == 0)
{
_cameraModels[i].setImageSize(size);
}