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
00041 #ifndef PCL_FEATURES_IMPL_VFH_H_
00042 #define PCL_FEATURES_IMPL_VFH_H_
00043
00044 #include <pcl/features/vfh.h>
00045 #include <pcl/features/pfh.h>
00046 #include <pcl/common/common.h>
00047
00049 template<typename PointInT, typename PointNT, typename PointOutT> bool
00050 pcl::VFHEstimation<PointInT, PointNT, PointOutT>::initCompute ()
00051 {
00052 if (input_->points.size () < 2 || (surface_ && surface_->points.size () < 2))
00053 {
00054 PCL_ERROR ("[pcl::VFHEstimation::initCompute] Input dataset must have at least 2 points!\n");
00055 return (false);
00056 }
00057 if (search_radius_ == 0 && k_ == 0)
00058 k_ = 1;
00059 return (Feature<PointInT, PointOutT>::initCompute ());
00060 }
00061
00063 template<typename PointInT, typename PointNT, typename PointOutT> void
00064 pcl::VFHEstimation<PointInT, PointNT, PointOutT>::compute (PointCloudOut &output)
00065 {
00066 if (!initCompute ())
00067 {
00068 output.width = output.height = 0;
00069 output.points.clear ();
00070 return;
00071 }
00072
00073 output.header = input_->header;
00074
00075
00076
00077
00078
00079 output.width = output.height = 1;
00080 output.is_dense = input_->is_dense;
00081 output.points.resize (1);
00082
00083
00084 computeFeature (output);
00085
00086 Feature<PointInT, PointOutT>::deinitCompute ();
00087 }
00088
00090 template<typename PointInT, typename PointNT, typename PointOutT> void
00091 pcl::VFHEstimation<PointInT, PointNT, PointOutT>::computePointSPFHSignature (const Eigen::Vector4f ¢roid_p,
00092 const Eigen::Vector4f ¢roid_n,
00093 const pcl::PointCloud<PointInT> &cloud,
00094 const pcl::PointCloud<PointNT> &normals,
00095 const std::vector<int> &indices)
00096 {
00097 Eigen::Vector4f pfh_tuple;
00098
00099 hist_f1_.setZero (nr_bins_f1_);
00100 hist_f2_.setZero (nr_bins_f2_);
00101 hist_f3_.setZero (nr_bins_f3_);
00102 hist_f4_.setZero (nr_bins_f4_);
00103
00104
00105
00106
00107
00108
00109
00110
00111
00112
00113 double distance_normalization_factor = 1.0;
00114 if (normalize_distances_)
00115 {
00116 Eigen::Vector4f max_pt;
00117 pcl::getMaxDistance (cloud, indices, centroid_p, max_pt);
00118 max_pt[3] = 0;
00119 distance_normalization_factor = (centroid_p - max_pt).norm ();
00120 }
00121
00122
00123 float hist_incr;
00124 if (normalize_bins_)
00125 hist_incr = 100.0f / static_cast<float> (indices.size () - 1);
00126 else
00127 hist_incr = 1.0f;
00128
00129 float hist_incr_size_component;
00130 if (size_component_)
00131 hist_incr_size_component = hist_incr;
00132 else
00133 hist_incr_size_component = 0.0;
00134
00135
00136 for (size_t idx = 0; idx < indices.size (); ++idx)
00137 {
00138
00139 if (!computePairFeatures (centroid_p, centroid_n, cloud.points[indices[idx]].getVector4fMap (),
00140 normals.points[indices[idx]].getNormalVector4fMap (), pfh_tuple[0], pfh_tuple[1],
00141 pfh_tuple[2], pfh_tuple[3]))
00142 continue;
00143
00144
00145 int h_index = static_cast<int> (floor (nr_bins_f1_ * ((pfh_tuple[0] + M_PI) * d_pi_)));
00146 if (h_index < 0)
00147 h_index = 0;
00148 if (h_index >= nr_bins_f1_)
00149 h_index = nr_bins_f1_ - 1;
00150 hist_f1_ (h_index) += hist_incr;
00151
00152 h_index = static_cast<int> (floor (nr_bins_f2_ * ((pfh_tuple[1] + 1.0) * 0.5)));
00153 if (h_index < 0)
00154 h_index = 0;
00155 if (h_index >= nr_bins_f2_)
00156 h_index = nr_bins_f2_ - 1;
00157 hist_f2_ (h_index) += hist_incr;
00158
00159 h_index = static_cast<int> (floor (nr_bins_f3_ * ((pfh_tuple[2] + 1.0) * 0.5)));
00160 if (h_index < 0)
00161 h_index = 0;
00162 if (h_index >= nr_bins_f3_)
00163 h_index = nr_bins_f3_ - 1;
00164 hist_f3_ (h_index) += hist_incr;
00165
00166 if (normalize_distances_)
00167 h_index = static_cast<int> (floor (nr_bins_f4_ * (pfh_tuple[3] / distance_normalization_factor)));
00168 else
00169 h_index = static_cast<int> (pcl_round (pfh_tuple[3] * 100));
00170
00171 if (h_index < 0)
00172 h_index = 0;
00173 if (h_index >= nr_bins_f4_)
00174 h_index = nr_bins_f4_ - 1;
00175
00176 hist_f4_ (h_index) += hist_incr_size_component;
00177 }
00178 }
00180 template <typename PointInT, typename PointNT, typename PointOutT> void
00181 pcl::VFHEstimation<PointInT, PointNT, PointOutT>::computeFeature (PointCloudOut &output)
00182 {
00183
00184 Eigen::Vector4f xyz_centroid;
00185
00186 if (use_given_centroid_)
00187 xyz_centroid = centroid_to_use_;
00188 else
00189 compute3DCentroid (*surface_, *indices_, xyz_centroid);
00190
00191
00192 Eigen::Vector4f normal_centroid = Eigen::Vector4f::Zero ();
00193 int cp = 0;
00194
00195
00196 if (use_given_normal_)
00197 normal_centroid = normal_to_use_;
00198 else
00199 {
00200 if (normals_->is_dense)
00201 {
00202 for (size_t i = 0; i < indices_->size (); ++i)
00203 {
00204 normal_centroid += normals_->points[(*indices_)[i]].getNormalVector4fMap ();
00205 cp++;
00206 }
00207 }
00208
00209 else
00210 {
00211 for (size_t i = 0; i < indices_->size (); ++i)
00212 {
00213 if (!pcl_isfinite (normals_->points[(*indices_)[i]].normal[0])
00214 ||
00215 !pcl_isfinite (normals_->points[(*indices_)[i]].normal[1])
00216 ||
00217 !pcl_isfinite (normals_->points[(*indices_)[i]].normal[2]))
00218 continue;
00219 normal_centroid += normals_->points[(*indices_)[i]].getNormalVector4fMap ();
00220 cp++;
00221 }
00222 }
00223 normal_centroid /= static_cast<float> (cp);
00224 }
00225
00226
00227 Eigen::Vector4f viewpoint (vpx_, vpy_, vpz_, 0);
00228 Eigen::Vector4f d_vp_p = viewpoint - xyz_centroid;
00229 d_vp_p.normalize ();
00230
00231
00232 computePointSPFHSignature (xyz_centroid, normal_centroid, *surface_, *normals_, *indices_);
00233
00234
00235 output.points.resize (1);
00236 output.width = 1;
00237 output.height = 1;
00238
00239
00240 for (int d = 0; d < hist_f1_.size (); ++d)
00241 output.points[0].histogram[d + 0] = hist_f1_[d];
00242
00243 size_t data_size = hist_f1_.size ();
00244 for (int d = 0; d < hist_f2_.size (); ++d)
00245 output.points[0].histogram[d + data_size] = hist_f2_[d];
00246
00247 data_size += hist_f2_.size ();
00248 for (int d = 0; d < hist_f3_.size (); ++d)
00249 output.points[0].histogram[d + data_size] = hist_f3_[d];
00250
00251 data_size += hist_f3_.size ();
00252 for (int d = 0; d < hist_f4_.size (); ++d)
00253 output.points[0].histogram[d + data_size] = hist_f4_[d];
00254
00255
00256 hist_vp_.setZero (nr_bins_vp_);
00257
00258 double hist_incr;
00259 if (normalize_bins_)
00260 hist_incr = 100.0 / static_cast<double> (indices_->size ());
00261 else
00262 hist_incr = 1.0;
00263
00264 for (size_t i = 0; i < indices_->size (); ++i)
00265 {
00266 Eigen::Vector4f normal (normals_->points[(*indices_)[i]].normal[0],
00267 normals_->points[(*indices_)[i]].normal[1],
00268 normals_->points[(*indices_)[i]].normal[2], 0);
00269
00270 double alpha = (normal.dot (d_vp_p) + 1.0) * 0.5;
00271 int fi = static_cast<int> (floor (alpha * static_cast<double> (hist_vp_.size ())));
00272 if (fi < 0)
00273 fi = 0;
00274 if (fi > (static_cast<int> (hist_vp_.size ()) - 1))
00275 fi = static_cast<int> (hist_vp_.size ()) - 1;
00276
00277 hist_vp_ [fi] += static_cast<float> (hist_incr);
00278 }
00279 data_size += hist_f4_.size ();
00280
00281 for (int d = 0; d < hist_vp_.size (); ++d)
00282 output.points[0].histogram[d + data_size] = hist_vp_[d];
00283 }
00284
00285 #define PCL_INSTANTIATE_VFHEstimation(T,NT,OutT) template class PCL_EXPORTS pcl::VFHEstimation<T,NT,OutT>;
00286
00287 #endif // PCL_FEATURES_IMPL_VFH_H_