Skip to content

Commit

Permalink
Merge pull request #1415 from SergioRAgostinho/voxel_grid_xyzrgba
Browse files Browse the repository at this point in the history 
Fix for #1413
  • Loading branch information
@jspricke
jspricke committed Nov 17, 2015
2 parents e211f71 + bb057d4 commit 88b5b83
Show file tree
Hide file tree
Showing 4 changed files with 306 additions and 121 deletions.
106 changes: 22 additions & 84 deletions filters/include/pcl/filters/impl/voxel_grid.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -38,6 +38,7 @@
#ifndef PCL_FILTERS_IMPL_VOXEL_GRID_H_
#define PCL_FILTERS_IMPL_VOXEL_GRID_H_

#include <pcl/common/centroid.h>
#include <pcl/common/common.h>
#include <pcl/common/io.h>
#include <pcl/filters/voxel_grid.h>
Expand Down Expand Up @@ -259,22 +260,7 @@ pcl::VoxelGrid<PointT>::applyFilter (PointCloud &output)
// Set up the division multiplier
divb_mul_ = Eigen::Vector4i (1, div_b_[0], div_b_[0] * div_b_[1], 0);

int centroid_size = 4;
if (downsample_all_data_)
centroid_size = boost::mpl::size<FieldList>::value;

// ---[ RGB special case
std::vector<pcl::PCLPointField> fields;
int rgba_index = -1;
rgba_index = pcl::getFieldIndex (*input_, "rgb", fields);
if (rgba_index == -1)
rgba_index = pcl::getFieldIndex (*input_, "rgba", fields);
if (rgba_index >= 0)
{
rgba_index = fields[rgba_index].offset;
centroid_size += 3;
}

// Storage for mapping leaf and pointcloud indexes
std::vector<cloud_point_index_idx> index_vector;
index_vector.reserve (indices_->size ());

Expand Down Expand Up @@ -407,86 +393,38 @@ pcl::VoxelGrid<PointT>::applyFilter (PointCloud &output)
}

index = 0;
Eigen::VectorXf centroid = Eigen::VectorXf::Zero (centroid_size);
Eigen::VectorXf temporary = Eigen::VectorXf::Zero (centroid_size);

for (unsigned int cp = 0; cp < first_and_last_indices_vector.size (); ++cp)
{
// calculate centroid - sum values from all input points, that have the same idx value in index_vector array
unsigned int first_index = first_and_last_indices_vector[cp].first;
unsigned int last_index = first_and_last_indices_vector[cp].second;
if (!downsample_all_data_)
{
centroid[0] = input_->points[index_vector[first_index].cloud_point_index].x;
centroid[1] = input_->points[index_vector[first_index].cloud_point_index].y;
centroid[2] = input_->points[index_vector[first_index].cloud_point_index].z;
}
else
{
// ---[ RGB special case
if (rgba_index >= 0)
{
// Fill r/g/b data, assuming that the order is BGRA
pcl::RGB rgb;
memcpy (&rgb, reinterpret_cast<const char*> (&input_->points[index_vector[first_index].cloud_point_index]) + rgba_index, sizeof (RGB));
centroid[centroid_size-3] = rgb.r;
centroid[centroid_size-2] = rgb.g;
centroid[centroid_size-1] = rgb.b;
}
pcl::for_each_type <FieldList> (NdCopyPointEigenFunctor <PointT> (input_->points[index_vector[first_index].cloud_point_index], centroid));
}

for (unsigned int i = first_index + 1; i < last_index; ++i)
{
if (!downsample_all_data_)
{
centroid[0] += input_->points[index_vector[i].cloud_point_index].x;
centroid[1] += input_->points[index_vector[i].cloud_point_index].y;
centroid[2] += input_->points[index_vector[i].cloud_point_index].z;
}
else
{
// ---[ RGB special case
if (rgba_index >= 0)
{
// Fill r/g/b data, assuming that the order is BGRA
pcl::RGB rgb;
memcpy (&rgb, reinterpret_cast<const char*> (&input_->points[index_vector[i].cloud_point_index]) + rgba_index, sizeof (RGB));
temporary[centroid_size-3] = rgb.r;
temporary[centroid_size-2] = rgb.g;
temporary[centroid_size-1] = rgb.b;
}
pcl::for_each_type <FieldList> (NdCopyPointEigenFunctor <PointT> (input_->points[index_vector[i].cloud_point_index], temporary));
centroid += temporary;
}
}
unsigned int first_index = first_and_last_indices_vector[cp].first;
unsigned int last_index = first_and_last_indices_vector[cp].second;

// index is centroid final position in resulting PointCloud
if (save_leaf_layout_)
leaf_layout_[index_vector[first_index].idx] = index;

centroid /= static_cast<float> (last_index - first_index);

// store centroid
// Do we need to process all the fields?
if (!downsample_all_data_)
//Limit downsampling to coords
if (!downsample_all_data_)
{
output.points[index].x = centroid[0];
output.points[index].y = centroid[1];
output.points[index].z = centroid[2];
Eigen::Vector4f centroid (Eigen::Vector4f::Zero ());

for (unsigned int li = first_index; li < last_index; ++li)
centroid += input_->points[index_vector[li].cloud_point_index].getVector4fMap ();

centroid /= static_cast<float> (last_index - first_index);
output.points[index].getVector4fMap () = centroid;
}
else
else
{
pcl::for_each_type<FieldList> (pcl::NdCopyEigenPointFunctor <PointT> (centroid, output.points[index]));
// ---[ RGB special case
if (rgba_index >= 0)
{
// pack r/g/b into rgb
float r = centroid[centroid_size-3], g = centroid[centroid_size-2], b = centroid[centroid_size-1];
int rgb = (static_cast<int> (r) << 16) | (static_cast<int> (g) << 8) | static_cast<int> (b);
memcpy (reinterpret_cast<char*> (&output.points[index]) + rgba_index, &rgb, sizeof (float));
}
CentroidPoint<PointT> centroid;

// fill in the accumulator with leaf points
for (unsigned int li = first_index; li < last_index; ++li)
centroid.add (input_->points[index_vector[li].cloud_point_index]);

centroid.get (output.points[index]);
}

++index;
}
output.width = static_cast<uint32_t> (output.points.size ());
Expand Down
37 changes: 19 additions & 18 deletions filters/include/pcl/filters/impl/voxel_grid_covariance.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -115,7 +115,7 @@ pcl::VoxelGridCovariance<PointT>::applyFilter (PointCloud &output)
if (rgba_index >= 0)
{
rgba_index = fields[rgba_index].offset;
centroid_size += 3;
centroid_size += 4;
}

// If we don't want to process the entire cloud, but rather filter points far away from the viewpoint first...
Expand Down Expand Up @@ -185,17 +185,17 @@ pcl::VoxelGridCovariance<PointT>::applyFilter (PointCloud &output)
{
// Copy all the fields
Eigen::VectorXf centroid = Eigen::VectorXf::Zero (centroid_size);
pcl::for_each_type<FieldList> (NdCopyPointEigenFunctor<PointT> (input_->points[cp], centroid));
// ---[ RGB special case
if (rgba_index >= 0)
{
// fill r/g/b data
int rgb;
memcpy (&rgb, reinterpret_cast<const char*> (&input_->points[cp]) + rgba_index, sizeof (int));
centroid[centroid_size - 3] = static_cast<float> ((rgb >> 16) & 0x0000ff);
centroid[centroid_size - 2] = static_cast<float> ((rgb >> 8) & 0x0000ff);
centroid[centroid_size - 1] = static_cast<float> ((rgb) & 0x0000ff);
// Fill r/g/b data, assuming that the order is BGRA
const pcl::RGB& rgb = *reinterpret_cast<const RGB*> (reinterpret_cast<const char*> (&input_->points[cp]) + rgba_index);
centroid[centroid_size - 4] = rgb.a;
centroid[centroid_size - 3] = rgb.r;
centroid[centroid_size - 2] = rgb.g;
centroid[centroid_size - 1] = rgb.b;
}
pcl::for_each_type<FieldList> (NdCopyPointEigenFunctor<PointT> (input_->points[cp], centroid));
leaf.centroid += centroid;
}
++leaf.nr_points;
Expand Down Expand Up @@ -245,17 +245,17 @@ pcl::VoxelGridCovariance<PointT>::applyFilter (PointCloud &output)
{
// Copy all the fields
Eigen::VectorXf centroid = Eigen::VectorXf::Zero (centroid_size);
pcl::for_each_type<FieldList> (NdCopyPointEigenFunctor<PointT> (input_->points[cp], centroid));
// ---[ RGB special case
if (rgba_index >= 0)
{
// Fill r/g/b data, assuming that the order is BGRA
int rgb;
memcpy (&rgb, reinterpret_cast<const char*> (&input_->points[cp]) + rgba_index, sizeof (int));
centroid[centroid_size - 3] = static_cast<float> ((rgb >> 16) & 0x0000ff);
centroid[centroid_size - 2] = static_cast<float> ((rgb >> 8) & 0x0000ff);
centroid[centroid_size - 1] = static_cast<float> ((rgb) & 0x0000ff);
const pcl::RGB& rgb = *reinterpret_cast<const RGB*> (reinterpret_cast<const char*> (&input_->points[cp]) + rgba_index);
centroid[centroid_size - 4] = rgb.a;
centroid[centroid_size - 3] = rgb.r;
centroid[centroid_size - 2] = rgb.g;
centroid[centroid_size - 1] = rgb.b;
}
pcl::for_each_type<FieldList> (NdCopyPointEigenFunctor<PointT> (input_->points[cp], centroid));
leaf.centroid += centroid;
}
++leaf.nr_points;
Expand Down Expand Up @@ -313,10 +313,11 @@ pcl::VoxelGridCovariance<PointT>::applyFilter (PointCloud &output)
// ---[ RGB special case
if (rgba_index >= 0)
{
// pack r/g/b into rgb
float r = leaf.centroid[centroid_size - 3], g = leaf.centroid[centroid_size - 2], b = leaf.centroid[centroid_size - 1];
int rgb = (static_cast<int> (r)) << 16 | (static_cast<int> (g)) << 8 | (static_cast<int> (b));
memcpy (reinterpret_cast<char*> (&output.points.back ()) + rgba_index, &rgb, sizeof (float));
pcl::RGB& rgb = *reinterpret_cast<RGB*> (reinterpret_cast<char*> (&output.points.back ()) + rgba_index);
rgb.a = leaf.centroid[centroid_size - 4];
rgb.r = leaf.centroid[centroid_size - 3];
rgb.g = leaf.centroid[centroid_size - 2];
rgb.b = leaf.centroid[centroid_size - 1];
}
}

Expand Down
42 changes: 23 additions & 19 deletions filters/src/voxel_grid.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -258,23 +258,25 @@ pcl::VoxelGrid<pcl::PCLPointCloud2>::applyFilter (PCLPointCloud2 &output)
Eigen::Vector4f pt = Eigen::Vector4f::Zero ();

int centroid_size = 4;
if (downsample_all_data_)
centroid_size = static_cast<int> (input_->fields.size ());

int rgba_index = -1;

// ---[ RGB special case
// if the data contains "rgba" or "rgb", add an extra field for r/g/b in centroid
for (int d = 0; d < centroid_size; ++d)
if (downsample_all_data_)
{
if (input_->fields[d].name == std::string ("rgba") || input_->fields[d].name == std::string ("rgb"))
centroid_size = static_cast<int> (input_->fields.size ());

// ---[ RGB special case
// if the data contains "rgba" or "rgb", add an extra field for r/g/b in centroid
for (int d = 0; d < centroid_size; ++d)
{
rgba_index = d;
centroid_size += 3;
break;
if (input_->fields[d].name == std::string ("rgba") || input_->fields[d].name == std::string ("rgb"))
{
rgba_index = d;
centroid_size += 4;
break;
}
}
}

// If we don't want to process the entire cloud, but rather filter points far away from the viewpoint first...
if (!filter_field_name_.empty ())
{
Expand Down Expand Up @@ -457,9 +459,10 @@ pcl::VoxelGrid<pcl::PCLPointCloud2>::applyFilter (PCLPointCloud2 &output)
{
pcl::RGB rgb;
memcpy (&rgb, &input_->data[point_offset + input_->fields[rgba_index].offset], sizeof (RGB));
centroid[centroid_size-3] = rgb.r;
centroid[centroid_size-2] = rgb.g;
centroid[centroid_size-1] = rgb.b;
centroid[centroid_size-4] = rgb.r;
centroid[centroid_size-3] = rgb.g;
centroid[centroid_size-2] = rgb.b;
centroid[centroid_size-1] = rgb.a;
}
// Copy all the fields
for (size_t d = 0; d < input_->fields.size (); ++d)
Expand Down Expand Up @@ -487,9 +490,10 @@ pcl::VoxelGrid<pcl::PCLPointCloud2>::applyFilter (PCLPointCloud2 &output)
{
pcl::RGB rgb;
memcpy (&rgb, &input_->data[point_offset + input_->fields[rgba_index].offset], sizeof (RGB));
temporary[centroid_size-3] = rgb.r;
temporary[centroid_size-2] = rgb.g;
temporary[centroid_size-1] = rgb.b;
temporary[centroid_size-4] = rgb.r;
temporary[centroid_size-3] = rgb.g;
temporary[centroid_size-2] = rgb.b;
temporary[centroid_size-1] = rgb.a;
}
// Copy all the fields
for (size_t d = 0; d < input_->fields.size (); ++d)
Expand Down Expand Up @@ -526,8 +530,8 @@ pcl::VoxelGrid<pcl::PCLPointCloud2>::applyFilter (PCLPointCloud2 &output)
// full extra r/g/b centroid field
if (rgba_index >= 0)
{
float r = centroid[centroid_size-3], g = centroid[centroid_size-2], b = centroid[centroid_size-1];
int rgb = (static_cast<int> (r) << 16) | (static_cast<int> (g) << 8) | static_cast<int> (b);
float r = centroid[centroid_size-4], g = centroid[centroid_size-3], b = centroid[centroid_size-2], a = centroid[centroid_size-1];
int rgb = (static_cast<int> (a) << 24) | (static_cast<int> (r) << 16) | (static_cast<int> (g) << 8) | static_cast<int> (b);
memcpy (&output.data[point_offset + output.fields[rgba_index].offset], &rgb, sizeof (float));
}
}
Expand Down
Loading

0 comments on commit 88b5b83

Please sign in to comment.