40 #ifndef PCL_FEATURES_IMPL_PPF_H_
41 #define PCL_FEATURES_IMPL_PPF_H_
43 #include <pcl/features/ppf.h>
44 #include <pcl/features/pfh.h>
47 template <
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT>
59 template <
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT>
void
63 output.points.resize (indices_->size () * input_->points.size ());
65 output.width =
static_cast<std::uint32_t
> (output.points.size ());
66 output.is_dense =
true;
69 for (std::size_t index_i = 0; index_i < indices_->size (); ++index_i)
71 std::size_t i = (*indices_)[index_i];
72 for (std::size_t j = 0 ; j < input_->points.size (); ++j)
79 normals_->points[i].getNormalVector4fMap (),
80 input_->points[j].getVector4fMap (),
81 normals_->points[j].getNormalVector4fMap (),
82 p.f1, p.f2, p.f3, p.f4))
85 Eigen::Vector3f model_reference_point = input_->points[i].getVector3fMap (),
86 model_reference_normal = normals_->points[i].getNormalVector3fMap (),
87 model_point = input_->points[j].getVector3fMap ();
88 float rotation_angle = std::acos (model_reference_normal.dot (Eigen::Vector3f::UnitX ()));
89 bool parallel_to_x = (model_reference_normal.y() == 0.0f && model_reference_normal.z() == 0.0f);
90 Eigen::Vector3f rotation_axis = (parallel_to_x)?(Eigen::Vector3f::UnitY ()):(model_reference_normal.cross (Eigen::Vector3f::UnitX ()). normalized());
91 Eigen::AngleAxisf rotation_mg (rotation_angle, rotation_axis);
92 Eigen::Affine3f transform_mg (Eigen::Translation3f ( rotation_mg * ((-1) * model_reference_point)) * rotation_mg);
94 Eigen::Vector3f model_point_transformed = transform_mg * model_point;
95 float angle = std::atan2 ( -model_point_transformed(2), model_point_transformed(1));
96 if (std::sin (angle) * model_point_transformed(2) < 0.0f)
102 PCL_ERROR (
"[pcl::%s::computeFeature] Computing pair feature vector between points %u and %u went wrong.\n", getClassName ().c_str (), i, j);
103 p.f1 = p.f2 = p.f3 = p.f4 = p.alpha_m = std::numeric_limits<float>::quiet_NaN ();
104 output.is_dense =
false;
111 p.f1 = p.f2 = p.f3 = p.f4 = p.alpha_m = std::numeric_limits<float>::quiet_NaN ();
112 output.is_dense =
false;
115 output.points[index_i*input_->points.size () + j] = p;
120 #define PCL_INSTANTIATE_PPFEstimation(T,NT,OutT) template class PCL_EXPORTS pcl::PPFEstimation<T,NT,OutT>;
123 #endif // PCL_FEATURES_IMPL_PPF_H_