i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-08 04:20:20 +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
+17 -2
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())
{
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
+75 -70
View File
@@ -691,94 +691,99 @@ void DatabaseViewer::closeEvent(QCloseEvent* event)
}
event->accept();
if(linksAdded_.size() || linksRefined_.size() || linksRemoved_.size())
if(dbDriver_)
{
QMessageBox::StandardButton button = QMessageBox::question(this,
tr("Links modified"),
tr("Some links are modified (%1 added, %2 refined, %3 removed), do you want to save them?")
.arg(linksAdded_.size()).arg(linksRefined_.size()).arg(linksRemoved_.size()),
QMessageBox::Cancel | QMessageBox::Yes | QMessageBox::No,
QMessageBox::Cancel);
if(button == QMessageBox::Yes)
if(linksAdded_.size() || linksRefined_.size() || linksRemoved_.size())
{
// Added links
for(std::multimap<int, rtabmap::Link>::iterator iter=linksAdded_.begin(); iter!=linksAdded_.end(); ++iter)
QMessageBox::StandardButton button = QMessageBox::question(this,
tr("Links modified"),
tr("Some links are modified (%1 added, %2 refined, %3 removed), do you want to save them?")
.arg(linksAdded_.size()).arg(linksRefined_.size()).arg(linksRemoved_.size()),
QMessageBox::Cancel | QMessageBox::Yes | QMessageBox::No,
QMessageBox::Cancel);
if(button == QMessageBox::Yes)
{
std::multimap<int, rtabmap::Link>::iterator refinedIter = rtabmap::graph::findLink(linksRefined_, iter->second.from(), iter->second.to());
if(refinedIter != linksRefined_.end())
// Added links
for(std::multimap<int, rtabmap::Link>::iterator iter=linksAdded_.begin(); iter!=linksAdded_.end(); ++iter)
{
dbDriver_->addLink(refinedIter->second);
dbDriver_->addLink(refinedIter->second.inverse());
std::multimap<int, rtabmap::Link>::iterator refinedIter = rtabmap::graph::findLink(linksRefined_, iter->second.from(), iter->second.to());
if(refinedIter != linksRefined_.end())
{
dbDriver_->addLink(refinedIter->second);
dbDriver_->addLink(refinedIter->second.inverse());
}
else
{
dbDriver_->addLink(iter->second);
dbDriver_->addLink(iter->second.inverse());
}
}
else
//Refined links
for(std::multimap<int, rtabmap::Link>::iterator iter=linksRefined_.begin(); iter!=linksRefined_.end(); ++iter)
{
dbDriver_->addLink(iter->second);
dbDriver_->addLink(iter->second.inverse());
if(!containsLink(linksAdded_, iter->second.from(), iter->second.to()))
{
dbDriver_->updateLink(iter->second);
dbDriver_->updateLink(iter->second.inverse());
}
}
// Rejected links
for(std::multimap<int, rtabmap::Link>::iterator iter=linksRemoved_.begin(); iter!=linksRemoved_.end(); ++iter)
{
dbDriver_->removeLink(iter->second.to(), iter->second.from());
dbDriver_->removeLink(iter->second.from(), iter->second.to());
}
linksAdded_.clear();
linksRefined_.clear();
linksRemoved_.clear();
}
//Refined links
for(std::multimap<int, rtabmap::Link>::iterator iter=linksRefined_.begin(); iter!=linksRefined_.end(); ++iter)
if(button != QMessageBox::Yes && button != QMessageBox::No)
{
if(!containsLink(linksAdded_, iter->second.from(), iter->second.to()))
{
dbDriver_->updateLink(iter->second);
dbDriver_->updateLink(iter->second.inverse());
}
event->ignore();
}
// Rejected links
for(std::multimap<int, rtabmap::Link>::iterator iter=linksRemoved_.begin(); iter!=linksRemoved_.end(); ++iter)
{
dbDriver_->removeLink(iter->second.to(), iter->second.from());
dbDriver_->removeLink(iter->second.from(), iter->second.to());
}
linksAdded_.clear();
linksRefined_.clear();
linksRemoved_.clear();
}
if(button != QMessageBox::Yes && button != QMessageBox::No)
if(event->isAccepted() &&
generatedLocalMaps_.size() &&
uStrNumCmp(dbDriver_->getDatabaseVersion(), "0.11.10") >= 0)
{
event->ignore();
}
}
QMessageBox::StandardButton button = QMessageBox::question(this,
tr("Local occupancy grid maps modified"),
tr("%1 occupancy grid maps are modified, do you want to "
"save them? This will overwrite occupancy grids saved in the database.")
.arg(generatedLocalMaps_.size()),
QMessageBox::Cancel | QMessageBox::Yes | QMessageBox::No,
QMessageBox::Cancel);
if(event->isAccepted() && generatedLocalMaps_.size())
{
QMessageBox::StandardButton button = QMessageBox::question(this,
tr("Local occupancy grid maps modified"),
tr("%1 occupancy grid maps are modified, do you want to "
"save them? This will overwrite occupancy grids saved in the database.")
.arg(generatedLocalMaps_.size()),
QMessageBox::Cancel | QMessageBox::Yes | QMessageBox::No,
QMessageBox::Cancel);
if(button == QMessageBox::Yes)
{
// Rejected links
UASSERT(generatedLocalMaps_.size() == generatedLocalMapsInfo_.size());
std::map<int, std::pair<cv::Mat, cv::Mat> >::iterator mapIter = generatedLocalMaps_.begin();
std::map<int, std::pair<float, cv::Point3f> >::iterator infoIter = generatedLocalMapsInfo_.begin();
for(; mapIter!=generatedLocalMaps_.end(); ++mapIter, ++infoIter)
if(button == QMessageBox::Yes)
{
UASSERT(mapIter->first == infoIter->first);
dbDriver_->updateOccupancyGrid(
mapIter->first,
mapIter->second.first,
mapIter->second.second,
infoIter->second.first,
infoIter->second.second);
// Rejected links
UASSERT(generatedLocalMaps_.size() == generatedLocalMapsInfo_.size());
std::map<int, std::pair<cv::Mat, cv::Mat> >::iterator mapIter = generatedLocalMaps_.begin();
std::map<int, std::pair<float, cv::Point3f> >::iterator infoIter = generatedLocalMapsInfo_.begin();
for(; mapIter!=generatedLocalMaps_.end(); ++mapIter, ++infoIter)
{
UASSERT(mapIter->first == infoIter->first);
dbDriver_->updateOccupancyGrid(
mapIter->first,
mapIter->second.first,
mapIter->second.second,
infoIter->second.first,
infoIter->second.second);
}
generatedLocalMaps_.clear();
generatedLocalMapsInfo_.clear();
localMaps_.clear();
}
generatedLocalMaps_.clear();
generatedLocalMapsInfo_.clear();
localMaps_.clear();
}
if(button != QMessageBox::Yes && button != QMessageBox::No)
{
event->ignore();
if(button != QMessageBox::Yes && button != QMessageBox::No)
{
event->ignore();
}
}
}
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));
}
}
}