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 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;
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 ());
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;
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...
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
PPFRGBEstimation()
Empty Constructor.
std::string feature_name_
The feature name.
IndicesPtr indices_
A pointer to the vector of point indices to use.
KdTreePtr tree_
A pointer to the spatial search object.
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)
uint32_t height
The point cloud height (if organized as an image-structure).
const std::string & getClassName() const
Get a string representation of the name of this class.
uint32_t width
The point cloud width (if organized as an image-structure).
PointCloud represents the base class in PCL for storing collections of 3D points. ...
PointCloudNConstPtr normals_
A pointer to the input dataset that contains the point normals of the XYZ dataset.
PointCloudConstPtr input_
The input point cloud dataset.
void resize(size_t n)
Resize the cloud.
Feature represents the base feature class.
double search_radius_
The nearest neighbors search radius for each point.