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

DbViewer: added option to regenerate optimized 2d map from OctoMap (https://github.com/introlab/rtabmap_ros/issues/213)

Browse Source
This commit is contained in:
matlabbe
2022-10-23 12:16:35 -07:00
parent 6e8f43916c
commit 97a150b180
1 changed files with 49 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files

58
guilib/src/DatabaseViewer.cpp
View File

@@ -3226,17 +3226,57 @@ void DatabaseViewer::regenerateSavedMap()
return; return;
} }
//
#ifdef RTABMAP_OCTOMAP
QStringList types;
types.push_back("Default occupancy grid");
types.push_back("From OctoMap projection");
bool ok;
QString type = QInputDialog::getItem(this, tr("Which type?"), tr("Poses:"), types, 0, false, &ok);
if(!ok)
{
return;
}
#endif
//update scans //update scans
UINFO("Update local maps list..."); UINFO("Update local maps list...");
OccupancyGrid grid(ui_->parameters_toolbox->getParameters());
for(std::map<int, std::pair<std::pair<cv::Mat, cv::Mat>, cv::Mat> >::iterator iter=localMaps_.begin(); iter!=localMaps_.end(); ++iter)
{
grid.addToCache(iter->first, iter->second.first.first, iter->second.first.second, iter->second.second);
}
grid.update(graphes_.back());
float xMin, yMin; float xMin, yMin;
cv::Mat map = grid.getMap(xMin, yMin); cv::Mat map;
float gridCellSize = Parameters::defaultGridCellSize();
Parameters::parse(ui_->parameters_toolbox->getParameters(), Parameters::kGridCellSize(), gridCellSize);
#ifdef RTABMAP_OCTOMAP
if(type.compare("From OctoMap projection") == 0)
{
//create local octomap
OctoMap octomap(ui_->parameters_toolbox->getParameters());
for(std::map<int, std::pair<std::pair<cv::Mat, cv::Mat>, cv::Mat> >::iterator iter=localMaps_.begin(); iter!=localMaps_.end(); ++iter)
{
if(iter->second.first.first.channels() == 2 || iter->second.first.second.channels() == 2)
{
QMessageBox::warning(this, tr(""),
tr("Some local occupancy grids are 2D, but OctoMap requires 3D local "
"occupancy grids. Select default occupancy grid or generate "
"3D local occupancy grids (\"Grid/3D\" core parameter)."));
return;
}
octomap.addToCache(iter->first, iter->second.first.first, iter->second.first.second, iter->second.second, localMapsInfo_.at(iter->first).second);
}
octomap.update(graphes_.back());
map = octomap.createProjectionMap(xMin, yMin, gridCellSize, 0);
}
else
#endif
{
OccupancyGrid grid(ui_->parameters_toolbox->getParameters());
for(std::map<int, std::pair<std::pair<cv::Mat, cv::Mat>, cv::Mat> >::iterator iter=localMaps_.begin(); iter!=localMaps_.end(); ++iter)
{
grid.addToCache(iter->first, iter->second.first.first, iter->second.first.second, iter->second.second);
}
grid.update(graphes_.back());
map = grid.getMap(xMin, yMin);
}
if(map.empty()) if(map.empty())
{ {
@@ -3244,7 +3284,7 @@ void DatabaseViewer::regenerateSavedMap()
} }
else else
{ {
dbDriver_->save2DMap(map, xMin, yMin, grid.getCellSize()); dbDriver_->save2DMap(map, xMin, yMin, gridCellSize);
Transform lastlocalizationPose; Transform lastlocalizationPose;
dbDriver_->loadOptimizedPoses(&lastlocalizationPose); dbDriver_->loadOptimizedPoses(&lastlocalizationPose);
if(lastlocalizationPose.isNull() && !graphes_.back().empty()) if(lastlocalizationPose.isNull() && !graphes_.back().empty())
@@ -6701,7 +6741,7 @@ void DatabaseViewer::sliderIterationsValueChanged(int value)
localMapsInfo.insert(*localMapsInfo_.find(ids[i])); localMapsInfo.insert(*localMapsInfo_.find(ids[i]));
} }
} }
else else if(ids.at(i)>0)
{ {
SensorData data; SensorData data;
dbDriver_->getNodeData(ids.at(i), data, false, false, false); dbDriver_->getNodeData(ids.at(i), data, false, false, false);