void CreateGridMesh(
const VoxelGrid<Discretizer>& vg,
std::vector<Vector3>& vertices)
{
std::vector<Vector3> voxel_mesh;
CreateBoxMesh(vg.res().x(), vg.res().y(), vg.res().z(), voxel_mesh);
vertices.reserve(voxel_mesh.size() * vg.sizeX() * vg.sizeY() * vg.sizeZ());
for (int x = 0; x < vg.sizeX(); ++x) {
for (int y = 0; y < vg.sizeY(); ++y) {
for (int z = 0; z < vg.sizeZ(); ++z) {
const MemoryCoord mc(x, y, z);
const WorldCoord wc(vg.memoryToWorld(mc));
Vector3 wp(wc.x, wc.y, wc.z);
std::printf("%d, %d, %d -> %0.3f, %0.3f, %0.3f\n", x, y, z, wc.x, wc.y, wc.z);
// translate all triangles by the voxel position
for (const Vector3& v : voxel_mesh) {
vertices.push_back(v + wp);
}
}
}
}
}
static void downsampling(PointCloudPtr cloudPCLInput, PointCloudPtr cloudPCLOutput, double dLeafSize)
{
VoxelGrid<PointT> downsampler;
downsampler.setInputCloud(cloudPCLInput);
downsampler.setLeafSize(dLeafSize, dLeafSize, dLeafSize);
downsampler.filter(*cloudPCLOutput);
}
/**
* Implements the Voxel Grid Filter.
* Gets the leafs size as arguments (floating point).
* Returns a pointer to the filtered cloud.
*/
PointCloud<pointType>::Ptr FilterHandler::voxelGridFilter(float xLeafSize,
float yLeafSize,
float zLeafSize)
{
VoxelGrid<pointType> sor;
sor.setInputCloud(_cloud);
sor.setLeafSize(xLeafSize, yLeafSize, zLeafSize);
sor.filter(*_cloud);
io::savePCDFile(_output, *_cloud, true);
return _cloud;
}
/**
* Reducing the number of elements in a point cloud using a
* voxel grid with configured leaf size.
* The main goal is to increase processing speed.
*/
void scaleCloud(
TheiaCloudPtr in,
double inLeafSize,
TheiaCloudPtr out
){
VoxelGrid<TheiaPoint> grid;
grid.setLeafSize(inLeafSize, inLeafSize, inLeafSize);
grid.setInputCloud(in);
grid.filter(*out);
}
// Subsample cloud for faster matching and processing, while filling in normals.
void PointcloudProc::reduceCloud(const PointCloud<PointXYZRGB>& input, PointCloud<PointXYZRGBNormal>& output) const
{
PointCloud<PointXYZRGB> cloud_nan_filtered, cloud_box_filtered, cloud_voxel_reduced;
PointCloud<Normal> normals;
PointCloud<PointXYZRGBNormal> cloud_normals;
std::vector<int> indices;
// Filter out nans.
removeNaNFromPointCloud(input, cloud_nan_filtered, indices);
indices.clear();
// Filter out everything outside a [200x200x200] box.
Eigen::Vector4f min_pt(-100, -100, -100, -100);
Eigen::Vector4f max_pt(100, 100, 100, 100);
getPointsInBox(cloud_nan_filtered, min_pt, max_pt, indices);
ExtractIndices<PointXYZRGB> boxfilter;
boxfilter.setInputCloud(boost::make_shared<const PointCloud<PointXYZRGB> >(cloud_nan_filtered));
boxfilter.setIndices (boost::make_shared<vector<int> > (indices));
boxfilter.filter(cloud_box_filtered);
// Reduce pointcloud
VoxelGrid<PointXYZRGB> voxelfilter;
voxelfilter.setInputCloud (boost::make_shared<const PointCloud<PointXYZRGB> > (cloud_box_filtered));
voxelfilter.setLeafSize (0.05, 0.05, 0.05);
// voxelfilter.setLeafSize (0.1, 0.1, 0.1);
voxelfilter.filter (cloud_voxel_reduced);
// Compute normals
NormalEstimation<PointXYZRGB, Normal> normalest;
normalest.setViewPoint(0, 0, 0);
normalest.setSearchMethod (boost::make_shared<search::KdTree<PointXYZRGB> > ());
//normalest.setKSearch (10);
normalest.setRadiusSearch (0.25);
// normalest.setRadiusSearch (0.4);
normalest.setInputCloud(boost::make_shared<const PointCloud<PointXYZRGB> >(cloud_voxel_reduced));
normalest.compute(normals);
pcl::concatenateFields (cloud_voxel_reduced, normals, cloud_normals);
// Filter based on curvature
PassThrough<PointXYZRGBNormal> normalfilter;
normalfilter.setFilterFieldName("curvature");
// normalfilter.setFilterLimits(0.0, 0.2);
normalfilter.setFilterLimits(0.0, 0.2);
normalfilter.setInputCloud(boost::make_shared<const PointCloud<PointXYZRGBNormal> >(cloud_normals));
normalfilter.filter(output);
}
OccupancyGrid::OccupancyGrid(
const VoxelGrid& voxel_grid,
const size_t density_channel_index,
const float occupancy_threshold)
: m_grid(
voxel_grid.get_xres(),
voxel_grid.get_yres(),
voxel_grid.get_zres(),
1)
{
initialize(
voxel_grid,
density_channel_index,
occupancy_threshold);
}
PointCloud<PointXYZI>::Ptr PointCloudFunctions::downSampleCloud(pcl::PointCloud<PointXYZI>::Ptr inputCloud, float leafSize, bool save, string fileNameToSave)
{
PointCloud<PointXYZI>::Ptr downsampled(new PointCloud<PointXYZI> ());
VoxelGrid<PointXYZI> sor;
sor.setInputCloud (inputCloud);
sor.setFilterLimits(0, 2000);
sor.setLeafSize (leafSize, leafSize, leafSize);
sor.filter (*downsampled);
if (save)
{
savePCDFileASCII (fileNameToSave, *downsampled);
}
return downsampled;
}
void write_voxel_grid(
const char* filename,
const VoxelGrid& grid)
{
FILE* file = fopen(filename, "wt");
if (file == 0)
return;
const size_t xres = grid.get_xres();
const size_t yres = grid.get_yres();
const size_t zres = grid.get_zres();
const size_t channel_count = grid.get_channel_count();
for (size_t z = 0; z < zres; ++z)
{
fprintf(file, "z " FMT_SIZE_T "\n\n", z);
for (size_t y = 0; y < yres; ++y)
{
for (size_t x = 0; x < xres; ++x)
{
if (x > 0)
fprintf(file, " ");
const float* voxel = grid.voxel(x, y, z);
for (size_t i = 0; i < channel_count; ++i)
{
if (i > 0)
fprintf(file, ",");
fprintf(file, "%f", voxel[i]);
}
}
fprintf(file, "\n");
}
fprintf(file, "\n");
}
fclose(file);
}
void callback( const sensor_msgs::ImageConstPtr& dep, const CameraInfoConstPtr& cam_info)
{
Time begin = Time::now();
// Debug info
cerr << "Recieved frame..." << endl;
cerr << "Cam info: fx:" << cam_info->K[0] << " fy:" << cam_info->K[4] << " cx:" << cam_info->K[2] <<" cy:" << cam_info->K[5] << endl;
cerr << "Depth image h:" << dep->height << " w:" << dep->width << " e:" << dep->encoding << " " << dep->step << endl;
//get image from message
cv_bridge::CvImagePtr cv_image = cv_bridge::toCvCopy(dep);
Mat depth = cv_image->image;
Normals normal(depth, cam_info);
PointCloud<pcl::PointXYZ>::Ptr cloud (new PointCloud<PointXYZ>);
for (int i = 0; i < normal.m_points.rows; ++i)
for (int j = 0; j < normal.m_points.cols; ++j)
{
Vec3f vector = normal.m_points.at<Vec3f>(i, j);
//pcl::Vec
cloud->push_back(pcl::PointXYZ(vector[0], vector[1], vector[2]));
}
VoxelGrid<PointXYZ> voxelgrid;
voxelgrid.setInputCloud(cloud);
voxelgrid.setLeafSize(0.05, 0.05, 0.05);
voxelgrid.filter(*cloud);
cloud->header.frame_id = OUTPUT_POINT_CLOUD_FRAMEID;
stringstream name;
name << "model_" << modelNo << ".pcd";
io::savePCDFile(name.str(), *cloud);
++modelNo;
pub.publish(cloud);
Time end = ros::Time::now();
cerr << "Computation time: " << (end-begin).nsec/1000000.0 << " ms." << endl;
cerr << "=========================================================" << endl;
}
bool VoxelmapTest::runTest()
{
std::vector<bool> testResults;
{
{
// Basic Test, simple grid.
std::vector<float> boxVert;
std::vector<size_t> boxInd;
makeBoxVertexIndices(Vector3(1.2f), Vector3(0.0f), boxVert, boxInd);
float voxelWidth = 1.0f; // With box size 1.2f, we should have center voxel empty, but immediately surrounded voxels full.
{
VoxelGrid* resultGrid = VoxelGridFactory::generateVoxelGridFromMesh((const float*)&boxVert[0], boxVert.size() / 3, &boxInd[0], boxInd.size() / 3, voxelWidth);
int numVoxels = resultGrid->numVoxels();
bool hasCorrectNumEntries = numVoxels == 26; //27 - 1 [This test is set up so that the central box is empty]
assert(hasCorrectNumEntries);
testResults.push_back(hasCorrectNumEntries);
}
{
// Add another box around 4.0, 4.0, 4.0
makeBoxVertexIndices(Vector3(1.2f), Vector3(4.0f), boxVert, boxInd);
VoxelGrid* resultGrid = VoxelGridFactory::generateVoxelGridFromMesh((const float*)&boxVert[0], boxVert.size() / 3, &boxInd[0], boxInd.size() / 3, voxelWidth);
int numVoxels = resultGrid->numVoxels();
bool hasCorrectNumEntries = numVoxels == 52; //27 - 1 [This test is set up so that the central box is empty]
assert(hasCorrectNumEntries);
testResults.push_back(hasCorrectNumEntries);
}
}
{
// Finer grid test
// Basic Test, simple grid.
std::vector<float> boxVert;
std::vector<size_t> boxInd;
makeBoxVertexIndices(Vector3(1.2f), Vector3(0.0f), boxVert, boxInd);
float voxelWidth = 0.2f; // With box size 1.2f, we should have center voxel empty, but immediately surrounded voxels full.
{
VoxelGrid* resultGrid = VoxelGridFactory::generateVoxelGridFromMesh((const float*)&boxVert[0], boxVert.size() / 3, &boxInd[0], boxInd.size() / 3, voxelWidth);
int numVoxels = resultGrid->numVoxels();
bool hasCorrectNumEntries = numVoxels == 7 * 7 * 7 - 5 * 5 * 5;
assert(hasCorrectNumEntries);
testResults.push_back(hasCorrectNumEntries);
}
}
}
for (auto testResult : testResults)
{
if (!testResult)
{
return false;
}
}
return true;
}
float OccupancyGrid::get_density_sum(
const VoxelGrid& voxel_grid,
const size_t density_channel_index,
const size_t x,
const size_t y,
const size_t z) const
{
float density_sum = 0.0f;
for (int dx = -1; dx <= +1; ++dx)
{
for (int dy = -1; dy <= +1; ++dy)
{
for (int dz = -1; dz <= +1; ++dz)
{
const int ix = static_cast<int>(x) + dx;
const int iy = static_cast<int>(y) + dy;
const int iz = static_cast<int>(z) + dz;
if (ix < 0 ||
iy < 0 ||
iz < 0 ||
ix >= static_cast<int>(voxel_grid.get_xres()) ||
iy >= static_cast<int>(voxel_grid.get_yres()) ||
iz >= static_cast<int>(voxel_grid.get_zres()))
continue;
const float* voxel = voxel_grid.voxel(ix, iy, iz);
assert(voxel[density_channel_index] >= 0.0f);
density_sum += voxel[density_channel_index];
}
}
}
return density_sum;
}
PartitionCellCount::
PartitionCellCount(const VoxelGrid & grid, Rect3i halfCellBounds,
int runlineDirection ) :
mGridDescription(grid.gridDescription()),
mNumCells(8),
mHalfCellBounds(halfCellBounds),
m_nnx(halfCellBounds.size(0)+1),
m_nny(halfCellBounds.size(1)+1),
m_nnz(halfCellBounds.size(2)+1)
{
long long allocSize = m_nnx*m_nny*m_nnz;
if (mMaterialIndexHalfCells.max_size() < allocSize)
{
cerr << "Warning: PartitionCellCount is going to attempt to allocate a "
<< m_nnx << "x" << m_nny << "x" << m_nnz << " cell array with "
"std::vector; the total size is " << allocSize << " and the vector"
" maximum size is " << mMaterialIndexHalfCells.max_size()
<< ", so this will likely fail." << endl;
}
mMaterialIndexHalfCells.resize(m_nnx*m_nny*m_nnz);
calcMaterialIndices(grid, runlineDirection);
allocateAuxiliaryDataSpace(grid);
}
void
compute (const sensor_msgs::PointCloud2::ConstPtr &input, sensor_msgs::PointCloud2 &output,
float leaf_x, float leaf_y, float leaf_z, const std::string &field, double fmin, double fmax)
{
TicToc tt;
tt.tic ();
print_highlight ("Computing ");
VoxelGrid<sensor_msgs::PointCloud2> grid;
grid.setInputCloud (input);
grid.setFilterFieldName (field);
grid.setFilterLimits (fmin, fmax);
grid.setLeafSize (leaf_x, leaf_y, leaf_z);
grid.filter (output);
print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" ms : "); print_value ("%d", output.width * output.height); print_info (" points]\n");
}
int main(int argc, char** argv)
{
if(argc <= 1 || console::find_argument(argc, argv, "-h") >= 0) {
printUsage(argv[0]);
}
//read file
vector<string> paths;
if(console::find_argument(argc,argv,"--file")>= 0) {
vector<int> indices(pcl::console::parse_file_extension_argument(argc, argv, "pcd"));
if (pcl::console::find_argument(argc, argv, "--save") >= 0) {
indices.erase(indices.end()-1);
}
Utilities::getFiles(argv, indices, paths);
indices.clear();
indices = pcl::console::parse_file_extension_argument(argc, argv, "ply");
Utilities::getFiles(argv, indices, paths);
}
// or read a folder
if(console::find_argument(argc,argv,"--folder")>= 0) {
Utilities::getFiles(argv[pcl::console::find_argument(argc, argv, "--folder") + 1], paths);
}
vector<PCLPointCloud2> cloud_blob;
PointCloud<PointXYZ>::Ptr ptr_cloud (new PointCloud<PointXYZ>);
Utilities::read(paths, cloud_blob);
Utilities::convert2XYZ(cloud_blob, ptr_cloud);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZ>);
float media = 50, devest = 1.0, size;
string axis ("z");
string savePath;
if(console::find_argument(argc,argv,"--save")>= 0) {
savePath = argv[console::find_argument(argc,argv,"--save") + 1];
}
Timer timer;
Log* ptr_log;
Log log(savePath);
ptr_log = &log;
string configuration("Filter:\n");
/* Statistical Filter */
if(console::find_argument(argc,argv,"-s")>= 0) {
if(!isAlpha(argv[console::find_argument(argc,argv,"-s") + 1])) {
media = atof(argv[console::find_argument(argc,argv,"-s") + 1]);
devest = atof(argv[console::find_argument(argc,argv,"-s") + 2]);
}
StatisticalOutlierRemoval<PointXYZ> sor;
sor.setInputCloud (ptr_cloud);
sor.setMeanK (media);
sor.setStddevMulThresh (devest);
sor.filter (*cloud_filtered);
configuration += "Statistical Outlier Removal\n";
configuration += "media: "+ to_string(media) +"\n";
configuration += "Desvest: "+ to_string(devest) +"\n";
configuration += "total point after filer: "+ to_string(cloud_filtered->height * cloud_filtered->width) +"\n";
configuration += "Time to complete: "+ timer.report() +"\n";
cout << configuration << endl;
ptr_log->write(configuration);
}
timer.reset();
/* Voxel Filter */
if(console::find_argument(argc,argv, "-v") >= 0) {
if(!isAlpha(argv[console::find_argument(argc,argv,"-v") + 1])) {
size = atof(argv[console::find_argument(argc,argv,"-v") + 1]);
}
// Create the filtering object
VoxelGrid<PointXYZ> sor;
sor.setInputCloud (ptr_cloud);
sor.setLeafSize (0.01f, 0.01f, 0.01f);
sor.filter (*cloud_filtered);
configuration += "Voxel Grid\n";
configuration += "size of voxel: "+ to_string(size) +"\n";
configuration += "lief size: "+ to_string(0.01) +","+ to_string(0.01) +"," +to_string(0.01)+"\n";
configuration += "total point after filer: "+ to_string(cloud_filtered->height * cloud_filtered->width) +"\n";
configuration += "Time to complete: "+ timer.report() +"\n";
cout << configuration << endl;
ptr_log->write(configuration);
}
timer.reset();
/* PassThroug Filter */
if(console::find_argument(argc,argv, "-p") >= 0) {
axis = argv[console::find_argument(argc,argv,"-p") + 1];
// Create the filtering object
pcl::PassThrough<pcl::PointXYZ> pass;
pass.setInputCloud (ptr_cloud);
pass.setFilterFieldName (axis);
pass.setFilterLimits (0.0, 1.0);
//pass.setFilterLimitsNegative (true);
pass.filter (*cloud_filtered);
configuration += "PassThroug\n";
configuration += "axis: "+ axis +"\n";
configuration += "range: "+ to_string(0.0) +","+ to_string(1.0) + "\n";
configuration += "total point after filer: "+ to_string(cloud_filtered->height * cloud_filtered->width) +"\n";
configuration += "Time to complete: "+ timer.report() +"\n";
cout << configuration << endl;
ptr_log->write(configuration);
}
/* Save */
if(console::find_argument(argc,argv,"--save")>= 0) {
int how_many_files = atoi(argv[console::find_argument(argc,argv,"--save") + 2]);
Utilities::writePCDFile(ptr_cloud, savePath, how_many_files );
}
ptr_log->close();
return 0;
}
int main(int argc, char** argv)
{
if (argc < 2)
{
cout << "Input a PCD file name...\n";
return 0;
}
PointCloud<PointXYZ>::Ptr cloud(new PointCloud<PointXYZ>), cloud_f(new PointCloud<PointXYZ>);
PCDReader reader;
reader.read(argv[1], *cloud);
cout << "PointCloud before filtering has: " << cloud->points.size() << " data points.\n";
PointCloud<PointXYZ>::Ptr cloud_filtered(new PointCloud<PointXYZ>);
VoxelGrid<PointXYZ> vg;
vg.setInputCloud(cloud);
vg.setLeafSize(0.01f, 0.01f, 0.01f);
vg.filter(*cloud_filtered);
cout << "PointCloud after filtering has: " << cloud_filtered->points.size() << " data points.\n";
SACSegmentation<PointXYZ> seg;
PointIndices::Ptr inliers(new PointIndices);
PointCloud<PointXYZ>::Ptr cloud_plane(new PointCloud<PointXYZ>);
ModelCoefficients::Ptr coefficients(new ModelCoefficients);
seg.setOptimizeCoefficients(true);
seg.setModelType(SACMODEL_PLANE);
seg.setMethodType(SAC_RANSAC);
seg.setMaxIterations(100);
seg.setDistanceThreshold(0.02);
int i=0, nr_points = (int)cloud_filtered->points.size();
while (cloud_filtered->points.size() > 0.3 * nr_points)
{
seg.setInputCloud(cloud_filtered);
seg.segment(*inliers, *coefficients);
if (inliers->indices.size() == 0)
{
cout << "Coud not estimate a planar model for the given dataset.\n";
break;
}
ExtractIndices<PointXYZ> extract;
extract.setInputCloud(cloud_filtered);
extract.setIndices(inliers);
extract.setNegative(false);
extract.filter(*cloud_plane);
cout << "PointCloud representing the planar component has: " << cloud_filtered->points.size() << " data points.\n";
extract.setNegative(true);
extract.filter(*cloud_f);
cloud_filtered->swap(*cloud_f);
}
search::KdTree<PointXYZ>::Ptr kdtree(new search::KdTree<PointXYZ>);
kdtree->setInputCloud(cloud_filtered);
vector<PointIndices> cluster_indices;
EuclideanClusterExtraction<PointXYZ> ece;
ece.setClusterTolerance(0.02);
ece.setMinClusterSize(100);
ece.setMaxClusterSize(25000);
ece.setSearchMethod(kdtree);
ece.setInputCloud(cloud_filtered);
ece.extract(cluster_indices);
PCDWriter writer;
int j = 0;
for (vector<PointIndices>::const_iterator it=cluster_indices.begin(); it != cluster_indices.end(); ++it)
{
PointCloud<PointXYZ>::Ptr cluster_cloud(new PointCloud<PointXYZ>);
for (vector<int>::const_iterator pit=it->indices.begin(); pit != it->indices.end(); ++pit)
cluster_cloud->points.push_back(cloud_filtered->points[*pit]);
cluster_cloud->width = cluster_cloud->points.size();
cluster_cloud->height = 1;
cluster_cloud->is_dense = true;
cout << "PointCloud representing a cluster has: " << cluster_cloud->points.size() << " data points.\n";
stringstream ss;
ss << "cloud_cluster_" << j << ".pcd";
writer.write<PointXYZ>(ss.str(), *cluster_cloud, false);
j++;
}
return 0;
}
void
compute (const sensor_msgs::PointCloud2::ConstPtr &input, sensor_msgs::PointCloud2 &output,
double search_radius, bool sqr_gauss_param_set, double sqr_gauss_param,
bool use_polynomial_fit, int polynomial_order)
{
PointCloud<PointXYZ>::Ptr xyz_cloud_pre (new pcl::PointCloud<PointXYZ> ()),
xyz_cloud (new pcl::PointCloud<PointXYZ> ());
fromROSMsg (*input, *xyz_cloud_pre);
// Filter the NaNs from the cloud
for (size_t i = 0; i < xyz_cloud_pre->size (); ++i)
if (pcl_isfinite (xyz_cloud_pre->points[i].x))
xyz_cloud->push_back (xyz_cloud_pre->points[i]);
xyz_cloud->header = xyz_cloud_pre->header;
xyz_cloud->height = 1;
xyz_cloud->width = xyz_cloud->size ();
xyz_cloud->is_dense = false;
// io::savePCDFile ("test.pcd", *xyz_cloud);
PointCloud<PointXYZ>::Ptr xyz_cloud_smoothed (new PointCloud<PointXYZ> ());
MovingLeastSquares<PointXYZ, Normal> mls;
mls.setInputCloud (xyz_cloud);
mls.setSearchRadius (search_radius);
if (sqr_gauss_param_set) mls.setSqrGaussParam (sqr_gauss_param);
mls.setPolynomialFit (use_polynomial_fit);
mls.setPolynomialOrder (polynomial_order);
// mls.setUpsamplingMethod (MovingLeastSquares<PointXYZ, Normal>::SAMPLE_LOCAL_PLANE);
mls.setUpsamplingMethod (MovingLeastSquares<PointXYZ, Normal>::UNIFORM_DENSITY);
// mls.setUpsamplingMethod (MovingLeastSquares<PointXYZ, Normal>::FILL_HOLES);
// mls.setUpsamplingMethod (MovingLeastSquares<PointXYZ, Normal>::NONE);
mls.setFillingStepSize (0.02);
mls.setPointDensity (50000*search_radius); // 300 points in a 5 cm radius
mls.setUpsamplingRadius (0.025);
mls.setUpsamplingStepSize (0.015);
search::KdTree<PointXYZ>::Ptr tree (new search::KdTree<PointXYZ> ());
mls.setSearchMethod (tree);
PointCloud<Normal>::Ptr mls_normals (new PointCloud<Normal> ());
mls.setOutputNormals (mls_normals);
PCL_INFO ("Computing smoothed surface and normals with search_radius %f , sqr_gaussian_param %f, polynomial fitting %d, polynomial order %d\n",
mls.getSearchRadius(), mls.getSqrGaussParam(), mls.getPolynomialFit(), mls.getPolynomialOrder());
TicToc tt;
tt.tic ();
mls.reconstruct (*xyz_cloud_smoothed);
print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" ms : "); print_value ("%d", xyz_cloud_smoothed->width * xyz_cloud_smoothed->height); print_info (" points]\n");
sensor_msgs::PointCloud2 output_positions, output_normals;
// printf ("sizes: %d %d %d\n", xyz_cloud_smoothed->width, xyz_cloud_smoothed->height, xyz_cloud_smoothed->size ());
toROSMsg (*xyz_cloud_smoothed, output_positions);
toROSMsg (*mls_normals, output_normals);
concatenateFields (output_positions, output_normals, output);
PointCloud<PointXYZ> xyz_vg;
VoxelGrid<PointXYZ> vg;
vg.setInputCloud (xyz_cloud_smoothed);
vg.setLeafSize (0.005, 0.005, 0.005);
vg.filter (xyz_vg);
sensor_msgs::PointCloud2 xyz_vg_2;
toROSMsg (xyz_vg, xyz_vg_2);
pcl::io::savePCDFile ("cloud_vg.pcd", xyz_vg_2, Eigen::Vector4f::Zero (),
Eigen::Quaternionf::Identity (), true);
}
/*! @brief runs the whole processing pipeline for FPFH features
*
* @note At the moment the evaluation results will be printed to console.
*
* @param[in] in the labeled input point cloud
* @param[out] ref_out the reference point cloud after the preprocessing steps
* @param[out] fpfh_out the labeled point cloud after the classifing process
*/
void processFPFH(const PointCloud<PointXYZRGB>::Ptr in,
PointCloud<PointXYZRGB>::Ptr ref_out,
PointCloud<PointXYZRGB>::Ptr fpfh_out)
{
PointCloud<Normal>::Ptr n(new PointCloud<Normal>());
PointCloud<FPFHSignature33>::Ptr fpfh(new PointCloud<FPFHSignature33>());
// Passthrough filtering (needs to be done to remove NaNs)
cout << "FPFH: Pass (with " << in->points.size() << " points)" << endl;
PassThrough<PointXYZRGB> pass;
pass.setInputCloud(in);
pass.setFilterFieldName("z");
pass.setFilterLimits(0.0f, pass_depth_);
pass.filter(*ref_out);
// Optional voxelgrid filtering
if (fpfh_vox_enable_)
{
cout << "FPFH: Voxel (with " << ref_out->points.size() << " points)" << endl;
VoxelGrid<PointXYZRGB> vox;
vox.setInputCloud(ref_out);
vox.setLeafSize(fpfh_vox_, fpfh_vox_, fpfh_vox_);
vox.filter(*ref_out);
}
#ifdef PCL_VERSION_COMPARE //fuerte
pcl::search::KdTree<PointXYZRGB>::Ptr tree (new pcl::search::KdTree<PointXYZRGB>());
#else //electric
pcl::KdTreeFLANN<PointXYZRGB>::Ptr tree (new pcl::KdTreeFLANN<PointXYZRGB> ());
#endif
//KdTree<PointXYZRGB>::Ptr tree(new KdTreeFLANN<PointXYZRGB>());
tree->setInputCloud(ref_out);
// Optional surface smoothing
if(fpfh_mls_enable_)
{
cout << "FPFH: MLS (with " << ref_out->points.size() << " points)" << endl;
#ifdef PCL_VERSION_COMPARE
std::cerr << "MLS has changed completely in PCL 1.7! Requires redesign of entire program" << std::endl;
exit(0);
#else
MovingLeastSquares<PointXYZRGB, Normal> mls;
mls.setInputCloud(ref_out);
mls.setOutputNormals(n);
mls.setPolynomialFit(true);
mls.setPolynomialOrder(2);
mls.setSearchMethod(tree);
mls.setSearchRadius(fpfh_rn_);
mls.reconstruct(*ref_out);
#endif
cout << "FPFH: flip normals (with " << ref_out->points.size() << " points)" << endl;
for (size_t i = 0; i < ref_out->points.size(); ++i)
{
flipNormalTowardsViewpoint(ref_out->points[i], 0.0f, 0.0f, 0.0f,
n->points[i].normal[0],
n->points[i].normal[1],
n->points[i].normal[2]);
}
}
else
{
cout << "FPFH: Normals (with " << ref_out->points.size() << " points)" << endl;
NormalEstimation<PointXYZRGB, Normal> norm;
norm.setInputCloud(ref_out);
norm.setSearchMethod(tree);
norm.setRadiusSearch(fpfh_rn_);
norm.compute(*n);
}
// FPFH estimation
#ifdef PCL_VERSION_COMPARE //fuerte
tree.reset(new pcl::search::KdTree<PointXYZRGB>());
#else //electric
tree.reset(new KdTreeFLANN<PointXYZRGB> ());
#endif
tree->setInputCloud(ref_out);
cout << "FPFH: estimation (with " << ref_out->points.size() << " points)" << endl;
FPFHEstimation<PointXYZRGB, Normal, FPFHSignature33> fpfhE;
fpfhE.setInputCloud(ref_out);
fpfhE.setInputNormals(n);
fpfhE.setSearchMethod(tree);
fpfhE.setRadiusSearch(fpfh_rf_);
fpfhE.compute(*fpfh);
cout << "FPFH: classification " << endl;
*fpfh_out = *ref_out;
CvSVM svm;
svm.load(fpfh_svm_model_.c_str());
cv::Mat fpfh_histo(1, 33, CV_32FC1);
int exp_rgb, pre_rgb, predict;
cob_3d_mapping_common::LabelResults stats(fl2label(fpfh_rn_),fl2label(fpfh_rf_),fpfh_mls_enable_);
for (size_t idx = 0; idx < ref_out->points.size(); idx++)
{
exp_rgb = *reinterpret_cast<int*>(&ref_out->points[idx].rgb); // expected label
memcpy(fpfh_histo.ptr<float>(0), fpfh->points[idx].histogram, sizeof(fpfh->points[idx].histogram));
predict = (int)svm.predict(fpfh_histo);
//cout << predict << endl;
switch(predict)
{
case SVM_PLANE:
pre_rgb = LBL_PLANE;
if (exp_rgb != LBL_PLANE && exp_rgb != LBL_UNDEF) stats.fp[EVAL_PLANE]++;
break;
case SVM_EDGE:
pre_rgb = LBL_EDGE;
if (exp_rgb != LBL_EDGE && exp_rgb != LBL_UNDEF) stats.fp[EVAL_EDGE]++;
if (exp_rgb != LBL_COR && exp_rgb != LBL_EDGE && exp_rgb != LBL_UNDEF) stats.fp[EVAL_EDGECORNER]++;
break;
case SVM_COR:
pre_rgb = LBL_COR;
if (exp_rgb != LBL_COR && exp_rgb != LBL_UNDEF) stats.fp[EVAL_COR]++;
if (exp_rgb != LBL_COR && exp_rgb != LBL_EDGE && exp_rgb != LBL_UNDEF) stats.fp[EVAL_EDGECORNER]++;
break;
case SVM_SPH:
pre_rgb = LBL_SPH;
if (exp_rgb != LBL_SPH && exp_rgb != LBL_UNDEF) stats.fp[EVAL_SPH]++;
if (exp_rgb != LBL_SPH && exp_rgb != LBL_CYL && exp_rgb != LBL_UNDEF) stats.fp[EVAL_CURVED]++;
break;
case SVM_CYL:
pre_rgb = LBL_CYL;
if (exp_rgb != LBL_CYL && exp_rgb != LBL_UNDEF) stats.fp[EVAL_CYL]++;
if (exp_rgb != LBL_SPH && exp_rgb != LBL_CYL && exp_rgb != LBL_UNDEF) stats.fp[EVAL_CURVED]++;
break;
default:
pre_rgb = LBL_UNDEF;
break;
}
switch(exp_rgb)
{
case LBL_PLANE:
if (pre_rgb != exp_rgb) stats.fn[EVAL_PLANE]++;
stats.exp[EVAL_PLANE]++;
break;
case LBL_EDGE:
if (pre_rgb != exp_rgb)
{
stats.fn[EVAL_EDGE]++;
if (pre_rgb != LBL_COR) stats.fn[EVAL_EDGECORNER]++;
}
stats.exp[EVAL_EDGE]++;
stats.exp[EVAL_EDGECORNER]++;
break;
case LBL_COR:
if (pre_rgb != exp_rgb)
{
stats.fn[EVAL_COR]++;
if (pre_rgb != LBL_EDGE) stats.fn[EVAL_EDGECORNER]++;
}
stats.exp[EVAL_COR]++;
stats.exp[EVAL_EDGECORNER]++;
break;
case LBL_SPH:
if (pre_rgb != exp_rgb)
{
stats.fn[EVAL_SPH]++;
if (pre_rgb != LBL_CYL) stats.fn[EVAL_CURVED]++;
}
stats.exp[EVAL_SPH]++;
stats.exp[EVAL_CURVED]++;
break;
case LBL_CYL:
if (pre_rgb != exp_rgb)
{
stats.fn[EVAL_CYL]++;
if (pre_rgb != LBL_SPH) stats.fn[EVAL_CURVED]++;
}
stats.exp[EVAL_CYL]++;
stats.exp[EVAL_CURVED]++;
break;
default:
stats.undef++;
break;
}
fpfh_out->points[idx].rgb = *reinterpret_cast<float*>(&pre_rgb);
}
cout << "FPFH:\n" << stats << endl << endl;
}
void processRSD(const PointCloud<PointXYZRGB>::Ptr in,
PointCloud<PointXYZRGB>::Ptr ref_out,
PointCloud<PointXYZRGB>::Ptr rsd_out)
{
PointCloud<Normal>::Ptr n(new PointCloud<Normal>());
PointCloud<PrincipalRadiiRSD>::Ptr rsd(new PointCloud<PrincipalRadiiRSD>());
// passthrough filtering (needed to remove NaNs)
cout << "RSD: Pass (with " << in->points.size() << " points)" << endl;
PassThrough<PointXYZRGB> pass;
pass.setInputCloud(in);
pass.setFilterFieldName("z");
pass.setFilterLimits(0.0f, pass_depth_);
pass.filter(*ref_out);
// optional voxelgrid filtering
if (rsd_vox_enable_)
{
cout << "RSD: Voxel (with " << ref_out->points.size() << " points)" << endl;
VoxelGrid<PointXYZRGB> vox;
vox.setInputCloud(ref_out);
vox.setLeafSize(rsd_vox_, rsd_vox_, rsd_vox_);
vox.filter(*ref_out);
}
#ifdef PCL_VERSION_COMPARE //fuerte
pcl::search::KdTree<PointXYZRGB>::Ptr tree (new pcl::search::KdTree<PointXYZRGB>());
#else //electric
KdTreeFLANN<PointXYZRGB>::Ptr tree (new pcl::KdTreeFLANN<PointXYZRGB> ());
#endif
tree->setInputCloud(ref_out);
// optional surface smoothing
if(rsd_mls_enable_)
{
cout << "RSD: MLS (with " << ref_out->points.size() << " points)" << endl;
#ifdef PCL_VERSION_COMPARE
std::cerr << "MLS has changed completely in PCL 1.7! Requires redesign of entire program" << std::endl;
exit(0);
#else
MovingLeastSquares<PointXYZRGB, Normal> mls;
mls.setInputCloud(ref_out);
mls.setOutputNormals(n);
mls.setPolynomialFit(true);
mls.setPolynomialOrder(2);
mls.setSearchMethod(tree);
mls.setSearchRadius(rsd_rn_);
mls.reconstruct(*ref_out);
#endif
cout << "RSD: flip normals (with " << ref_out->points.size() << " points)" << endl;
for (size_t i = 0; i < ref_out->points.size(); ++i)
{
flipNormalTowardsViewpoint(ref_out->points[i], 0.0f, 0.0f, 0.0f,
n->points[i].normal[0],
n->points[i].normal[1],
n->points[i].normal[2]);
}
}
else
{
cout << "RSD: Normals (with " << ref_out->points.size() << " points)" << endl;
NormalEstimation<PointXYZRGB, Normal> norm;
norm.setInputCloud(ref_out);
norm.setSearchMethod(tree);
norm.setRadiusSearch(rsd_rn_);
norm.compute(*n);
}
tree->setInputCloud(ref_out);
// RSD estimation
cout << "RSD: estimation (with " << ref_out->points.size() << " points)" << endl;
RSDEstimation<PointXYZRGB, Normal, PrincipalRadiiRSD> rsdE;
rsdE.setInputCloud(ref_out);
rsdE.setInputNormals(n);
rsdE.setSearchMethod(tree);
rsdE.setPlaneRadius(r_limit_);
rsdE.setRadiusSearch(rsd_rf_);
rsdE.compute(*rsd);
cout << "RSD: classification " << endl;
*rsd_out = *ref_out;
// apply RSD rules for classification
int exp_rgb, pre_rgb;
float r_max,r_min;
cob_3d_mapping_common::LabelResults stats(fl2label(rsd_rn_),fl2label(rsd_rf_),rsd_mls_enable_);
for (size_t idx = 0; idx < ref_out->points.size(); idx++)
{
exp_rgb = *reinterpret_cast<int*>(&ref_out->points[idx].rgb); // expected label
r_max = rsd->points[idx].r_max;
r_min = rsd->points[idx].r_min;
if ( r_min > r_high )
{
pre_rgb = LBL_PLANE;
if (exp_rgb != LBL_PLANE && exp_rgb != LBL_UNDEF) stats.fp[EVAL_PLANE]++;
}
else if (r_min < r_low)
{
if (r_max < r_r2 * r_min)
{
pre_rgb = LBL_COR;
if (exp_rgb != LBL_COR && exp_rgb != LBL_UNDEF) stats.fp[EVAL_COR]++;
}
else
{
pre_rgb = LBL_EDGE;
if (exp_rgb != LBL_EDGE && exp_rgb != LBL_UNDEF) stats.fp[EVAL_EDGE]++;
}
// special case: combined class for corner and edge
if (exp_rgb != LBL_COR && exp_rgb != LBL_EDGE && exp_rgb != LBL_UNDEF)
stats.fp[EVAL_EDGECORNER]++;
}
else
{
if (r_max < r_r1 * r_min)
{
pre_rgb = LBL_SPH;
if (exp_rgb != LBL_SPH && exp_rgb != LBL_UNDEF) stats.fp[EVAL_SPH]++;
}
else
{
pre_rgb = LBL_CYL;
if (exp_rgb != LBL_CYL && exp_rgb != LBL_UNDEF) stats.fp[EVAL_CYL]++;
}
// special case: combined class for sphere and cylinder
if (exp_rgb != LBL_SPH && exp_rgb != LBL_CYL && exp_rgb != LBL_UNDEF)
stats.fp[EVAL_CURVED]++;
}
switch(exp_rgb)
{
case LBL_PLANE:
if (pre_rgb != exp_rgb) stats.fn[EVAL_PLANE]++;
stats.exp[EVAL_PLANE]++;
break;
case LBL_EDGE:
if (pre_rgb != exp_rgb)
{
stats.fn[EVAL_EDGE]++;
if (pre_rgb != LBL_COR) stats.fn[EVAL_EDGECORNER]++;
}
stats.exp[EVAL_EDGE]++;
stats.exp[EVAL_EDGECORNER]++;
break;
case LBL_COR:
if (pre_rgb != exp_rgb)
{
stats.fn[EVAL_COR]++;
if (pre_rgb != LBL_EDGE) stats.fn[EVAL_EDGECORNER]++;
}
stats.exp[EVAL_COR]++;
stats.exp[EVAL_EDGECORNER]++;
break;
case LBL_SPH:
if (pre_rgb != exp_rgb)
{
stats.fn[EVAL_SPH]++;
if (pre_rgb != LBL_CYL) stats.fn[EVAL_CURVED]++;
}
stats.exp[EVAL_SPH]++;
stats.exp[EVAL_CURVED]++;
break;
case LBL_CYL:
if (pre_rgb != exp_rgb)
{
stats.fn[EVAL_CYL]++;
if (pre_rgb != LBL_SPH) stats.fn[EVAL_CURVED]++;
}
stats.exp[EVAL_CYL]++;
stats.exp[EVAL_CURVED]++;
break;
default:
stats.undef++;
break;
}
rsd_out->points[idx].rgb = *reinterpret_cast<float*>(&pre_rgb);
}
cout << "RSD:\n" << stats << endl << endl;
}
void CreateIndexedGridMesh(
const VoxelGrid<Discretizer>& vg,
std::vector<Vector3>& vertices,
std::vector<int>& indices)
{
// create all the vertices
const size_t vertex_count = (vg.sizeX() + 1) * (vg.sizeY() + 1) * (vg.sizeZ() + 1);
vertices.reserve(vertex_count);
for (int ix = 0; ix < vg.sizeX() + 1; ix++) {
for (int iy = 0; iy < vg.sizeY() + 1; iy++) {
for (int iz = 0; iz < vg.sizeZ() + 1; iz++) {
MemoryCoord mc(ix, iy, iz);
WorldCoord wc(vg.memoryToWorld(mc));
Vector3 p =
Vector3(wc.x, wc.y, wc.z) - 0.5 * vg.res();
vertices.push_back(p);
}
}
}
const size_t voxel_count = vg.sizeX() * vg.sizeY() * vg.sizeZ();
const size_t xplane_count = vg.sizeY() * vg.sizeZ();
const size_t yplane_count = vg.sizeX() * vg.sizeZ();
const size_t zplane_count = vg.sizeX() * vg.sizeY();
indices.reserve(6 * voxel_count * + 2 * xplane_count + 2 * yplane_count + 3 * zplane_count);
// for every voxel there are 12 triangles
for (int ix = 0; ix < vg.sizeX(); ++ix) {
for (int iy = 0; iy < vg.sizeY(); ++iy) {
for (int iz = 0; iz < vg.sizeZ(); ++iz) {
MemoryCoord a(ix, iy, iz);
MemoryCoord b(ix, iy, iz + 1);
MemoryCoord c(ix, iy + 1, iz);
MemoryCoord d(ix, iy + 1, iz + 1);
MemoryCoord e(ix + 1, iy, iz);
MemoryCoord f(ix + 1, iy, iz + 1);
MemoryCoord g(ix + 1, iy + 1, iz);
MemoryCoord h(ix + 1, iy + 1, iz + 1);
auto to_index = [&](const MemoryCoord& c) -> size_t
{
return c.x * (vg.sizeY() + 1) * (vg.sizeZ() + 1) +
c.y * (vg.sizeZ() + 1) +
c.z;
};
// back face
indices.push_back(to_index(a));
indices.push_back(to_index(b));
indices.push_back(to_index(d));
indices.push_back(to_index(a));
indices.push_back(to_index(d));
indices.push_back(to_index(c));
// left face
indices.push_back(to_index(a));
indices.push_back(to_index(f));
indices.push_back(to_index(b));
indices.push_back(to_index(a));
indices.push_back(to_index(e));
indices.push_back(to_index(f));
// bottom face
indices.push_back(to_index(a));
indices.push_back(to_index(g));
indices.push_back(to_index(e));
indices.push_back(to_index(a));
indices.push_back(to_index(c));
indices.push_back(to_index(g));
// front face?
if (ix == vg.sizeX() - 1) {
indices.push_back(to_index(f));
indices.push_back(to_index(g));
indices.push_back(to_index(e));
indices.push_back(to_index(f));
indices.push_back(to_index(h));
indices.push_back(to_index(g));
}
// right face?
if (iy == vg.sizeY() - 1) {
indices.push_back(to_index(h));
indices.push_back(to_index(d));
indices.push_back(to_index(c));
indices.push_back(to_index(h));
indices.push_back(to_index(c));
indices.push_back(to_index(g));
}
// top face?
if (iz == vg.sizeZ() - 1) {
indices.push_back(to_index(b));
indices.push_back(to_index(d));
indices.push_back(to_index(h));
indices.push_back(to_index(b));
indices.push_back(to_index(h));
indices.push_back(to_index(f));
}
}
}
}
}
