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

Added Map color overlay on 3D cloud (press key '2' on keyboard)

Browse Source 
Fixed loading all node ids from database, ignoring children

git-svn-id: http://rtabmap.googlecode.com/svn/trunk/rtabmap@1813 f169173b-cf89-36c8-b27e-44dbe73f0c83
This commit is contained in:
matlabbe
2014-10-01 21:44:46 +00:00
parent 317d382a24
commit c441cc1d0f
10 changed files with 101 additions and 27 deletions
Download Patch File Download Diff File Expand all files Collapse all files

15
guilib/include/rtabmap/gui/CloudViewer.h
View File

@@ -80,28 +80,33 @@ public:
bool addOrUpdateCloud(
const std::string & id,
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
const Transform & pose = Transform::getIdentity());
const Transform & pose = Transform::getIdentity(),
const QColor & color = Qt::gray);
bool addOrUpdateCloud(
const std::string & id,
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
const Transform & pose = Transform::getIdentity());
const Transform & pose = Transform::getIdentity(),
const QColor & color = Qt::gray);
bool addCloud(
const std::string & id,
const pcl::PCLPointCloud2Ptr & binaryCloud,
const Transform & pose,
bool rgb);
bool rgb,
const QColor & color = Qt::gray);
bool addCloud(
const std::string & id,
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
const Transform & pose = Transform::getIdentity());
const Transform & pose = Transform::getIdentity(),
const QColor & color = Qt::gray);
bool addCloud(
const std::string & id,
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
const Transform & pose = Transform::getIdentity());
const Transform & pose = Transform::getIdentity(),
const QColor & color = Qt::gray);
bool addCloudMesh(
const std::string & id,

1
guilib/include/rtabmap/gui/MainWindow.h
View File

@@ -255,6 +255,7 @@ private:
QMap<int, float> _depthFysMap;
QMap<int, float> _depthCxsMap;
QMap<int, float> _depthCysMap;
QMap<int, int> _mapIds;
QMap<int, Transform> _localTransformsMap;
std::map<int, Transform> _currentPosesMap;
QMap<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr > _createdClouds;

25
guilib/src/CloudViewer.cpp
View File

@@ -209,11 +209,12 @@ bool CloudViewer::updateCloud(
bool CloudViewer::addOrUpdateCloud(
const std::string & id,
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
const Transform & pose)
const Transform & pose,
const QColor & color)
{
if(!updateCloud(id, cloud, pose))
{
return addCloud(id, cloud, pose);
return addCloud(id, cloud, pose, color);
}
return true;
}
@@ -221,11 +222,12 @@ bool CloudViewer::addOrUpdateCloud(
bool CloudViewer::addOrUpdateCloud(
const std::string & id,
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
const Transform & pose)
const Transform & pose,
const QColor & color)
{
if(!updateCloud(id, cloud, pose))
{
return addCloud(id, cloud, pose);
return addCloud(id, cloud, pose, color);
}
return true;
}
@@ -234,7 +236,8 @@ bool CloudViewer::addCloud(
const std::string & id,
const pcl::PCLPointCloud2Ptr & binaryCloud,
const Transform & pose,
bool rgb)
bool rgb,
const QColor & color)
{
if(!_addedClouds.contains(id))
{
@@ -247,7 +250,7 @@ bool CloudViewer::addCloud(
if(_visualizer->addPointCloud (binaryCloud, colorHandler, origin, orientation, id))
{
// white
colorHandler.reset (new pcl::visualization::PointCloudColorHandlerCustom<pcl::PCLPointCloud2> (binaryCloud, 255, 255, 255));
colorHandler.reset (new pcl::visualization::PointCloudColorHandlerCustom<pcl::PCLPointCloud2> (binaryCloud, color.red(), color.green(), color.blue()));
_visualizer->addPointCloud (binaryCloud, colorHandler, origin, orientation, id);
// x,y,z
@@ -277,7 +280,8 @@ bool CloudViewer::addCloud(
bool CloudViewer::addCloud(
const std::string & id,
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
const Transform & pose)
const Transform & pose,
const QColor & color)
{
if(!_addedClouds.contains(id))
{
@@ -285,7 +289,7 @@ bool CloudViewer::addCloud(
pcl::PCLPointCloud2Ptr binaryCloud(new pcl::PCLPointCloud2);
pcl::toPCLPointCloud2(*cloud, *binaryCloud);
return addCloud(id, binaryCloud, pose, true);
return addCloud(id, binaryCloud, pose, true, color);
}
return false;
}
@@ -293,7 +297,8 @@ bool CloudViewer::addCloud(
bool CloudViewer::addCloud(
const std::string & id,
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
const Transform & pose)
const Transform & pose,
const QColor & color)
{
if(!_addedClouds.contains(id))
{
@@ -301,7 +306,7 @@ bool CloudViewer::addCloud(
pcl::PCLPointCloud2Ptr binaryCloud(new pcl::PCLPointCloud2);
pcl::toPCLPointCloud2(*cloud, *binaryCloud);
return addCloud(id, binaryCloud, pose, false);
return addCloud(id, binaryCloud, pose, false, color);
}
return false;
}

8
guilib/src/DatabaseViewer.cpp
View File

@@ -781,7 +781,13 @@ void DatabaseViewer::view3DMap()
cloud = rtabmap::util3d::transformPointCloud(cloud, localTransform);
viewer->addCloud(uFormat("cloud%d", iter->first), cloud, pose);
QColor color = Qt::red;
int mapId = memory_->getMapId(iter->first);
if(mapId >= 0)
{
color = (Qt::GlobalColor)(mapId % 12 + 7 );
}
viewer->addCloud(uFormat("cloud%d", iter->first), cloud, pose, color);
UINFO("Generated %d (%d points)", iter->first, cloud->size());
progressDialog.appendText(QString("Generated %1 (%2 points)").arg(iter->first).arg(cloud->size()));

47
guilib/src/MainWindow.cpp
View File

@@ -747,6 +747,14 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
_depths2DMap.insert(iter->first, iter->second);
}
}
// map ids
for(std::map<int, int>::const_iterator iter = stat.getMapIds().begin();
iter != stat.getMapIds().end();
++iter)
{
_mapIds.insert(iter->first, iter->second);
}
}
int rehearsed = (int)uValue(stat.data(), Statistics::kMemoryRehearsal_merged(), 0.0f);
@@ -1335,7 +1343,13 @@ void MainWindow::createAndAddCloudToMap(int nodeId, const Transform & pose)
cloud->clear();
pcl::copyPointCloud(*cloudWithNormals, *cloud);
}
if(!_ui->widget_cloudViewer->addOrUpdateCloud(cloudName, cloud, pose))
QColor color = Qt::gray;
int mapId = _mapIds.value(nodeId, -1);
if(mapId >= 0)
{
color = (Qt::GlobalColor)(mapId % 12 + 7 );
}
if(!_ui->widget_cloudViewer->addOrUpdateCloud(cloudName, cloud, pose, color))
{
UERROR("Adding cloud %d to viewer failed!", nodeId);
}
@@ -1360,6 +1374,12 @@ void MainWindow::createAndAddScanToMap(int nodeId, const Transform & pose)
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;
cv::Mat depth2d = util3d::uncompressData(_depths2DMap.value(nodeId));
cloud = util3d::depth2DToPointCloud(depth2d);
QColor color = Qt::red;
int mapId = _mapIds.value(nodeId, -1);
if(mapId >= 0)
{
color = (Qt::GlobalColor)(mapId % 12 + 7 );
}
if(!_ui->widget_cloudViewer->addOrUpdateCloud(scanName, cloud, pose))
{
UERROR("Adding cloud %d to viewer failed!", nodeId);
@@ -1463,6 +1483,7 @@ void MainWindow::processRtabmapEvent3DMap(const rtabmap::RtabmapEvent3DMap & eve
UINFO(" depthFys = %d", event.getDepthFys().size());
UINFO(" depthCxs = %d", event.getDepthCxs().size());
UINFO(" depthCys = %d", event.getDepthCys().size());
UINFO(" map ids = %d", event.getMapIds().size());
UINFO(" localTransforms = %d", event.getLocalTransforms().size());
UINFO(" poses = %d", event.getPoses().size());
UINFO(" constraints = %d", event.getConstraints().size());
@@ -1523,6 +1544,15 @@ void MainWindow::processRtabmapEvent3DMap(const rtabmap::RtabmapEvent3DMap & eve
_initProgressDialog->appendText(tr("Inserted %1 depth cy parameters.").arg(_depthCysMap.size()));
_initProgressDialog->incrementStep();
for(std::map<int, int>::const_iterator iter = event.getMapIds().begin();
iter!=event.getMapIds().end();
++iter)
{
_mapIds.insert(iter->first, iter->second);
}
_initProgressDialog->appendText(tr("Inserted %1 map ids").arg(_mapIds.size()));
_initProgressDialog->incrementStep();
for(std::map<int, std::vector<unsigned char> >::const_iterator iter = event.getDepths2d().begin();
iter!=event.getDepths2d().end();
++iter)
@@ -2715,6 +2745,7 @@ void MainWindow::clearTheCache()
_depthFysMap.clear();
_depthCxsMap.clear();
_depthCysMap.clear();
_mapIds.clear();
_localTransformsMap.clear();
_createdClouds.clear();
_createdScans.clear();
@@ -3047,6 +3078,13 @@ void MainWindow::viewScans()
{
_initProgressDialog->appendText(tr("Viewing the scan %1 (%2 points)...").arg(iter->first).arg(iter->second->size()));
_initProgressDialog->incrementStep();
QColor color = Qt::red;
int mapId = _mapIds.value(iter->first, -1);
if(mapId >= 0)
{
color = (Qt::GlobalColor)(mapId % 12 + 7 );
}
viewer->addCloud(uFormat("cloud%d",iter->first), iter->second, iter->first>0?_currentPosesMap.at(iter->first):Transform::getIdentity());
_initProgressDialog->appendText(tr("Viewing the scan %1 (%2 points)... done.").arg(iter->first).arg(iter->second->size()));
}
@@ -3213,6 +3251,13 @@ void MainWindow::viewClouds()
{
_initProgressDialog->appendText(tr("Viewing the cloud %1 (%2 points)...").arg(iter->first).arg(iter->second->size()));
_initProgressDialog->incrementStep();
QColor color = Qt::gray;
int mapId = _mapIds.value(iter->first, -1);
if(mapId >= 0)
{
color = (Qt::GlobalColor)(mapId % 12 + 7 );
}
viewer->addCloud(uFormat("cloud%d",iter->first), iter->second, iter->first>0?_currentPosesMap.at(iter->first):Transform::getIdentity());
_initProgressDialog->appendText(tr("Viewing the cloud %1 (%2 points)... done.").arg(iter->first).arg(iter->second->size()));
}