Skip to content

Commit

Permalink
Fixes PointCloudLibrary#1413
Browse files Browse the repository at this point in the history 
Voxel grid now handles alpha values.
  • Loading branch information
@SergioRAgostinho
SergioRAgostinho committed Nov 8, 2015
1 parent 887b3ea commit 7f0bc33
Show file tree
Hide file tree
Showing 3 changed files with 118 additions and 18 deletions.
20 changes: 11 additions & 9 deletions filters/include/pcl/filters/impl/voxel_grid.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -272,7 +272,7 @@ pcl::VoxelGrid<PointT>::applyFilter (PointCloud &output)
if (rgba_index >= 0)
{
rgba_index = fields[rgba_index].offset;
centroid_size += 3;
centroid_size += 4;
}

std::vector<cloud_point_index_idx> index_vector;
Expand Down Expand Up @@ -429,9 +429,10 @@ pcl::VoxelGrid<PointT>::applyFilter (PointCloud &output)
// 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;
centroid[centroid_size-4] = rgb.r;
centroid[centroid_size-3] = rgb.g;
centroid[centroid_size-2] = rgb.b;
centroid[centroid_size-1] = rgb.a;
}
pcl::for_each_type <FieldList> (NdCopyPointEigenFunctor <PointT> (input_->points[index_vector[first_index].cloud_point_index], centroid));
}
Expand All @@ -452,9 +453,10 @@ pcl::VoxelGrid<PointT>::applyFilter (PointCloud &output)
// 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;
temporary[centroid_size-4] = rgb.r;
temporary[centroid_size-3] = rgb.g;
temporary[centroid_size-2] = rgb.b;
temporary[centroid_size-1] = rgb.a;
}
pcl::for_each_type <FieldList> (NdCopyPointEigenFunctor <PointT> (input_->points[index_vector[i].cloud_point_index], temporary));
centroid += temporary;
Expand Down Expand Up @@ -482,8 +484,8 @@ pcl::VoxelGrid<PointT>::applyFilter (PointCloud &output)
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);
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 (reinterpret_cast<char*> (&output.points[index]) + rgba_index, &rgb, sizeof (float));
}
}
Expand Down
20 changes: 11 additions & 9 deletions filters/src/voxel_grid.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -270,7 +270,7 @@ pcl::VoxelGrid<pcl::PCLPointCloud2>::applyFilter (PCLPointCloud2 &output)
if (input_->fields[d].name == std::string ("rgba") || input_->fields[d].name == std::string ("rgb"))
{
rgba_index = d;
centroid_size += 3;
centroid_size += 4;
break;
}
}
Expand Down Expand Up @@ -457,9 +457,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 +488,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 +528,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
96 changes: 96 additions & 0 deletions test/filters/test_filters.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -780,6 +780,102 @@ TEST (VoxelGrid_RGB, Filters)
}
}

//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
TEST (VoxelGrid_RGBA, Filters)
{
PCLPointCloud2 cloud_rgba_blob_;
PCLPointCloud2::Ptr cloud_rgba_blob_ptr_;
PointCloud<PointXYZRGBA> cloud_rgba_;
PointCloud<PointXYZRGBA>::Ptr cloud_rgba_ptr_;

int col_r[] = {214, 193, 180, 164, 133, 119, 158, 179, 178, 212};
int col_g[] = {10, 39, 219, 231, 142, 169, 84, 158, 139, 214};
int col_b[] = {101, 26, 46, 189, 211, 154, 246, 16, 139, 153};
int col_a[] = {232, 161, 24, 71, 139, 244, 246, 40, 247, 244};
float ave_r = 0.0f;
float ave_b = 0.0f;
float ave_g = 0.0f;
float ave_a = 0.0f;
for (int i = 0; i < 10; ++i)
{
ave_r += static_cast<float> (col_r[i]);
ave_g += static_cast<float> (col_g[i]);
ave_b += static_cast<float> (col_b[i]);
ave_a += static_cast<float> (col_a[i]);
}
ave_r /= 10.0f;
ave_g /= 10.0f;
ave_b /= 10.0f;
ave_a /= 10.0f;

for (int i = 0; i < 10; ++i)
{
PointXYZRGBA pt;
int rgba = (col_a[i] << 24) | (col_r[i] << 16) | (col_g[i] << 8) | col_b[i];
pt.x = 0.0f;
pt.y = 0.0f;
pt.z = 0.0f;
pt.rgba = *reinterpret_cast<uint32_t*> (&rgba);
cloud_rgba_.points.push_back (pt);
}

toPCLPointCloud2 (cloud_rgba_, cloud_rgba_blob_);
cloud_rgba_blob_ptr_.reset (new PCLPointCloud2 (cloud_rgba_blob_));
cloud_rgba_ptr_.reset (new PointCloud<PointXYZRGBA> (cloud_rgba_));

PointCloud<PointXYZRGBA> output_rgba;
VoxelGrid<PointXYZRGBA> grid_rgba;

grid_rgba.setLeafSize (0.03f, 0.03f, 0.03f);
grid_rgba.setInputCloud (cloud_rgba_ptr_);
grid_rgba.filter (output_rgba);

EXPECT_EQ (int (output_rgba.points.size ()), 1);
EXPECT_EQ (int (output_rgba.width), 1);
EXPECT_EQ (int (output_rgba.height), 1);
EXPECT_EQ (bool (output_rgba.is_dense), true);
{
int rgba;
int r,g,b,a;
memcpy (&rgba, &(output_rgba.points[0].rgba), sizeof(int));
a = (rgba >> 24) & 0xFF; r = (rgba >> 16) & 0xFF; g = (rgba >> 8 ) & 0xFF; b = (rgba >> 0 ) & 0xFF;
EXPECT_NEAR (output_rgba.points[0].x, 0.0, 1e-4);
EXPECT_NEAR (output_rgba.points[0].y, 0.0, 1e-4);
EXPECT_NEAR (output_rgba.points[0].z, 0.0, 1e-4);
EXPECT_NEAR (r, ave_r, 1.0);
EXPECT_NEAR (g, ave_g, 1.0);
EXPECT_NEAR (b, ave_b, 1.0);
EXPECT_NEAR (a, ave_a, 1.0);
}

VoxelGrid<PCLPointCloud2> grid2;
PCLPointCloud2 output_rgba_blob;

grid2.setLeafSize (0.03f, 0.03f, 0.03f);
grid2.setInputCloud (cloud_rgba_blob_ptr_);
grid2.filter (output_rgba_blob);

fromPCLPointCloud2 (output_rgba_blob, output_rgba);

EXPECT_EQ (int (output_rgba.points.size ()), 1);
EXPECT_EQ (int (output_rgba.width), 1);
EXPECT_EQ (int (output_rgba.height), 1);
EXPECT_EQ (bool (output_rgba.is_dense), true);
{
int rgba;
int r,g,b,a;
memcpy (&rgba, &(output_rgba.points[0].rgba), sizeof(int));
a = (rgba >> 24) & 0xFF; r = (rgba >> 16) & 0xFF; g = (rgba >> 8 ) & 0xFF; b = (rgba >> 0 ) & 0xFF;
EXPECT_NEAR (output_rgba.points[0].x, 0.0, 1e-4);
EXPECT_NEAR (output_rgba.points[0].y, 0.0, 1e-4);
EXPECT_NEAR (output_rgba.points[0].z, 0.0, 1e-4);
EXPECT_NEAR (r, ave_r, 1.0);
EXPECT_NEAR (g, ave_g, 1.0);
EXPECT_NEAR (b, ave_b, 1.0);
EXPECT_NEAR (a, ave_a, 1.0);
}
}

#if 0
////////////////////////////////////////////////////////////////////////////////
float getRandomNumber (float max = 1.0, float min = 0.0)
Expand Down

0 comments on commit 7f0bc33

Please sign in to comment.