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

Added util3d::segmentObstaclesFromGround() method, templated some PCL methods (new rtabmap/core/impl/util3d.hpp)

Browse Source 
git-svn-id: http://rtabmap.googlecode.com/svn/trunk/rtabmap@1921 f169173b-cf89-36c8-b27e-44dbe73f0c83
This commit is contained in:
matlabbe
2014-10-24 16:58:32 +00:00
parent 2bd32e5a95
commit 60b0fd2e98
12 changed files with 658 additions and 626 deletions
Download Patch File Download Diff File Expand all files Collapse all files

28
guilib/src/DatabaseViewer.cpp
View File

@@ -803,10 +803,10 @@ void DatabaseViewer::view3DMap()
if(maxDepth)
{
cloud = rtabmap::util3d::passThrough(cloud, "z", 0, maxDepth);
cloud = rtabmap::util3d::passThrough<pcl::PointXYZRGB>(cloud, "z", 0, maxDepth);
}
cloud = rtabmap::util3d::transformPointCloud(cloud, localTransform);
cloud = rtabmap::util3d::transformPointCloud<pcl::PointXYZRGB>(cloud, localTransform);
QColor color = Qt::red;
int mapId = memory_->getMapId(iter->first);
@@ -929,10 +929,10 @@ void DatabaseViewer::generate3DMap()
if(maxDepth)
{
cloud = rtabmap::util3d::passThrough(cloud, "z", 0, maxDepth);
cloud = rtabmap::util3d::passThrough<pcl::PointXYZRGB>(cloud, "z", 0, maxDepth);
}
cloud = rtabmap::util3d::transformPointCloud(cloud, pose*localTransform);
cloud = rtabmap::util3d::transformPointCloud<pcl::PointXYZRGB>(cloud, pose*localTransform);
std::string name = uFormat("%s/node%d.pcd", path.toStdString().c_str(), iter->first);
pcl::io::savePCDFile(name, *cloud);
UINFO("Saved %s (%d points)", name.c_str(), cloud->size());
@@ -1427,8 +1427,8 @@ void DatabaseViewer::updateConstraintView(const rtabmap::Link & link,
1);
}
cloudA = rtabmap::util3d::removeNaNFromPointCloud(cloudA);
cloudA = rtabmap::util3d::transformPointCloud(cloudA, localTransformA);
cloudA = rtabmap::util3d::removeNaNFromPointCloud<pcl::PointXYZRGB>(cloudA);
cloudA = rtabmap::util3d::transformPointCloud<pcl::PointXYZRGB>(cloudA, localTransformA);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudB;
if(depthB.type() == CV_8UC1)
@@ -1450,14 +1450,14 @@ void DatabaseViewer::updateConstraintView(const rtabmap::Link & link,
1);
}
cloudB = rtabmap::util3d::removeNaNFromPointCloud(cloudB);
cloudB = rtabmap::util3d::transformPointCloud(cloudB, t*localTransformB);
cloudB = rtabmap::util3d::removeNaNFromPointCloud<pcl::PointXYZRGB>(cloudB);
cloudB = rtabmap::util3d::transformPointCloud<pcl::PointXYZRGB>(cloudB, t*localTransformB);
//cloud 2d
pcl::PointCloud<pcl::PointXYZ>::Ptr scanA, scanB;
scanA = rtabmap::util3d::depth2DToPointCloud(depth2dA);
scanB = rtabmap::util3d::depth2DToPointCloud(depth2dB);
scanB = rtabmap::util3d::transformPointCloud(scanB, t);
scanB = rtabmap::util3d::transformPointCloud<pcl::PointXYZ>(scanB, t);
if(cloudA->size())
{
@@ -1746,8 +1746,8 @@ void DatabaseViewer::refineConstraint(int from, int to)
//voxelize
if(ui_->doubleSpinBox_icp_voxel->value() > 0.0f)
{
oldCloud = util3d::voxelize(oldCloud, ui_->doubleSpinBox_icp_voxel->value());
newCloud = util3d::voxelize(newCloud, ui_->doubleSpinBox_icp_voxel->value());
oldCloud = util3d::voxelize<pcl::PointXYZ>(oldCloud, ui_->doubleSpinBox_icp_voxel->value());
newCloud = util3d::voxelize<pcl::PointXYZ>(newCloud, ui_->doubleSpinBox_icp_voxel->value());
}
if(newCloud->size() && oldCloud->size())
@@ -1794,13 +1794,13 @@ void DatabaseViewer::refineConstraint(int from, int to)
pcl::PointCloud<pcl::PointNormal>::Ptr cloudANormals = util3d::computeNormals(cloudA, ui_->spinBox_icp_normalKSearch->value());
pcl::PointCloud<pcl::PointNormal>::Ptr cloudBNormals = util3d::computeNormals(cloudB, ui_->spinBox_icp_normalKSearch->value());
cloudANormals = util3d::removeNaNNormalsFromPointCloud(cloudANormals);
cloudANormals = util3d::removeNaNNormalsFromPointCloud<pcl::PointNormal>(cloudANormals);
if(cloudA->size() != cloudANormals->size())
{
UWARN("removed nan normals...");
}
cloudBNormals = util3d::removeNaNNormalsFromPointCloud(cloudBNormals);
cloudBNormals = util3d::removeNaNNormalsFromPointCloud<pcl::PointNormal>(cloudBNormals);
if(cloudB->size() != cloudBNormals->size())
{
UWARN("removed nan normals...");
@@ -1848,7 +1848,7 @@ void DatabaseViewer::refineConstraint(int from, int to)
}
if(ui_->dockWidget_constraints->isVisible())
{
cloudB = util3d::transformPointCloud(cloudB, transform);
cloudB = util3d::transformPointCloud<pcl::PointXYZ>(cloudB, transform);
this->updateConstraintView(newLink, cloudA, cloudB);
}
}

18
guilib/src/LoopClosureViewer.cpp
View File

@@ -166,17 +166,17 @@ void LoopClosureViewer::updateView(const Transform & transform)
sA_->getDepthFx(), sA_->getDepthFy(),
decimation);
}
cloudA = util3d::removeNaNFromPointCloud<pcl::PointXYZRGB>(cloudA);
if(maxDepth>0.0)
{
{
cloudA = util3d::passThrough<pcl::PointXYZRGB>(cloudA, "z", 0, maxDepth);
}
if(samples>0 && (int)cloudA->size() > samples)
{
{
cloudA = util3d::sampling<pcl::PointXYZRGB>(cloudA, samples);
}
}
cloudA = util3d::transformPointCloud<pcl::PointXYZRGB>(cloudA, sA_->getLocalTransform());
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudB;
@@ -198,23 +198,23 @@ void LoopClosureViewer::updateView(const Transform & transform)
sB_->getDepthFx(), sB_->getDepthFy(),
decimation);
}
cloudB = util3d::removeNaNFromPointCloud<pcl::PointXYZRGB>(cloudB);
if(maxDepth>0.0)
{
{
cloudB = util3d::passThrough<pcl::PointXYZRGB>(cloudB, "z", 0, maxDepth);
}
if(samples>0 && (int)cloudB->size() > samples)
{
{
cloudB = util3d::sampling<pcl::PointXYZRGB>(cloudB, samples);
}
}
cloudB = util3d::transformPointCloud<pcl::PointXYZRGB>(cloudB, t*sB_->getLocalTransform());
//cloud 2d
pcl::PointCloud<pcl::PointXYZ>::Ptr scanA, scanB;
scanA = util3d::depth2DToPointCloud(depth2dA);
scanB = util3d::depth2DToPointCloud(depth2dB);
scanB = util3d::depth2DToPointCloud(depth2dB);
scanB = util3d::transformPointCloud<pcl::PointXYZ>(scanB, t);
ui_->label_idA->setText(QString("[%1 (%2) -> %3 (%4)]").arg(sB_->id()).arg(cloudB->size()).arg(sA_->id()).arg(cloudA->size()));

28
guilib/src/MainWindow.cpp
View File

@@ -636,7 +636,7 @@ void MainWindow::processOdometry(const rtabmap::SensorData & data, int quality,
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;
cloud = util3d::depth2DToPointCloud(data.depth2d());
cloud = util3d::transformPointCloud(cloud, pose);
cloud = util3d::transformPointCloud<pcl::PointXYZ>(cloud, pose);
if(!_ui->widget_cloudViewer->addOrUpdateCloud("scanOdom", cloud, _odometryCorrection))
{
UERROR("Adding scanOdom to viewer failed!");
@@ -3178,13 +3178,13 @@ bool MainWindow::getExportedScans(std::map<int, pcl::PointCloud<pcl::PointXYZ>::
{
if(assemble)
{
*assembledScans += *util3d::transformPointCloud(scan, iter->second);;
*assembledScans += *util3d::transformPointCloud<pcl::PointXYZ>(scan, iter->second);;
if(count++ % 100 == 0)
{
if(assembledScans->size() && voxel)
{
assembledScans = util3d::voxelize(assembledScans, voxel);
assembledScans = util3d::voxelize<pcl::PointXYZ>(assembledScans, voxel);
}
}
}
@@ -3211,7 +3211,7 @@ bool MainWindow::getExportedScans(std::map<int, pcl::PointCloud<pcl::PointXYZ>::
{
if(voxel && assembledScans->size())
{
assembledScans = util3d::voxelize(assembledScans, voxel);
assembledScans = util3d::voxelize<pcl::PointXYZ>(assembledScans, voxel);
}
if(assembledScans->size())
{
@@ -3529,7 +3529,7 @@ void MainWindow::saveClouds(const std::map<int, pcl::PointCloud<pcl::PointXYZRGB
if(iter->second->size())
{
pcl::PointCloud<pcl::PointXYZRGB>::Ptr transformedCloud;
transformedCloud = util3d::transformPointCloud(iter->second, _currentPosesMap.at(iter->first));
transformedCloud = util3d::transformPointCloud<pcl::PointXYZRGB>(iter->second, _currentPosesMap.at(iter->first));
QString pathFile = path+QDir::separator()+QString("%1%2.%3").arg(prefix).arg(iter->first).arg(suffix);
bool success =false;
@@ -3630,7 +3630,7 @@ void MainWindow::saveMeshes(const std::map<int, pcl::PolygonMesh::Ptr> & meshes)
mesh.polygons = iter->second->polygons;
pcl::PointCloud<pcl::PointXYZRGB>::Ptr tmp(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::fromPCLPointCloud2(iter->second->cloud, *tmp);
tmp = util3d::transformPointCloud(tmp, _currentPosesMap.at(iter->first));
tmp = util3d::transformPointCloud<pcl::PointXYZRGB>(tmp, _currentPosesMap.at(iter->first));
pcl::toPCLPointCloud2(*tmp, mesh.cloud);
QString pathFile = path+QDir::separator()+QString("%1%2.%3").arg(prefix).arg(iter->first).arg(suffix);
@@ -3736,7 +3736,7 @@ void MainWindow::saveScans(const std::map<int, pcl::PointCloud<pcl::PointXYZ>::P
if(iter->second->size())
{
pcl::PointCloud<pcl::PointXYZ>::Ptr transformedCloud;
transformedCloud = util3d::transformPointCloud(iter->second, _currentPosesMap.at(iter->first));
transformedCloud = util3d::transformPointCloud<pcl::PointXYZ>(iter->second, _currentPosesMap.at(iter->first));
QString pathFile = path+QDir::separator()+QString("%1%2.%3").arg(prefix).arg(iter->first).arg(suffix);
bool success =false;
@@ -3814,24 +3814,24 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr MainWindow::createCloud(
bool filtered = false;
if(cloud->size() && maxDepth)
{
cloud = util3d::passThrough(cloud, "z", 0, maxDepth);
cloud = util3d::passThrough<pcl::PointXYZRGB>(cloud, "z", 0, maxDepth);
filtered = true;
}
if(cloud->size() && voxelSize)
{
cloud = util3d::voxelize(cloud, voxelSize);
cloud = util3d::voxelize<pcl::PointXYZRGB>(cloud, voxelSize);
filtered = true;
}
if(cloud->size() && !filtered)
{
cloud = util3d::removeNaNFromPointCloud (cloud);
cloud = util3d::removeNaNFromPointCloud<pcl::PointXYZRGB>(cloud);
}
if(cloud->size())
{
cloud = util3d::transformPointCloud(cloud, pose * localTransform);
cloud = util3d::transformPointCloud<pcl::PointXYZRGB>(cloud, pose * localTransform);
}
}
UDEBUG("Generated cloud %d (pts=%d) time=%fs", id, (int)cloud->size(), timer.ticks());
@@ -3874,7 +3874,7 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr MainWindow::getAssembledCloud(
}
else if(uContains(_createdClouds, iter->first))
{
cloud = util3d::transformPointCloud(_createdClouds.at(iter->first), iter->second);
cloud = util3d::transformPointCloud<pcl::PointXYZRGB>(_createdClouds.at(iter->first), iter->second);
}
if(cloud->size())
@@ -3903,7 +3903,7 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr MainWindow::getAssembledCloud(
{
if(assembledCloud->size() && assembledVoxelSize)
{
assembledCloud = util3d::voxelize(assembledCloud, assembledVoxelSize);
assembledCloud = util3d::voxelize<pcl::PointXYZRGB>(assembledCloud, assembledVoxelSize);
}
}
}
@@ -3917,7 +3917,7 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr MainWindow::getAssembledCloud(
if(assembledCloud->size() && assembledVoxelSize)
{
assembledCloud = util3d::voxelize(assembledCloud, assembledVoxelSize);
assembledCloud = util3d::voxelize<pcl::PointXYZRGB>(assembledCloud, assembledVoxelSize);
}
return assembledCloud;

4
guilib/src/OdometryViewer.cpp
View File

@@ -119,10 +119,10 @@ void OdometryViewer::processData()
if(voxelSize_ > 0.0f)
{
cloud = util3d::voxelize(cloud, voxelSize_);
cloud = util3d::voxelize<pcl::PointXYZRGB>(cloud, voxelSize_);
}
cloud = util3d::transformPointCloud(cloud, data.localTransform());
cloud = util3d::transformPointCloud<pcl::PointXYZRGB>(cloud, data.localTransform());
if(!data.pose().isNull())
{