template <typename PointInT, typename PointNT, typename PointOutT> void pcl16::PPFRGBRegionEstimation<PointInT, PointNT, PointOutT>::computeFeature (PointCloudOut &output) { PCL16_INFO ("before computing output size: %u\n", output.size ()); output.resize (indices_->size ()); for (int index_i = 0; index_i < static_cast<int> (indices_->size ()); ++index_i) { int i = (*indices_)[index_i]; std::vector<int> nn_indices; std::vector<float> nn_distances; tree_->radiusSearch (i, static_cast<float> (search_radius_), nn_indices, nn_distances); PointOutT average_feature_nn; average_feature_nn.alpha_m = 0; average_feature_nn.f1 = average_feature_nn.f2 = average_feature_nn.f3 = average_feature_nn.f4 = average_feature_nn.r_ratio = average_feature_nn.g_ratio = average_feature_nn.b_ratio = 0.0f; for (std::vector<int>::iterator nn_it = nn_indices.begin (); nn_it != nn_indices.end (); ++nn_it) { int j = *nn_it; if (i != j) { float f1, f2, f3, f4, r_ratio, g_ratio, b_ratio; if (pcl16::computeRGBPairFeatures (input_->points[i].getVector4fMap (), normals_->points[i].getNormalVector4fMap (), input_->points[i].getRGBVector4i (), input_->points[j].getVector4fMap (), normals_->points[j].getNormalVector4fMap (), input_->points[j].getRGBVector4i (), f1, f2, f3, f4, r_ratio, g_ratio, b_ratio)) { average_feature_nn.f1 += f1; average_feature_nn.f2 += f2; average_feature_nn.f3 += f3; average_feature_nn.f4 += f4; average_feature_nn.r_ratio += r_ratio; average_feature_nn.g_ratio += g_ratio; average_feature_nn.b_ratio += b_ratio; } else { PCL16_ERROR ("[pcl16::%s::computeFeature] Computing pair feature vector between points %zu and %zu went wrong.\n", getClassName ().c_str (), i, j); } } } float normalization_factor = static_cast<float> (nn_indices.size ()); average_feature_nn.f1 /= normalization_factor; average_feature_nn.f2 /= normalization_factor; average_feature_nn.f3 /= normalization_factor; average_feature_nn.f4 /= normalization_factor; average_feature_nn.r_ratio /= normalization_factor; average_feature_nn.g_ratio /= normalization_factor; average_feature_nn.b_ratio /= normalization_factor; output.points[index_i] = average_feature_nn; } PCL16_INFO ("Output size: %u\n", output.points.size ()); }
template <typename PointInT, typename PointOutT, typename IntensityT> void pcl::BriskKeypoint2D<PointInT, PointOutT, IntensityT>::detectKeypoints (PointCloudOut &output) { // image size const int width = int (input_->width); const int height = int (input_->height); // destination for intensity data; will be forwarded to BRISK std::vector<unsigned char> image_data (width*height); for (size_t i = 0; i < image_data.size (); ++i) image_data[i] = static_cast<unsigned char> (intensity_ ((*input_)[i])); pcl::keypoints::brisk::ScaleSpace brisk_scale_space (octaves_); brisk_scale_space.constructPyramid (image_data, width, height); // Check if the template types are the same. If true, avoid a copy. // The PointOutT MUST be registered using the POINT_CLOUD_REGISTER_POINT_STRUCT macro! if (isSamePointType<PointOutT, pcl::PointWithScale> ()) brisk_scale_space.getKeypoints (threshold_, output.points); else { pcl::PointCloud<pcl::PointWithScale> output_temp; brisk_scale_space.getKeypoints (threshold_, output_temp.points); pcl::copyPointCloud<pcl::PointWithScale, PointOutT> (output_temp, output); } // we do not change the denseness output.width = int (output.points.size ()); output.height = 1; output.is_dense = false; // set to false to be sure // 2nd pass to remove the invalid set of 3D keypoints if (remove_invalid_3D_keypoints_) { PointCloudOut output_clean; for (size_t i = 0; i < output.size (); ++i) { PointOutT pt; // Interpolate its position in 3D, as the "u" and "v" are subpixel accurate bilinearInterpolation (input_, output[i].x, output[i].y, pt); // Check if the point is finite if (pcl::isFinite (pt)) output_clean.push_back (output[i]); } output = output_clean; output.is_dense = true; // set to true as there's no keypoint at an invalid XYZ } }
template <typename PointInT, typename PointOutT, typename NormalT> void pcl::TrajkovicKeypoint3D<PointInT, PointOutT, NormalT>::detectKeypoints (PointCloudOut &output) { response_.reset (new pcl::PointCloud<float> (input_->width, input_->height)); const Normals &normals = *normals_; const PointCloudIn &input = *input_; pcl::PointCloud<float>& response = *response_; const int w = static_cast<int> (input_->width) - half_window_size_; const int h = static_cast<int> (input_->height) - half_window_size_; if (method_ == FOUR_CORNERS) { #ifdef _OPENMP #pragma omp parallel for num_threads (threads_) #endif for(int j = half_window_size_; j < h; ++j) { for(int i = half_window_size_; i < w; ++i) { if (!isFinite (input (i,j))) continue; const NormalT ¢er = normals (i,j); if (!isFinite (center)) continue; int count = 0; const NormalT &up = getNormalOrNull (i, j-half_window_size_, count); const NormalT &down = getNormalOrNull (i, j+half_window_size_, count); const NormalT &left = getNormalOrNull (i-half_window_size_, j, count); const NormalT &right = getNormalOrNull (i+half_window_size_, j, count); // Get rid of isolated points if (!count) continue; float sn1 = squaredNormalsDiff (up, center); float sn2 = squaredNormalsDiff (down, center); float r1 = sn1 + sn2; float r2 = squaredNormalsDiff (right, center) + squaredNormalsDiff (left, center); float d = std::min (r1, r2); if (d < first_threshold_) continue; sn1 = sqrt (sn1); sn2 = sqrt (sn2); float b1 = normalsDiff (right, up) * sn1; b1+= normalsDiff (left, down) * sn2; float b2 = normalsDiff (right, down) * sn2; b2+= normalsDiff (left, up) * sn1; float B = std::min (b1, b2); float A = r2 - r1 - 2*B; response (i,j) = ((B < 0) && ((B + A) > 0)) ? r1 - ((B*B)/A) : d; } } } else { #ifdef _OPENMP #pragma omp parallel for num_threads (threads_) #endif for(int j = half_window_size_; j < h; ++j) { for(int i = half_window_size_; i < w; ++i) { if (!isFinite (input (i,j))) continue; const NormalT ¢er = normals (i,j); if (!isFinite (center)) continue; int count = 0; const NormalT &up = getNormalOrNull (i, j-half_window_size_, count); const NormalT &down = getNormalOrNull (i, j+half_window_size_, count); const NormalT &left = getNormalOrNull (i-half_window_size_, j, count); const NormalT &right = getNormalOrNull (i+half_window_size_, j, count); const NormalT &upleft = getNormalOrNull (i-half_window_size_, j-half_window_size_, count); const NormalT &upright = getNormalOrNull (i+half_window_size_, j-half_window_size_, count); const NormalT &downleft = getNormalOrNull (i-half_window_size_, j+half_window_size_, count); const NormalT &downright = getNormalOrNull (i+half_window_size_, j+half_window_size_, count); // Get rid of isolated points if (!count) continue; std::vector<float> r (4,0); r[0] = squaredNormalsDiff (up, center); r[0]+= squaredNormalsDiff (down, center); r[1] = squaredNormalsDiff (upright, center); r[1]+= squaredNormalsDiff (downleft, center); r[2] = squaredNormalsDiff (right, center); r[2]+= squaredNormalsDiff (left, center); r[3] = squaredNormalsDiff (downright, center); r[3]+= squaredNormalsDiff (upleft, center); float d = *(std::min_element (r.begin (), r.end ())); if (d < first_threshold_) continue; std::vector<float> B (4,0); std::vector<float> A (4,0); std::vector<float> sumAB (4,0); B[0] = normalsDiff (upright, up) * normalsDiff (up, center); B[0]+= normalsDiff (downleft, down) * normalsDiff (down, center); B[1] = normalsDiff (right, upright) * normalsDiff (upright, center); B[1]+= normalsDiff (left, downleft) * normalsDiff (downleft, center); B[2] = normalsDiff (downright, right) * normalsDiff (downright, center); B[2]+= normalsDiff (upleft, left) * normalsDiff (upleft, center); B[3] = normalsDiff (down, downright) * normalsDiff (downright, center); B[3]+= normalsDiff (up, upleft) * normalsDiff (upleft, center); A[0] = r[1] - r[0] - B[0] - B[0]; A[1] = r[2] - r[1] - B[1] - B[1]; A[2] = r[3] - r[2] - B[2] - B[2]; A[3] = r[0] - r[3] - B[3] - B[3]; sumAB[0] = A[0] + B[0]; sumAB[1] = A[1] + B[1]; sumAB[2] = A[2] + B[2]; sumAB[3] = A[3] + B[3]; if ((*std::max_element (B.begin (), B.end ()) < 0) && (*std::min_element (sumAB.begin (), sumAB.end ()) > 0)) { std::vector<float> D (4,0); D[0] = B[0] * B[0] / A[0]; D[1] = B[1] * B[1] / A[1]; D[2] = B[2] * B[2] / A[2]; D[3] = B[3] * B[3] / A[3]; response (i,j) = *(std::min (D.begin (), D.end ())); } else response (i,j) = d; } } } // Non maximas suppression std::vector<int> indices = *indices_; std::sort (indices.begin (), indices.end (), boost::bind (&TrajkovicKeypoint3D::greaterCornernessAtIndices, this, _1, _2)); output.clear (); output.reserve (input_->size ()); std::vector<bool> occupency_map (indices.size (), false); const int width (input_->width); const int height (input_->height); const int occupency_map_size (indices.size ()); #ifdef _OPENMP #pragma omp parallel for shared (output) num_threads (threads_) #endif for (int i = 0; i < indices.size (); ++i) { int idx = indices[i]; if ((response_->points[idx] < second_threshold_) || occupency_map[idx]) continue; PointOutT p; p.getVector3fMap () = input_->points[idx].getVector3fMap (); p.intensity = response_->points [idx]; #ifdef _OPENMP #pragma omp critical #endif { output.push_back (p); keypoints_indices_->indices.push_back (idx); } const int x = idx % width; const int y = idx / width; const int u_end = std::min (width, x + half_window_size_); const int v_end = std::min (height, y + half_window_size_); for(int v = std::max (0, y - half_window_size_); v < v_end; ++v) for(int u = std::max (0, x - half_window_size_); u < u_end; ++u) occupency_map[v*width + u] = true; } output.height = 1; output.width = static_cast<uint32_t> (output.size()); // we don not change the denseness output.is_dense = true; }
template <typename PointInT, typename PointOutT> void pcl::MovingLeastSquares<PointInT, PointOutT>::process (PointCloudOut &output) { // Check if normals have to be computed/saved if (compute_normals_) { normals_.reset (new NormalCloud); // Copy the header normals_->header = input_->header; // Clear the fields in case the method exits before computation normals_->width = normals_->height = 0; normals_->points.clear (); } // Copy the header output.header = input_->header; output.width = output.height = 0; output.points.clear (); if (search_radius_ <= 0 || sqr_gauss_param_ <= 0) { PCL_ERROR ("[pcl::%s::reconstruct] Invalid search radius (%f) or Gaussian parameter (%f)!\n", getClassName ().c_str (), search_radius_, sqr_gauss_param_); return; } if (!initCompute ()) return; // Initialize the spatial locator if (!tree_) { KdTreePtr tree; if (input_->isOrganized ()) tree.reset (new pcl::search::OrganizedNeighbor<PointInT> ()); else tree.reset (new pcl::search::KdTree<PointInT> (false)); setSearchMethod (tree); } // Send the surface dataset to the spatial locator tree_->setInputCloud (input_, indices_); // Initialize random number generator if necessary switch (upsample_method_) { case (RANDOM_UNIFORM_DENSITY): { boost::mt19937 *rng = new boost::mt19937 (static_cast<unsigned int>(std::time(0))); float tmp = static_cast<float> (search_radius_ / 2.0f); boost::uniform_real<float> *uniform_distrib = new boost::uniform_real<float> (-tmp, tmp); rng_uniform_distribution_ = new boost::variate_generator<boost::mt19937, boost::uniform_real<float> > (*rng, *uniform_distrib); break; } case (VOXEL_GRID_DILATION): { mls_results_.resize (input_->size ()); break; } default: break; } // Perform the actual surface reconstruction performProcessing (output); if (compute_normals_) { normals_->height = 1; normals_->width = static_cast<uint32_t> (normals_->size ()); // TODO!!! MODIFY TO PER-CLOUD COPYING - much faster than per-point for (unsigned int i = 0; i < output.size (); ++i) { typedef typename pcl::traits::fieldList<PointOutT>::type FieldList; pcl::for_each_type<FieldList> (SetIfFieldExists<PointOutT, float> (output.points[i], "normal_x", normals_->points[i].normal_x)); pcl::for_each_type<FieldList> (SetIfFieldExists<PointOutT, float> (output.points[i], "normal_y", normals_->points[i].normal_y)); pcl::for_each_type<FieldList> (SetIfFieldExists<PointOutT, float> (output.points[i], "normal_z", normals_->points[i].normal_z)); pcl::for_each_type<FieldList> (SetIfFieldExists<PointOutT, float> (output.points[i], "curvature", normals_->points[i].curvature)); } } // Set proper widths and heights for the clouds output.height = 1; output.width = static_cast<uint32_t> (output.size ()); deinitCompute (); }
template <typename PointInT, typename PointOutT, typename IntensityT> void pcl::HarrisKeypoint2D<PointInT, PointOutT, IntensityT>::detectKeypoints (PointCloudOut &output) { derivatives_cols_.resize (input_->width, input_->height); derivatives_rows_.resize (input_->width, input_->height); //Compute cloud intensities first derivatives along columns and rows //!!! nsallem 20120220 : we don't test here for density so if one term in nan the result is nan int w = static_cast<int> (input_->width) - 1; int h = static_cast<int> (input_->height) - 1; // j = 0 --> j-1 out of range ; use 0 // i = 0 --> i-1 out of range ; use 0 derivatives_cols_(0,0) = (intensity_ ((*input_) (0,1)) - intensity_ ((*input_) (0,0))) * 0.5; derivatives_rows_(0,0) = (intensity_ ((*input_) (1,0)) - intensity_ ((*input_) (0,0))) * 0.5; // #ifdef _OPENMP // //#pragma omp parallel for shared (derivatives_cols_, input_) num_threads (threads_) // #pragma omp parallel for num_threads (threads_) // #endif for(int i = 1; i < w; ++i) { derivatives_cols_(i,0) = (intensity_ ((*input_) (i,1)) - intensity_ ((*input_) (i,0))) * 0.5; } derivatives_rows_(w,0) = (intensity_ ((*input_) (w,0)) - intensity_ ((*input_) (w-1,0))) * 0.5; derivatives_cols_(w,0) = (intensity_ ((*input_) (w,1)) - intensity_ ((*input_) (w,0))) * 0.5; // #ifdef _OPENMP // //#pragma omp parallel for shared (derivatives_cols_, derivatives_rows_, input_) num_threads (threads_) // #pragma omp parallel for num_threads (threads_) // #endif for(int j = 1; j < h; ++j) { // i = 0 --> i-1 out of range ; use 0 derivatives_rows_(0,j) = (intensity_ ((*input_) (1,j)) - intensity_ ((*input_) (0,j))) * 0.5; for(int i = 1; i < w; ++i) { // derivative with respect to rows derivatives_rows_(i,j) = (intensity_ ((*input_) (i+1,j)) - intensity_ ((*input_) (i-1,j))) * 0.5; // derivative with respect to cols derivatives_cols_(i,j) = (intensity_ ((*input_) (i,j+1)) - intensity_ ((*input_) (i,j-1))) * 0.5; } // i = w --> w+1 out of range ; use w derivatives_rows_(w,j) = (intensity_ ((*input_) (w,j)) - intensity_ ((*input_) (w-1,j))) * 0.5; } // j = h --> j+1 out of range use h derivatives_cols_(0,h) = (intensity_ ((*input_) (0,h)) - intensity_ ((*input_) (0,h-1))) * 0.5; derivatives_rows_(0,h) = (intensity_ ((*input_) (1,h)) - intensity_ ((*input_) (0,h))) * 0.5; // #ifdef _OPENMP // //#pragma omp parallel for shared (derivatives_cols_, input_) num_threads (threads_) // #pragma omp parallel for num_threads (threads_) // #endif for(int i = 1; i < w; ++i) { derivatives_cols_(i,h) = (intensity_ ((*input_) (i,h)) - intensity_ ((*input_) (i,h-1))) * 0.5; } derivatives_rows_(w,h) = (intensity_ ((*input_) (w,h)) - intensity_ ((*input_) (w-1,h))) * 0.5; derivatives_cols_(w,h) = (intensity_ ((*input_) (w,h)) - intensity_ ((*input_) (w,h-1))) * 0.5; float highest_response_; switch (method_) { case HARRIS: responseHarris(*response_, highest_response_); break; case NOBLE: responseNoble(*response_, highest_response_); break; case LOWE: responseLowe(*response_, highest_response_); break; case TOMASI: responseTomasi(*response_, highest_response_); break; } if (!nonmax_) output = *response_; else { threshold_*= highest_response_; std::sort (indices_->begin (), indices_->end (), boost::bind (&HarrisKeypoint2D::greaterIntensityAtIndices, this, _1, _2)); output.clear (); output.reserve (response_->size()); std::vector<bool> occupency_map (response_->size (), false); int width (response_->width); int height (response_->height); const int occupency_map_size (occupency_map.size ()); #ifdef _OPENMP #pragma omp parallel for shared (output, occupency_map) private (width, height) num_threads(threads_) #endif for (int idx = 0; idx < occupency_map_size; ++idx) { if (occupency_map[idx] || response_->points [indices_->at (idx)].intensity < threshold_ || !isFinite (response_->points[idx])) continue; #ifdef _OPENMP #pragma omp critical #endif output.push_back (response_->at (indices_->at (idx))); int u_end = std::min (width, indices_->at (idx) % width + min_distance_); int v_end = std::min (height, indices_->at (idx) / width + min_distance_); for(int u = std::max (0, indices_->at (idx) % width - min_distance_); u < u_end; ++u) for(int v = std::max (0, indices_->at (idx) / width - min_distance_); v < v_end; ++v) occupency_map[v*input_->width+u] = true; } // if (refine_) // refineCorners (output); output.height = 1; output.width = static_cast<uint32_t> (output.size()); } // we don not change the denseness output.is_dense = input_->is_dense; }
template <typename PointInT, typename PointOutT> void pcl::MovingLeastSquares<PointInT, PointOutT>::process (PointCloudOut &output) { // Reset or initialize the collection of indices corresponding_input_indices_.reset (new PointIndices); // Check if normals have to be computed/saved if (compute_normals_) { normals_.reset (new NormalCloud); // Copy the header normals_->header = input_->header; // Clear the fields in case the method exits before computation normals_->width = normals_->height = 0; normals_->points.clear (); } // Copy the header output.header = input_->header; output.width = output.height = 0; output.points.clear (); if (search_radius_ <= 0 || sqr_gauss_param_ <= 0) { PCL_ERROR ("[pcl::%s::process] Invalid search radius (%f) or Gaussian parameter (%f)!\n", getClassName ().c_str (), search_radius_, sqr_gauss_param_); return; } // Check if distinct_cloud_ was set if (upsample_method_ == DISTINCT_CLOUD && !distinct_cloud_) { PCL_ERROR ("[pcl::%s::process] Upsample method was set to DISTINCT_CLOUD, but no distinct cloud was specified.\n", getClassName ().c_str ()); return; } if (!initCompute ()) return; // Initialize the spatial locator if (!tree_) { KdTreePtr tree; if (input_->isOrganized ()) tree.reset (new pcl::search::OrganizedNeighbor<PointInT> ()); else tree.reset (new pcl::search::KdTree<PointInT> (false)); setSearchMethod (tree); } // Send the surface dataset to the spatial locator tree_->setInputCloud (input_); switch (upsample_method_) { // Initialize random number generator if necessary case (RANDOM_UNIFORM_DENSITY): { rng_alg_.seed (static_cast<unsigned> (std::time (0))); float tmp = static_cast<float> (search_radius_ / 2.0f); boost::uniform_real<float> uniform_distrib (-tmp, tmp); rng_uniform_distribution_.reset (new boost::variate_generator<boost::mt19937&, boost::uniform_real<float> > (rng_alg_, uniform_distrib)); break; } case (VOXEL_GRID_DILATION): case (DISTINCT_CLOUD): { if (!cache_mls_results_) PCL_WARN ("The cache mls results is forced when using upsampling method VOXEL_GRID_DILATION or DISTINCT_CLOUD.\n"); cache_mls_results_ = true; break; } default: break; } if (cache_mls_results_) { mls_results_.resize (input_->size ()); } else { mls_results_.resize (1); // Need to have a reference to a single dummy result. } // Perform the actual surface reconstruction performProcessing (output); if (compute_normals_) { normals_->height = 1; normals_->width = static_cast<uint32_t> (normals_->size ()); for (unsigned int i = 0; i < output.size (); ++i) { typedef typename pcl::traits::fieldList<PointOutT>::type FieldList; pcl::for_each_type<FieldList> (SetIfFieldExists<PointOutT, float> (output.points[i], "normal_x", normals_->points[i].normal_x)); pcl::for_each_type<FieldList> (SetIfFieldExists<PointOutT, float> (output.points[i], "normal_y", normals_->points[i].normal_y)); pcl::for_each_type<FieldList> (SetIfFieldExists<PointOutT, float> (output.points[i], "normal_z", normals_->points[i].normal_z)); pcl::for_each_type<FieldList> (SetIfFieldExists<PointOutT, float> (output.points[i], "curvature", normals_->points[i].curvature)); } } // Set proper widths and heights for the clouds output.height = 1; output.width = static_cast<uint32_t> (output.size ()); deinitCompute (); }