00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039
00040 #ifndef PCL_FEATURES_IMPL_NORMAL_3D_OMP_H_
00041 #define PCL_FEATURES_IMPL_NORMAL_3D_OMP_H_
00042
00043 #include <pcl/features/normal_3d_omp.h>
00044
00046 template <typename PointInT> void
00047 pcl::NormalEstimationOMP<PointInT, Eigen::MatrixXf>::computeFeatureEigen (pcl::PointCloud<Eigen::MatrixXf> &output)
00048 {
00049 float vpx, vpy, vpz;
00050 getViewPoint (vpx, vpy, vpz);
00051 output.is_dense = true;
00052
00053
00054 output.points.resize (indices_->size (), 4);
00055
00056
00057 #if defined(_WIN32) || ((__GNUC__ > 4) && (__GNUC_MINOR__ > 2))
00058 #pragma omp parallel for schedule (dynamic, threads_)
00059 #endif
00060
00061 for (int idx = 0; idx < static_cast<int> (indices_->size ()); ++idx)
00062 {
00063
00064
00065 std::vector<int> nn_indices (k_);
00066 std::vector<float> nn_dists (k_);
00067
00068 if (!isFinite ((*input_)[(*indices_)[idx]]) ||
00069 this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists) == 0)
00070 {
00071 output.points (idx, 0) = output.points (idx, 1) = output.points (idx, 2) = output.points (idx, 3) = std::numeric_limits<float>::quiet_NaN ();
00072 output.is_dense = false;
00073 continue;
00074 }
00075
00076
00077 Eigen::Vector4f xyz_centroid;
00078
00079 compute3DCentroid (*surface_, nn_indices, xyz_centroid);
00080
00081
00082 EIGEN_ALIGN16 Eigen::Matrix3f covariance_matrix;
00083
00084 computeCovarianceMatrix (*surface_, nn_indices, xyz_centroid, covariance_matrix);
00085
00086
00087 solvePlaneParameters (covariance_matrix,
00088 output.points (idx, 0), output.points (idx, 1), output.points (idx, 2), output.points (idx, 3));
00089
00090 flipNormalTowardsViewpoint (input_->points[(*indices_)[idx]], vpx, vpy, vpz,
00091 output.points (idx, 0), output.points (idx, 1), output.points (idx, 2));
00092 }
00093 }
00094
00096 template <typename PointInT, typename PointOutT> void
00097 pcl::NormalEstimationOMP<PointInT, PointOutT>::computeFeature (PointCloudOut &output)
00098 {
00099 float vpx, vpy, vpz;
00100 getViewPoint (vpx, vpy, vpz);
00101
00102 output.is_dense = true;
00103
00104 #pragma omp parallel for schedule (dynamic, threads_)
00105 for (int idx = 0; idx < static_cast<int> (indices_->size ()); ++idx)
00106 {
00107
00108
00109 std::vector<int> nn_indices (k_);
00110 std::vector<float> nn_dists (k_);
00111
00112 if (!isFinite ((*input_)[(*indices_)[idx]]) ||
00113 this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists) == 0)
00114 {
00115 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 ();
00116
00117 output.is_dense = false;
00118 continue;
00119 }
00120
00121
00122 Eigen::Vector4f xyz_centroid;
00123
00124 compute3DCentroid (*surface_, nn_indices, xyz_centroid);
00125
00126
00127 EIGEN_ALIGN16 Eigen::Matrix3f covariance_matrix;
00128
00129 computeCovarianceMatrix (*surface_, nn_indices, xyz_centroid, covariance_matrix);
00130
00131
00132 solvePlaneParameters (covariance_matrix,
00133 output.points[idx].normal[0], output.points[idx].normal[1], output.points[idx].normal[2], output.points[idx].curvature);
00134
00135 flipNormalTowardsViewpoint (input_->points[(*indices_)[idx]], vpx, vpy, vpz,
00136 output.points[idx].normal[0], output.points[idx].normal[1], output.points[idx].normal[2]);
00137 }
00138 }
00139
00140 #define PCL_INSTANTIATE_NormalEstimationOMP(T,NT) template class PCL_EXPORTS pcl::NormalEstimationOMP<T,NT>;
00141
00142 #endif // PCL_FEATURES_IMPL_NORMAL_3D_OMP_H_
00143