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

Adding util3d::cloudsFromSensorData to be able to show in DBViewer individual clouds for each camera (#1182)

Browse Source 
* splitting multicam generated clouds

* make sure returned cloud is valid (can be empty)
This commit is contained in:
matlabbe
2023-12-13 12:15:29 -08:00
committed by GitHub
parent f56875db4a
commit be3e6c538c
3 changed files with 277 additions and 138 deletions
Download Patch File Download Diff File Expand all files Collapse all files

60
corelib/include/rtabmap/core/util3d.h
View File

@@ -144,6 +144,43 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_CORE_EXPORT cloudFromStereoImages
std::vector<int> * validIndices = 0,
const ParametersMap & parameters = ParametersMap());
/**
* Create a XYZ cloud from the images contained in SensorData, one for each camera
*
* @param sensorData, the sensor data.
* @param decimation, images are decimated by this factor before projecting points to 3D. The factor
* should be a factor of the image width and height.
* @param maxDepth, maximum depth of the projected points (farther points are set to null in case of an organized cloud).
* @param minDepth, minimum depth of the projected points (closer points are set to null in case of an organized cloud).
* @param validIndices, the indices of valid points in the cloud
* @param stereoParameters, stereo optional parameters (in case it is stereo data)
* @param roiRatios, [left, right, top, bottom] region of interest (in ratios) of the image projected.
* @return XYZ cloud(s), one per camera
*/
std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> RTABMAP_CORE_EXPORT cloudsFromSensorData(
const SensorData & sensorData,
int decimation = 1,
float maxDepth = 0.0f,
float minDepth = 0.0f,
std::vector<pcl::IndicesPtr> * validIndices = 0,
const ParametersMap & stereoParameters = ParametersMap(),
const std::vector<float> & roiRatios = std::vector<float>()); // ignored for stereo
/**
* Create a XYZ cloud from the images contained in SensorData. If there is only one camera,
* the returned cloud is organized. Otherwise, all NaN
* points are removed and the cloud will be dense.
*
* @param sensorData, the sensor data.
* @param decimation, images are decimated by this factor before projecting points to 3D. The factor
* should be a factor of the image width and height.
* @param maxDepth, maximum depth of the projected points (farther points are set to null in case of an organized cloud).
* @param minDepth, minimum depth of the projected points (closer points are set to null in case of an organized cloud).
* @param validIndices, the indices of valid points in the cloud
* @param stereoParameters, stereo optional parameters (in case it is stereo data)
* @param roiRatios, [left, right, top, bottom] region of interest (in ratios) of the image projected.
* @return a XYZ cloud.
*/
pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_CORE_EXPORT cloudFromSensorData(
const SensorData & sensorData,
int decimation = 1,
@@ -153,6 +190,28 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_CORE_EXPORT cloudFromSensorData(
const ParametersMap & stereoParameters = ParametersMap(),
const std::vector<float> & roiRatios = std::vector<float>()); // ignored for stereo
/**
* Create an RGB cloud from the images contained in SensorData, one for each camera
*
* @param sensorData, the sensor data.
* @param decimation, images are decimated by this factor before projecting points to 3D. The factor
* should be a factor of the image width and height.
* @param maxDepth, maximum depth of the projected points (farther points are set to null in case of an organized cloud).
* @param minDepth, minimum depth of the projected points (closer points are set to null in case of an organized cloud).
* @param validIndices, the indices of valid points in the cloud
* @param stereoParameters, stereo optional parameters (in case it is stereo data)
* @param roiRatios, [left, right, top, bottom] region of interest (in ratios) of the image projected.
* @return RGB cloud(s), one per camera
*/
std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr> RTABMAP_CORE_EXPORT cloudsRGBFromSensorData(
const SensorData & sensorData,
int decimation = 1,
float maxDepth = 0.0f,
float minDepth = 0.0f,
std::vector<pcl::IndicesPtr > * validIndices = 0,
const ParametersMap & stereoParameters = ParametersMap(),
const std::vector<float> & roiRatios = std::vector<float>()); // ignored for stereo
/**
* Create an RGB cloud from the images contained in SensorData. If there is only one camera,
* the returned cloud is organized. Otherwise, all NaN
@@ -164,6 +223,7 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_CORE_EXPORT cloudFromSensorData(
* @param maxDepth, maximum depth of the projected points (farther points are set to null in case of an organized cloud).
* @param minDepth, minimum depth of the projected points (closer points are set to null in case of an organized cloud).
* @param validIndices, the indices of valid points in the cloud
* @param stereoParameters, stereo optional parameters (in case it is stereo data)
* @param roiRatios, [left, right, top, bottom] region of interest (in ratios) of the image projected.
* @return a RGB cloud.
*/

203
corelib/src/util3d.cpp
View File

@@ -850,12 +850,12 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFromStereoImages(
validIndices);
}
pcl::PointCloud<pcl::PointXYZ>::Ptr cloudFromSensorData(
std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> cloudsFromSensorData(
const SensorData & sensorData,
int decimation,
float maxDepth,
float minDepth,
std::vector<int> * validIndices,
std::vector<pcl::IndicesPtr> * validIndices,
const ParametersMap & stereoParameters,
const std::vector<float> & roiRatios)
{
@@ -864,7 +864,7 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr cloudFromSensorData(
decimation = 1;
}
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> clouds;
if(!sensorData.depthRaw().empty() && sensorData.cameraModels().size())
{
@@ -873,6 +873,11 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr cloudFromSensorData(
int subImageWidth = sensorData.depthRaw().cols/sensorData.cameraModels().size();
for(unsigned int i=0; i<sensorData.cameraModels().size(); ++i)
{
clouds.push_back(pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>));
if(validIndices)
{
validIndices->push_back(pcl::IndicesPtr(new std::vector<int>()));
}
if(sensorData.cameraModels()[i].isValidForProjection())
{
cv::Mat depth = cv::Mat(sensorData.depthRaw(), cv::Rect(subImageWidth*i, 0, subImageWidth, sensorData.depthRaw().rows));
@@ -928,7 +933,7 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr cloudFromSensorData(
decimation,
maxDepth,
minDepth,
sensorData.cameraModels().size()==1?validIndices:0);
validIndices?validIndices->back().get():0);
if(tmp->size())
{
@@ -936,16 +941,7 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr cloudFromSensorData(
{
tmp = util3d::transformPointCloud(tmp, model.localTransform());
}
if(sensorData.cameraModels().size() > 1)
{
tmp = util3d::removeNaNFromPointCloud(tmp);
*cloud += *tmp;
}
else
{
cloud = tmp;
}
clouds.back() = tmp;
}
}
else
@@ -974,6 +970,11 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr cloudFromSensorData(
int subImageWidth = sensorData.rightRaw().cols/sensorData.stereoCameraModels().size();
for(unsigned int i=0; i<sensorData.stereoCameraModels().size(); ++i)
{
clouds.push_back(pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>));
if(validIndices)
{
validIndices->push_back(pcl::IndicesPtr(new std::vector<int>()));
}
if(sensorData.stereoCameraModels()[i].isValidForProjection())
{
cv::Mat left(leftMono, cv::Rect(subImageWidth*i, 0, subImageWidth, leftMono.rows));
@@ -1014,7 +1015,7 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr cloudFromSensorData(
decimation,
maxDepth,
minDepth,
validIndices);
validIndices?validIndices->back().get():0);
if(tmp->size())
{
@@ -1022,16 +1023,7 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr cloudFromSensorData(
{
tmp = util3d::transformPointCloud(tmp, model.localTransform());
}
if(sensorData.stereoCameraModels().size() > 1)
{
tmp = util3d::removeNaNFromPointCloud(tmp);
*cloud += *tmp;
}
else
{
cloud = tmp;
}
clouds.back() = tmp;
}
}
else
@@ -1041,19 +1033,10 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr cloudFromSensorData(
}
}
if(!cloud->empty() && cloud->is_dense && validIndices)
{
//generate indices for all points (they are all valid)
validIndices->resize(cloud->size());
for(unsigned int i=0; i<cloud->size(); ++i)
{
validIndices->at(i) = i;
}
}
return cloud;
return clouds;
}
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudRGBFromSensorData(
pcl::PointCloud<pcl::PointXYZ>::Ptr cloudFromSensorData(
const SensorData & sensorData,
int decimation,
float maxDepth,
@@ -1061,13 +1044,66 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudRGBFromSensorData(
std::vector<int> * validIndices,
const ParametersMap & stereoParameters,
const std::vector<float> & roiRatios)
{
std::vector<pcl::IndicesPtr> validIndicesV;
std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> clouds = cloudsFromSensorData(
sensorData,
decimation,
maxDepth,
minDepth,
validIndices?&validIndicesV:0,
stereoParameters,
roiRatios);
if(validIndices)
{
UASSERT(validIndicesV.size() == clouds.size());
}
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
if(clouds.size() == 1)
{
cloud = clouds[0];
if(validIndices)
{
*validIndices = *validIndicesV[0];
}
}
else
{
for(size_t i=0; i<clouds.size(); ++i)
{
*cloud += *util3d::removeNaNFromPointCloud(clouds[i]);
}
if(validIndices)
{
//generate indices for all points (they are all valid)
validIndices->resize(cloud->size());
for(size_t i=0; i<cloud->size(); ++i)
{
validIndices->at(i) = i;
}
}
}
return cloud;
}
std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr> cloudsRGBFromSensorData(
const SensorData & sensorData,
int decimation,
float maxDepth,
float minDepth,
std::vector<pcl::IndicesPtr> * validIndices,
const ParametersMap & stereoParameters,
const std::vector<float> & roiRatios)
{
if(decimation == 0)
{
decimation = 1;
}
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr> clouds;
if(!sensorData.imageRaw().empty() && !sensorData.depthRaw().empty() && sensorData.cameraModels().size())
{
@@ -1082,6 +1118,11 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudRGBFromSensorData(
for(unsigned int i=0; i<sensorData.cameraModels().size(); ++i)
{
clouds.push_back(pcl::PointCloud<pcl::PointXYZRGB>::Ptr(new pcl::PointCloud<pcl::PointXYZRGB>));
if(validIndices)
{
validIndices->push_back(pcl::IndicesPtr(new std::vector<int>()));
}
if(sensorData.cameraModels()[i].isValidForProjection())
{
cv::Mat rgb(sensorData.imageRaw(), cv::Rect(subRGBWidth*i, 0, subRGBWidth, sensorData.imageRaw().rows));
@@ -1128,7 +1169,7 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudRGBFromSensorData(
decimation,
maxDepth,
minDepth,
sensorData.cameraModels().size() == 1?validIndices:0);
validIndices?validIndices->back().get():0);
if(tmp->size())
{
@@ -1136,16 +1177,7 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudRGBFromSensorData(
{
tmp = util3d::transformPointCloud(tmp, model.localTransform());
}
if(sensorData.cameraModels().size() > 1)
{
tmp = util3d::removeNaNFromPointCloud(tmp);
*cloud += *tmp;
}
else
{
cloud = tmp;
}
clouds.back() = tmp;
}
}
else
@@ -1164,6 +1196,11 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudRGBFromSensorData(
int subImageWidth = sensorData.rightRaw().cols/sensorData.stereoCameraModels().size();
for(unsigned int i=0; i<sensorData.stereoCameraModels().size(); ++i)
{
clouds.push_back(pcl::PointCloud<pcl::PointXYZRGB>::Ptr(new pcl::PointCloud<pcl::PointXYZRGB>));
if(validIndices)
{
validIndices->push_back(pcl::IndicesPtr(new std::vector<int>()));
}
if(sensorData.stereoCameraModels()[i].isValidForProjection())
{
cv::Mat left(sensorData.imageRaw(), cv::Rect(subImageWidth*i, 0, subImageWidth, sensorData.imageRaw().rows));
@@ -1205,7 +1242,7 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudRGBFromSensorData(
decimation,
maxDepth,
minDepth,
validIndices,
validIndices?validIndices->back().get():0,
stereoParameters);
if(tmp->size())
@@ -1214,16 +1251,7 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudRGBFromSensorData(
{
tmp = util3d::transformPointCloud(tmp, model.localTransform());
}
if(sensorData.stereoCameraModels().size() > 1)
{
tmp = util3d::removeNaNFromPointCloud(tmp);
*cloud += *tmp;
}
else
{
cloud = tmp;
}
clouds.back() = tmp;
}
}
else
@@ -1233,16 +1261,59 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudRGBFromSensorData(
}
}
if(cloud->is_dense && validIndices)
return clouds;
}
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudRGBFromSensorData(
const SensorData & sensorData,
int decimation,
float maxDepth,
float minDepth,
std::vector<int> * validIndices,
const ParametersMap & stereoParameters,
const std::vector<float> & roiRatios)
{
std::vector<pcl::IndicesPtr> validIndicesV;
std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr> clouds = cloudsRGBFromSensorData(
sensorData,
decimation,
maxDepth,
minDepth,
validIndices?&validIndicesV:0,
stereoParameters,
roiRatios);
if(validIndices)
{
//generate indices for all points (they are all valid)
validIndices->resize(cloud->size());
for(unsigned int i=0; i<cloud->size(); ++i)
{
validIndices->at(i) = i;
}
UASSERT(validIndicesV.size() == clouds.size());
}
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
if(clouds.size() == 1)
{
cloud = clouds[0];
if(validIndices)
{
*validIndices = *validIndicesV[0];
}
}
else
{
for(size_t i=0; i<clouds.size(); ++i)
{
*cloud += *util3d::removeNaNFromPointCloud(clouds[i]);
}
if(validIndices)
{
//generate indices for all points (they are all valid)
validIndices->resize(cloud->size());
for(size_t i=0; i<cloud->size(); ++i)
{
validIndices->at(i) = i;
}
}
}
return cloud;
}

154
guilib/src/DatabaseViewer.cpp
View File

@@ -4876,14 +4876,8 @@ void DatabaseViewer::update(int value,
{
cloudViewer_->removeAllLines();
cloudViewer_->removeAllFrustums();
cloudViewer_->removeCloud("mesh");
cloudViewer_->removeCloud("cloud");
cloudViewer_->removeCloud("scan");
cloudViewer_->removeCloud("map");
cloudViewer_->removeCloud("ground");
cloudViewer_->removeCloud("obstacles");
cloudViewer_->removeCloud("empty_cells");
cloudViewer_->removeCloud("words");
cloudViewer_->removeAllClouds();
cloudViewer_->removeOctomap();
Transform pose = Transform::getIdentity();
@@ -5047,8 +5041,8 @@ void DatabaseViewer::update(int value,
{
if(!data.imageRaw().empty())
{
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
pcl::IndicesPtr indices(new std::vector<int>);
std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr> clouds;
std::vector<pcl::IndicesPtr> allIndices;
if(!data.depthRaw().empty() && data.cameraModels().size()==1)
{
cv::Mat depth = data.depthRaw();
@@ -5056,96 +5050,110 @@ void DatabaseViewer::update(int value,
{
depth = util2d::fillDepthHoles(depth, ui_->spinBox_mesh_fillDepthHoles->value(), float(ui_->spinBox_mesh_depthError->value())/100.0f);
}
cloud = util3d::cloudFromDepthRGB(
pcl::IndicesPtr indices(new std::vector<int>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = util3d::cloudFromDepthRGB(
data.imageRaw(),
depth,
data.cameraModels()[0],
ui_->spinBox_decimation->value(),0,0,indices.get());
if(indices->size())
{
cloud = util3d::transformPointCloud(cloud, data.cameraModels()[0].localTransform());
clouds.push_back(util3d::transformPointCloud(cloud, data.cameraModels()[0].localTransform()));
allIndices.push_back(indices);
}
}
else
{
cloud = util3d::cloudRGBFromSensorData(data, ui_->spinBox_decimation->value(), 0, 0, indices.get(), ui_->parameters_toolbox->getParameters());
clouds = util3d::cloudsRGBFromSensorData(data, ui_->spinBox_decimation->value(), 0, 0, &allIndices, ui_->parameters_toolbox->getParameters());
}
if(indices->size())
UASSERT(clouds.size() == allIndices.size());
for(size_t i=0; i<allIndices.size(); ++i)
{
if(ui_->doubleSpinBox_voxelSize->value() > 0.0)
if(allIndices[i]->size())
{
cloud = util3d::voxelize(cloud, indices, ui_->doubleSpinBox_voxelSize->value());
}
if(ui_->checkBox_showMesh->isChecked() && !cloud->is_dense)
{
Eigen::Vector3f viewpoint(0.0f,0.0f,0.0f);
if(data.cameraModels().size() && !data.cameraModels()[0].localTransform().isNull())
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = clouds[i];
pcl::IndicesPtr indices = allIndices[i];
if(ui_->doubleSpinBox_voxelSize->value() > 0.0)
{
viewpoint[0] = data.cameraModels()[0].localTransform().x();
viewpoint[1] = data.cameraModels()[0].localTransform().y();
viewpoint[2] = data.cameraModels()[0].localTransform().z();
cloud = util3d::voxelize(cloud, indices, ui_->doubleSpinBox_voxelSize->value());
}
else if(data.stereoCameraModels().size() && !data.stereoCameraModels()[0].localTransform().isNull())
{
viewpoint[0] = data.stereoCameraModels()[0].localTransform().x();
viewpoint[1] = data.stereoCameraModels()[0].localTransform().y();
viewpoint[2] = data.stereoCameraModels()[0].localTransform().z();
}
std::vector<pcl::Vertices> polygons = util3d::organizedFastMesh(
cloud,
float(ui_->spinBox_mesh_angleTolerance->value())*M_PI/180.0f,
ui_->checkBox_mesh_quad->isChecked(),
ui_->spinBox_mesh_triangleSize->value(),
viewpoint);
if(ui_->spinBox_mesh_minClusterSize->value())
if(ui_->checkBox_showMesh->isChecked() && !cloud->is_dense)
{
// filter polygons
std::vector<std::set<int> > neighbors;
std::vector<std::set<int> > vertexToPolygons;
util3d::createPolygonIndexes(polygons,
cloud->size(),
neighbors,
vertexToPolygons);
std::list<std::list<int> > clusters = util3d::clusterPolygons(
neighbors,
ui_->spinBox_mesh_minClusterSize->value());
std::vector<pcl::Vertices> filteredPolygons(polygons.size());
int oi=0;
for(std::list<std::list<int> >::iterator iter=clusters.begin(); iter!=clusters.end(); ++iter)
Eigen::Vector3f viewpoint(0.0f,0.0f,0.0f);
if(data.cameraModels().size() && !data.cameraModels()[0].localTransform().isNull())
{
for(std::list<int>::iterator jter=iter->begin(); jter!=iter->end(); ++jter)
{
filteredPolygons[oi++] = polygons.at(*jter);
}
viewpoint[0] = data.cameraModels()[0].localTransform().x();
viewpoint[1] = data.cameraModels()[0].localTransform().y();
viewpoint[2] = data.cameraModels()[0].localTransform().z();
}
filteredPolygons.resize(oi);
polygons = filteredPolygons;
}
else if(data.stereoCameraModels().size() && !data.stereoCameraModels()[0].localTransform().isNull())
{
viewpoint[0] = data.stereoCameraModels()[0].localTransform().x();
viewpoint[1] = data.stereoCameraModels()[0].localTransform().y();
viewpoint[2] = data.stereoCameraModels()[0].localTransform().z();
}
std::vector<pcl::Vertices> polygons = util3d::organizedFastMesh(
cloud,
float(ui_->spinBox_mesh_angleTolerance->value())*M_PI/180.0f,
ui_->checkBox_mesh_quad->isChecked(),
ui_->spinBox_mesh_triangleSize->value(),
viewpoint);
cloudViewer_->addCloudMesh("mesh", cloud, polygons, pose);
}
if(ui_->checkBox_showCloud->isChecked())
{
cloudViewer_->addCloud("cloud", cloud, pose);
if(ui_->spinBox_mesh_minClusterSize->value())
{
// filter polygons
std::vector<std::set<int> > neighbors;
std::vector<std::set<int> > vertexToPolygons;
util3d::createPolygonIndexes(polygons,
cloud->size(),
neighbors,
vertexToPolygons);
std::list<std::list<int> > clusters = util3d::clusterPolygons(
neighbors,
ui_->spinBox_mesh_minClusterSize->value());
std::vector<pcl::Vertices> filteredPolygons(polygons.size());
int oi=0;
for(std::list<std::list<int> >::iterator iter=clusters.begin(); iter!=clusters.end(); ++iter)
{
for(std::list<int>::iterator jter=iter->begin(); jter!=iter->end(); ++jter)
{
filteredPolygons[oi++] = polygons.at(*jter);
}
}
filteredPolygons.resize(oi);
polygons = filteredPolygons;
}
cloudViewer_->addCloudMesh(uFormat("mesh_%d", i), cloud, polygons, pose);
}
if(ui_->checkBox_showCloud->isChecked())
{
cloudViewer_->addCloud(uFormat("cloud_%d", i), cloud, pose);
}
}
}
}
else if(ui_->checkBox_showCloud->isChecked())
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;
pcl::IndicesPtr indices(new std::vector<int>);
cloud = util3d::cloudFromSensorData(data, ui_->spinBox_decimation->value(), 0, 0, indices.get(), ui_->parameters_toolbox->getParameters());
if(indices->size())
{
if(ui_->doubleSpinBox_voxelSize->value() > 0.0)
{
cloud = util3d::voxelize(cloud, indices, ui_->doubleSpinBox_voxelSize->value());
}
std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> clouds;
std::vector<pcl::IndicesPtr> allIndices;
cloudViewer_->addCloud("cloud", cloud, pose);
clouds = util3d::cloudsFromSensorData(data, ui_->spinBox_decimation->value(), 0, 0, &allIndices, ui_->parameters_toolbox->getParameters());
UASSERT(clouds.size() == allIndices.size());
for(size_t i=0; i<allIndices.size(); ++i)
{
if(allIndices[i]->size())
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = clouds[i];
pcl::IndicesPtr indices = allIndices[i];
if(ui_->doubleSpinBox_voxelSize->value() > 0.0)
{
cloud = util3d::voxelize(cloud, indices, ui_->doubleSpinBox_voxelSize->value());
}
cloudViewer_->addCloud(uFormat("cloud_%d", i), cloud, pose);
}
}
}
}