void
pcl::VoxelGridLabel::applyFilter (PointCloud &output)
{
// Has the input dataset been set already?
if (!input_)
{
PCL_WARN ("[pcl::%s::applyFilter] No input dataset given!\n", getClassName ().c_str ());
output.width = output.height = 0;
output.points.clear ();
return;
}
// Copy the header (and thus the frame_id) + allocate enough space for points
output.height = 1; // downsampling breaks the organized structure
output.is_dense = true; // we filter out invalid points
Eigen::Vector4f min_p, max_p;
// Get the minimum and maximum dimensions
if (!filter_field_name_.empty ()) // If we don't want to process the entire cloud...
getMinMax3D<pcl::PointXYZRGBL>(input_, filter_field_name_, static_cast<float> (filter_limit_min_), static_cast<float> (filter_limit_max_), min_p, max_p, filter_limit_negative_);
else
getMinMax3D<pcl::PointXYZRGBL>(*input_, min_p, max_p);
// Compute the minimum and maximum bounding box values
min_b_[0] = static_cast<int> (floor (min_p[0] * inverse_leaf_size_[0]));
max_b_[0] = static_cast<int> (floor (max_p[0] * inverse_leaf_size_[0]));
min_b_[1] = static_cast<int> (floor (min_p[1] * inverse_leaf_size_[1]));
max_b_[1] = static_cast<int> (floor (max_p[1] * inverse_leaf_size_[1]));
min_b_[2] = static_cast<int> (floor (min_p[2] * inverse_leaf_size_[2]));
max_b_[2] = static_cast<int> (floor (max_p[2] * inverse_leaf_size_[2]));
// Compute the number of divisions needed along all axis
div_b_ = max_b_ - min_b_ + Eigen::Vector4i::Ones ();
div_b_[3] = 0;
// 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<sensor_msgs::PointField> 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;
}
// ---[ Label special case
int label_index = -1;
label_index = pcl::getFieldIndex (*input_, "label", fields);
std::vector<cloud_point_index_idx> index_vector;
index_vector.reserve(input_->points.size());
// 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 ())
{
// Get the distance field index
std::vector<sensor_msgs::PointField> fields;
int distance_idx = pcl::getFieldIndex (*input_, filter_field_name_, fields);
if (distance_idx == -1)
PCL_WARN ("[pcl::%s::applyFilter] Invalid filter field name. Index is %d.\n", getClassName ().c_str (), distance_idx);
// First pass: go over all points and insert them into the index_vector vector
// with calculated idx. Points with the same idx value will contribute to the
// same point of resulting CloudPoint
for (unsigned int cp = 0; cp < static_cast<unsigned int> (input_->points.size ()); ++cp)
{
if (!input_->is_dense)
// Check if the point is invalid
if (!pcl_isfinite (input_->points[cp].x) ||
!pcl_isfinite (input_->points[cp].y) ||
!pcl_isfinite (input_->points[cp].z))
continue;
// Get the distance value
const uint8_t* pt_data = reinterpret_cast<const uint8_t*> (&input_->points[cp]);
float distance_value = 0;
memcpy (&distance_value, pt_data + fields[distance_idx].offset, sizeof (float));
if (filter_limit_negative_)
{
// Use a threshold for cutting out points which inside the interval
if ((distance_value < filter_limit_max_) && (distance_value > filter_limit_min_))
continue;
}
else
{
// Use a threshold for cutting out points which are too close/far away
if ((distance_value > filter_limit_max_) || (distance_value < filter_limit_min_))
continue;
}
int ijk0 = static_cast<int> (floor (input_->points[cp].x * inverse_leaf_size_[0]) - min_b_[0]);
int ijk1 = static_cast<int> (floor (input_->points[cp].y * inverse_leaf_size_[1]) - min_b_[1]);
int ijk2 = static_cast<int> (floor (input_->points[cp].z * inverse_leaf_size_[2]) - min_b_[2]);
// Compute the centroid leaf index
int idx = ijk0 * divb_mul_[0] + ijk1 * divb_mul_[1] + ijk2 * divb_mul_[2];
index_vector.push_back (cloud_point_index_idx (static_cast<unsigned int> (idx), cp));
}
}
// No distance filtering, process all data
else
{
// First pass: go over all points and insert them into the index_vector vector
// with calculated idx. Points with the same idx value will contribute to the
// same point of resulting CloudPoint
for (unsigned int cp = 0; cp < static_cast<unsigned int> (input_->points.size ()); ++cp)
{
if (!input_->is_dense)
// Check if the point is invalid
if (!pcl_isfinite (input_->points[cp].x) ||
!pcl_isfinite (input_->points[cp].y) ||
!pcl_isfinite (input_->points[cp].z))
continue;
int ijk0 = static_cast<int> (floor (input_->points[cp].x * inverse_leaf_size_[0]) - min_b_[0]);
int ijk1 = static_cast<int> (floor (input_->points[cp].y * inverse_leaf_size_[1]) - min_b_[1]);
int ijk2 = static_cast<int> (floor (input_->points[cp].z * inverse_leaf_size_[2]) - min_b_[2]);
// Compute the centroid leaf index
int idx = ijk0 * divb_mul_[0] + ijk1 * divb_mul_[1] + ijk2 * divb_mul_[2];
index_vector.push_back (cloud_point_index_idx (static_cast<unsigned int> (idx), cp));
}
}
// Second pass: sort the index_vector vector using value representing target cell as index
// in effect all points belonging to the same output cell will be next to each other
std::sort (index_vector.begin (), index_vector.end (), std::less<cloud_point_index_idx> ());
// Third pass: count output cells
// we need to skip all the same, adjacenent idx values
unsigned int total = 0;
unsigned int index = 0;
while (index < index_vector.size ())
{
unsigned int i = index + 1;
while (i < index_vector.size () && index_vector[i].idx == index_vector[index].idx)
++i;
++total;
index = i;
}
// Fourth pass: compute centroids, insert them into their final position
output.points.resize (total);
if (save_leaf_layout_)
{
try
{
// Resizing won't reset old elements to -1. If leaf_layout_ has been used previously, it needs to be re-initialized to -1
uint32_t new_layout_size = div_b_[0]*div_b_[1]*div_b_[2];
//This is the number of elements that need to be re-initialized to -1
uint32_t reinit_size = std::min (static_cast<unsigned int> (new_layout_size), static_cast<unsigned int> (leaf_layout_.size()));
for (uint32_t i = 0; i < reinit_size; i++)
{
leaf_layout_[i] = -1;
}
leaf_layout_.resize (new_layout_size, -1);
}
catch (std::bad_alloc&)
{
throw PCLException("VoxelGrid bin size is too low; impossible to allocate memory for layout",
"voxel_grid.hpp", "applyFilter");
}
catch (std::length_error&)
{
throw PCLException("VoxelGrid bin size is too low; impossible to allocate memory for layout",
"voxel_grid.hpp", "applyFilter");
}
}
index = 0;
Eigen::VectorXf centroid = Eigen::VectorXf::Zero (centroid_size);
Eigen::VectorXf temporary = Eigen::VectorXf::Zero (centroid_size);
for (unsigned int cp = 0; cp < index_vector.size ();)
{
std::map<int, int> labels;
// calculate centroid - sum values from all input points, that have the same idx value in index_vector array
if (!downsample_all_data_)
{
centroid[0] = input_->points[index_vector[cp].cloud_point_index].x;
centroid[1] = input_->points[index_vector[cp].cloud_point_index].y;
centroid[2] = input_->points[index_vector[cp].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[cp].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;
}
// ---[ Label special case
if (label_index >= 0)
{
// store the label in a map data structure
uint32_t label = input_->points[index_vector[cp].cloud_point_index].label;
std::map<int, int>::iterator it = labels.find (label);
if (it == labels.end ())
labels.insert (labels.begin (), std::pair<int, int> (label, 1));
else
it->second = it->second++;
}
pcl::for_each_type <FieldList> (NdCopyPointEigenFunctor <pcl::PointXYZRGBL> (input_->points[index_vector[cp].cloud_point_index], centroid));
}
unsigned int i = cp + 1;
while (i < index_vector.size () && index_vector[i].idx == index_vector[cp].idx)
{
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 <pcl::PointXYZRGBL> (input_->points[index_vector[i].cloud_point_index], temporary));
centroid += temporary;
}
++i;
}
// index is centroid final position in resulting PointCloud
if (save_leaf_layout_)
leaf_layout_[index_vector[cp].idx] = index;
centroid /= static_cast<float> (i - cp);
// store centroid
// Do we need to process all the fields?
if (!downsample_all_data_)
{
output.points[index].x = centroid[0];
output.points[index].y = centroid[1];
output.points[index].z = centroid[2];
}
else
{
pcl::for_each_type<FieldList> (pcl::NdCopyEigenPointFunctor <pcl::PointXYZRGBL> (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));
}
if (label_index >= 0)
{
// find the label with the highest occurrence
std::map<int, int>::iterator it = labels.begin ();
int n_occurrence = it->second;
int label = it->first;
if (it != labels.end ())
{
for (it = labels.begin (); it != labels.end (); it++)
{
if (n_occurrence < it->second)
{
n_occurrence = it->second;
label = it->first;
}
}
}
output.points[index].label = label;
}
}
cp = i;
++index;
}
output.width = static_cast<uint32_t> (output.points.size ());
}
void
pcl::VoxelGrid<sensor_msgs::PointCloud2>::applyFilter (PointCloud2 &output)
{
// If fields x/y/z are not present, we cannot downsample
if (x_idx_ == -1 || y_idx_ == -1 || z_idx_ == -1)
{
PCL_ERROR ("[pcl::%s::applyFilter] Input dataset doesn't have x-y-z coordinates!\n", getClassName ().c_str ());
output.width = output.height = 0;
output.data.clear ();
return;
}
int nr_points = input_->width * input_->height;
// Copy the header (and thus the frame_id) + allocate enough space for points
output.height = 1; // downsampling breaks the organized structure
if (downsample_all_data_)
{
output.fields = input_->fields;
output.point_step = input_->point_step;
}
else
{
output.fields.resize (4);
output.fields[0] = input_->fields[x_idx_];
output.fields[0].offset = 0;
output.fields[1] = input_->fields[y_idx_];
output.fields[1].offset = 4;
output.fields[2] = input_->fields[z_idx_];
output.fields[2].offset = 8;
output.fields[3].name = "rgba";
output.fields[3].offset = 12;
output.fields[3].datatype = sensor_msgs::PointField::FLOAT32;
output.point_step = 16;
}
output.is_bigendian = input_->is_bigendian;
output.row_step = input_->row_step;
output.is_dense = true; // we filter out invalid points
Eigen::Vector4f min_p, max_p;
// Get the minimum and maximum dimensions
if (!filter_field_name_.empty ()) // If we don't want to process the entire cloud...
getMinMax3D (input_, x_idx_, y_idx_, z_idx_, filter_field_name_,
static_cast<float> (filter_limit_min_),
static_cast<float> (filter_limit_max_), min_p, max_p, filter_limit_negative_);
else
getMinMax3D (input_, x_idx_, y_idx_, z_idx_, min_p, max_p);
// Compute the minimum and maximum bounding box values
min_b_[0] = static_cast<int> (floor (min_p[0] * inverse_leaf_size_[0]));
max_b_[0] = static_cast<int> (floor (max_p[0] * inverse_leaf_size_[0]));
min_b_[1] = static_cast<int> (floor (min_p[1] * inverse_leaf_size_[1]));
max_b_[1] = static_cast<int> (floor (max_p[1] * inverse_leaf_size_[1]));
min_b_[2] = static_cast<int> (floor (min_p[2] * inverse_leaf_size_[2]));
max_b_[2] = static_cast<int> (floor (max_p[2] * inverse_leaf_size_[2]));
// Compute the number of divisions needed along all axis
div_b_ = max_b_ - min_b_ + Eigen::Vector4i::Ones ();
div_b_[3] = 0;
std::vector<cloud_point_index_idx> index_vector;
index_vector.reserve (nr_points);
// Create the first xyz_offset, and set up the division multiplier
Eigen::Array4i xyz_offset (input_->fields[x_idx_].offset,
input_->fields[y_idx_].offset,
input_->fields[z_idx_].offset,
0);
divb_mul_ = Eigen::Vector4i (1, div_b_[0], div_b_[0] * div_b_[1], 0);
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 (input_->fields[d].name == std::string ("rgba") || input_->fields[d].name == std::string ("rgb"))
{
rgba_index = d;
centroid_size += 3;
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 ())
{
// Get the distance field index
int distance_idx = pcl::getFieldIndex (*input_, filter_field_name_);
// @todo fixme
if (input_->fields[distance_idx].datatype != sensor_msgs::PointField::FLOAT32)
{
PCL_ERROR ("[pcl::%s::applyFilter] Distance filtering requested, but distances are not float/double in the dataset! Only FLOAT32/FLOAT64 distances are supported right now.\n", getClassName ().c_str ());
output.width = output.height = 0;
output.data.clear ();
return;
}
// First pass: go over all points and insert them into the index_vector vector
// with calculated idx. Points with the same idx value will contribute to the
// same point of resulting CloudPoint
float distance_value = 0;
for (int cp = 0; cp < nr_points; ++cp)
{
int point_offset = cp * input_->point_step;
// Get the distance value
memcpy (&distance_value, &input_->data[point_offset + input_->fields[distance_idx].offset], sizeof (float));
if (filter_limit_negative_)
{
// Use a threshold for cutting out points which inside the interval
if (distance_value < filter_limit_max_ && distance_value > filter_limit_min_)
{
xyz_offset += input_->point_step;
continue;
}
}
else
{
// Use a threshold for cutting out points which are too close/far away
if (distance_value > filter_limit_max_ || distance_value < filter_limit_min_)
{
xyz_offset += input_->point_step;
continue;
}
}
// Unoptimized memcpys: assume fields x, y, z are in random order
memcpy (&pt[0], &input_->data[xyz_offset[0]], sizeof (float));
memcpy (&pt[1], &input_->data[xyz_offset[1]], sizeof (float));
memcpy (&pt[2], &input_->data[xyz_offset[2]], sizeof (float));
// Check if the point is invalid
if (!pcl_isfinite (pt[0]) ||
!pcl_isfinite (pt[1]) ||
!pcl_isfinite (pt[2]))
{
xyz_offset += input_->point_step;
continue;
}
int ijk0 = static_cast<int> (floor (pt[0] * inverse_leaf_size_[0]) - min_b_[0]);
int ijk1 = static_cast<int> (floor (pt[1] * inverse_leaf_size_[1]) - min_b_[1]);
int ijk2 = static_cast<int> (floor (pt[2] * inverse_leaf_size_[2]) - min_b_[2]);
// Compute the centroid leaf index
int idx = ijk0 * divb_mul_[0] + ijk1 * divb_mul_[1] + ijk2 * divb_mul_[2];
index_vector.push_back (cloud_point_index_idx (idx, cp));
xyz_offset += input_->point_step;
}
}
// No distance filtering, process all data
else
{
// First pass: go over all points and insert them into the right leaf
for (int cp = 0; cp < nr_points; ++cp)
{
// Unoptimized memcpys: assume fields x, y, z are in random order
memcpy (&pt[0], &input_->data[xyz_offset[0]], sizeof (float));
memcpy (&pt[1], &input_->data[xyz_offset[1]], sizeof (float));
memcpy (&pt[2], &input_->data[xyz_offset[2]], sizeof (float));
// Check if the point is invalid
if (!pcl_isfinite (pt[0]) ||
!pcl_isfinite (pt[1]) ||
!pcl_isfinite (pt[2]))
{
xyz_offset += input_->point_step;
continue;
}
int ijk0 = static_cast<int> (floor (pt[0] * inverse_leaf_size_[0]) - min_b_[0]);
int ijk1 = static_cast<int> (floor (pt[1] * inverse_leaf_size_[1]) - min_b_[1]);
int ijk2 = static_cast<int> (floor (pt[2] * inverse_leaf_size_[2]) - min_b_[2]);
// Compute the centroid leaf index
int idx = ijk0 * divb_mul_[0] + ijk1 * divb_mul_[1] + ijk2 * divb_mul_[2];
index_vector.push_back (cloud_point_index_idx (idx, cp));
xyz_offset += input_->point_step;
}
}
// Second pass: sort the index_vector vector using value representing target cell as index
// in effect all points belonging to the same output cell will be next to each other
std::sort (index_vector.begin (), index_vector.end (), std::less<cloud_point_index_idx> ());
// Third pass: count output cells
// we need to skip all the same, adjacenent idx values
unsigned int total=0;
unsigned int index=0;
while (index < index_vector.size ())
{
unsigned int i = index + 1;
while (i < index_vector.size () && index_vector[i].idx == index_vector[index].idx)
++i;
++total;
index = i;
}
// Fourth pass: compute centroids, insert them into their final position
output.width = total;
output.row_step = output.point_step * output.width;
output.data.resize (output.width * output.point_step);
if (save_leaf_layout_)
{
try
{
leaf_layout_.resize (div_b_[0]*div_b_[1]*div_b_[2], -1);
}
catch (std::bad_alloc&)
{
throw PCLException("VoxelGrid bin size is too low; impossible to allocate memory for layout",
"voxel_grid.cpp", "applyFilter");
}
catch (std::length_error&)
{
throw PCLException("VoxelGrid bin size is too low; impossible to allocate memory for layout",
"voxel_grid.cpp", "applyFilter");
}
}
// If we downsample each field, the {x,y,z}_idx_ offsets should correspond in input_ and output
if (downsample_all_data_)
xyz_offset = Eigen::Array4i (output.fields[x_idx_].offset,
output.fields[y_idx_].offset,
output.fields[z_idx_].offset,
0);
else
// If not, we must have created a new xyzw cloud
xyz_offset = Eigen::Array4i (0, 4, 8, 12);
index=0;
Eigen::VectorXf centroid = Eigen::VectorXf::Zero (centroid_size);
Eigen::VectorXf temporary = Eigen::VectorXf::Zero (centroid_size);
for (unsigned int cp = 0; cp < index_vector.size ();)
{
int point_offset = index_vector[cp].cloud_point_index * input_->point_step;
// Do we need to process all the fields?
if (!downsample_all_data_)
{
memcpy (&pt[0], &input_->data[point_offset+input_->fields[x_idx_].offset], sizeof (float));
memcpy (&pt[1], &input_->data[point_offset+input_->fields[y_idx_].offset], sizeof (float));
memcpy (&pt[2], &input_->data[point_offset+input_->fields[z_idx_].offset], sizeof (float));
centroid[0] = pt[0];
centroid[1] = pt[1];
centroid[2] = pt[2];
centroid[3] = 0;
}
else
{
// ---[ RGB special case
// fill extra r/g/b centroid field
if (rgba_index >= 0)
{
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;
}
// Copy all the fields
for (unsigned int d = 0; d < input_->fields.size (); ++d)
memcpy (¢roid[d], &input_->data[point_offset + input_->fields[d].offset], field_sizes_[d]);
}
unsigned int i = cp + 1;
while (i < index_vector.size () && index_vector[i].idx == index_vector[cp].idx)
{
int point_offset = index_vector[i].cloud_point_index * input_->point_step;
if (!downsample_all_data_)
{
memcpy (&pt[0], &input_->data[point_offset+input_->fields[x_idx_].offset], sizeof (float));
memcpy (&pt[1], &input_->data[point_offset+input_->fields[y_idx_].offset], sizeof (float));
memcpy (&pt[2], &input_->data[point_offset+input_->fields[z_idx_].offset], sizeof (float));
centroid[0] += pt[0];
centroid[1] += pt[1];
centroid[2] += pt[2];
}
else
{
// ---[ RGB special case
// fill extra r/g/b centroid field
if (rgba_index >= 0)
{
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;
}
// Copy all the fields
for (unsigned int d = 0; d < input_->fields.size (); ++d)
memcpy (&temporary[d], &input_->data[point_offset + input_->fields[d].offset], field_sizes_[d]);
centroid+=temporary;
}
++i;
}
// Save leaf layout information for fast access to cells relative to current position
if (save_leaf_layout_)
leaf_layout_[index_vector[cp].idx] = index;
// Normalize the centroid
centroid /= static_cast<float> (i - cp);
// Do we need to process all the fields?
if (!downsample_all_data_)
{
// Copy the data
memcpy (&output.data[xyz_offset[0]], ¢roid[0], sizeof (float));
memcpy (&output.data[xyz_offset[1]], ¢roid[1], sizeof (float));
memcpy (&output.data[xyz_offset[2]], ¢roid[2], sizeof (float));
xyz_offset += output.point_step;
}
else
{
int point_offset = index * output.point_step;
// Copy all the fields
for (size_t d = 0; d < output.fields.size (); ++d)
memcpy (&output.data[point_offset + output.fields[d].offset], ¢roid[d], field_sizes_[d]);
// ---[ RGB special case
// 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);
memcpy (&output.data[point_offset + output.fields[rgba_index].offset], &rgb, sizeof (float));
}
}
cp = i;
++index;
}
}
