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

increased graph::getConnectedGraph() performance

Browse Source
This commit is contained in:
matlabbe
2015-10-14 10:03:36 -04:00
parent 5f488ed269
commit a8d83c895c
2 changed files with 84 additions and 94 deletions
Download Patch File Download Diff File Expand all files Collapse all files

50
corelib/src/Graph.cpp
View File

@@ -236,6 +236,13 @@ void Optimizer::getConnectedGraph(
std::set<int> nextDepth;
nextDepth.insert(fromId);
int d = 0;
std::multimap<int, int> biLinks;
for(std::multimap<int, Link>::const_iterator iter=linksIn.begin(); iter!=linksIn.end(); ++iter)
{
biLinks.insert(std::make_pair(iter->second.from(), iter->second.to()));
biLinks.insert(std::make_pair(iter->second.to(), iter->second.from()));
}
while((depth == 0 || d < depth) && nextDepth.size())
{
curentDepth = nextDepth;
@@ -248,39 +255,22 @@ void Optimizer::getConnectedGraph(
ids.insert(*jter);
posesOut.insert(*posesIn.find(*jter));
for(std::multimap<int, Link>::const_iterator iter=linksIn.begin(); iter!=linksIn.end(); ++iter)
for(std::multimap<int, int>::const_iterator iter=biLinks.find(*jter); iter!=biLinks.end() && iter->first==*jter; ++iter)
{
if(iter->second.from() == *jter)
int nextId = iter->second;
if(ids.find(nextId) == ids.end() && uContains(posesIn, nextId))
{
if(ids.find(iter->second.to()) == ids.end() && uContains(posesIn, iter->second.to()))
{
nextDepth.insert(iter->second.to());
if(depth == 0 || d < depth-1)
{
linksOut.insert(*iter);
}
else if(curentDepth.find(iter->second.to()) != curentDepth.end() ||
ids.find(iter->second.to()) != ids.end())
{
linksOut.insert(*iter);
}
}
}
else if(iter->second.to() == *jter)
{
if(ids.find(iter->second.from()) == ids.end() && uContains(posesIn, iter->second.from()))
{
nextDepth.insert(iter->second.from());
nextDepth.insert(nextId);
if(depth == 0 || d < depth-1)
{
linksOut.insert(*iter);
}
else if(curentDepth.find(iter->second.from()) != curentDepth.end() ||
ids.find(iter->second.from()) != ids.end())
{
linksOut.insert(*iter);
}
std::map<int, Link>::const_iterator kter = graph::findLink(linksIn, *jter, nextId);
if(depth == 0 || d < depth-1)
{
linksOut.insert(*kter);
}
else if(curentDepth.find(nextId) != curentDepth.end() ||
ids.find(nextId) != ids.end())
{
linksOut.insert(*kter);
}
}
}

128
guilib/src/DatabaseViewer.cpp
View File

@@ -2679,73 +2679,82 @@ void DatabaseViewer::sliderIterationsValueChanged(int value)
if(memory_ && value >=0 && value < (int)graphes_.size())
{
std::map<int, rtabmap::Transform> & graph = uValueAt(graphes_, value);
if(ui_->dockWidget_graphView->isVisible() && localMaps_.size() == 0)
std::map<int, rtabmap::Transform> graphFiltered = graph;
if(ui_->groupBox_posefiltering->isChecked())
{
graphFiltered = graph::radiusPosesFiltering(graph,
ui_->doubleSpinBox_posefilteringRadius->value(),
ui_->doubleSpinBox_posefilteringAngle->value()*CV_PI/180.0);
}
if(ui_->dockWidget_graphView->isVisible())
{
//update scans
UINFO("Update local maps list...");
std::vector<int> ids = uKeys(graph);
std::vector<int> ids = uKeys(graphFiltered);
for(unsigned int i=0; i<ids.size(); ++i)
{
UTimer time;
bool added = false;
if(ui_->groupBox_gridFromProjection->isChecked())
if(localMaps_.find(ids[i]) == localMaps_.end())
{
SensorData data = memory_->getNodeData(ids.at(i), true);
if(!data.depthOrRightRaw().empty())
UTimer time;
bool added = false;
if(ui_->groupBox_gridFromProjection->isChecked())
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;
cloud = util3d::cloudFromSensorData(data,
ui_->spinBox_projDecimation->value(),
ui_->doubleSpinBox_projMaxDepth->value(),
ui_->doubleSpinBox_gridCellSize->value());
if(cloud->size())
SensorData data = memory_->getNodeData(ids.at(i), true);
if(!data.depthOrRightRaw().empty())
{
UTimer timer;
float cellSize = ui_->doubleSpinBox_gridCellSize->value();
float groundNormalMaxAngle = M_PI_4;
int minClusterSize = 20;
cv::Mat ground, obstacles;
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;
cloud = util3d::cloudFromSensorData(data,
ui_->spinBox_projDecimation->value(),
ui_->doubleSpinBox_projMaxDepth->value(),
ui_->doubleSpinBox_gridCellSize->value());
util3d::occupancy2DFromCloud3D<pcl::PointXYZ>(
cloud,
ground, obstacles,
cellSize,
groundNormalMaxAngle,
minClusterSize);
if(!ground.empty() || !obstacles.empty())
if(cloud->size())
{
localMaps_.insert(std::make_pair(ids.at(i), std::make_pair(ground, obstacles)));
added = true;
UTimer timer;
float cellSize = ui_->doubleSpinBox_gridCellSize->value();
float groundNormalMaxAngle = M_PI_4;
int minClusterSize = 20;
cv::Mat ground, obstacles;
util3d::occupancy2DFromCloud3D<pcl::PointXYZ>(
cloud,
ground, obstacles,
cellSize,
groundNormalMaxAngle,
minClusterSize);
if(!ground.empty() || !obstacles.empty())
{
localMaps_.insert(std::make_pair(ids.at(i), std::make_pair(ground, obstacles)));
added = true;
}
}
}
}
}
else
{
SensorData data = memory_->getNodeData(ids.at(i), false);
if(!data.laserScanCompressed().empty())
else
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;
cv::Mat laserScan;
data.uncompressDataConst(0, 0, &laserScan);
cv::Mat ground, obstacles;
util3d::occupancy2DFromLaserScan(
laserScan,
ground,
obstacles,
ui_->doubleSpinBox_gridCellSize->value(),
ui_->checkBox_gridFillUnkownSpace->isChecked(),
data.laserScanMaxRange());
localMaps_.insert(std::make_pair(ids.at(i), std::make_pair(ground, obstacles)));
added = true;
SensorData data = memory_->getNodeData(ids.at(i), false);
if(!data.laserScanCompressed().empty())
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;
cv::Mat laserScan;
data.uncompressDataConst(0, 0, &laserScan);
cv::Mat ground, obstacles;
util3d::occupancy2DFromLaserScan(
laserScan,
ground,
obstacles,
ui_->doubleSpinBox_gridCellSize->value(),
ui_->checkBox_gridFillUnkownSpace->isChecked(),
data.laserScanMaxRange());
localMaps_.insert(std::make_pair(ids.at(i), std::make_pair(ground, obstacles)));
added = true;
}
}
if(added)
{
UINFO("Processed grid map %d/%d (%fs)", i+1, (int)ids.size(), time.ticks());
}
}
if(added)
{
UINFO("Processed grid map %d/%d (%fs)", i+1, (int)ids.size(), time.ticks());
}
}
UINFO("Update local maps list... done");
@@ -2759,17 +2768,7 @@ void DatabaseViewer::sliderIterationsValueChanged(int value)
cv::Mat map;
QTime time;
time.start();
if(ui_->groupBox_posefiltering->isChecked())
{
std::map<int, rtabmap::Transform> graphFiltered = graph::radiusPosesFiltering(graph,
ui_->doubleSpinBox_posefilteringRadius->value(),
ui_->doubleSpinBox_posefilteringAngle->value()*CV_PI/180.0);
map = rtabmap::util3d::create2DMapFromOccupancyLocalMaps(graphFiltered, localMaps_, cell, xMin, yMin, 0, ui_->checkBox_gridErode->isChecked());
}
else
{
map = rtabmap::util3d::create2DMapFromOccupancyLocalMaps(graph, localMaps_, cell, xMin, yMin, 0, ui_->checkBox_gridErode->isChecked());
}
map = rtabmap::util3d::create2DMapFromOccupancyLocalMaps(graphFiltered, localMaps_, cell, xMin, yMin, 0, ui_->checkBox_gridErode->isChecked());
if(!map.empty())
{
ui_->graphViewer->updateMap(rtabmap::util3d::convertMap2Image8U(map), cell, xMin, yMin);
@@ -2966,6 +2965,7 @@ void DatabaseViewer::updateGraphView()
}
std::map<int, rtabmap::Transform> posesOut;
std::multimap<int, rtabmap::Link> linksOut;
UINFO("Get connected graph (%d poses, %d links)", (int)poses.size(), (int)links.size());
optimizer->getConnectedGraph(
fromId,
poses,
@@ -2973,7 +2973,7 @@ void DatabaseViewer::updateGraphView()
posesOut,
linksOut,
ui_->spinBox_optimizationDepth->value());
UINFO("Connected graph of %d poses and %d links", (int)posesOut.size(), (int)linksOut.size());
QTime time;
time.start();
std::map<int, rtabmap::Transform> finalPoses = optimizer->optimize(fromId, posesOut, linksOut, &graphes_);