Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Fix for #1413 #1415

Merged
merged 1 commit into from
Nov 17, 2015
Merged

Fix for #1413 #1415

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
106 changes: 22 additions & 84 deletions filters/include/pcl/filters/impl/voxel_grid.hpp
View file
Open in desktop
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
Open in desktop
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
Open in desktop
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