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_PFH_H_
00041 #define PCL_FEATURES_IMPL_PFH_H_
00042
00043 #include <pcl/features/pfh.h>
00044
00046 template <typename PointInT, typename PointNT, typename PointOutT> bool
00047 pcl::PFHEstimation<PointInT, PointNT, PointOutT>::computePairFeatures (
00048 const pcl::PointCloud<PointInT> &cloud, const pcl::PointCloud<PointNT> &normals,
00049 int p_idx, int q_idx, float &f1, float &f2, float &f3, float &f4)
00050 {
00051 pcl::computePairFeatures (cloud.points[p_idx].getVector4fMap (), normals.points[p_idx].getNormalVector4fMap (),
00052 cloud.points[q_idx].getVector4fMap (), normals.points[q_idx].getNormalVector4fMap (),
00053 f1, f2, f3, f4);
00054 return (true);
00055 }
00056
00058 template <typename PointInT, typename PointNT, typename PointOutT> void
00059 pcl::PFHEstimation<PointInT, PointNT, PointOutT>::computePointPFHSignature (
00060 const pcl::PointCloud<PointInT> &cloud, const pcl::PointCloud<PointNT> &normals,
00061 const std::vector<int> &indices, int nr_split, Eigen::VectorXf &pfh_histogram)
00062 {
00063 int h_index, h_p;
00064
00065
00066 pfh_histogram.setZero ();
00067
00068
00069 float hist_incr = 100.0f / static_cast<float> (indices.size () * (indices.size () - 1) / 2);
00070
00071 std::pair<int, int> key;
00072
00073 for (size_t i_idx = 0; i_idx < indices.size (); ++i_idx)
00074 {
00075 for (size_t j_idx = 0; j_idx < i_idx; ++j_idx)
00076 {
00077
00078 if (!isFinite (cloud.points[indices[i_idx]]) || !isFinite (cloud.points[indices[j_idx]]))
00079 continue;
00080
00081 if (use_cache_)
00082 {
00083
00084 int p1, p2;
00085
00086
00087 p1 = indices[i_idx];
00088 p2 = indices[j_idx];
00089
00090
00091
00092
00093
00094
00095 key = std::pair<int, int> (p1, p2);
00096
00097
00098 std::map<std::pair<int, int>, Eigen::Vector4f, std::less<std::pair<int, int> >, Eigen::aligned_allocator<Eigen::Vector4f> >::iterator fm_it = feature_map_.find (key);
00099 if (fm_it != feature_map_.end ())
00100 pfh_tuple_ = fm_it->second;
00101 else
00102 {
00103
00104 if (!computePairFeatures (cloud, normals, indices[i_idx], indices[j_idx],
00105 pfh_tuple_[0], pfh_tuple_[1], pfh_tuple_[2], pfh_tuple_[3]))
00106 continue;
00107 }
00108 }
00109 else
00110 if (!computePairFeatures (cloud, normals, indices[i_idx], indices[j_idx],
00111 pfh_tuple_[0], pfh_tuple_[1], pfh_tuple_[2], pfh_tuple_[3]))
00112 continue;
00113
00114
00115 f_index_[0] = static_cast<int> (floor (nr_split * ((pfh_tuple_[0] + M_PI) * d_pi_)));
00116 if (f_index_[0] < 0) f_index_[0] = 0;
00117 if (f_index_[0] >= nr_split) f_index_[0] = nr_split - 1;
00118
00119 f_index_[1] = static_cast<int> (floor (nr_split * ((pfh_tuple_[1] + 1.0) * 0.5)));
00120 if (f_index_[1] < 0) f_index_[1] = 0;
00121 if (f_index_[1] >= nr_split) f_index_[1] = nr_split - 1;
00122
00123 f_index_[2] = static_cast<int> (floor (nr_split * ((pfh_tuple_[2] + 1.0) * 0.5)));
00124 if (f_index_[2] < 0) f_index_[2] = 0;
00125 if (f_index_[2] >= nr_split) f_index_[2] = nr_split - 1;
00126
00127
00128 h_index = 0;
00129 h_p = 1;
00130 for (int d = 0; d < 3; ++d)
00131 {
00132 h_index += h_p * f_index_[d];
00133 h_p *= nr_split;
00134 }
00135 pfh_histogram[h_index] += hist_incr;
00136
00137 if (use_cache_)
00138 {
00139
00140 feature_map_[key] = pfh_tuple_;
00141
00142
00143 key_list_.push (key);
00144
00145 if (key_list_.size () > max_cache_size_)
00146 {
00147
00148 feature_map_.erase (key_list_.back ());
00149 key_list_.pop ();
00150 }
00151 }
00152 }
00153 }
00154 }
00155
00157 template <typename PointInT, typename PointNT, typename PointOutT> void
00158 pcl::PFHEstimation<PointInT, PointNT, PointOutT>::computeFeature (PointCloudOut &output)
00159 {
00160
00161 feature_map_.clear ();
00162 std::queue<std::pair<int, int> > empty;
00163 std::swap (key_list_, empty);
00164
00165 pfh_histogram_.setZero (nr_subdiv_ * nr_subdiv_ * nr_subdiv_);
00166
00167
00168
00169 std::vector<int> nn_indices (k_);
00170 std::vector<float> nn_dists (k_);
00171
00172 output.is_dense = true;
00173
00174 if (input_->is_dense)
00175 {
00176
00177 for (size_t idx = 0; idx < indices_->size (); ++idx)
00178 {
00179 if (this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists) == 0)
00180 {
00181 for (int d = 0; d < pfh_histogram_.size (); ++d)
00182 output.points[idx].histogram[d] = std::numeric_limits<float>::quiet_NaN ();
00183
00184 output.is_dense = false;
00185 continue;
00186 }
00187
00188
00189 computePointPFHSignature (*surface_, *normals_, nn_indices, nr_subdiv_, pfh_histogram_);
00190
00191
00192 for (int d = 0; d < pfh_histogram_.size (); ++d)
00193 output.points[idx].histogram[d] = pfh_histogram_[d];
00194 }
00195 }
00196 else
00197 {
00198
00199 for (size_t idx = 0; idx < indices_->size (); ++idx)
00200 {
00201 if (!isFinite ((*input_)[(*indices_)[idx]]) ||
00202 this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists) == 0)
00203 {
00204 for (int d = 0; d < pfh_histogram_.size (); ++d)
00205 output.points[idx].histogram[d] = std::numeric_limits<float>::quiet_NaN ();
00206
00207 output.is_dense = false;
00208 continue;
00209 }
00210
00211
00212 computePointPFHSignature (*surface_, *normals_, nn_indices, nr_subdiv_, pfh_histogram_);
00213
00214
00215 for (int d = 0; d < pfh_histogram_.size (); ++d)
00216 output.points[idx].histogram[d] = pfh_histogram_[d];
00217 }
00218 }
00219 }
00220
00222 template <typename PointInT, typename PointNT> void
00223 pcl::PFHEstimation<PointInT, PointNT, Eigen::MatrixXf>::computeFeatureEigen (pcl::PointCloud<Eigen::MatrixXf> &output)
00224 {
00225
00226 output.channels["pfh"].name = "pfh";
00227 output.channels["pfh"].offset = 0;
00228 output.channels["pfh"].size = 4;
00229 output.channels["pfh"].count = nr_subdiv_ * nr_subdiv_ * nr_subdiv_;
00230 output.channels["pfh"].datatype = sensor_msgs::PointField::FLOAT32;
00231
00232
00233 feature_map_.clear ();
00234 std::queue<std::pair<int, int> > empty;
00235 std::swap (key_list_, empty);
00236 pfh_histogram_.setZero (nr_subdiv_ * nr_subdiv_ * nr_subdiv_);
00237
00238
00239 output.points.resize (indices_->size (), nr_subdiv_ * nr_subdiv_ * nr_subdiv_);
00240
00241 std::vector<int> nn_indices (k_);
00242 std::vector<float> nn_dists (k_);
00243
00244 output.is_dense = true;
00245
00246 if (input_->is_dense)
00247 {
00248
00249 for (size_t idx = 0; idx < indices_->size (); ++idx)
00250 {
00251 if (this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists) == 0)
00252 {
00253 output.points.row (idx).setConstant (std::numeric_limits<float>::quiet_NaN ());
00254 output.is_dense = false;
00255 continue;
00256 }
00257
00258
00259 computePointPFHSignature (*surface_, *normals_, nn_indices, nr_subdiv_, pfh_histogram_);
00260 output.points.row (idx) = pfh_histogram_;
00261 }
00262 }
00263 else
00264 {
00265
00266 for (size_t idx = 0; idx < indices_->size (); ++idx)
00267 {
00268 if (!isFinite ((*input_)[(*indices_)[idx]]) ||
00269 this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists) == 0)
00270 {
00271 output.points.row (idx).setConstant (std::numeric_limits<float>::quiet_NaN ());
00272 output.is_dense = false;
00273 continue;
00274 }
00275
00276
00277 computePointPFHSignature (*surface_, *normals_, nn_indices, nr_subdiv_, pfh_histogram_);
00278 output.points.row (idx) = pfh_histogram_;
00279 }
00280 }
00281 }
00282
00283 #define PCL_INSTANTIATE_PFHEstimation(T,NT,OutT) template class PCL_EXPORTS pcl::PFHEstimation<T,NT,OutT>;
00284
00285 #endif // PCL_FEATURES_IMPL_PFH_H_
00286