38 #ifndef PCL_FEATURES_IMPL_PPFRGB_H_ 39 #define PCL_FEATURES_IMPL_PPFRGB_H_ 41 #include <pcl/features/ppfrgb.h> 42 #include <pcl/features/pfhrgb.h> 45 template <
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT>
57 template <
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT>
void 61 output.points.resize (indices_->size () * input_->points.size ());
63 output.width = static_cast<uint32_t> (output.points.size ());
66 for (
size_t index_i = 0; index_i < indices_->size (); ++index_i)
68 size_t i = (*indices_)[index_i];
69 for (
size_t j = 0 ; j < input_->points.size (); ++j)
75 (input_->points[i].getVector4fMap (), normals_->points[i].getNormalVector4fMap (), input_->points[i].getRGBVector4i (),
76 input_->points[j].getVector4fMap (), normals_->points[j].getNormalVector4fMap (), input_->points[j].getRGBVector4i (),
77 p.f1, p.f2, p.f3, p.f4, p.r_ratio, p.g_ratio, p.b_ratio))
80 Eigen::Vector3f model_reference_point = input_->points[i].getVector3fMap (),
81 model_reference_normal = normals_->points[i].getNormalVector3fMap (),
82 model_point = input_->points[j].getVector3fMap ();
83 Eigen::AngleAxisf rotation_mg (acosf (model_reference_normal.dot (Eigen::Vector3f::UnitX ())),
84 model_reference_normal.cross (Eigen::Vector3f::UnitX ()).normalized ());
85 Eigen::Affine3f transform_mg = Eigen::Translation3f ( rotation_mg * ((-1) * model_reference_point)) * rotation_mg;
87 Eigen::Vector3f model_point_transformed = transform_mg * model_point;
88 float angle = atan2f ( -model_point_transformed(2), model_point_transformed(1));
89 if (sin (angle) * model_point_transformed(2) < 0.0f)
95 PCL_ERROR (
"[pcl::%s::computeFeature] Computing pair feature vector between points %lu and %lu went wrong.\n", getClassName ().c_str (), i, j);
96 p.f1 = p.f2 = p.f3 = p.f4 = p.alpha_m = p.r_ratio = p.g_ratio = p.b_ratio = 0.f;
102 p.f1 = p.f2 = p.f3 = p.f4 = p.alpha_m = p.r_ratio = p.g_ratio = p.b_ratio = 0.f;
104 output.points[index_i*input_->points.size () + j] = p;
113 template <
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT>
121 template <
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT>
void 124 PCL_INFO (
"before computing output size: %u\n", output.size ());
125 output.resize (indices_->size ());
126 for (
int index_i = 0; index_i < static_cast<int> (indices_->size ()); ++index_i)
128 int i = (*indices_)[index_i];
129 std::vector<int> nn_indices;
130 std::vector<float> nn_distances;
131 tree_->radiusSearch (i, static_cast<float> (search_radius_), nn_indices, nn_distances);
133 PointOutT average_feature_nn;
134 average_feature_nn.alpha_m = 0;
135 average_feature_nn.f1 = average_feature_nn.f2 = average_feature_nn.f3 = average_feature_nn.f4 =
136 average_feature_nn.r_ratio = average_feature_nn.g_ratio = average_feature_nn.b_ratio = 0.0f;
138 for (std::vector<int>::iterator nn_it = nn_indices.begin (); nn_it != nn_indices.end (); ++nn_it)
143 float f1, f2, f3, f4, r_ratio, g_ratio, b_ratio;
145 (input_->points[i].getVector4fMap (), normals_->points[i].getNormalVector4fMap (), input_->points[i].getRGBVector4i (),
146 input_->points[j].getVector4fMap (), normals_->points[j].getNormalVector4fMap (), input_->points[j].getRGBVector4i (),
147 f1, f2, f3, f4, r_ratio, g_ratio, b_ratio))
149 average_feature_nn.f1 += f1;
150 average_feature_nn.f2 += f2;
151 average_feature_nn.f3 += f3;
152 average_feature_nn.f4 += f4;
153 average_feature_nn.r_ratio += r_ratio;
154 average_feature_nn.g_ratio += g_ratio;
155 average_feature_nn.b_ratio += b_ratio;
159 PCL_ERROR (
"[pcl::%s::computeFeature] Computing pair feature vector between points %lu and %lu went wrong.\n", getClassName ().c_str (), i, j);
164 float normalization_factor = static_cast<float> (nn_indices.size ());
165 average_feature_nn.f1 /= normalization_factor;
166 average_feature_nn.f2 /= normalization_factor;
167 average_feature_nn.f3 /= normalization_factor;
168 average_feature_nn.f4 /= normalization_factor;
169 average_feature_nn.r_ratio /= normalization_factor;
170 average_feature_nn.g_ratio /= normalization_factor;
171 average_feature_nn.b_ratio /= normalization_factor;
172 output.points[index_i] = average_feature_nn;
174 PCL_INFO (
"Output size: %u\n", output.points.size ());
178 #define PCL_INSTANTIATE_PPFRGBEstimation(T,NT,OutT) template class PCL_EXPORTS pcl::PPFRGBEstimation<T,NT,OutT>; 179 #define PCL_INSTANTIATE_PPFRGBRegionEstimation(T,NT,OutT) template class PCL_EXPORTS pcl::PPFRGBRegionEstimation<T,NT,OutT>; 181 #endif // PCL_FEATURES_IMPL_PPFRGB_H_ search::KdTree is a wrapper class which inherits the pcl::KdTree class for performing search function...
PPFRGBEstimation()
Empty Constructor.
std::string feature_name_
The feature name.
PCL_EXPORTS bool computeRGBPairFeatures(const Eigen::Vector4f &p1, const Eigen::Vector4f &n1, const Eigen::Vector4i &colors1, const Eigen::Vector4f &p2, const Eigen::Vector4f &n2, const Eigen::Vector4i &colors2, float &f1, float &f2, float &f3, float &f4, float &f5, float &f6, float &f7)
Feature represents the base feature class.