void
pcl_ros::VFHEstimation::emptyPublish (const PointCloudInConstPtr &cloud)
{
PointCloudOut output;
output.header = cloud->header;
pub_output_.publish (output.makeShared ());
}
void
pcl_ros::BoundaryEstimation::computePublish (const PointCloudInConstPtr &cloud,
const PointCloudNConstPtr &normals,
const PointCloudInConstPtr &surface,
const IndicesPtr &indices)
{
// Set the parameters
impl_.setKSearch (k_);
impl_.setRadiusSearch (search_radius_);
// Initialize the spatial locator
initTree (spatial_locator_type_, tree_, k_);
impl_.setSearchMethod (tree_);
// Set the inputs
impl_.setInputCloud (cloud);
impl_.setIndices (indices);
impl_.setSearchSurface (surface);
impl_.setInputNormals (normals);
// Estimate the feature
PointCloudOut output;
impl_.compute (output);
// Enforce that the TF frame and the timestamp are copied
output.header = cloud->header;
pub_output_.publish (output.makeShared ());
}
template <typename PointInT, typename PointNT, typename PointOutT> void
pcl16::PPFRGBRegionEstimation<PointInT, PointNT, PointOutT>::computeFeature (PointCloudOut &output)
{
PCL16_INFO ("before computing output size: %u\n", output.size ());
output.resize (indices_->size ());
for (int index_i = 0; index_i < static_cast<int> (indices_->size ()); ++index_i)
{
int i = (*indices_)[index_i];
std::vector<int> nn_indices;
std::vector<float> nn_distances;
tree_->radiusSearch (i, static_cast<float> (search_radius_), nn_indices, nn_distances);
PointOutT average_feature_nn;
average_feature_nn.alpha_m = 0;
average_feature_nn.f1 = average_feature_nn.f2 = average_feature_nn.f3 = average_feature_nn.f4 =
average_feature_nn.r_ratio = average_feature_nn.g_ratio = average_feature_nn.b_ratio = 0.0f;
for (std::vector<int>::iterator nn_it = nn_indices.begin (); nn_it != nn_indices.end (); ++nn_it)
{
int j = *nn_it;
if (i != j)
{
float f1, f2, f3, f4, r_ratio, g_ratio, b_ratio;
if (pcl16::computeRGBPairFeatures
(input_->points[i].getVector4fMap (), normals_->points[i].getNormalVector4fMap (), input_->points[i].getRGBVector4i (),
input_->points[j].getVector4fMap (), normals_->points[j].getNormalVector4fMap (), input_->points[j].getRGBVector4i (),
f1, f2, f3, f4, r_ratio, g_ratio, b_ratio))
{
average_feature_nn.f1 += f1;
average_feature_nn.f2 += f2;
average_feature_nn.f3 += f3;
average_feature_nn.f4 += f4;
average_feature_nn.r_ratio += r_ratio;
average_feature_nn.g_ratio += g_ratio;
average_feature_nn.b_ratio += b_ratio;
}
else
{
PCL16_ERROR ("[pcl16::%s::computeFeature] Computing pair feature vector between points %zu and %zu went wrong.\n", getClassName ().c_str (), i, j);
}
}
}
float normalization_factor = static_cast<float> (nn_indices.size ());
average_feature_nn.f1 /= normalization_factor;
average_feature_nn.f2 /= normalization_factor;
average_feature_nn.f3 /= normalization_factor;
average_feature_nn.f4 /= normalization_factor;
average_feature_nn.r_ratio /= normalization_factor;
average_feature_nn.g_ratio /= normalization_factor;
average_feature_nn.b_ratio /= normalization_factor;
output.points[index_i] = average_feature_nn;
}
PCL16_INFO ("Output size: %u\n", output.points.size ());
}
template <typename PointInT, typename PointOutT> void
pcl::MovingLeastSquares<PointInT, PointOutT>::addProjectedPointNormal (int index,
const Eigen::Vector3d &point,
const Eigen::Vector3d &normal,
double curvature,
PointCloudOut &projected_points,
NormalCloud &projected_points_normals,
PointIndices &corresponding_input_indices) const
{
PointOutT aux;
aux.x = static_cast<float> (point[0]);
aux.y = static_cast<float> (point[1]);
aux.z = static_cast<float> (point[2]);
// Copy additional point information if available
copyMissingFields (input_->points[index], aux);
projected_points.push_back (aux);
corresponding_input_indices.indices.push_back (index);
if (compute_normals_)
{
pcl::Normal aux_normal;
aux_normal.normal_x = static_cast<float> (normal[0]);
aux_normal.normal_y = static_cast<float> (normal[1]);
aux_normal.normal_z = static_cast<float> (normal[2]);
aux_normal.curvature = curvature;
projected_points_normals.push_back (aux_normal);
}
}
template <typename PointInT, typename PointOutT> void
pcl::MovingLeastSquares<PointInT, PointOutT>::performProcessing (PointCloudOut &output)
{
// Compute the number of coefficients
nr_coeff_ = (order_ + 1) * (order_ + 2) / 2;
// Allocate enough space to hold the results of nearest neighbor searches
// \note resize is irrelevant for a radiusSearch ().
std::vector<int> nn_indices;
std::vector<float> nn_sqr_dists;
// For all points
for (size_t cp = 0; cp < indices_->size (); ++cp)
{
// Get the initial estimates of point positions and their neighborhoods
if (!searchForNeighbors ((*indices_)[cp], nn_indices, nn_sqr_dists))
continue;
// Check the number of nearest neighbors for normal estimation (and later
// for polynomial fit as well)
if (nn_indices.size () < 3)
continue;
PointCloudOut projected_points;
NormalCloud projected_points_normals;
// Get a plane approximating the local surface's tangent and project point onto it
int index = (*indices_)[cp];
computeMLSPointNormal (index, nn_indices, nn_sqr_dists, projected_points, projected_points_normals, *corresponding_input_indices_, mls_results_[index]);
// Copy all information from the input cloud to the output points (not doing any interpolation)
for (size_t pp = 0; pp < projected_points.size (); ++pp)
copyMissingFields (input_->points[(*indices_)[cp]], projected_points[pp]);
// Append projected points to output
output.insert (output.end (), projected_points.begin (), projected_points.end ());
if (compute_normals_)
normals_->insert (normals_->end (), projected_points_normals.begin (), projected_points_normals.end ());
}
// Perform the distinct-cloud or voxel-grid upsampling
performUpsampling (output);
}
template <typename PointInT, typename PointNT, typename PointOutT> void
pcl::GFPFHEstimation<PointInT, PointNT, PointOutT>::computeFeature (PointCloudOut &output)
{
pcl::octree::OctreePointCloudSearch<PointInT> octree (octree_leaf_size_);
octree.setInputCloud (input_);
octree.addPointsFromInputCloud ();
typename pcl::PointCloud<PointInT>::VectorType occupied_cells;
octree.getOccupiedVoxelCenters (occupied_cells);
// Determine the voxels crosses along the line segments
// formed by every pair of occupied cells.
std::vector< std::vector<int> > line_histograms;
for (size_t i = 0; i < occupied_cells.size (); ++i)
{
Eigen::Vector3f origin = occupied_cells[i].getVector3fMap ();
for (size_t j = i+1; j < occupied_cells.size (); ++j)
{
typename pcl::PointCloud<PointInT>::VectorType intersected_cells;
Eigen::Vector3f end = occupied_cells[j].getVector3fMap ();
octree.getApproxIntersectedVoxelCentersBySegment (origin, end, intersected_cells, 0.5f);
// Intersected cells are ordered from closest to furthest w.r.t. the origin.
std::vector<int> histogram;
for (size_t k = 0; k < intersected_cells.size (); ++k)
{
std::vector<int> indices;
octree.voxelSearch (intersected_cells[k], indices);
int label = emptyLabel ();
if (indices.size () != 0)
{
label = getDominantLabel (indices);
}
histogram.push_back (label);
}
line_histograms.push_back(histogram);
}
}
std::vector< std::vector<int> > transition_histograms;
computeTransitionHistograms (line_histograms, transition_histograms);
std::vector<float> distances;
computeDistancesToMean (transition_histograms, distances);
std::vector<float> gfpfh_histogram;
computeDistanceHistogram (distances, gfpfh_histogram);
output.clear ();
output.width = 1;
output.height = 1;
output.points.resize (1);
std::copy (gfpfh_histogram.begin (), gfpfh_histogram.end (), output.points[0].histogram);
}
void
pcl_ros::NormalEstimation::computePublish (const PointCloudInConstPtr &cloud,
const PointCloudInConstPtr &surface,
const IndicesPtr &indices)
{
// Set the parameters
impl_.setKSearch (k_);
impl_.setRadiusSearch (search_radius_);
// Set the inputs
impl_.setInputCloud (cloud);
impl_.setIndices (indices);
impl_.setSearchSurface (surface);
// Estimate the feature
PointCloudOut output;
impl_.compute (output);
// Publish a Boost shared ptr const data
// Enforce that the TF frame and the timestamp are copied
output.header = cloud->header;
pub_output_.publish (output.makeShared ());
}
void
pcl::recognition::ORROctree::getFullLeavesPoints (PointCloudOut& out) const
{
out.resize(full_leaves_.size ());
size_t i = 0;
// Now iterate over all full leaves and compute the normals and average points
for ( vector<ORROctree::Node*>::const_iterator it = full_leaves_.begin() ; it != full_leaves_.end() ; ++it, ++i )
{
out[i].x = (*it)->getData ()->getPoint ()[0];
out[i].y = (*it)->getData ()->getPoint ()[1];
out[i].z = (*it)->getData ()->getPoint ()[2];
}
}
template <typename PointInT, typename PointOutT, typename IntensityT> void
pcl::BriskKeypoint2D<PointInT, PointOutT, IntensityT>::detectKeypoints (PointCloudOut &output)
{
// image size
const int width = int (input_->width);
const int height = int (input_->height);
// destination for intensity data; will be forwarded to BRISK
std::vector<unsigned char> image_data (width*height);
for (size_t i = 0; i < image_data.size (); ++i)
image_data[i] = static_cast<unsigned char> (intensity_ ((*input_)[i]));
pcl::keypoints::brisk::ScaleSpace brisk_scale_space (octaves_);
brisk_scale_space.constructPyramid (image_data, width, height);
// Check if the template types are the same. If true, avoid a copy.
// The PointOutT MUST be registered using the POINT_CLOUD_REGISTER_POINT_STRUCT macro!
if (isSamePointType<PointOutT, pcl::PointWithScale> ())
brisk_scale_space.getKeypoints (threshold_, output.points);
else
{
pcl::PointCloud<pcl::PointWithScale> output_temp;
brisk_scale_space.getKeypoints (threshold_, output_temp.points);
pcl::copyPointCloud<pcl::PointWithScale, PointOutT> (output_temp, output);
}
// we do not change the denseness
output.width = int (output.points.size ());
output.height = 1;
output.is_dense = false; // set to false to be sure
// 2nd pass to remove the invalid set of 3D keypoints
if (remove_invalid_3D_keypoints_)
{
PointCloudOut output_clean;
for (size_t i = 0; i < output.size (); ++i)
{
PointOutT pt;
// Interpolate its position in 3D, as the "u" and "v" are subpixel accurate
bilinearInterpolation (input_, output[i].x, output[i].y, pt);
// Check if the point is finite
if (pcl::isFinite (pt))
output_clean.push_back (output[i]);
}
output = output_clean;
output.is_dense = true; // set to true as there's no keypoint at an invalid XYZ
}
}
template <typename PointInT, typename PointOutT> void
pcl::MovingLeastSquares<PointInT, PointOutT>::performProcessing (PointCloudOut &output)
{
// Compute the number of coefficients
nr_coeff_ = (order_ + 1) * (order_ + 2) / 2;
// Allocate enough space to hold the results of nearest neighbor searches
// \note resize is irrelevant for a radiusSearch ().
std::vector<int> nn_indices;
std::vector<float> nn_sqr_dists;
// For all points
for (size_t cp = 0; cp < indices_->size (); ++cp)
{
// Get the initial estimates of point positions and their neighborhoods
if (!searchForNeighbors (int (cp), nn_indices, nn_sqr_dists))
continue;
// Check the number of nearest neighbors for normal estimation (and later
// for polynomial fit as well)
if (nn_indices.size () < 3)
continue;
PointCloudOut projected_points;
NormalCloud projected_points_normals;
// Get a plane approximating the local surface's tangent and project point onto it
computeMLSPointNormal (int (cp), *input_, nn_indices, nn_sqr_dists, projected_points, projected_points_normals);
// Append projected points to output
output.insert (output.end (), projected_points.begin (), projected_points.end ());
if (compute_normals_)
normals_->insert (normals_->end (), projected_points_normals.begin (), projected_points_normals.end ());
}
// For the voxel grid upsampling method, generate the voxel grid and dilate it
// Then, project the newly obtained points to the MLS surface
if (upsample_method_ == VOXEL_GRID_DILATION)
{
MLSVoxelGrid voxel_grid (input_, indices_, voxel_size_);
for (int iteration = 0; iteration < dilation_iteration_num_; ++iteration)
voxel_grid.dilate ();
BOOST_FOREACH (typename MLSVoxelGrid::HashMap::value_type voxel, voxel_grid.voxel_grid_)
{
// Get 3D position of point
Eigen::Vector3f pos;
voxel_grid.getPosition (voxel.first, pos);
PointInT p;
p.x = pos[0];
p.y = pos[1];
p.z = pos[2];
std::vector<int> nn_indices;
std::vector<float> nn_dists;
tree_->nearestKSearch (p, 1, nn_indices, nn_dists);
int input_index = nn_indices.front ();
// If the closest point did not have a valid MLS fitting result
// OR if it is too far away from the sampled point
if (mls_results_[input_index].valid == false)
continue;
Eigen::Vector3f add_point = p.getVector3fMap (),
input_point = input_->points[input_index].getVector3fMap ();
Eigen::Vector3d aux = mls_results_[input_index].u;
Eigen::Vector3f u = aux.cast<float> ();
aux = mls_results_[input_index].v;
Eigen::Vector3f v = aux.cast<float> ();
float u_disp = (add_point - input_point).dot (u),
v_disp = (add_point - input_point).dot (v);
PointOutT result_point;
pcl::Normal result_normal;
projectPointToMLSSurface (u_disp, v_disp,
mls_results_[input_index].u, mls_results_[input_index].v,
mls_results_[input_index].plane_normal,
mls_results_[input_index].curvature,
input_point,
mls_results_[input_index].c_vec,
mls_results_[input_index].num_neighbors,
result_point, result_normal);
float d_before = (pos - input_point).norm (),
d_after = (result_point.getVector3fMap () - input_point). norm();
if (d_after > d_before)
continue;
output.push_back (result_point);
if (compute_normals_)
normals_->push_back (result_normal);
}
}
template <typename PointInT, typename PointOutT> void
pcl::MovingLeastSquares<PointInT, PointOutT>::computeMLSPointNormal (int index,
const PointCloudIn &input,
const std::vector<int> &nn_indices,
std::vector<float> &nn_sqr_dists,
PointCloudOut &projected_points,
NormalCloud &projected_points_normals)
{
// Compute the plane coefficients
//pcl::computePointNormal<PointInT> (*input_, nn_indices, model_coefficients, curvature);
EIGEN_ALIGN16 Eigen::Matrix3f covariance_matrix;
Eigen::Vector4f xyz_centroid;
// Estimate the XYZ centroid
pcl::compute3DCentroid (input, nn_indices, xyz_centroid);
//pcl::compute3DCentroid (input, nn_indices, xyz_centroid);
pcl::computeCovarianceMatrix (input, nn_indices, xyz_centroid, covariance_matrix);
// Compute the 3x3 covariance matrix
EIGEN_ALIGN16 Eigen::Vector3f::Scalar eigen_value;
EIGEN_ALIGN16 Eigen::Vector3f eigen_vector;
Eigen::Vector4f model_coefficients;
pcl::eigen33 (covariance_matrix, eigen_value, eigen_vector);
model_coefficients.head<3> () = eigen_vector;
model_coefficients[3] = 0;
model_coefficients[3] = -1 * model_coefficients.dot (xyz_centroid);
// Projected query point
Eigen::Vector3f point = input[(*indices_)[index]].getVector3fMap ();
float distance = point.dot (model_coefficients.head<3> ()) + model_coefficients[3];
point -= distance * model_coefficients.head<3> ();
float curvature = covariance_matrix.trace ();
// Compute the curvature surface change
if (curvature != 0)
curvature = fabsf (eigen_value / curvature);
// Get a copy of the plane normal easy access
Eigen::Vector3d plane_normal = model_coefficients.head<3> ().cast<double> ();
// Vector in which the polynomial coefficients will be put
Eigen::VectorXd c_vec;
// Local coordinate system (Darboux frame)
Eigen::Vector3d v (0.0f, 0.0f, 0.0f), u (0.0f, 0.0f, 0.0f);
// Perform polynomial fit to update point and normal
////////////////////////////////////////////////////
if (polynomial_fit_ && static_cast<int> (nn_indices.size ()) >= nr_coeff_)
{
// Update neighborhood, since point was projected, and computing relative
// positions. Note updating only distances for the weights for speed
std::vector<Eigen::Vector3d> de_meaned (nn_indices.size ());
for (size_t ni = 0; ni < nn_indices.size (); ++ni)
{
de_meaned[ni][0] = input_->points[nn_indices[ni]].x - point[0];
de_meaned[ni][1] = input_->points[nn_indices[ni]].y - point[1];
de_meaned[ni][2] = input_->points[nn_indices[ni]].z - point[2];
nn_sqr_dists[ni] = static_cast<float> (de_meaned[ni].dot (de_meaned[ni]));
}
// Allocate matrices and vectors to hold the data used for the polynomial fit
Eigen::VectorXd weight_vec (nn_indices.size ());
Eigen::MatrixXd P (nr_coeff_, nn_indices.size ());
Eigen::VectorXd f_vec (nn_indices.size ());
Eigen::MatrixXd P_weight; // size will be (nr_coeff_, nn_indices.size ());
Eigen::MatrixXd P_weight_Pt (nr_coeff_, nr_coeff_);
// Get local coordinate system (Darboux frame)
v = plane_normal.unitOrthogonal ();
u = plane_normal.cross (v);
// Go through neighbors, transform them in the local coordinate system,
// save height and the evaluation of the polynome's terms
double u_coord, v_coord, u_pow, v_pow;
for (size_t ni = 0; ni < nn_indices.size (); ++ni)
{
// (re-)compute weights
weight_vec (ni) = exp (-nn_sqr_dists[ni] / sqr_gauss_param_);
// transforming coordinates
u_coord = de_meaned[ni].dot (u);
v_coord = de_meaned[ni].dot (v);
f_vec (ni) = de_meaned[ni].dot (plane_normal);
// compute the polynomial's terms at the current point
int j = 0;
u_pow = 1;
for (int ui = 0; ui <= order_; ++ui)
{
v_pow = 1;
for (int vi = 0; vi <= order_ - ui; ++vi)
{
P (j++, ni) = u_pow * v_pow;
v_pow *= v_coord;
}
u_pow *= u_coord;
}
}
// Computing coefficients
P_weight = P * weight_vec.asDiagonal ();
P_weight_Pt = P_weight * P.transpose ();
c_vec = P_weight * f_vec;
P_weight_Pt.llt ().solveInPlace (c_vec);
}
switch (upsample_method_)
{
case (NONE):
{
Eigen::Vector3d normal = plane_normal;
if (polynomial_fit_ && static_cast<int> (nn_indices.size ()) >= nr_coeff_ && pcl_isfinite (c_vec[0]))
{
point += (c_vec[0] * plane_normal).cast<float> ();
// Compute tangent vectors using the partial derivates evaluated at (0,0) which is c_vec[order_+1] and c_vec[1]
if (compute_normals_)
normal = plane_normal - c_vec[order_ + 1] * u - c_vec[1] * v;
}
PointOutT aux;
aux.x = point[0];
aux.y = point[1];
aux.z = point[2];
projected_points.push_back (aux);
if (compute_normals_)
{
pcl::Normal aux_normal;
aux_normal.normal_x = static_cast<float> (normal[0]);
aux_normal.normal_y = static_cast<float> (normal[1]);
aux_normal.normal_z = static_cast<float> (normal[2]);
aux_normal.curvature = curvature;
projected_points_normals.push_back (aux_normal);
}
break;
}
case (SAMPLE_LOCAL_PLANE):
{
// Uniformly sample a circle around the query point using the radius and step parameters
for (float u_disp = -static_cast<float> (upsampling_radius_); u_disp <= upsampling_radius_; u_disp += static_cast<float> (upsampling_step_))
for (float v_disp = -static_cast<float> (upsampling_radius_); v_disp <= upsampling_radius_; v_disp += static_cast<float> (upsampling_step_))
if (u_disp*u_disp + v_disp*v_disp < upsampling_radius_*upsampling_radius_)
{
PointOutT projected_point;
pcl::Normal projected_normal;
projectPointToMLSSurface (u_disp, v_disp, u, v, plane_normal, curvature, point, c_vec,
static_cast<int> (nn_indices.size ()),
projected_point, projected_normal);
projected_points.push_back (projected_point);
if (compute_normals_)
projected_points_normals.push_back (projected_normal);
}
break;
}
case (RANDOM_UNIFORM_DENSITY):
{
// Compute the local point density and add more samples if necessary
int num_points_to_add = static_cast<int> (floor (desired_num_points_in_radius_ / 2.0 / static_cast<double> (nn_indices.size ())));
// Just add the query point, because the density is good
if (num_points_to_add <= 0)
{
// Just add the current point
Eigen::Vector3d normal = plane_normal;
if (polynomial_fit_ && static_cast<int> (nn_indices.size ()) >= nr_coeff_ && pcl_isfinite (c_vec[0]))
{
// Projection onto MLS surface along Darboux normal to the height at (0,0)
point += (c_vec[0] * plane_normal).cast<float> ();
// Compute tangent vectors using the partial derivates evaluated at (0,0) which is c_vec[order_+1] and c_vec[1]
if (compute_normals_)
normal = plane_normal - c_vec[order_ + 1] * u - c_vec[1] * v;
}
PointOutT aux;
aux.x = point[0];
aux.y = point[1];
aux.z = point[2];
projected_points.push_back (aux);
if (compute_normals_)
{
pcl::Normal aux_normal;
aux_normal.normal_x = static_cast<float> (normal[0]);
aux_normal.normal_y = static_cast<float> (normal[1]);
aux_normal.normal_z = static_cast<float> (normal[2]);
aux_normal.curvature = curvature;
projected_points_normals.push_back (aux_normal);
}
}
else
{
// Sample the local plane
for (int num_added = 0; num_added < num_points_to_add;)
{
float u_disp = (*rng_uniform_distribution_) (),
v_disp = (*rng_uniform_distribution_) ();
// Check if inside circle; if not, try another coin flip
if (u_disp * u_disp + v_disp * v_disp > search_radius_ * search_radius_/4)
continue;
PointOutT projected_point;
pcl::Normal projected_normal;
projectPointToMLSSurface (u_disp, v_disp, u, v, plane_normal, curvature, point, c_vec,
static_cast<int> (nn_indices.size ()),
projected_point, projected_normal);
projected_points.push_back (projected_point);
if (compute_normals_)
projected_points_normals.push_back (projected_normal);
num_added ++;
}
}
break;
}
case (VOXEL_GRID_DILATION):
{
// Take all point pairs and sample space between them in a grid-fashion
// \note consider only point pairs with increasing indices
MLSResult result (plane_normal, u, v, c_vec, static_cast<int> (nn_indices.size ()), curvature);
mls_results_[index] = result;
break;
}
}
}
template <typename PointInT, typename PointOutT> void
pcl::MovingLeastSquares<PointInT, PointOutT>::process (PointCloudOut &output)
{
// Reset or initialize the collection of indices
corresponding_input_indices_.reset (new PointIndices);
// Check if normals have to be computed/saved
if (compute_normals_)
{
normals_.reset (new NormalCloud);
// Copy the header
normals_->header = input_->header;
// Clear the fields in case the method exits before computation
normals_->width = normals_->height = 0;
normals_->points.clear ();
}
// Copy the header
output.header = input_->header;
output.width = output.height = 0;
output.points.clear ();
if (search_radius_ <= 0 || sqr_gauss_param_ <= 0)
{
PCL_ERROR ("[pcl::%s::process] Invalid search radius (%f) or Gaussian parameter (%f)!\n", getClassName ().c_str (), search_radius_, sqr_gauss_param_);
return;
}
// Check if distinct_cloud_ was set
if (upsample_method_ == DISTINCT_CLOUD && !distinct_cloud_)
{
PCL_ERROR ("[pcl::%s::process] Upsample method was set to DISTINCT_CLOUD, but no distinct cloud was specified.\n", getClassName ().c_str ());
return;
}
if (!initCompute ())
return;
// Initialize the spatial locator
if (!tree_)
{
KdTreePtr tree;
if (input_->isOrganized ())
tree.reset (new pcl::search::OrganizedNeighbor<PointInT> ());
else
tree.reset (new pcl::search::KdTree<PointInT> (false));
setSearchMethod (tree);
}
// Send the surface dataset to the spatial locator
tree_->setInputCloud (input_);
switch (upsample_method_)
{
// Initialize random number generator if necessary
case (RANDOM_UNIFORM_DENSITY):
{
rng_alg_.seed (static_cast<unsigned> (std::time (0)));
float tmp = static_cast<float> (search_radius_ / 2.0f);
boost::uniform_real<float> uniform_distrib (-tmp, tmp);
rng_uniform_distribution_.reset (new boost::variate_generator<boost::mt19937&, boost::uniform_real<float> > (rng_alg_, uniform_distrib));
break;
}
case (VOXEL_GRID_DILATION):
case (DISTINCT_CLOUD):
{
if (!cache_mls_results_)
PCL_WARN ("The cache mls results is forced when using upsampling method VOXEL_GRID_DILATION or DISTINCT_CLOUD.\n");
cache_mls_results_ = true;
break;
}
default:
break;
}
if (cache_mls_results_)
{
mls_results_.resize (input_->size ());
}
else
{
mls_results_.resize (1); // Need to have a reference to a single dummy result.
}
// Perform the actual surface reconstruction
performProcessing (output);
if (compute_normals_)
{
normals_->height = 1;
normals_->width = static_cast<uint32_t> (normals_->size ());
for (unsigned int i = 0; i < output.size (); ++i)
{
typedef typename pcl::traits::fieldList<PointOutT>::type FieldList;
pcl::for_each_type<FieldList> (SetIfFieldExists<PointOutT, float> (output.points[i], "normal_x", normals_->points[i].normal_x));
pcl::for_each_type<FieldList> (SetIfFieldExists<PointOutT, float> (output.points[i], "normal_y", normals_->points[i].normal_y));
pcl::for_each_type<FieldList> (SetIfFieldExists<PointOutT, float> (output.points[i], "normal_z", normals_->points[i].normal_z));
pcl::for_each_type<FieldList> (SetIfFieldExists<PointOutT, float> (output.points[i], "curvature", normals_->points[i].curvature));
}
}
// Set proper widths and heights for the clouds
output.height = 1;
output.width = static_cast<uint32_t> (output.size ());
deinitCompute ();
}
template <typename PointInT, typename PointOutT, typename IntensityT> void
pcl::HarrisKeypoint2D<PointInT, PointOutT, IntensityT>::detectKeypoints (PointCloudOut &output)
{
derivatives_cols_.resize (input_->width, input_->height);
derivatives_rows_.resize (input_->width, input_->height);
//Compute cloud intensities first derivatives along columns and rows
//!!! nsallem 20120220 : we don't test here for density so if one term in nan the result is nan
int w = static_cast<int> (input_->width) - 1;
int h = static_cast<int> (input_->height) - 1;
// j = 0 --> j-1 out of range ; use 0
// i = 0 --> i-1 out of range ; use 0
derivatives_cols_(0,0) = (intensity_ ((*input_) (0,1)) - intensity_ ((*input_) (0,0))) * 0.5;
derivatives_rows_(0,0) = (intensity_ ((*input_) (1,0)) - intensity_ ((*input_) (0,0))) * 0.5;
// #ifdef _OPENMP
// //#pragma omp parallel for shared (derivatives_cols_, input_) num_threads (threads_)
// #pragma omp parallel for num_threads (threads_)
// #endif
for(int i = 1; i < w; ++i)
{
derivatives_cols_(i,0) = (intensity_ ((*input_) (i,1)) - intensity_ ((*input_) (i,0))) * 0.5;
}
derivatives_rows_(w,0) = (intensity_ ((*input_) (w,0)) - intensity_ ((*input_) (w-1,0))) * 0.5;
derivatives_cols_(w,0) = (intensity_ ((*input_) (w,1)) - intensity_ ((*input_) (w,0))) * 0.5;
// #ifdef _OPENMP
// //#pragma omp parallel for shared (derivatives_cols_, derivatives_rows_, input_) num_threads (threads_)
// #pragma omp parallel for num_threads (threads_)
// #endif
for(int j = 1; j < h; ++j)
{
// i = 0 --> i-1 out of range ; use 0
derivatives_rows_(0,j) = (intensity_ ((*input_) (1,j)) - intensity_ ((*input_) (0,j))) * 0.5;
for(int i = 1; i < w; ++i)
{
// derivative with respect to rows
derivatives_rows_(i,j) = (intensity_ ((*input_) (i+1,j)) - intensity_ ((*input_) (i-1,j))) * 0.5;
// derivative with respect to cols
derivatives_cols_(i,j) = (intensity_ ((*input_) (i,j+1)) - intensity_ ((*input_) (i,j-1))) * 0.5;
}
// i = w --> w+1 out of range ; use w
derivatives_rows_(w,j) = (intensity_ ((*input_) (w,j)) - intensity_ ((*input_) (w-1,j))) * 0.5;
}
// j = h --> j+1 out of range use h
derivatives_cols_(0,h) = (intensity_ ((*input_) (0,h)) - intensity_ ((*input_) (0,h-1))) * 0.5;
derivatives_rows_(0,h) = (intensity_ ((*input_) (1,h)) - intensity_ ((*input_) (0,h))) * 0.5;
// #ifdef _OPENMP
// //#pragma omp parallel for shared (derivatives_cols_, input_) num_threads (threads_)
// #pragma omp parallel for num_threads (threads_)
// #endif
for(int i = 1; i < w; ++i)
{
derivatives_cols_(i,h) = (intensity_ ((*input_) (i,h)) - intensity_ ((*input_) (i,h-1))) * 0.5;
}
derivatives_rows_(w,h) = (intensity_ ((*input_) (w,h)) - intensity_ ((*input_) (w-1,h))) * 0.5;
derivatives_cols_(w,h) = (intensity_ ((*input_) (w,h)) - intensity_ ((*input_) (w,h-1))) * 0.5;
float highest_response_;
switch (method_)
{
case HARRIS:
responseHarris(*response_, highest_response_);
break;
case NOBLE:
responseNoble(*response_, highest_response_);
break;
case LOWE:
responseLowe(*response_, highest_response_);
break;
case TOMASI:
responseTomasi(*response_, highest_response_);
break;
}
if (!nonmax_)
output = *response_;
else
{
threshold_*= highest_response_;
std::sort (indices_->begin (), indices_->end (),
boost::bind (&HarrisKeypoint2D::greaterIntensityAtIndices, this, _1, _2));
output.clear ();
output.reserve (response_->size());
std::vector<bool> occupency_map (response_->size (), false);
int width (response_->width);
int height (response_->height);
const int occupency_map_size (occupency_map.size ());
#ifdef _OPENMP
#pragma omp parallel for shared (output, occupency_map) private (width, height) num_threads(threads_)
#endif
for (int idx = 0; idx < occupency_map_size; ++idx)
{
if (occupency_map[idx] || response_->points [indices_->at (idx)].intensity < threshold_ || !isFinite (response_->points[idx]))
continue;
#ifdef _OPENMP
#pragma omp critical
#endif
output.push_back (response_->at (indices_->at (idx)));
int u_end = std::min (width, indices_->at (idx) % width + min_distance_);
int v_end = std::min (height, indices_->at (idx) / width + min_distance_);
for(int u = std::max (0, indices_->at (idx) % width - min_distance_); u < u_end; ++u)
for(int v = std::max (0, indices_->at (idx) / width - min_distance_); v < v_end; ++v)
occupency_map[v*input_->width+u] = true;
}
// if (refine_)
// refineCorners (output);
output.height = 1;
output.width = static_cast<uint32_t> (output.size());
}
// we don not change the denseness
output.is_dense = input_->is_dense;
}
template <typename PointInT, typename PointOutT> void
pcl::BilateralUpsampling<PointInT, PointOutT>::performProcessing (PointCloudOut &output)
{
output.resize (input_->size ());
float nan = std::numeric_limits<float>::quiet_NaN ();
Eigen::MatrixXf val_exp_depth_matrix;
Eigen::VectorXf val_exp_rgb_vector;
computeDistances (val_exp_depth_matrix, val_exp_rgb_vector);
for (int x = 0; x < static_cast<int> (input_->width); ++x)
for (int y = 0; y < static_cast<int> (input_->height); ++y)
{
int start_window_x = std::max (x - window_size_, 0),
start_window_y = std::max (y - window_size_, 0),
end_window_x = std::min (x + window_size_, static_cast<int> (input_->width)),
end_window_y = std::min (y + window_size_, static_cast<int> (input_->height));
float sum = 0.0f,
norm_sum = 0.0f;
for (int x_w = start_window_x; x_w < end_window_x; ++ x_w)
for (int y_w = start_window_y; y_w < end_window_y; ++ y_w)
{
float dx = float (x - x_w),
dy = float (y - y_w);
float val_exp_depth = val_exp_depth_matrix(dx+window_size_, dy+window_size_);
float d_color = static_cast<float> (
abs (input_->points[y_w * input_->width + x_w].r - input_->points[y * input_->width + x].r) +
abs (input_->points[y_w * input_->width + x_w].g - input_->points[y * input_->width + x].g) +
abs (input_->points[y_w * input_->width + x_w].b - input_->points[y * input_->width + x].b));
float val_exp_rgb = val_exp_rgb_vector(Eigen::VectorXf::Index(d_color));
if (pcl_isfinite (input_->points[y_w*input_->width + x_w].z))
{
sum += val_exp_depth * val_exp_rgb * input_->points[y_w*input_->width + x_w].z;
norm_sum += val_exp_depth * val_exp_rgb;
}
}
output.points[y*input_->width + x].r = input_->points[y*input_->width + x].r;
output.points[y*input_->width + x].g = input_->points[y*input_->width + x].g;
output.points[y*input_->width + x].b = input_->points[y*input_->width + x].b;
if (norm_sum != 0.0f)
{
float depth = sum / norm_sum;
Eigen::Vector3f pc (static_cast<float> (x) * depth, static_cast<float> (y) * depth, depth);
Eigen::Vector3f pw (unprojection_matrix_ * pc);
output.points[y*input_->width + x].x = pw[0];
output.points[y*input_->width + x].y = pw[1];
output.points[y*input_->width + x].z = pw[2];
}
else
{
output.points[y*input_->width + x].x = nan;
output.points[y*input_->width + x].y = nan;
output.points[y*input_->width + x].z = nan;
}
}
output.header = input_->header;
output.width = input_->width;
output.height = input_->height;
}
template <typename PointInT, typename PointOutT> void
pcl::MovingLeastSquares<PointInT, PointOutT>::performUpsampling (PointCloudOut &output)
{
if (upsample_method_ == DISTINCT_CLOUD)
{
for (size_t dp_i = 0; dp_i < distinct_cloud_->size (); ++dp_i) // dp_i = distinct_point_i
{
// Distinct cloud may have nan points, skip them
if (!pcl_isfinite (distinct_cloud_->points[dp_i].x))
continue;
// Get 3D position of point
//Eigen::Vector3f pos = distinct_cloud_->points[dp_i].getVector3fMap ();
std::vector<int> nn_indices;
std::vector<float> nn_dists;
tree_->nearestKSearch (distinct_cloud_->points[dp_i], 1, nn_indices, nn_dists);
int input_index = nn_indices.front ();
// If the closest point did not have a valid MLS fitting result
// OR if it is too far away from the sampled point
if (mls_results_[input_index].valid == false)
continue;
Eigen::Vector3d add_point = distinct_cloud_->points[dp_i].getVector3fMap ().template cast<double> ();
float u_disp = static_cast<float> ((add_point - mls_results_[input_index].mean).dot (mls_results_[input_index].u_axis)),
v_disp = static_cast<float> ((add_point - mls_results_[input_index].mean).dot (mls_results_[input_index].v_axis));
PointOutT result_point;
pcl::Normal result_normal;
projectPointToMLSSurface (u_disp, v_disp,
mls_results_[input_index].u_axis, mls_results_[input_index].v_axis,
mls_results_[input_index].plane_normal,
mls_results_[input_index].mean,
mls_results_[input_index].curvature,
mls_results_[input_index].c_vec,
mls_results_[input_index].num_neighbors,
result_point, result_normal);
// Copy additional point information if available
copyMissingFields (input_->points[input_index], result_point);
// Store the id of the original point
corresponding_input_indices_->indices.push_back (input_index);
output.push_back (result_point);
if (compute_normals_)
normals_->push_back (result_normal);
}
}
// For the voxel grid upsampling method, generate the voxel grid and dilate it
// Then, project the newly obtained points to the MLS surface
if (upsample_method_ == VOXEL_GRID_DILATION)
{
MLSVoxelGrid voxel_grid (input_, indices_, voxel_size_);
for (int iteration = 0; iteration < dilation_iteration_num_; ++iteration)
voxel_grid.dilate ();
for (typename MLSVoxelGrid::HashMap::iterator m_it = voxel_grid.voxel_grid_.begin (); m_it != voxel_grid.voxel_grid_.end (); ++m_it)
{
// Get 3D position of point
Eigen::Vector3f pos;
voxel_grid.getPosition (m_it->first, pos);
PointInT p;
p.x = pos[0];
p.y = pos[1];
p.z = pos[2];
std::vector<int> nn_indices;
std::vector<float> nn_dists;
tree_->nearestKSearch (p, 1, nn_indices, nn_dists);
int input_index = nn_indices.front ();
// If the closest point did not have a valid MLS fitting result
// OR if it is too far away from the sampled point
if (mls_results_[input_index].valid == false)
continue;
Eigen::Vector3d add_point = p.getVector3fMap ().template cast<double> ();
float u_disp = static_cast<float> ((add_point - mls_results_[input_index].mean).dot (mls_results_[input_index].u_axis)),
v_disp = static_cast<float> ((add_point - mls_results_[input_index].mean).dot (mls_results_[input_index].v_axis));
PointOutT result_point;
pcl::Normal result_normal;
projectPointToMLSSurface (u_disp, v_disp,
mls_results_[input_index].u_axis, mls_results_[input_index].v_axis,
mls_results_[input_index].plane_normal,
mls_results_[input_index].mean,
mls_results_[input_index].curvature,
mls_results_[input_index].c_vec,
mls_results_[input_index].num_neighbors,
result_point, result_normal);
// Copy additional point information if available
copyMissingFields (input_->points[input_index], result_point);
// Store the id of the original point
corresponding_input_indices_->indices.push_back (input_index);
output.push_back (result_point);
if (compute_normals_)
normals_->push_back (result_normal);
}
}
}
template <typename PointInT, typename PointOutT> void
pcl::MovingLeastSquares<PointInT, PointOutT>::process (PointCloudOut &output)
{
// Check if normals have to be computed/saved
if (compute_normals_)
{
normals_.reset (new NormalCloud);
// Copy the header
normals_->header = input_->header;
// Clear the fields in case the method exits before computation
normals_->width = normals_->height = 0;
normals_->points.clear ();
}
// Copy the header
output.header = input_->header;
output.width = output.height = 0;
output.points.clear ();
if (search_radius_ <= 0 || sqr_gauss_param_ <= 0)
{
PCL_ERROR ("[pcl::%s::reconstruct] Invalid search radius (%f) or Gaussian parameter (%f)!\n", getClassName ().c_str (), search_radius_, sqr_gauss_param_);
return;
}
if (!initCompute ())
return;
// Initialize the spatial locator
if (!tree_)
{
KdTreePtr tree;
if (input_->isOrganized ())
tree.reset (new pcl::search::OrganizedNeighbor<PointInT> ());
else
tree.reset (new pcl::search::KdTree<PointInT> (false));
setSearchMethod (tree);
}
// Send the surface dataset to the spatial locator
tree_->setInputCloud (input_, indices_);
// Initialize random number generator if necessary
switch (upsample_method_)
{
case (RANDOM_UNIFORM_DENSITY):
{
boost::mt19937 *rng = new boost::mt19937 (static_cast<unsigned int>(std::time(0)));
float tmp = static_cast<float> (search_radius_ / 2.0f);
boost::uniform_real<float> *uniform_distrib = new boost::uniform_real<float> (-tmp, tmp);
rng_uniform_distribution_ = new boost::variate_generator<boost::mt19937, boost::uniform_real<float> > (*rng, *uniform_distrib);
break;
}
case (VOXEL_GRID_DILATION):
{
mls_results_.resize (input_->size ());
break;
}
default:
break;
}
// Perform the actual surface reconstruction
performProcessing (output);
if (compute_normals_)
{
normals_->height = 1;
normals_->width = static_cast<uint32_t> (normals_->size ());
// TODO!!! MODIFY TO PER-CLOUD COPYING - much faster than per-point
for (unsigned int i = 0; i < output.size (); ++i)
{
typedef typename pcl::traits::fieldList<PointOutT>::type FieldList;
pcl::for_each_type<FieldList> (SetIfFieldExists<PointOutT, float> (output.points[i], "normal_x", normals_->points[i].normal_x));
pcl::for_each_type<FieldList> (SetIfFieldExists<PointOutT, float> (output.points[i], "normal_y", normals_->points[i].normal_y));
pcl::for_each_type<FieldList> (SetIfFieldExists<PointOutT, float> (output.points[i], "normal_z", normals_->points[i].normal_z));
pcl::for_each_type<FieldList> (SetIfFieldExists<PointOutT, float> (output.points[i], "curvature", normals_->points[i].curvature));
}
}
// Set proper widths and heights for the clouds
output.height = 1;
output.width = static_cast<uint32_t> (output.size ());
deinitCompute ();
}
template <typename PointInT, typename PointOutT> void
pcl::MovingLeastSquares<PointInT, PointOutT>::performProcessing (PointCloudOut &output)
{
// Compute the number of coefficients
nr_coeff_ = (order_ + 1) * (order_ + 2) / 2;
// Allocate enough space to hold the results of nearest neighbor searches
// \note resize is irrelevant for a radiusSearch ().
std::vector<int> nn_indices;
std::vector<float> nn_sqr_dists;
typedef typename pcl::traits::fieldList<typename PointCloudIn::PointType>::type FieldListInput;
typedef typename pcl::traits::fieldList<typename PointCloudOut::PointType>::type FieldListOutput;
// For all points
for (size_t cp = 0; cp < indices_->size (); ++cp)
{
// Get the initial estimates of point positions and their neighborhoods
if (!searchForNeighbors (int (cp), nn_indices, nn_sqr_dists))
continue;
// Check the number of nearest neighbors for normal estimation (and later
// for polynomial fit as well)
if (nn_indices.size () < 3)
continue;
PointCloudOut projected_points;
NormalCloud projected_points_normals;
// Get a plane approximating the local surface's tangent and project point onto it
computeMLSPointNormal (int (cp), *input_, nn_indices, nn_sqr_dists, projected_points, projected_points_normals);
/// Copy RGB information if available
float rgb_input;
bool rgb_exists_input;
pcl::for_each_type<FieldListInput> (pcl::CopyIfFieldExists<typename PointCloudIn::PointType, float> (
input_->points[(*indices_)[cp]], "rgb", rgb_exists_input, rgb_input));
if (rgb_exists_input)
{
for (size_t pp = 0; pp < projected_points.size (); ++pp)
pcl::for_each_type<FieldListOutput> (pcl::SetIfFieldExists<typename PointCloudOut::PointType, float> (
projected_points.points[pp], "rgb", rgb_input));
}
// Append projected points to output
output.insert (output.end (), projected_points.begin (), projected_points.end ());
if (compute_normals_)
normals_->insert (normals_->end (), projected_points_normals.begin (), projected_points_normals.end ());
}
if (upsample_method_ == DISTINCT_CLOUD)
{
for (size_t dp_i = 0; dp_i < distinct_cloud_->size (); ++dp_i) // dp_i = distinct_point_i
{
// Distinct cloud may have nan points, skip them
if (!pcl_isfinite (distinct_cloud_->points[dp_i].x))
continue;
// Get 3D position of point
//Eigen::Vector3f pos = distinct_cloud_->points[dp_i].getVector3fMap ();
std::vector<int> nn_indices;
std::vector<float> nn_dists;
tree_->nearestKSearch (distinct_cloud_->points[dp_i], 1, nn_indices, nn_dists);
int input_index = nn_indices.front ();
// If the closest point did not have a valid MLS fitting result
// OR if it is too far away from the sampled point
if (mls_results_[input_index].valid == false)
continue;
Eigen::Vector3f add_point = distinct_cloud_->points[dp_i].getVector3fMap (),
input_point = input_->points[input_index].getVector3fMap ();
Eigen::Vector3d aux = mls_results_[input_index].u;
Eigen::Vector3f u = aux.cast<float> ();
aux = mls_results_[input_index].v;
Eigen::Vector3f v = aux.cast<float> ();
float u_disp = (add_point - input_point).dot (u),
v_disp = (add_point - input_point).dot (v);
PointOutT result_point;
pcl::Normal result_normal;
projectPointToMLSSurface (u_disp, v_disp,
mls_results_[input_index].u, mls_results_[input_index].v,
mls_results_[input_index].plane_normal,
mls_results_[input_index].curvature,
input_point,
mls_results_[input_index].c_vec,
mls_results_[input_index].num_neighbors,
result_point, result_normal);
/// Copy RGB information if available
float rgb_input;
bool rgb_exists_input;
pcl::for_each_type<FieldListInput> (pcl::CopyIfFieldExists<typename PointCloudIn::PointType, float> (
input_->points[input_index], "rgb", rgb_exists_input, rgb_input));
if (rgb_exists_input)
{
pcl::for_each_type<FieldListOutput> (pcl::SetIfFieldExists<typename PointCloudOut::PointType, float> (
result_point, "rgb", rgb_input));
}
output.push_back (result_point);
if (compute_normals_)
normals_->push_back (result_normal);
}
}
// For the voxel grid upsampling method, generate the voxel grid and dilate it
// Then, project the newly obtained points to the MLS surface
if (upsample_method_ == VOXEL_GRID_DILATION)
{
MLSVoxelGrid voxel_grid (input_, indices_, voxel_size_);
for (int iteration = 0; iteration < dilation_iteration_num_; ++iteration)
voxel_grid.dilate ();
for (typename MLSVoxelGrid::HashMap::iterator m_it = voxel_grid.voxel_grid_.begin (); m_it != voxel_grid.voxel_grid_.end (); ++m_it)
{
// Get 3D position of point
Eigen::Vector3f pos;
voxel_grid.getPosition (m_it->first, pos);
PointInT p;
p.x = pos[0];
p.y = pos[1];
p.z = pos[2];
std::vector<int> nn_indices;
std::vector<float> nn_dists;
tree_->nearestKSearch (p, 1, nn_indices, nn_dists);
int input_index = nn_indices.front ();
// If the closest point did not have a valid MLS fitting result
// OR if it is too far away from the sampled point
if (mls_results_[input_index].valid == false)
continue;
Eigen::Vector3f add_point = p.getVector3fMap (),
input_point = input_->points[input_index].getVector3fMap ();
Eigen::Vector3d aux = mls_results_[input_index].u;
Eigen::Vector3f u = aux.cast<float> ();
aux = mls_results_[input_index].v;
Eigen::Vector3f v = aux.cast<float> ();
float u_disp = (add_point - input_point).dot (u),
v_disp = (add_point - input_point).dot (v);
PointOutT result_point;
pcl::Normal result_normal;
projectPointToMLSSurface (u_disp, v_disp,
mls_results_[input_index].u, mls_results_[input_index].v,
mls_results_[input_index].plane_normal,
mls_results_[input_index].curvature,
input_point,
mls_results_[input_index].c_vec,
mls_results_[input_index].num_neighbors,
result_point, result_normal);
float d_before = (pos - input_point).norm (),
d_after = (result_point.getVector3fMap () - input_point). norm();
if (d_after > d_before)
continue;
/// Copy RGB information if available
float rgb_input;
bool rgb_exists_input;
pcl::for_each_type<FieldListInput> (pcl::CopyIfFieldExists<typename PointCloudIn::PointType, float> (
input_->points[input_index], "rgb", rgb_exists_input, rgb_input));
if (rgb_exists_input)
{
pcl::for_each_type<FieldListOutput> (pcl::SetIfFieldExists<typename PointCloudOut::PointType, float> (
result_point, "rgb", rgb_input));
}
output.push_back (result_point);
if (compute_normals_)
normals_->push_back (result_normal);
}
}
}
template <typename PointInT, typename PointOutT, typename NormalT> void
pcl::TrajkovicKeypoint3D<PointInT, PointOutT, NormalT>::detectKeypoints (PointCloudOut &output)
{
response_.reset (new pcl::PointCloud<float> (input_->width, input_->height));
const Normals &normals = *normals_;
const PointCloudIn &input = *input_;
pcl::PointCloud<float>& response = *response_;
const int w = static_cast<int> (input_->width) - half_window_size_;
const int h = static_cast<int> (input_->height) - half_window_size_;
if (method_ == FOUR_CORNERS)
{
#ifdef _OPENMP
#pragma omp parallel for num_threads (threads_)
#endif
for(int j = half_window_size_; j < h; ++j)
{
for(int i = half_window_size_; i < w; ++i)
{
if (!isFinite (input (i,j))) continue;
const NormalT ¢er = normals (i,j);
if (!isFinite (center)) continue;
int count = 0;
const NormalT &up = getNormalOrNull (i, j-half_window_size_, count);
const NormalT &down = getNormalOrNull (i, j+half_window_size_, count);
const NormalT &left = getNormalOrNull (i-half_window_size_, j, count);
const NormalT &right = getNormalOrNull (i+half_window_size_, j, count);
// Get rid of isolated points
if (!count) continue;
float sn1 = squaredNormalsDiff (up, center);
float sn2 = squaredNormalsDiff (down, center);
float r1 = sn1 + sn2;
float r2 = squaredNormalsDiff (right, center) + squaredNormalsDiff (left, center);
float d = std::min (r1, r2);
if (d < first_threshold_) continue;
sn1 = sqrt (sn1);
sn2 = sqrt (sn2);
float b1 = normalsDiff (right, up) * sn1;
b1+= normalsDiff (left, down) * sn2;
float b2 = normalsDiff (right, down) * sn2;
b2+= normalsDiff (left, up) * sn1;
float B = std::min (b1, b2);
float A = r2 - r1 - 2*B;
response (i,j) = ((B < 0) && ((B + A) > 0)) ? r1 - ((B*B)/A) : d;
}
}
}
else
{
#ifdef _OPENMP
#pragma omp parallel for num_threads (threads_)
#endif
for(int j = half_window_size_; j < h; ++j)
{
for(int i = half_window_size_; i < w; ++i)
{
if (!isFinite (input (i,j))) continue;
const NormalT ¢er = normals (i,j);
if (!isFinite (center)) continue;
int count = 0;
const NormalT &up = getNormalOrNull (i, j-half_window_size_, count);
const NormalT &down = getNormalOrNull (i, j+half_window_size_, count);
const NormalT &left = getNormalOrNull (i-half_window_size_, j, count);
const NormalT &right = getNormalOrNull (i+half_window_size_, j, count);
const NormalT &upleft = getNormalOrNull (i-half_window_size_, j-half_window_size_, count);
const NormalT &upright = getNormalOrNull (i+half_window_size_, j-half_window_size_, count);
const NormalT &downleft = getNormalOrNull (i-half_window_size_, j+half_window_size_, count);
const NormalT &downright = getNormalOrNull (i+half_window_size_, j+half_window_size_, count);
// Get rid of isolated points
if (!count) continue;
std::vector<float> r (4,0);
r[0] = squaredNormalsDiff (up, center);
r[0]+= squaredNormalsDiff (down, center);
r[1] = squaredNormalsDiff (upright, center);
r[1]+= squaredNormalsDiff (downleft, center);
r[2] = squaredNormalsDiff (right, center);
r[2]+= squaredNormalsDiff (left, center);
r[3] = squaredNormalsDiff (downright, center);
r[3]+= squaredNormalsDiff (upleft, center);
float d = *(std::min_element (r.begin (), r.end ()));
if (d < first_threshold_) continue;
std::vector<float> B (4,0);
std::vector<float> A (4,0);
std::vector<float> sumAB (4,0);
B[0] = normalsDiff (upright, up) * normalsDiff (up, center);
B[0]+= normalsDiff (downleft, down) * normalsDiff (down, center);
B[1] = normalsDiff (right, upright) * normalsDiff (upright, center);
B[1]+= normalsDiff (left, downleft) * normalsDiff (downleft, center);
B[2] = normalsDiff (downright, right) * normalsDiff (downright, center);
B[2]+= normalsDiff (upleft, left) * normalsDiff (upleft, center);
B[3] = normalsDiff (down, downright) * normalsDiff (downright, center);
B[3]+= normalsDiff (up, upleft) * normalsDiff (upleft, center);
A[0] = r[1] - r[0] - B[0] - B[0];
A[1] = r[2] - r[1] - B[1] - B[1];
A[2] = r[3] - r[2] - B[2] - B[2];
A[3] = r[0] - r[3] - B[3] - B[3];
sumAB[0] = A[0] + B[0];
sumAB[1] = A[1] + B[1];
sumAB[2] = A[2] + B[2];
sumAB[3] = A[3] + B[3];
if ((*std::max_element (B.begin (), B.end ()) < 0) &&
(*std::min_element (sumAB.begin (), sumAB.end ()) > 0))
{
std::vector<float> D (4,0);
D[0] = B[0] * B[0] / A[0];
D[1] = B[1] * B[1] / A[1];
D[2] = B[2] * B[2] / A[2];
D[3] = B[3] * B[3] / A[3];
response (i,j) = *(std::min (D.begin (), D.end ()));
}
else
response (i,j) = d;
}
}
}
// Non maximas suppression
std::vector<int> indices = *indices_;
std::sort (indices.begin (), indices.end (),
boost::bind (&TrajkovicKeypoint3D::greaterCornernessAtIndices, this, _1, _2));
output.clear ();
output.reserve (input_->size ());
std::vector<bool> occupency_map (indices.size (), false);
const int width (input_->width);
const int height (input_->height);
const int occupency_map_size (indices.size ());
#ifdef _OPENMP
#pragma omp parallel for shared (output) num_threads (threads_)
#endif
for (int i = 0; i < indices.size (); ++i)
{
int idx = indices[i];
if ((response_->points[idx] < second_threshold_) || occupency_map[idx])
continue;
PointOutT p;
p.getVector3fMap () = input_->points[idx].getVector3fMap ();
p.intensity = response_->points [idx];
#ifdef _OPENMP
#pragma omp critical
#endif
{
output.push_back (p);
keypoints_indices_->indices.push_back (idx);
}
const int x = idx % width;
const int y = idx / width;
const int u_end = std::min (width, x + half_window_size_);
const int v_end = std::min (height, y + half_window_size_);
for(int v = std::max (0, y - half_window_size_); v < v_end; ++v)
for(int u = std::max (0, x - half_window_size_); u < u_end; ++u)
occupency_map[v*width + u] = true;
}
output.height = 1;
output.width = static_cast<uint32_t> (output.size());
// we don not change the denseness
output.is_dense = true;
}
template <typename PointInT, typename PointOutT> void
pcl::MovingLeastSquaresOMP<PointInT, PointOutT>::performProcessing (PointCloudOut &output)
{
// Compute the number of coefficients
nr_coeff_ = (order_ + 1) * (order_ + 2) / 2;
// (Maximum) number of threads
unsigned int threads = threads_ == 0 ? 1 : threads_;
// Create temporaries for each thread in order to avoid synchronization
typename PointCloudOut::CloudVectorType projected_points (threads);
typename NormalCloud::CloudVectorType projected_points_normals (threads);
std::vector<PointIndices> corresponding_input_indices (threads);
// For all points
#pragma omp parallel for schedule (dynamic,1000) num_threads (threads)
for (int cp = 0; cp < static_cast<int> (indices_->size ()); ++cp)
{
// Allocate enough space to hold the results of nearest neighbor searches
// \note resize is irrelevant for a radiusSearch ().
std::vector<int> nn_indices;
std::vector<float> nn_sqr_dists;
// Get the initial estimates of point positions and their neighborhoods
if (this->searchForNeighbors ((*indices_)[cp], nn_indices, nn_sqr_dists))
{
// Check the number of nearest neighbors for normal estimation (and later
// for polynomial fit as well)
if (nn_indices.size () >= 3)
{
// This thread's ID (range 0 to threads-1)
int tn = omp_get_thread_num ();
// Size of projected points before computeMLSPointNormal () adds points
size_t pp_size = projected_points[tn].size ();
// Get a plane approximating the local surface's tangent and project point onto it
int index = (*indices_)[cp];
this->computeMLSPointNormal (index, nn_indices, nn_sqr_dists, projected_points[tn], projected_points_normals[tn], corresponding_input_indices[tn], this->mls_results_[index]);
// Copy all information from the input cloud to the output points (not doing any interpolation)
for (size_t pp = pp_size; pp < projected_points[tn].size (); ++pp)
this->copyMissingFields (input_->points[(*indices_)[cp]], projected_points[tn][pp]);
}
}
}
// Combine all threads' results into the output vectors
for (unsigned int tn = 0; tn < threads; ++tn)
{
output.insert (output.end (), projected_points[tn].begin (), projected_points[tn].end ());
corresponding_input_indices_->indices.insert (corresponding_input_indices_->indices.end (),
corresponding_input_indices[tn].indices.begin (), corresponding_input_indices[tn].indices.end ());
if (compute_normals_)
normals_->insert (normals_->end (), projected_points_normals[tn].begin (), projected_points_normals[tn].end ());
}
// Perform the distinct-cloud or voxel-grid upsampling
this->performUpsampling (output);
}
