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

Matching map colors to path/cloud/scan

Browse Source
This commit is contained in:
matlabbe
2015-05-07 15:32:56 -04:00
parent 3642e2fbf1
commit dd7d28898b
3 changed files with 18 additions and 19 deletions
Download Patch File Download Diff File Expand all files Collapse all files

13
guilib/src/CloudViewer.cpp
View File

@@ -552,7 +552,7 @@ void CloudViewer::addOrUpdateGraph(
if(graph->size())
{
_graphes.insert(std::make_pair(id, graph));
_graphes.insert(id);
pcl::PolygonMesh mesh;
pcl::Vertices vertices;
@@ -565,6 +565,9 @@ void CloudViewer::addOrUpdateGraph(
mesh.polygons.push_back(vertices);
_visualizer->addPolylineFromPolygonMesh(mesh, id);
_visualizer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, color.redF(), color.greenF(), color.blueF(), id);
this->addOrUpdateCloud(id+"_nodes", graph, Transform::getIdentity(), color);
this->setCloudPointSize(id+"_nodes", 5);
}
}
@@ -580,16 +583,18 @@ void CloudViewer::removeGraph(const std::string & id)
{
_visualizer->removeShape(id);
_graphes.erase(id);
removeCloud(id+"_nodes");
}
}
void CloudViewer::removeAllGraphs()
{
for(std::map<std::string, pcl::PointCloud<pcl::PointXYZ>::Ptr >::iterator iter = _graphes.begin(); iter!=_graphes.end(); ++iter)
std::set<std::string> graphes = _graphes;
for(std::set<std::string>::iterator iter = graphes.begin(); iter!=graphes.end(); ++iter)
{
_visualizer->removeShape(iter->first);
this->removeGraph(*iter);
}
_graphes.clear();
UASSERT(_graphes.empty());
}
bool CloudViewer::isTrajectoryShown() const

22
guilib/src/MainWindow.cpp
View File

@@ -1433,12 +1433,11 @@ void MainWindow::updateMapCloud(
{
// Find all graphs
std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr > graphs;
pcl::PointCloud<pcl::PointXYZ>::Ptr graphNodes(new pcl::PointCloud<pcl::PointXYZ>);
graphNodes->resize(_currentPosesMap.size());
int oi = 0;
for(std::map<int, Transform>::iterator iter=_currentPosesMap.begin(); iter!=_currentPosesMap.end(); ++iter)
{
int mapId = uValue(_currentMapIds, iter->first, -1);
//edges
std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr >::iterator kter = graphs.find(mapId);
if(kter == graphs.end())
{
@@ -1446,7 +1445,6 @@ void MainWindow::updateMapCloud(
}
pcl::PointXYZ pt(iter->second.x(), iter->second.y(), iter->second.z());
kter->second->push_back(pt);
(*graphNodes)[oi++] = pcl::PointXYZ(pt);
}
// add graphs
@@ -1455,14 +1453,10 @@ void MainWindow::updateMapCloud(
QColor color = Qt::gray;
if(iter->first >= 0)
{
color = (Qt::GlobalColor)((iter->first+2) % 12 + 7 );
color = (Qt::GlobalColor)((iter->first+3) % 12 + 7 );
}
_ui->widget_cloudViewer->addOrUpdateGraph(uFormat("graph_%d", iter->first), iter->second, color);
}
// add nodes
_ui->widget_cloudViewer->addOrUpdateCloud("graph_nodes", graphNodes, Transform::getIdentity(), Qt::green);
_ui->widget_cloudViewer->setCloudPointSize("graph_nodes", 5);
}
// Update occupancy grid map in 3D map view and graph view
@@ -1649,7 +1643,7 @@ void MainWindow::createAndAddCloudToMap(int nodeId, const Transform & pose, int
QColor color = Qt::gray;
if(mapId >= 0)
{
color = (Qt::GlobalColor)(mapId % 12 + 7 );
color = (Qt::GlobalColor)(mapId+3 % 12 + 7 );
}
if(!_ui->widget_cloudViewer->addOrUpdateCloud(cloudName, cloud, pose, color))
{
@@ -1666,7 +1660,7 @@ void MainWindow::createAndAddCloudToMap(int nodeId, const Transform & pose, int
QColor color = Qt::gray;
if(mapId >= 0)
{
color = (Qt::GlobalColor)(mapId % 12 + 7 );
color = (Qt::GlobalColor)(mapId+3 % 12 + 7 );
}
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
cloud->resize(iter->getWords3().size());
@@ -1721,12 +1715,12 @@ void MainWindow::createAndAddScanToMap(int nodeId, const Transform & pose, int m
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;
cloud = util3d::laserScanToPointCloud(depth2D);
QColor color = Qt::red;
QColor color = Qt::gray;
if(mapId >= 0)
{
color = (Qt::GlobalColor)(mapId % 12 + 7 );
color = (Qt::GlobalColor)(mapId+3 % 12 + 7 );
}
if(!_ui->widget_cloudViewer->addOrUpdateCloud(scanName, cloud, pose))
if(!_ui->widget_cloudViewer->addOrUpdateCloud(scanName, cloud, pose, color))
{
UERROR("Adding cloud %d to viewer failed!", nodeId);
}