bool
pcl::getEigenAsPointCloud (Eigen::MatrixXf &in, pcl::PCLPointCloud2 &out)
{
// Get X-Y-Z indices
int x_idx = getFieldIndex (out, "x");
int y_idx = getFieldIndex (out, "y");
int z_idx = getFieldIndex (out, "z");
if (x_idx == -1 || y_idx == -1 || z_idx == -1)
{
PCL_ERROR ("Output dataset has no X-Y-Z coordinates set up as fields! Cannot convert from Eigen format.\n");
return (false);
}
if (out.fields[x_idx].datatype != pcl::PCLPointField::FLOAT32 ||
out.fields[y_idx].datatype != pcl::PCLPointField::FLOAT32 ||
out.fields[z_idx].datatype != pcl::PCLPointField::FLOAT32)
{
PCL_ERROR ("X-Y-Z coordinates not floats. Currently only floats are supported.\n");
return (false);
}
if (in.cols () != static_cast<int>(out.width * out.height))
{
PCL_ERROR ("Number of points in the point cloud differs from the Eigen matrix. Cannot continue.\n");
return (false);
}
size_t npts = in.cols ();
Eigen::Array4i xyz_offset (out.fields[x_idx].offset, out.fields[y_idx].offset, out.fields[z_idx].offset, 0);
// Copy the input dataset into Eigen format
for (size_t i = 0; i < npts; ++i)
{
// Unoptimized memcpys: assume fields x, y, z are in random order
memcpy (&out.data[xyz_offset[0]], &in (0, i), sizeof (float));
memcpy (&out.data[xyz_offset[1]], &in (1, i), sizeof (float));
memcpy (&out.data[xyz_offset[2]], &in (2, i), sizeof (float));
xyz_offset += out.point_step;
}
return (true);
}
void
pcl::getMinMax3D (const sensor_msgs::PointCloud2ConstPtr &cloud, int x_idx, int y_idx, int z_idx,
Eigen::Vector4f &min_pt, Eigen::Vector4f &max_pt)
{
// @todo fix this
if (cloud->fields[x_idx].datatype != sensor_msgs::PointField::FLOAT32 ||
cloud->fields[y_idx].datatype != sensor_msgs::PointField::FLOAT32 ||
cloud->fields[z_idx].datatype != sensor_msgs::PointField::FLOAT32)
{
PCL_ERROR ("[pcl::getMinMax3D] XYZ dimensions are not float type!\n");
return;
}
Eigen::Array4f min_p, max_p;
min_p.setConstant (FLT_MAX);
max_p.setConstant (-FLT_MAX);
int nr_points = cloud->width * cloud->height;
Eigen::Array4f pt = Eigen::Array4f::Zero ();
Eigen::Array4i xyz_offset (cloud->fields[x_idx].offset, cloud->fields[y_idx].offset, cloud->fields[z_idx].offset, 0);
for (int cp = 0; cp < nr_points; ++cp)
{
// Unoptimized memcpys: assume fields x, y, z are in random order
memcpy (&pt[0], &cloud->data[xyz_offset[0]], sizeof (float));
memcpy (&pt[1], &cloud->data[xyz_offset[1]], sizeof (float));
memcpy (&pt[2], &cloud->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 += cloud->point_step;
continue;
}
xyz_offset += cloud->point_step;
min_p = (min_p.min) (pt);
max_p = (max_p.max) (pt);
}
min_pt = min_p;
max_pt = max_p;
}
bool
pcl::getPointCloudAsEigen (const pcl::PCLPointCloud2 &in, Eigen::MatrixXf &out)
{
// Get X-Y-Z indices
int x_idx = getFieldIndex (in, "x");
int y_idx = getFieldIndex (in, "y");
int z_idx = getFieldIndex (in, "z");
if (x_idx == -1 || y_idx == -1 || z_idx == -1)
{
PCL_ERROR ("Input dataset has no X-Y-Z coordinates! Cannot convert to Eigen format.\n");
return (false);
}
if (in.fields[x_idx].datatype != pcl::PCLPointField::FLOAT32 ||
in.fields[y_idx].datatype != pcl::PCLPointField::FLOAT32 ||
in.fields[z_idx].datatype != pcl::PCLPointField::FLOAT32)
{
PCL_ERROR ("X-Y-Z coordinates not floats. Currently only floats are supported.\n");
return (false);
}
size_t npts = in.width * in.height;
out = Eigen::MatrixXf::Ones (4, npts);
Eigen::Array4i xyz_offset (in.fields[x_idx].offset, in.fields[y_idx].offset, in.fields[z_idx].offset, 0);
// Copy the input dataset into Eigen format
for (size_t i = 0; i < npts; ++i)
{
// Unoptimized memcpys: assume fields x, y, z are in random order
memcpy (&out (0, i), &in.data[xyz_offset[0]], sizeof (float));
memcpy (&out (1, i), &in.data[xyz_offset[1]], sizeof (float));
memcpy (&out (2, i), &in.data[xyz_offset[2]], sizeof (float));
xyz_offset += in.point_step;
}
return (true);
}
void
pcl::getMinMax3D (const sensor_msgs::PointCloud2ConstPtr &cloud, int x_idx, int y_idx, int z_idx,
const std::string &distance_field_name, float min_distance, float max_distance,
Eigen::Vector4f &min_pt, Eigen::Vector4f &max_pt, bool limit_negative)
{
// @todo fix this
if (cloud->fields[x_idx].datatype != sensor_msgs::PointField::FLOAT32 ||
cloud->fields[y_idx].datatype != sensor_msgs::PointField::FLOAT32 ||
cloud->fields[z_idx].datatype != sensor_msgs::PointField::FLOAT32)
{
PCL_ERROR ("[pcl::getMinMax3D] XYZ dimensions are not float type!\n");
return;
}
Eigen::Array4f min_p, max_p;
min_p.setConstant (FLT_MAX);
max_p.setConstant (-FLT_MAX);
// Get the distance field index
int distance_idx = pcl::getFieldIndex (*cloud, distance_field_name);
// @todo fix this
if (cloud->fields[distance_idx].datatype != sensor_msgs::PointField::FLOAT32)
{
PCL_ERROR ("[pcl::getMinMax3D] Filtering dimensions is not float type!\n");
return;
}
int nr_points = cloud->width * cloud->height;
Eigen::Array4f pt = Eigen::Array4f::Zero ();
Eigen::Array4i xyz_offset (cloud->fields[x_idx].offset,
cloud->fields[y_idx].offset,
cloud->fields[z_idx].offset,
0);
float distance_value = 0;
for (int cp = 0; cp < nr_points; ++cp)
{
int point_offset = cp * cloud->point_step;
// Get the distance value
memcpy (&distance_value, &cloud->data[point_offset + cloud->fields[distance_idx].offset], sizeof (float));
if (limit_negative)
{
// Use a threshold for cutting out points which inside the interval
if ((distance_value < max_distance) && (distance_value > min_distance))
{
xyz_offset += cloud->point_step;
continue;
}
}
else
{
// Use a threshold for cutting out points which are too close/far away
if ((distance_value > max_distance) || (distance_value < min_distance))
{
xyz_offset += cloud->point_step;
continue;
}
}
// Unoptimized memcpys: assume fields x, y, z are in random order
memcpy (&pt[0], &cloud->data[xyz_offset[0]], sizeof (float));
memcpy (&pt[1], &cloud->data[xyz_offset[1]], sizeof (float));
memcpy (&pt[2], &cloud->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 += cloud->point_step;
continue;
}
xyz_offset += cloud->point_step;
min_p = (min_p.min) (pt);
max_p = (max_p.max) (pt);
}
min_pt = min_p;
max_pt = max_p;
}
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_, filter_limit_min_, 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] = (int)(floor (min_p[0] * inverse_leaf_size_[0]));
max_b_[0] = (int)(floor (max_p[0] * inverse_leaf_size_[0]));
min_b_[1] = (int)(floor (min_p[1] * inverse_leaf_size_[1]));
max_b_[1] = (int)(floor (max_p[1] * inverse_leaf_size_[1]));
min_b_[2] = (int)(floor (min_p[2] * inverse_leaf_size_[2]));
max_b_[2] = (int)(floor (max_p[2] * inverse_leaf_size_[2]));
// min_b_ = (min_p.array () * inverse_leaf_size_).cast<int> ();
// max_b_ = (max_p.array () * inverse_leaf_size_).cast<int> ();
// Compute the number of divisions needed along all axis
div_b_ = max_b_ - min_b_ + Eigen::Vector4i::Ones ();
div_b_[3] = 0;
// Clear the leaves
leaves_.clear ();
// 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 = 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;
}
}
Eigen::Vector4i ijk = Eigen::Vector4i::Zero ();
// 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 right leaf
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;
}
ijk[0] = (int)(floor (pt[0] * inverse_leaf_size_[0]));
ijk[1] = (int)(floor (pt[1] * inverse_leaf_size_[1]));
ijk[2] = (int)(floor (pt[2] * inverse_leaf_size_[2]));
// Compute the centroid leaf index
int idx = (ijk - min_b_).dot (divb_mul_);
//int idx = (((pt.array () * inverse_leaf_size_).cast<int> ()).matrix () - min_b_).dot (divb_mul_);
Leaf& leaf = leaves_[idx];
if (leaf.nr_points == 0)
{
leaf.centroid.resize (centroid_size);
leaf.centroid.setZero ();
}
// Do we need to process all the fields?
if (!downsample_all_data_)
{
leaf.centroid.head<4> () += pt;
}
else
{
Eigen::VectorXf centroid = Eigen::VectorXf::Zero (centroid_size);
// ---[ RGB special case
// fill in 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]);
leaf.centroid += centroid;
}
++leaf.nr_points;
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;
}
ijk[0] = (int)(floor (pt[0] * inverse_leaf_size_[0]));
ijk[1] = (int)(floor (pt[1] * inverse_leaf_size_[1]));
ijk[2] = (int)(floor (pt[2] * inverse_leaf_size_[2]));
// Compute the centroid leaf index
int idx = (ijk - min_b_).dot (divb_mul_);
//int idx = (((pt.array () * inverse_leaf_size_).cast<int> ()).matrix () - min_b_).dot (divb_mul_);
Leaf& leaf = leaves_[idx];
if (leaf.nr_points == 0)
{
leaf.centroid.resize (centroid_size);
leaf.centroid.setZero ();
}
// Do we need to process all the fields?
if (!downsample_all_data_)
leaf.centroid.head<4> () += pt;
else
{
Eigen::VectorXf centroid = Eigen::VectorXf::Zero (centroid_size);
int point_offset = cp * input_->point_step;
// ---[ 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]);
leaf.centroid += centroid;
}
++leaf.nr_points;
xyz_offset += input_->point_step;
}
}
// Second pass: go over all leaves and compute centroids
int nr_p = 0;
// 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);
Eigen::VectorXf centroid;
output.width = leaves_.size ();
output.row_step = output.point_step * output.width;
output.data.resize (output.width * output.point_step);
if (save_leaf_layout_)
leaf_layout_.resize (div_b_[0] * div_b_[1] * div_b_[2], -1);
int cp = 0;
for (boost::unordered_map<size_t, Leaf>::const_iterator it = leaves_.begin (); it != leaves_.end (); ++it)
{
// Save leaf layout information for fast access to cells relative to current position
if (save_leaf_layout_)
leaf_layout_[it->first] = cp++;
// Normalize the centroid
const Leaf& leaf = it->second;
centroid = leaf.centroid / leaf.nr_points;
// 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 = nr_p * 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 = ((int)r) << 16 | ((int)g) << 8 | ((int)b);
memcpy (&output.data[point_offset + output.fields[rgba_index].offset], &rgb, sizeof(float));
}
}
++nr_p;
}
}
void
pcl::PassThrough<pcl::PCLPointCloud2>::applyFilter (PCLPointCloud2 &output)
{
if (!input_)
{
PCL_ERROR ("[pcl::%s::applyFilter] Input dataset not given!\n", getClassName ().c_str ());
output.width = output.height = 0;
output.data.clear ();
return;
}
// If fields x/y/z are not present, we cannot filter
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;
// Check if we're going to keep the organized structure of the cloud or not
if (keep_organized_)
{
if (filter_field_name_.empty ())
{
// Silly - if no filtering is actually done, and we want to keep the data organized,
// just copy everything. Any optimizations that can be done here???
output = *input_;
return;
}
output.width = input_->width;
output.height = input_->height;
// Check what the user value is: if !finite, set is_dense to false, true otherwise
if (!pcl_isfinite (user_filter_value_))
output.is_dense = false;
else
output.is_dense = input_->is_dense;
}
else
{
// Copy the header (and thus the frame_id) + allocate enough space for points
output.height = 1; // filtering breaks the organized structure
// Because we're doing explit checks for isfinite, is_dense = true
output.is_dense = true;
}
output.row_step = input_->row_step;
output.point_step = input_->point_step;
output.is_bigendian = input_->is_bigendian;
output.data.resize (input_->data.size ());
removed_indices_->resize (input_->data.size ());
int nr_p = 0;
int nr_removed_p = 0;
// Create the first xyz_offset
Eigen::Array4i xyz_offset (input_->fields[x_idx_].offset, input_->fields[y_idx_].offset,
input_->fields[z_idx_].offset, 0);
Eigen::Vector4f pt = Eigen::Vector4f::Zero ();
// 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_);
if (distance_idx == -1)
{
PCL_WARN ("[pcl::%s::applyFilter] Invalid filter field name. Index is %d.\n", getClassName ().c_str (), distance_idx);
output.width = output.height = 0;
output.data.clear ();
return;
}
// @todo fixme
if (input_->fields[distance_idx].datatype != pcl::PCLPointField::FLOAT32)
{
PCL_ERROR ("[pcl::%s::downsample] 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;
}
float badpt = std::numeric_limits<float>::quiet_NaN ();
// Check whether we need to store filtered valued in place
if (keep_organized_)
{
float distance_value = 0;
// Go over all points
for (int cp = 0; cp < nr_points; ++cp, xyz_offset += input_->point_step)
{
// Copy all the fields
memcpy (&output.data[cp * output.point_step], &input_->data[cp * output.point_step], output.point_step);
// Get the distance value
memcpy (&distance_value, &input_->data[cp * input_->point_step + 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_))
{
// Unoptimized memcpys: assume fields x, y, z are in random order
memcpy (&output.data[xyz_offset[0]], &badpt, sizeof (float));
memcpy (&output.data[xyz_offset[1]], &badpt, sizeof (float));
memcpy (&output.data[xyz_offset[2]], &badpt, sizeof (float));
continue;
}
else
{
if (extract_removed_indices_)
(*removed_indices_)[nr_removed_p++] = cp;
}
}
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_))
{
// Unoptimized memcpys: assume fields x, y, z are in random order
memcpy (&output.data[xyz_offset[0]], &badpt, sizeof (float));
memcpy (&output.data[xyz_offset[1]], &badpt, sizeof (float));
memcpy (&output.data[xyz_offset[2]], &badpt, sizeof (float));
continue;
}
else
{
if (extract_removed_indices_)
(*removed_indices_)[nr_removed_p++] = cp;
}
}
}
}
// Remove filtered points
else
{
// Go over all points
float distance_value = 0;
for (int cp = 0; cp < nr_points; ++cp, xyz_offset += input_->point_step)
{
// Get the distance value
memcpy (&distance_value, &input_->data[cp * input_->point_step + input_->fields[distance_idx].offset],
sizeof(float));
// Remove NAN/INF/-INF values. We expect passthrough to output clean valid data.
if (!pcl_isfinite (distance_value))
{
if (extract_removed_indices_)
(*removed_indices_)[nr_removed_p++] = cp;
continue;
}
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_)
{
if (extract_removed_indices_)
{
(*removed_indices_)[nr_removed_p] = cp;
nr_removed_p++;
}
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_)
{
if (extract_removed_indices_)
{
(*removed_indices_)[nr_removed_p] = cp;
nr_removed_p++;
}
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]))
{
if (extract_removed_indices_)
{
(*removed_indices_)[nr_removed_p] = cp;
nr_removed_p++;
}
continue;
}
// Copy all the fields
memcpy (&output.data[nr_p * output.point_step], &input_->data[cp * output.point_step], output.point_step);
nr_p++;
}
output.width = nr_p;
} // !keep_organized_
}
// No distance filtering, process all data. No need to check for is_organized here as we did it above
else
{
for (int cp = 0; cp < nr_points; ++cp, xyz_offset += input_->point_step)
{
// 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]))
{
if (extract_removed_indices_)
{
(*removed_indices_)[nr_removed_p] = cp;
nr_removed_p++;
}
continue;
}
// Copy all the fields
memcpy (&output.data[nr_p * output.point_step], &input_->data[cp * output.point_step], output.point_step);
nr_p++;
}
output.width = nr_p;
}
output.row_step = output.point_step * output.width;
output.data.resize (output.width * output.height * output.point_step);
removed_indices_->resize (nr_removed_p);
}
int
pcl::io::mesh2vtk (const pcl::PolygonMesh& mesh, vtkSmartPointer<vtkPolyData>& poly_data)
{
unsigned int nr_points = mesh.cloud.width * mesh.cloud.height;
unsigned int nr_polygons = static_cast<unsigned int> (mesh.polygons.size ());
// reset vtkPolyData object
poly_data = vtkSmartPointer<vtkPolyData>::New (); // OR poly_data->Reset();
vtkSmartPointer<vtkPoints> vtk_mesh_points = vtkSmartPointer<vtkPoints>::New ();
vtkSmartPointer<vtkCellArray> vtk_mesh_polygons = vtkSmartPointer<vtkCellArray>::New ();
poly_data->SetPoints (vtk_mesh_points);
// get field indices for x, y, z (as well as rgb and/or rgba)
int idx_x = -1, idx_y = -1, idx_z = -1, idx_rgb = -1, idx_rgba = -1, idx_normal_x = -1, idx_normal_y = -1, idx_normal_z = -1;
for (int d = 0; d < static_cast<int> (mesh.cloud.fields.size ()); ++d)
{
if (mesh.cloud.fields[d].name == "x") idx_x = d;
else if (mesh.cloud.fields[d].name == "y") idx_y = d;
else if (mesh.cloud.fields[d].name == "z") idx_z = d;
else if (mesh.cloud.fields[d].name == "rgb") idx_rgb = d;
else if (mesh.cloud.fields[d].name == "rgba") idx_rgba = d;
else if (mesh.cloud.fields[d].name == "normal_x") idx_normal_x = d;
else if (mesh.cloud.fields[d].name == "normal_y") idx_normal_y = d;
else if (mesh.cloud.fields[d].name == "normal_z") idx_normal_z = d;
}
if ( ( idx_x == -1 ) || ( idx_y == -1 ) || ( idx_z == -1 ) )
nr_points = 0;
// copy point data
vtk_mesh_points->SetNumberOfPoints (nr_points);
if (nr_points > 0)
{
Eigen::Vector4f pt = Eigen::Vector4f::Zero ();
Eigen::Array4i xyz_offset (mesh.cloud.fields[idx_x].offset, mesh.cloud.fields[idx_y].offset, mesh.cloud.fields[idx_z].offset, 0);
for (vtkIdType cp = 0; cp < static_cast<vtkIdType> (nr_points); ++cp, xyz_offset += mesh.cloud.point_step)
{
memcpy(&pt[0], &mesh.cloud.data[xyz_offset[0]], sizeof (float));
memcpy(&pt[1], &mesh.cloud.data[xyz_offset[1]], sizeof (float));
memcpy(&pt[2], &mesh.cloud.data[xyz_offset[2]], sizeof (float));
vtk_mesh_points->InsertPoint (cp, pt[0], pt[1], pt[2]);
}
}
// copy polygon data
if (nr_polygons > 0)
{
for (unsigned int i = 0; i < nr_polygons; i++)
{
unsigned int nr_points_in_polygon = static_cast<unsigned int> (mesh.polygons[i].vertices.size ());
vtk_mesh_polygons->InsertNextCell (nr_points_in_polygon);
for (unsigned int j = 0; j < nr_points_in_polygon; j++)
vtk_mesh_polygons->InsertCellPoint (mesh.polygons[i].vertices[j]);
}
poly_data->SetPolys (vtk_mesh_polygons);
}
// copy color information
if (idx_rgb != -1 || idx_rgba != -1)
{
vtkSmartPointer<vtkUnsignedCharArray> colors = vtkSmartPointer<vtkUnsignedCharArray>::New ();
colors->SetNumberOfComponents (3);
colors->SetName ("Colors");
pcl::RGB rgb;
int offset = (idx_rgb != -1) ? mesh.cloud.fields[idx_rgb].offset : mesh.cloud.fields[idx_rgba].offset;
for (vtkIdType cp = 0; cp < nr_points; ++cp)
{
memcpy (&rgb, &mesh.cloud.data[cp * mesh.cloud.point_step + offset], sizeof (RGB));
const unsigned char color[3] = {rgb.r, rgb.g, rgb.b};
colors->InsertNextTupleValue (color);
}
poly_data->GetPointData ()->SetScalars (colors);
}
// copy normal information
if (( idx_normal_x != -1 ) && ( idx_normal_y != -1 ) && ( idx_normal_z != -1 ))
{
vtkSmartPointer<vtkFloatArray> normals = vtkSmartPointer<vtkFloatArray>::New ();
normals->SetNumberOfComponents (3);
float nx = 0.0f, ny = 0.0f, nz = 0.0f;
for (vtkIdType cp = 0; cp < nr_points; ++cp)
{
memcpy (&nx, &mesh.cloud.data[cp*mesh.cloud.point_step+mesh.cloud.fields[idx_normal_x].offset], sizeof (float));
memcpy (&ny, &mesh.cloud.data[cp*mesh.cloud.point_step+mesh.cloud.fields[idx_normal_y].offset], sizeof (float));
memcpy (&nz, &mesh.cloud.data[cp*mesh.cloud.point_step+mesh.cloud.fields[idx_normal_z].offset], sizeof (float));
const float normal[3] = {nx, ny, nz};
normals->InsertNextTupleValue (normal);
}
poly_data->GetPointData()->SetNormals (normals);
}
if (poly_data->GetPoints () == NULL)
return (0);
return (static_cast<int> (poly_data->GetPoints ()->GetNumberOfPoints ()));
}
void
transformPointCloud (const Eigen::Matrix4f &transform, const sensor_msgs::PointCloud2 &in,
sensor_msgs::PointCloud2 &out)
{
// Get X-Y-Z indices
int x_idx = pcl::getFieldIndex (in, "x");
int y_idx = pcl::getFieldIndex (in, "y");
int z_idx = pcl::getFieldIndex (in, "z");
if (x_idx == -1 || y_idx == -1 || z_idx == -1)
{
ROS_ERROR ("Input dataset has no X-Y-Z coordinates! Cannot convert to Eigen format.");
return;
}
if (in.fields[x_idx].datatype != sensor_msgs::PointField::FLOAT32 ||
in.fields[y_idx].datatype != sensor_msgs::PointField::FLOAT32 ||
in.fields[z_idx].datatype != sensor_msgs::PointField::FLOAT32)
{
ROS_ERROR ("X-Y-Z coordinates not floats. Currently only floats are supported.");
return;
}
// Check if distance is available
int dist_idx = pcl::getFieldIndex (in, "distance");
// Copy the other data
if (&in != &out)
{
out.header = in.header;
out.height = in.height;
out.width = in.width;
out.fields = in.fields;
out.is_bigendian = in.is_bigendian;
out.point_step = in.point_step;
out.row_step = in.row_step;
out.is_dense = in.is_dense;
out.data.resize (in.data.size ());
// Copy everything as it's faster than copying individual elements
memcpy (&out.data[0], &in.data[0], in.data.size ());
}
Eigen::Array4i xyz_offset (in.fields[x_idx].offset, in.fields[y_idx].offset, in.fields[z_idx].offset, 0);
for (size_t i = 0; i < in.width * in.height; ++i)
{
Eigen::Vector4f pt (*(float*)&in.data[xyz_offset[0]], *(float*)&in.data[xyz_offset[1]], *(float*)&in.data[xyz_offset[2]], 1);
Eigen::Vector4f pt_out;
bool max_range_point = false;
int distance_ptr_offset = i*in.point_step + in.fields[dist_idx].offset;
float* distance_ptr = (dist_idx < 0 ? NULL : (float*)(&in.data[distance_ptr_offset]));
if (!std::isfinite (pt[0]) || !std::isfinite (pt[1]) || !std::isfinite (pt[2]))
{
if (distance_ptr==NULL || !std::isfinite(*distance_ptr)) // Invalid point
{
pt_out = pt;
}
else // max range point
{
pt[0] = *distance_ptr; // Replace x with the x value saved in distance
pt_out = transform * pt;
max_range_point = true;
//std::cout << pt[0]<<","<<pt[1]<<","<<pt[2]<<" => "<<pt_out[0]<<","<<pt_out[1]<<","<<pt_out[2]<<"\n";
}
}
else
{
pt_out = transform * pt;
}
if (max_range_point)
{
// Save x value in distance again
*(float*)(&out.data[distance_ptr_offset]) = pt_out[0];
pt_out[0] = std::numeric_limits<float>::quiet_NaN();
}
memcpy (&out.data[xyz_offset[0]], &pt_out[0], sizeof (float));
memcpy (&out.data[xyz_offset[1]], &pt_out[1], sizeof (float));
memcpy (&out.data[xyz_offset[2]], &pt_out[2], sizeof (float));
xyz_offset += in.point_step;
}
// Check if the viewpoint information is present
int vp_idx = pcl::getFieldIndex (in, "vp_x");
if (vp_idx != -1)
{
// Transform the viewpoint info too
for (size_t i = 0; i < out.width * out.height; ++i)
{
float *pstep = (float*)&out.data[i * out.point_step + out.fields[vp_idx].offset];
// Assume vp_x, vp_y, vp_z are consecutive
Eigen::Vector4f vp_in (pstep[0], pstep[1], pstep[2], 1);
Eigen::Vector4f vp_out = transform * vp_in;
pstep[0] = vp_out[0];
pstep[1] = vp_out[1];
pstep[2] = vp_out[2];
}
}
}
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;
}
}
