Point Cloud Library (PCL)
1.3.1
|
00001 #ifndef PCL_FEATURES_IMPL_PPFRGB_H_ 00002 #define PCL_FEATURES_IMPL_PPFRGB_H_ 00003 00004 #include "pcl/features/ppfrgb.h" 00005 #include "pcl/features/pfhrgb.h" 00006 00008 template <typename PointInT, typename PointNT, typename PointOutT> 00009 pcl::PPFRGBEstimation<PointInT, PointNT, PointOutT>::PPFRGBEstimation () 00010 : FeatureFromNormals <PointInT, PointNT, PointOutT> () 00011 { 00012 feature_name_ = "PPFRGBEstimation"; 00013 // Slight hack in order to pass the check for the presence of a search method in Feature::initCompute () 00014 Feature<PointInT, PointOutT>::tree_.reset (new pcl::search::KdTree <PointInT> ()); 00015 Feature<PointInT, PointOutT>::search_radius_ = 1.0f; 00016 }; 00017 00018 00020 template <typename PointInT, typename PointNT, typename PointOutT> void 00021 pcl::PPFRGBEstimation<PointInT, PointNT, PointOutT>::computeFeature (PointCloudOut &output) 00022 { 00023 // Initialize output container - overwrite the sizes done by Feature::initCompute () 00024 output.points.resize (indices_->size () * input_->points.size ()); 00025 output.height = 1; 00026 output.width = output.points.size (); 00027 00028 // Compute point pair features for every pair of points in the cloud 00029 for (size_t index_i = 0; index_i < indices_->size (); ++index_i) 00030 { 00031 size_t i = (*indices_)[index_i]; 00032 for (size_t j = 0 ; j < input_->points.size (); ++j) 00033 { 00034 PointOutT p; 00035 if (i != j) 00036 { 00037 if (pcl::computeRGBPairFeatures 00038 (input_->points[i].getVector4fMap (), normals_->points[i].getNormalVector4fMap (), input_->points[i].getRGBVector4i (), 00039 input_->points[j].getVector4fMap (), normals_->points[j].getNormalVector4fMap (), input_->points[j].getRGBVector4i (), 00040 p.f1, p.f2, p.f3, p.f4, p.r_ratio, p.g_ratio, p.b_ratio)) 00041 { 00042 // Calculate alpha_m angle 00043 Eigen::Vector3f model_reference_point = input_->points[i].getVector3fMap (), 00044 model_reference_normal = normals_->points[i].getNormalVector3fMap (), 00045 model_point = input_->points[j].getVector3fMap (); 00046 Eigen::AngleAxisf rotation_mg (acos (model_reference_normal.dot (Eigen::Vector3f::UnitX ())), 00047 model_reference_normal.cross (Eigen::Vector3f::UnitX ()).normalized ()); 00048 Eigen::Affine3f transform_mg = Eigen::Translation3f ( rotation_mg * ((-1) * model_reference_point)) * rotation_mg; 00049 00050 Eigen::Vector3f model_point_transformed = transform_mg * model_point; 00051 float angle = atan2f ( -model_point_transformed(2), model_point_transformed(1)); 00052 if (sin (angle) * model_point_transformed(2) < 0.0f) 00053 angle *= (-1); 00054 p.alpha_m = -angle; 00055 } 00056 else 00057 { 00058 PCL_ERROR ("[pcl::%s::computeFeature] Computing pair feature vector between points %zu and %zu went wrong.\n", getClassName ().c_str (), i, j); 00059 p.f1 = p.f2 = p.f3 = p.f4 = p.alpha_m = p.r_ratio = p.g_ratio = p.b_ratio = 0.f; 00060 } 00061 } 00062 // Do not calculate the feature for identity pairs (i, i) as they are not used 00063 // in the following computations 00064 else 00065 p.f1 = p.f2 = p.f3 = p.f4 = p.alpha_m = p.r_ratio = p.g_ratio = p.b_ratio = 0.f; 00066 00067 output.points[index_i*input_->points.size () + j] = p; 00068 } 00069 } 00070 } 00071 00072 00073 00076 template <typename PointInT, typename PointNT, typename PointOutT> 00077 pcl::PPFRGBRegionEstimation<PointInT, PointNT, PointOutT>::PPFRGBRegionEstimation () 00078 : FeatureFromNormals <PointInT, PointNT, PointOutT> () 00079 { 00080 feature_name_ = "PPFRGBEstimation"; 00081 }; 00082 00084 template <typename PointInT, typename PointNT, typename PointOutT> void 00085 pcl::PPFRGBRegionEstimation<PointInT, PointNT, PointOutT>::computeFeature (PointCloudOut &output) 00086 { 00087 PCL_INFO ("before computing output size: %u\n", output.size ()); 00088 output.resize (indices_->size ()); 00089 for (size_t index_i = 0; index_i < indices_->size (); ++index_i) 00090 { 00091 size_t i = (*indices_)[index_i]; 00092 std::vector<int> nn_indices; 00093 std::vector<float> nn_distances; 00094 tree_->radiusSearch (i, search_radius_, nn_indices, nn_distances); 00095 00096 PointOutT average_feature_nn; 00097 average_feature_nn.alpha_m = 0; 00098 average_feature_nn.f1 = average_feature_nn.f2 = average_feature_nn.f3 = average_feature_nn.f4 = 00099 average_feature_nn.r_ratio = average_feature_nn.g_ratio = average_feature_nn.b_ratio = 0.0f; 00100 00101 for (std::vector<int>::iterator nn_it = nn_indices.begin (); nn_it != nn_indices.end (); ++nn_it) 00102 { 00103 size_t j = *nn_it; 00104 if (i != j) 00105 { 00106 float f1, f2, f3, f4, r_ratio, g_ratio, b_ratio; 00107 if (pcl::computeRGBPairFeatures 00108 (input_->points[i].getVector4fMap (), normals_->points[i].getNormalVector4fMap (), input_->points[i].getRGBVector4i (), 00109 input_->points[j].getVector4fMap (), normals_->points[j].getNormalVector4fMap (), input_->points[j].getRGBVector4i (), 00110 f1, f2, f3, f4, r_ratio, g_ratio, b_ratio)) 00111 { 00112 average_feature_nn.f1 += f1; 00113 average_feature_nn.f2 += f2; 00114 average_feature_nn.f3 += f3; 00115 average_feature_nn.f4 += f4; 00116 average_feature_nn.r_ratio += r_ratio; 00117 average_feature_nn.g_ratio += g_ratio; 00118 average_feature_nn.b_ratio += b_ratio; 00119 } 00120 else 00121 { 00122 PCL_ERROR ("[pcl::%s::computeFeature] Computing pair feature vector between points %zu and %zu went wrong.\n", getClassName ().c_str (), i, j); 00123 } 00124 } 00125 } 00126 00127 float normalization_factor = nn_indices.size (); 00128 average_feature_nn.f1 /= normalization_factor; 00129 average_feature_nn.f2 /= normalization_factor; 00130 average_feature_nn.f3 /= normalization_factor; 00131 average_feature_nn.f4 /= normalization_factor; 00132 average_feature_nn.r_ratio /= normalization_factor; 00133 average_feature_nn.g_ratio /= normalization_factor; 00134 average_feature_nn.b_ratio /= normalization_factor; 00135 output.points[index_i] = average_feature_nn; 00136 } 00137 PCL_INFO ("Output size: %u\n", output.points.size ()); 00138 } 00139 00140 00141 #define PCL_INSTANTIATE_PPFRGBEstimation(T,NT,OutT) template class PCL_EXPORTS pcl::PPFRGBEstimation<T,NT,OutT>; 00142 #define PCL_INSTANTIATE_PPFRGBRegionEstimation(T,NT,OutT) template class PCL_EXPORTS pcl::PPFRGBRegionEstimation<T,NT,OutT>; 00143 00144 00145 00146 #endif // PCL_FEATURES_IMPL_PPFRGB_H_