41 #ifndef PCL_FEATURES_IMPL_CPPF_H_
42 #define PCL_FEATURES_IMPL_CPPF_H_
44 #include <pcl/features/cppf.h>
45 #include <pcl/features/pfh.h>
48 template <
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT>
60 template <
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT>
void
64 output.points.clear ();
65 output.points.reserve (indices_->size () * input_->points.size ());
66 output.is_dense =
true;
68 for (
const auto& i: *indices_)
70 for (std::size_t j = 0 ; j < input_->points.size (); ++j)
78 normals_->points[i].getNormalVector4fMap (),
79 input_->points[i].getRGBVector4i (),
80 input_->points[j].getVector4fMap (),
81 normals_->points[j].getNormalVector4fMap (),
82 input_->points[j].getRGBVector4i (),
83 p.f1, p.f2, p.f3, p.f4, p.f5, p.f6, p.f7, p.f8, p.f9, p.f10))
86 Eigen::Vector3f model_reference_point = input_->points[i].getVector3fMap (),
87 model_reference_normal = normals_->points[i].getNormalVector3fMap (),
88 model_point = input_->points[j].getVector3fMap ();
89 Eigen::AngleAxisf rotation_mg (std::acos (model_reference_normal.dot (Eigen::Vector3f::UnitX ())),
90 model_reference_normal.cross (Eigen::Vector3f::UnitX ()).normalized ());
91 Eigen::Affine3f transform_mg = Eigen::Translation3f ( rotation_mg * ((-1) * model_reference_point)) * rotation_mg;
93 Eigen::Vector3f model_point_transformed = transform_mg * model_point;
94 float angle = std::atan2 ( -model_point_transformed(2), model_point_transformed(1));
95 if (std::sin (angle) * model_point_transformed(2) < 0.0f)
101 PCL_ERROR (
"[pcl::%s::computeFeature] Computing pair feature vector between points %lu and %lu went wrong.\n", getClassName ().c_str (), i, j);
102 p.f1 = p.f2 = p.f3 = p.f4 = p.f5 = p.f6 = p.f7 = p.f8 = p.f9 = p.f10 = p.alpha_m = std::numeric_limits<float>::quiet_NaN ();
103 output.is_dense =
false;
108 p.f1 = p.f2 = p.f3 = p.f4 = p.f5 = p.f6 = p.f7 = p.f8 = p.f9 = p.f10 = p.alpha_m = std::numeric_limits<float>::quiet_NaN ();
109 output.is_dense =
false;
112 output.points.push_back (p);
117 output.width =
static_cast<std::uint32_t> (output.points.size ());
120 #define PCL_INSTANTIATE_CPPFEstimation(T,NT,OutT) template class PCL_EXPORTS pcl::CPPFEstimation<T,NT,OutT>;
123 #endif // PCL_FEATURES_IMPL_CPPF_H_