template <typename PointInT, typename PointOutT> void pcl::TBB_NormalEstimationTBB<PointInT, PointOutT>::operator () (const tbb::blocked_range <size_t> &r) const { float vpx, vpy, vpz; feature_->getViewPoint (vpx, vpy, vpz); // Iterating over the entire index vector for (size_t idx = r.begin (); idx != r.end (); ++idx) { std::vector<int> nn_indices (feature_->getKSearch ()); std::vector<float> nn_dists (feature_->getKSearch ()); feature_->searchForNeighbors ((*feature_->getIndices ())[idx], feature_->getSearchParameter (), nn_indices, nn_dists); // 16-bytes aligned placeholder for the XYZ centroid of a surface patch Eigen::Vector4f xyz_centroid; // Estimate the XYZ centroid compute3DCentroid (*feature_->getSearchSurface (), nn_indices, xyz_centroid); // Placeholder for the 3x3 covariance matrix at each surface patch EIGEN_ALIGN16 Eigen::Matrix3f covariance_matrix; // Compute the 3x3 covariance matrix computeCovarianceMatrix (*feature_->getSearchSurface (), nn_indices, xyz_centroid, covariance_matrix); // Get the plane normal and surface curvature solvePlaneParameters (covariance_matrix, output_.points[idx].normal[0], output_.points[idx].normal[1], output_.points[idx].normal[2], output_.points[idx].curvature); flipNormalTowardsViewpoint<PointInT> (feature_->getSearchSurface ()->points[idx], vpx, vpy, vpz, output_.points[idx].normal[0], output_.points[idx].normal[1], output_.points[idx].normal[2]); } }
inline void pcl::solvePlaneParameters (const Eigen::Matrix3f &covariance_matrix, const Eigen::Vector4f &point, Eigen::Vector4f &plane_parameters, float &curvature) { solvePlaneParameters (covariance_matrix, plane_parameters [0], plane_parameters [1], plane_parameters [2], curvature); plane_parameters[3] = 0; // Hessian form (D = nc . p_plane (centroid here) + p) plane_parameters[3] = -1 * plane_parameters.dot (point); }
template <typename PointInT> void pcl16::NormalEstimationOMP<PointInT, Eigen::MatrixXf>::computeFeatureEigen (pcl16::PointCloud<Eigen::MatrixXf> &output) { float vpx, vpy, vpz; getViewPoint (vpx, vpy, vpz); output.is_dense = true; // Resize the output dataset output.points.resize (indices_->size (), 4); // GCC 4.2.x seems to segfault with "internal compiler error" on MacOS X here #if defined(_WIN32) || ((__GNUC__ > 4) && (__GNUC_MINOR__ > 2)) #pragma omp parallel for schedule (dynamic, threads_) #endif // Iterating over the entire index vector for (int idx = 0; idx < static_cast<int> (indices_->size ()); ++idx) { // Allocate enough space to hold the results // \note This resize is irrelevant for a radiusSearch (). std::vector<int> nn_indices (k_); std::vector<float> nn_dists (k_); if (!isFinite ((*input_)[(*indices_)[idx]]) || this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists) == 0) { output.points (idx, 0) = output.points (idx, 1) = output.points (idx, 2) = output.points (idx, 3) = std::numeric_limits<float>::quiet_NaN (); output.is_dense = false; continue; } // 16-bytes aligned placeholder for the XYZ centroid of a surface patch Eigen::Vector4f xyz_centroid; // Estimate the XYZ centroid compute3DCentroid (*surface_, nn_indices, xyz_centroid); // Placeholder for the 3x3 covariance matrix at each surface patch EIGEN_ALIGN16 Eigen::Matrix3f covariance_matrix; // Compute the 3x3 covariance matrix computeCovarianceMatrix (*surface_, nn_indices, xyz_centroid, covariance_matrix); // Get the plane normal and surface curvature solvePlaneParameters (covariance_matrix, output.points (idx, 0), output.points (idx, 1), output.points (idx, 2), output.points (idx, 3)); flipNormalTowardsViewpoint (input_->points[(*indices_)[idx]], vpx, vpy, vpz, output.points (idx, 0), output.points (idx, 1), output.points (idx, 2)); } }
template <typename PointInT, typename PointOutT> void pcl16::NormalEstimationOMP<PointInT, PointOutT>::computeFeature (PointCloudOut &output) { float vpx, vpy, vpz; getViewPoint (vpx, vpy, vpz); output.is_dense = true; // Iterating over the entire index vector #pragma omp parallel for schedule (dynamic, threads_) for (int idx = 0; idx < static_cast<int> (indices_->size ()); ++idx) { // Allocate enough space to hold the results // \note This resize is irrelevant for a radiusSearch (). std::vector<int> nn_indices (k_); std::vector<float> nn_dists (k_); if (!isFinite ((*input_)[(*indices_)[idx]]) || this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists) == 0) { output.points[idx].normal[0] = output.points[idx].normal[1] = output.points[idx].normal[2] = output.points[idx].curvature = std::numeric_limits<float>::quiet_NaN (); output.is_dense = false; continue; } // 16-bytes aligned placeholder for the XYZ centroid of a surface patch Eigen::Vector4f xyz_centroid; // Estimate the XYZ centroid compute3DCentroid (*surface_, nn_indices, xyz_centroid); // Placeholder for the 3x3 covariance matrix at each surface patch EIGEN_ALIGN16 Eigen::Matrix3f covariance_matrix; // Compute the 3x3 covariance matrix computeCovarianceMatrix (*surface_, nn_indices, xyz_centroid, covariance_matrix); // Get the plane normal and surface curvature solvePlaneParameters (covariance_matrix, output.points[idx].normal[0], output.points[idx].normal[1], output.points[idx].normal[2], output.points[idx].curvature); flipNormalTowardsViewpoint (input_->points[(*indices_)[idx]], vpx, vpy, vpz, output.points[idx].normal[0], output.points[idx].normal[1], output.points[idx].normal[2]); } }