i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-03 16:27:46 +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
corelib/include/rtabmap/core/CameraModel.h
+1 -1
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;}
corelib/src/CameraModel.cpp
+31 -8
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();
corelib/src/DBReader.cpp
+16 -1
View File
@@ -162,10 +162,25 @@ bool DBReader::init(
StereoCameraModel stereoModel;
if(_dbDriver->getCalibration(*_ids.begin(), models, stereoModel))
{
if(models.size() && models.at(0).isValidForProjection())
if(models.size())
{
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())
{
_calibrated = true;
corelib/src/SensorData.cpp
+1 -1
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);
}
guilib/src/DatabaseViewer.cpp
+6 -1
View File
@@ -691,6 +691,8 @@ void DatabaseViewer::closeEvent(QCloseEvent* event)
}
event->accept();
if(dbDriver_)
{
if(linksAdded_.size() || linksRefined_.size() || linksRemoved_.size())
{
QMessageBox::StandardButton button = QMessageBox::question(this,
@@ -745,7 +747,9 @@ void DatabaseViewer::closeEvent(QCloseEvent* event)
}
}
if(event->isAccepted() && generatedLocalMaps_.size())
if(event->isAccepted() &&
generatedLocalMaps_.size() &&
uStrNumCmp(dbDriver_->getDatabaseVersion(), "0.11.10") >= 0)
{
QMessageBox::StandardButton button = QMessageBox::question(this,
tr("Local occupancy grid maps modified"),
@@ -781,6 +785,7 @@ void DatabaseViewer::closeEvent(QCloseEvent* event)
event->ignore();
}
}
}
if(event->isAccepted())
{
guilib/src/MainWindow.cpp
+6 -6
View File
@@ -991,7 +991,7 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom, bool dataI
int h = cloud->height;
UASSERT(w > 1 && h > 1);
textureMesh->tex_coordinates.resize(1);
int nPoints = textureMesh->cloud.data.size()/textureMesh->cloud.point_step;
int nPoints = (int)(textureMesh->cloud.data.size()/textureMesh->cloud.point_step);
textureMesh->tex_coordinates[0].resize(nPoints);
for(int i=0; i<nPoints; ++i)
{
@@ -2125,7 +2125,7 @@ void MainWindow::updateMapCloud(
jter->sensorData().uncompressDataConst(0, 0, 0, 0, &ground, &obstacles);
_gridLocalMaps.insert(std::make_pair(iter->first, std::make_pair(ground, obstacles)));
_gridViewPoints.insert(std::make_pair(iter->first, jter->sensorData().gridViewPoint()));
_cachedGridsMemoryUsage += ground.total()*ground.elemSize() + obstacles.total()*obstacles.elemSize();
_cachedGridsMemoryUsage += (long)(ground.total()*ground.elemSize() + obstacles.total()*obstacles.elemSize());
if (ground.cols || obstacles.cols)
{
@@ -2674,12 +2674,12 @@ std::pair<pcl::PointCloud<pcl::PointXYZRGB>::Ptr, pcl::IndicesPtr> MainWindow::c
int h = cloud->height;
UASSERT(w > 1 && h > 1);
textureMesh->tex_coordinates.resize(1);
unsigned int nPoints = outputFiltered->size();
int nPoints = (int)outputFiltered->size();
textureMesh->tex_coordinates[0].resize(nPoints);
for(unsigned int i=0; i<nPoints; ++i)
for(int i=0; i<nPoints; ++i)
{
//uv
UASSERT(i < denseToOrganizedIndices.size());
UASSERT(i < (int)denseToOrganizedIndices.size());
int originalVertex = denseToOrganizedIndices[i];
textureMesh->tex_coordinates[0][i] = Eigen::Vector2f(
float(originalVertex % w) / float(w), // u
@@ -2791,7 +2791,7 @@ std::pair<pcl::PointCloud<pcl::PointXYZRGB>::Ptr, pcl::IndicesPtr> MainWindow::c
if(_preferencesDialog->isCloudsKept())
{
_cachedClouds.insert(std::make_pair(nodeId, outputPair));
_createdCloudsMemoryUsage += output->size() * sizeof(pcl::PointXYZRGB) + indices->size()*sizeof(int);
_createdCloudsMemoryUsage += (long)(output->size() * sizeof(pcl::PointXYZRGB) + indices->size()*sizeof(int));
}
}
}