Go to the documentation of this file.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_FPFH_OMP_H_
00041 #define PCL_FEATURES_IMPL_FPFH_OMP_H_
00042
00043 #include <pcl/features/fpfh_omp.h>
00044
00046 template <typename PointInT, typename PointNT, typename PointOutT> void
00047 pcl::FPFHEstimationOMP<PointInT, PointNT, PointOutT>::computeFeature (PointCloudOut &output)
00048 {
00049 std::vector<int> spfh_indices_vec;
00050 std::vector<int> spfh_hist_lookup (surface_->points.size ());
00051
00052
00053
00054 if (surface_ != input_ ||
00055 indices_->size () != surface_->points.size ())
00056 {
00057 std::vector<int> nn_indices (k_);
00058 std::vector<float> nn_dists (k_);
00059
00060 std::set<int> spfh_indices_set;
00061 for (size_t idx = 0; idx < indices_->size (); ++idx)
00062 {
00063 int p_idx = (*indices_)[idx];
00064 if (this->searchForNeighbors (p_idx, search_parameter_, nn_indices, nn_dists) == 0)
00065 continue;
00066
00067 spfh_indices_set.insert (nn_indices.begin (), nn_indices.end ());
00068 }
00069 spfh_indices_vec.resize (spfh_indices_set.size ());
00070 std::copy (spfh_indices_set.begin (), spfh_indices_set.end (), spfh_indices_vec.begin ());
00071 }
00072 else
00073 {
00074
00075 spfh_indices_vec.resize (indices_->size ());
00076 for (int idx = 0; idx < static_cast<int> (indices_->size ()); ++idx)
00077 spfh_indices_vec[idx] = idx;
00078 }
00079
00080
00081 size_t data_size = spfh_indices_vec.size ();
00082 hist_f1_.setZero (data_size, nr_bins_f1_);
00083 hist_f2_.setZero (data_size, nr_bins_f2_);
00084 hist_f3_.setZero (data_size, nr_bins_f3_);
00085
00086
00087
00088 #pragma omp parallel for schedule (dynamic, threads_)
00089 for (int i = 0; i < static_cast<int> (spfh_indices_vec.size ()); ++i)
00090 {
00091
00092 int p_idx = spfh_indices_vec[i];
00093
00094
00095 std::vector<int> nn_indices (k_);
00096 std::vector<float> nn_dists (k_);
00097 if (this->searchForNeighbors (*surface_, p_idx, search_parameter_, nn_indices, nn_dists) == 0)
00098 continue;
00099
00100
00101 this->computePointSPFHSignature (*surface_, *normals_, p_idx, i, nn_indices, hist_f1_, hist_f2_, hist_f3_);
00102
00103
00104 spfh_hist_lookup[p_idx] = i;
00105 }
00106
00107
00108 int nr_bins = nr_bins_f1_ + nr_bins_f2_ + nr_bins_f3_;
00109
00110
00111 #pragma omp parallel for schedule (dynamic, threads_)
00112 for (int idx = 0; idx < static_cast<int> (indices_->size ()); ++idx)
00113 {
00114
00115 std::vector<int> nn_indices (k_);
00116 std::vector<float> nn_dists (k_);
00117 if (!isFinite ((*input_)[(*indices_)[idx]]) ||
00118 this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists) == 0)
00119 {
00120 for (int d = 0; d < nr_bins; ++d)
00121 output.points[idx].histogram[d] = std::numeric_limits<float>::quiet_NaN ();
00122
00123 output.is_dense = false;
00124 continue;
00125 }
00126
00127
00128
00129
00130 for (size_t i = 0; i < nn_indices.size (); ++i)
00131 nn_indices[i] = spfh_hist_lookup[nn_indices[i]];
00132
00133
00134 Eigen::VectorXf fpfh_histogram = Eigen::VectorXf::Zero (nr_bins);
00135 weightPointSPFHSignature (hist_f1_, hist_f2_, hist_f3_, nn_indices, nn_dists, fpfh_histogram);
00136
00137
00138 for (int d = 0; d < nr_bins; ++d)
00139 output.points[idx].histogram[d] = fpfh_histogram[d];
00140 }
00141
00142 }
00143
00144 #define PCL_INSTANTIATE_FPFHEstimationOMP(T,NT,OutT) template class PCL_EXPORTS pcl::FPFHEstimationOMP<T,NT,OutT>;
00145
00146 #endif // PCL_FEATURES_IMPL_FPFH_OMP_H_
00147