diff --git a/filters/include/pcl/filters/impl/voxel_grid.hpp b/filters/include/pcl/filters/impl/voxel_grid.hpp index 00677aa3c21..9f2db7a9ab1 100644 --- a/filters/include/pcl/filters/impl/voxel_grid.hpp +++ b/filters/include/pcl/filters/impl/voxel_grid.hpp @@ -38,6 +38,7 @@ #ifndef PCL_FILTERS_IMPL_VOXEL_GRID_H_ #define PCL_FILTERS_IMPL_VOXEL_GRID_H_ +#include #include #include #include @@ -259,22 +260,7 @@ pcl::VoxelGrid::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::value; - - // ---[ RGB special case - std::vector 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 index_vector; index_vector.reserve (indices_->size ()); @@ -407,86 +393,38 @@ pcl::VoxelGrid::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 (&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 (NdCopyPointEigenFunctor (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 (&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 (NdCopyPointEigenFunctor (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 (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 (last_index - first_index); + output.points[index].getVector4fMap () = centroid; } - else + else { - pcl::for_each_type (pcl::NdCopyEigenPointFunctor (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 (r) << 16) | (static_cast (g) << 8) | static_cast (b); - memcpy (reinterpret_cast (&output.points[index]) + rgba_index, &rgb, sizeof (float)); - } + CentroidPoint 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 (output.points.size ()); diff --git a/filters/include/pcl/filters/impl/voxel_grid_covariance.hpp b/filters/include/pcl/filters/impl/voxel_grid_covariance.hpp index b82ba2b8a24..0c7f9ac3846 100644 --- a/filters/include/pcl/filters/impl/voxel_grid_covariance.hpp +++ b/filters/include/pcl/filters/impl/voxel_grid_covariance.hpp @@ -115,7 +115,7 @@ pcl::VoxelGridCovariance::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... @@ -185,17 +185,17 @@ pcl::VoxelGridCovariance::applyFilter (PointCloud &output) { // Copy all the fields Eigen::VectorXf centroid = Eigen::VectorXf::Zero (centroid_size); + pcl::for_each_type (NdCopyPointEigenFunctor (input_->points[cp], centroid)); // ---[ RGB special case if (rgba_index >= 0) { - // fill r/g/b data - int rgb; - memcpy (&rgb, reinterpret_cast (&input_->points[cp]) + rgba_index, sizeof (int)); - centroid[centroid_size - 3] = static_cast ((rgb >> 16) & 0x0000ff); - centroid[centroid_size - 2] = static_cast ((rgb >> 8) & 0x0000ff); - centroid[centroid_size - 1] = static_cast ((rgb) & 0x0000ff); + // Fill r/g/b data, assuming that the order is BGRA + const pcl::RGB& rgb = *reinterpret_cast (reinterpret_cast (&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 (NdCopyPointEigenFunctor (input_->points[cp], centroid)); leaf.centroid += centroid; } ++leaf.nr_points; @@ -245,17 +245,17 @@ pcl::VoxelGridCovariance::applyFilter (PointCloud &output) { // Copy all the fields Eigen::VectorXf centroid = Eigen::VectorXf::Zero (centroid_size); + pcl::for_each_type (NdCopyPointEigenFunctor (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 (&input_->points[cp]) + rgba_index, sizeof (int)); - centroid[centroid_size - 3] = static_cast ((rgb >> 16) & 0x0000ff); - centroid[centroid_size - 2] = static_cast ((rgb >> 8) & 0x0000ff); - centroid[centroid_size - 1] = static_cast ((rgb) & 0x0000ff); + const pcl::RGB& rgb = *reinterpret_cast (reinterpret_cast (&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 (NdCopyPointEigenFunctor (input_->points[cp], centroid)); leaf.centroid += centroid; } ++leaf.nr_points; @@ -313,10 +313,11 @@ pcl::VoxelGridCovariance::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 (r)) << 16 | (static_cast (g)) << 8 | (static_cast (b)); - memcpy (reinterpret_cast (&output.points.back ()) + rgba_index, &rgb, sizeof (float)); + pcl::RGB& rgb = *reinterpret_cast (reinterpret_cast (&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]; } } diff --git a/filters/src/voxel_grid.cpp b/filters/src/voxel_grid.cpp index afe78df2579..62552a5312d 100644 --- a/filters/src/voxel_grid.cpp +++ b/filters/src/voxel_grid.cpp @@ -258,23 +258,25 @@ pcl::VoxelGrid::applyFilter (PCLPointCloud2 &output) Eigen::Vector4f pt = Eigen::Vector4f::Zero (); int centroid_size = 4; - if (downsample_all_data_) - centroid_size = static_cast (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 (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 ()) { @@ -457,9 +459,10 @@ pcl::VoxelGrid::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) @@ -487,9 +490,10 @@ pcl::VoxelGrid::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) @@ -526,8 +530,8 @@ pcl::VoxelGrid::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 (r) << 16) | (static_cast (g) << 8) | static_cast (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 (a) << 24) | (static_cast (r) << 16) | (static_cast (g) << 8) | static_cast (b); memcpy (&output.data[point_offset + output.fields[rgba_index].offset], &rgb, sizeof (float)); } } diff --git a/test/filters/test_filters.cpp b/test/filters/test_filters.cpp index 487b1ea0d67..b58248fada0 100644 --- a/test/filters/test_filters.cpp +++ b/test/filters/test_filters.cpp @@ -690,6 +690,152 @@ TEST (VoxelGrid, Filters) EXPECT_LE (output.points[neighbors2.at (0)].z - output.points[centroidIdx2].z, 0.02 * 2); } +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +TEST (VoxelGrid_No_DownsampleAllData, Filters) +{ + // Test the PointCloud method + PointCloud output; + VoxelGrid grid; + + grid.setLeafSize (0.02f, 0.02f, 0.02f); + grid.setDownsampleAllData(false); + grid.setInputCloud (cloud); + grid.filter (output); + + EXPECT_EQ (int (output.points.size ()), 103); + EXPECT_EQ (int (output.width), 103); + EXPECT_EQ (int (output.height), 1); + EXPECT_EQ (bool (output.is_dense), true); + + grid.setFilterFieldName ("z"); + grid.setFilterLimits (0.05, 0.1); + grid.filter (output); + + EXPECT_EQ (int (output.points.size ()), 14); + EXPECT_EQ (int (output.width), 14); + EXPECT_EQ (int (output.height), 1); + EXPECT_EQ (bool (output.is_dense), true); + + EXPECT_NEAR (output.points[0].x, -0.026125, 1e-4); + EXPECT_NEAR (output.points[0].y, 0.039788, 1e-4); + EXPECT_NEAR (output.points[0].z, 0.052827, 1e-4); + + EXPECT_NEAR (output.points[13].x, -0.073202, 1e-4); + EXPECT_NEAR (output.points[13].y, 0.1296, 1e-4); + EXPECT_NEAR (output.points[13].z, 0.051333, 1e-4); + + grid.setFilterLimitsNegative (true); + grid.setSaveLeafLayout(true); + grid.filter (output); + + EXPECT_EQ (int (output.points.size ()), 100); + EXPECT_EQ (int (output.width), 100); + EXPECT_EQ (int (output.height), 1); + EXPECT_EQ (bool (output.is_dense), true); + + // centroids should be identified correctly + EXPECT_EQ (grid.getCentroidIndex (output.points[0]), 0); + EXPECT_EQ (grid.getCentroidIndex (output.points[99]), 99); + EXPECT_EQ (grid.getCentroidIndexAt (grid.getGridCoordinates (-1,-1,-1)), -1); + //PCL_ERROR ("IGNORE PREVIOUS ERROR: testing it's functionality!\n"); + + // input point 195 [0.04872199893, 0.07376000285, 0.01743399911] + int centroidIdx = grid.getCentroidIndex (cloud->points[195]); + + // for arbitrary points, the centroid should be close + EXPECT_LE (fabs (output.points[centroidIdx].x - cloud->points[195].x), 0.02); + EXPECT_LE (fabs (output.points[centroidIdx].y - cloud->points[195].y), 0.02); + EXPECT_LE (fabs (output.points[centroidIdx].z - cloud->points[195].z), 0.02); + + // if getNeighborCentroidIndices works then the other helper functions work as well + EXPECT_EQ (grid.getNeighborCentroidIndices (output.points[0], Eigen::MatrixXi::Zero(3,1))[0], 0); + EXPECT_EQ (grid.getNeighborCentroidIndices (output.points[99], Eigen::MatrixXi::Zero(3,1))[0], 99); + + // neighboring centroid should be in the right position + Eigen::MatrixXi directions = Eigen::Vector3i (0, 0, 1); + vector neighbors = grid.getNeighborCentroidIndices (cloud->points[195], directions); + EXPECT_EQ (neighbors.size (), size_t (directions.cols ())); + EXPECT_NE (neighbors.at (0), -1); + EXPECT_LE (fabs (output.points[neighbors.at (0)].x - output.points[centroidIdx].x), 0.02); + EXPECT_LE (fabs (output.points[neighbors.at (0)].y - output.points[centroidIdx].y), 0.02); + EXPECT_LE ( output.points[neighbors.at (0)].z - output.points[centroidIdx].z, 0.02 * 2); + + // Test the pcl::PCLPointCloud2 method + VoxelGrid grid2; + + PCLPointCloud2 output_blob; + + grid2.setLeafSize (0.02f, 0.02f, 0.02f); + grid2.setDownsampleAllData(false); + grid2.setInputCloud (cloud_blob); + grid2.filter (output_blob); + + fromPCLPointCloud2 (output_blob, output); + + EXPECT_EQ (int (output.points.size ()), 103); + EXPECT_EQ (int (output.width), 103); + EXPECT_EQ (int (output.height), 1); + EXPECT_EQ (bool (output.is_dense), true); + + grid2.setFilterFieldName ("z"); + grid2.setFilterLimits (0.05, 0.1); + grid2.filter (output_blob); + + fromPCLPointCloud2 (output_blob, output); + + EXPECT_EQ (int (output.points.size ()), 14); + EXPECT_EQ (int (output.width), 14); + EXPECT_EQ (int (output.height), 1); + EXPECT_EQ (bool (output.is_dense), true); + + EXPECT_NEAR (output.points[0].x, -0.026125, 1e-4); + EXPECT_NEAR (output.points[0].y, 0.039788, 1e-4); + EXPECT_NEAR (output.points[0].z, 0.052827, 1e-4); + + EXPECT_NEAR (output.points[13].x, -0.073202, 1e-4); + EXPECT_NEAR (output.points[13].y, 0.1296, 1e-4); + EXPECT_NEAR (output.points[13].z, 0.051333, 1e-4); + + grid2.setFilterLimitsNegative (true); + grid2.setSaveLeafLayout(true); + grid2.filter (output_blob); + + fromPCLPointCloud2 (output_blob, output); + + EXPECT_EQ (int (output.points.size ()), 100); + EXPECT_EQ (int (output.width), 100); + EXPECT_EQ (int (output.height), 1); + EXPECT_EQ (bool (output.is_dense), true); + + // centroids should be identified correctly + EXPECT_EQ (grid2.getCentroidIndex (output.points[0].x, output.points[0].y, output.points[0].z), 0); + EXPECT_EQ (grid2.getCentroidIndex (output.points[99].x, output.points[99].y, output.points[99].z), 99); + EXPECT_EQ (grid2.getCentroidIndexAt (grid2.getGridCoordinates (-1,-1,-1)), -1); + //PCL_ERROR ("IGNORE PREVIOUS ERROR: testing it's functionality!\n"); + + // input point 195 [0.04872199893, 0.07376000285, 0.01743399911] + int centroidIdx2 = grid2.getCentroidIndex (0.048722f, 0.073760f, 0.017434f); + EXPECT_NE (centroidIdx2, -1); + + // for arbitrary points, the centroid should be close + EXPECT_LE (fabs (output.points[centroidIdx2].x - 0.048722), 0.02); + EXPECT_LE (fabs (output.points[centroidIdx2].y - 0.073760), 0.02); + EXPECT_LE (fabs (output.points[centroidIdx2].z - 0.017434), 0.02); + + // if getNeighborCentroidIndices works then the other helper functions work as well + EXPECT_EQ (grid2.getNeighborCentroidIndices (output.points[0].x, output.points[0].y, output.points[0].z, Eigen::MatrixXi::Zero(3,1))[0], 0); + EXPECT_EQ (grid2.getNeighborCentroidIndices (output.points[99].x, output.points[99].y, output.points[99].z, Eigen::MatrixXi::Zero(3,1))[0], 99); + + // neighboring centroid should be in the right position + Eigen::MatrixXi directions2 = Eigen::Vector3i (0, 0, 1); + vector neighbors2 = grid2.getNeighborCentroidIndices (0.048722f, 0.073760f, 0.017434f, directions2); + EXPECT_EQ (neighbors2.size (), size_t (directions2.cols ())); + EXPECT_NE (neighbors2.at (0), -1); + EXPECT_LE (fabs (output.points[neighbors2.at (0)].x - output.points[centroidIdx2].x), 0.02); + EXPECT_LE (fabs (output.points[neighbors2.at (0)].y - output.points[centroidIdx2].y), 0.02); + EXPECT_LE (output.points[neighbors2.at (0)].z - output.points[centroidIdx2].z, 0.02 * 2); +} + ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// TEST (VoxelGrid_RGB, Filters) { @@ -780,6 +926,102 @@ TEST (VoxelGrid_RGB, Filters) } } +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +TEST (VoxelGrid_RGBA, Filters) +{ + PCLPointCloud2 cloud_rgba_blob_; + PCLPointCloud2::Ptr cloud_rgba_blob_ptr_; + PointCloud cloud_rgba_; + PointCloud::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 (col_r[i]); + ave_g += static_cast (col_g[i]); + ave_b += static_cast (col_b[i]); + ave_a += static_cast (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 (&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 (cloud_rgba_)); + + PointCloud output_rgba; + VoxelGrid 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 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)