Go to the documentation of this file.00001
00063 #ifndef __IMPL_ORGANIZED_CURVATURE_ESTIMATION_OMP_H__
00064 #define __IMPL_ORGANIZED_CURVATURE_ESTIMATION_OMP_H__
00065
00066 #include "cob_3d_features/organized_curvature_estimation_omp.h"
00067
00068 template <typename PointInT, typename PointNT, typename PointLabelT, typename PointOutT> void
00069 cob_3d_features::OrganizedCurvatureEstimationOMP<PointInT,PointNT,PointLabelT,PointOutT>::computeFeature (PointCloudOut &output)
00070 {
00071 if (labels_->points.size() != input_->size())
00072 {
00073 labels_->points.resize(input_->size());
00074 labels_->height = input_->height;
00075 labels_->width = input_->width;
00076 }
00077 if (output.points.size() != input_->size())
00078 {
00079 output.points.resize(input_->size());
00080 output.height = input_->height;
00081 output.width = input_->width;
00082 }
00083
00084 int threadsize = 1;
00085
00086 #pragma omp parallel for schedule (dynamic, threadsize)
00087 for (size_t i=0; i < indices_->size(); ++i)
00088 {
00089 std::vector<int> nn_indices;
00090 std::vector<float> nn_distances;
00091 if (pcl_isnan(surface_->points[(*indices_)[i]].z) ||
00092 pcl_isnan(normals_->points[(*indices_)[i]].normal[2]))
00093 {
00094 labels_->points[(*indices_)[i]].label = I_NAN;
00095 }
00096 else if (this->searchForNeighborsInRange((*indices_)[i], nn_indices, nn_distances) != -1)
00097 {
00098 computePointCurvatures(*normals_, (*indices_)[i], nn_indices, nn_distances,
00099 output.points[(*indices_)[i]].principal_curvature[0],
00100 output.points[(*indices_)[i]].principal_curvature[1],
00101 output.points[(*indices_)[i]].principal_curvature[2],
00102 output.points[(*indices_)[i]].pc1,
00103 output.points[(*indices_)[i]].pc2,
00104 labels_->points[(*indices_)[i]].label);
00105 }
00106 else
00107 {
00108 labels_->points[(*indices_)[i]].label = I_NAN;
00109 }
00110 }
00111 }
00112
00113 #define PCL_INSTANTIATE_OrganizedCurvatureEstimationOMP(T,NT,LabelT,OutT) template class PCL_EXPORTS cob_3d_features::OrganizedCurvatureEstimationOMP<T,NT,LabelT,OutT>;
00114
00115 #endif