Solution::Solution(PointCloud<PointXYZRGB>::Ptr _cloud, string _name){
cloud = _cloud;
name = _name;
clustering_done=false;
clustering = new vector<int>(cloud->size(),0);
cluster_count=0;
//calculate maximum expansion of cloud
PointXYZRGB min;
PointXYZRGB max;
getMinMax3D(*cloud, min, max);
float expX=max.x-min.x;
float expY=max.y-min.y;
float expZ=max.z-min.z;
max_exp = std::max(expX,std::max(expY, expZ));
}
// according to http://www.pcl-users.org/Finding-oriented-bounding-box-of-a-cloud-td4024616.html
FitCube PCLTools::fitBox(PointCloud<PointXYZ>::Ptr cloud) {
FitCube retCube;
PCA<PointXYZ> pca;
PointCloud<PointXYZ> proj;
pca.setInputCloud(cloud);
pca.project(*cloud, proj);
PointXYZ proj_min;
PointXYZ proj_max;
getMinMax3D(proj, proj_min, proj_max);
PointXYZ min;
PointXYZ max;
pca.reconstruct(proj_min, min);
pca.reconstruct(proj_max, max);
// Rotation of PCA
Eigen::Matrix3f rot_mat = pca.getEigenVectors();
// Translation of PCA
Eigen::Vector3f cl_translation = pca.getMean().head(3);
Eigen::Matrix3f affine_trans;
// Reordering of principal components
affine_trans.col(2) << (rot_mat.col(0).cross(rot_mat.col(1))).normalized();
affine_trans.col(0) << rot_mat.col(0);
affine_trans.col(1) << rot_mat.col(1);
retCube.rotation = Eigen::Quaternionf(affine_trans);
Eigen::Vector4f t = pca.getMean();
retCube.translation = Eigen::Vector3f(t.x(), t.y(), t.z());
retCube.width = fabs(proj_max.x - proj_min.x);
retCube.height = fabs(proj_max.y - proj_min.y);
retCube.depth = fabs(proj_max.z - proj_min.z);
return retCube;
}
void add_remove_padding_hull (pcl::PointCloud<Point> &hull_in, pcl::PointCloud<Point> &hull_out, double padding)
{
// hull_out.points.resize(hull_in.points.size());
hull_out = hull_in;
Point point_max, point_min, point_center;
getMinMax3D (hull_in, point_min, point_max);
//Calculate the centroid of the hull
point_center.x = (point_max.x + point_min.x)/2;
point_center.y = (point_max.y + point_min.y)/2;
point_center.z = (point_max.z + point_min.z)/2;
for (unsigned long i = 0; i < hull_in.points.size(); i++)
{
double dist_to_center = sqrt((point_center.x - hull_in.points[i].x) * (point_center.x - hull_in.points[i].x) +
(point_center.y - hull_in.points[i].y) * (point_center.y - hull_in.points[i].y));
ROS_INFO("[%s] Dist to center: %lf", getName().c_str (), dist_to_center);
double angle;
angle= atan2((hull_in.points[i].y - point_center.y), (hull_in.points[i].x - point_center.x));
double new_dist_to_center = padding * dist_to_center;
hull_out.points[i].y = point_center.y + sin(angle) * new_dist_to_center;
hull_out.points[i].x = point_center.x + cos(angle) * new_dist_to_center;
}
}
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
NormalHoughProposer::houghVote(Entry &query, Entry &target, bin_t& bins)
{
// Compute the reference point for the R-table
Eigen::Vector4f centroid4;
compute3DCentroid(*(target.cloud), centroid4);
Eigen::Vector3f centroid(centroid4[0], centroid4[1], centroid4[2]);
assert(query.keypoints->size() == query.features->size());
assert(target.keypoints->size() == target.features->size());
// Figure out bin dimension
Eigen::Vector4f query_min4, query_max4;
getMinMax3D(*query.cloud, query_min4, query_max4);
Eigen::Vector3f query_min(query_min4[0], query_min4[1], query_min4[2]);
Eigen::Vector3f query_max(query_max4[0], query_max4[1], query_max4[2]);
Eigen::Affine3f t;
getTransformation(0, 0, 0, M_PI, 0.5, 1.5, t);
int correctly_matched = 0;
for (unsigned int i = 0; i < query.keypoints->size(); i++)
{
std::vector<int> feature_indices;
std::vector<float> sqr_distances;
int num_correspondences = 2;
if (!pcl_isfinite (query.features->points.row(i)(0)))
continue;
int num_found = target.tree->nearestKSearch(*query.features, i, num_correspondences, feature_indices, sqr_distances);
for (int j = 0; j < num_found; j++)
{
// For each one of the feature correspondences
int feature_index = feature_indices[j];
Eigen::Vector3f query_keypoint = query.keypoints->at(i).getVector3fMap();
Eigen::Vector3f target_keypoint = target.keypoints->at(feature_index).getVector3fMap();
target_keypoint = t * target_keypoint;
if ((query_keypoint - target_keypoint).norm() < 0.05)
{
correctly_matched++;
}
// Get corresponding target keypoint, and calculate its r to its centroid
PointNormal correspondence = target.keypoints->at(feature_index); // Since features correspond to the keypoints
Eigen::Vector3f r = correspondence.getVector3fMap() - centroid;
// Calculate the rotation transformation from the target normal to the query normal
Eigen::Vector3f target_normal = correspondence.getNormalVector3fMap();
target_normal.normalize();
Eigen::Vector3f query_normal = query.keypoints->at(i).getNormalVector3fMap();
query_normal.normalize();
double angle = acos( target_normal.dot(query_normal) / (target_normal.norm() * query_normal.norm()) );
Eigen::Vector3f axis = target_normal.normalized().cross(query_normal.normalized());
axis.normalize();
Eigen::Affine3f rot_transform;
rot_transform = Eigen::AngleAxisf (angle, axis);
// Check that the rotation matrix is correct
Eigen::Vector3f projected = rot_transform * target_normal;
projected.normalize();
// Transform r based on the difference between the normals
Eigen::Vector3f transformed_r = rot_transform * r;
for (int k = 0; k < num_angles_; k++)
{
float query_angle = (float(k) / num_angles_) * 2.0f * float (M_PI);
Eigen::Affine3f query_rot;
query_rot = Eigen::AngleAxisf(query_angle, query_normal.normalized());
Eigen::Vector3f guess_r = query_rot * transformed_r;
Eigen::Vector3f centroid_est = query.keypoints->at(i).getVector3fMap() - guess_r;
Eigen::Vector3f region = query_max - query_min;
Eigen::Vector3f bin_size = region / float (bins_);
Eigen::Vector3f diff = (centroid_est - query_min);
Eigen::Vector3f indices = diff.cwiseQuotient(bin_size);
castVotes(indices, bins);
}
}
}
}
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;
}
}
