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

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; std::set<int> nextDepth;
nextDepth.insert(fromId); nextDepth.insert(fromId);
int d = 0; 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()) while((depth == 0 || d < depth) && nextDepth.size())
{ {
curentDepth = nextDepth; curentDepth = nextDepth;
@@ -248,39 +255,22 @@ void Optimizer::getConnectedGraph(
ids.insert(*jter); ids.insert(*jter);
posesOut.insert(*posesIn.find(*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(nextId);
{
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());
if(depth == 0 || d < depth-1) std::map<int, Link>::const_iterator kter = graph::findLink(linksIn, *jter, nextId);
{ if(depth == 0 || d < depth-1)
linksOut.insert(*iter); {
} linksOut.insert(*kter);
else if(curentDepth.find(iter->second.from()) != curentDepth.end() || }
ids.find(iter->second.from()) != ids.end()) else if(curentDepth.find(nextId) != curentDepth.end() ||
{ ids.find(nextId) != ids.end())
linksOut.insert(*iter); {
} 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()) if(memory_ && value >=0 && value < (int)graphes_.size())
{ {
std::map<int, rtabmap::Transform> & graph = uValueAt(graphes_, value); 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 //update scans
UINFO("Update local maps list..."); UINFO("Update local maps list...");
std::vector<int> ids = uKeys(graphFiltered);
std::vector<int> ids = uKeys(graph);
for(unsigned int i=0; i<ids.size(); ++i) for(unsigned int i=0; i<ids.size(); ++i)
{ {
UTimer time; if(localMaps_.find(ids[i]) == localMaps_.end())
bool added = false;
if(ui_->groupBox_gridFromProjection->isChecked())
{ {
SensorData data = memory_->getNodeData(ids.at(i), true); UTimer time;
if(!data.depthOrRightRaw().empty()) bool added = false;
if(ui_->groupBox_gridFromProjection->isChecked())
{ {
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud; SensorData data = memory_->getNodeData(ids.at(i), true);
cloud = util3d::cloudFromSensorData(data, if(!data.depthOrRightRaw().empty())
ui_->spinBox_projDecimation->value(),
ui_->doubleSpinBox_projMaxDepth->value(),
ui_->doubleSpinBox_gridCellSize->value());
if(cloud->size())
{ {
UTimer timer; pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;
float cellSize = ui_->doubleSpinBox_gridCellSize->value(); cloud = util3d::cloudFromSensorData(data,
float groundNormalMaxAngle = M_PI_4; ui_->spinBox_projDecimation->value(),
int minClusterSize = 20; ui_->doubleSpinBox_projMaxDepth->value(),
cv::Mat ground, obstacles; ui_->doubleSpinBox_gridCellSize->value());
util3d::occupancy2DFromCloud3D<pcl::PointXYZ>( if(cloud->size())
cloud,
ground, obstacles,
cellSize,
groundNormalMaxAngle,
minClusterSize);
if(!ground.empty() || !obstacles.empty())
{ {
localMaps_.insert(std::make_pair(ids.at(i), std::make_pair(ground, obstacles))); UTimer timer;
added = true; 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
else
{
SensorData data = memory_->getNodeData(ids.at(i), false);
if(!data.laserScanCompressed().empty())
{ {
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud; SensorData data = memory_->getNodeData(ids.at(i), false);
cv::Mat laserScan; if(!data.laserScanCompressed().empty())
data.uncompressDataConst(0, 0, &laserScan); {
cv::Mat ground, obstacles; pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;
util3d::occupancy2DFromLaserScan( cv::Mat laserScan;
laserScan, data.uncompressDataConst(0, 0, &laserScan);
ground, cv::Mat ground, obstacles;
obstacles, util3d::occupancy2DFromLaserScan(
ui_->doubleSpinBox_gridCellSize->value(), laserScan,
ui_->checkBox_gridFillUnkownSpace->isChecked(), ground,
data.laserScanMaxRange()); obstacles,
localMaps_.insert(std::make_pair(ids.at(i), std::make_pair(ground, obstacles))); ui_->doubleSpinBox_gridCellSize->value(),
added = true; 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"); UINFO("Update local maps list... done");
@@ -2759,17 +2768,7 @@ void DatabaseViewer::sliderIterationsValueChanged(int value)
cv::Mat map; cv::Mat map;
QTime time; QTime time;
time.start(); time.start();
if(ui_->groupBox_posefiltering->isChecked()) map = rtabmap::util3d::create2DMapFromOccupancyLocalMaps(graphFiltered, localMaps_, cell, xMin, yMin, 0, ui_->checkBox_gridErode->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());
}
if(!map.empty()) if(!map.empty())
{ {
ui_->graphViewer->updateMap(rtabmap::util3d::convertMap2Image8U(map), cell, xMin, yMin); ui_->graphViewer->updateMap(rtabmap::util3d::convertMap2Image8U(map), cell, xMin, yMin);
@@ -2966,6 +2965,7 @@ void DatabaseViewer::updateGraphView()
} }
std::map<int, rtabmap::Transform> posesOut; std::map<int, rtabmap::Transform> posesOut;
std::multimap<int, rtabmap::Link> linksOut; std::multimap<int, rtabmap::Link> linksOut;
UINFO("Get connected graph (%d poses, %d links)", (int)poses.size(), (int)links.size());
optimizer->getConnectedGraph( optimizer->getConnectedGraph(
fromId, fromId,
poses, poses,
@@ -2973,7 +2973,7 @@ void DatabaseViewer::updateGraphView()
posesOut, posesOut,
linksOut, linksOut,
ui_->spinBox_optimizationDepth->value()); ui_->spinBox_optimizationDepth->value());
UINFO("Connected graph of %d poses and %d links", (int)posesOut.size(), (int)linksOut.size());
QTime time; QTime time;
time.start(); time.start();
std::map<int, rtabmap::Transform> finalPoses = optimizer->optimize(fromId, posesOut, linksOut, &graphes_); std::map<int, rtabmap::Transform> finalPoses = optimizer->optimize(fromId, posesOut, linksOut, &graphes_);