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 ()); }
void pcl::recognition::ORROctree::getFullLeavesPoints (PointCloudOut& out) const { out.resize(full_leaves_.size ()); size_t i = 0; // Now iterate over all full leaves and compute the normals and average points for ( vector<ORROctree::Node*>::const_iterator it = full_leaves_.begin() ; it != full_leaves_.end() ; ++it, ++i ) { out[i].x = (*it)->getData ()->getPoint ()[0]; out[i].y = (*it)->getData ()->getPoint ()[1]; out[i].z = (*it)->getData ()->getPoint ()[2]; } }
template <typename PointInT, typename PointOutT> void pcl::BilateralUpsampling<PointInT, PointOutT>::performProcessing (PointCloudOut &output) { output.resize (input_->size ()); float nan = std::numeric_limits<float>::quiet_NaN (); Eigen::MatrixXf val_exp_depth_matrix; Eigen::VectorXf val_exp_rgb_vector; computeDistances (val_exp_depth_matrix, val_exp_rgb_vector); for (int x = 0; x < static_cast<int> (input_->width); ++x) for (int y = 0; y < static_cast<int> (input_->height); ++y) { int start_window_x = std::max (x - window_size_, 0), start_window_y = std::max (y - window_size_, 0), end_window_x = std::min (x + window_size_, static_cast<int> (input_->width)), end_window_y = std::min (y + window_size_, static_cast<int> (input_->height)); float sum = 0.0f, norm_sum = 0.0f; for (int x_w = start_window_x; x_w < end_window_x; ++ x_w) for (int y_w = start_window_y; y_w < end_window_y; ++ y_w) { float dx = float (x - x_w), dy = float (y - y_w); float val_exp_depth = val_exp_depth_matrix(dx+window_size_, dy+window_size_); float d_color = static_cast<float> ( abs (input_->points[y_w * input_->width + x_w].r - input_->points[y * input_->width + x].r) + abs (input_->points[y_w * input_->width + x_w].g - input_->points[y * input_->width + x].g) + abs (input_->points[y_w * input_->width + x_w].b - input_->points[y * input_->width + x].b)); float val_exp_rgb = val_exp_rgb_vector(Eigen::VectorXf::Index(d_color)); if (pcl_isfinite (input_->points[y_w*input_->width + x_w].z)) { sum += val_exp_depth * val_exp_rgb * input_->points[y_w*input_->width + x_w].z; norm_sum += val_exp_depth * val_exp_rgb; } } output.points[y*input_->width + x].r = input_->points[y*input_->width + x].r; output.points[y*input_->width + x].g = input_->points[y*input_->width + x].g; output.points[y*input_->width + x].b = input_->points[y*input_->width + x].b; if (norm_sum != 0.0f) { float depth = sum / norm_sum; Eigen::Vector3f pc (static_cast<float> (x) * depth, static_cast<float> (y) * depth, depth); Eigen::Vector3f pw (unprojection_matrix_ * pc); output.points[y*input_->width + x].x = pw[0]; output.points[y*input_->width + x].y = pw[1]; output.points[y*input_->width + x].z = pw[2]; } else { output.points[y*input_->width + x].x = nan; output.points[y*input_->width + x].y = nan; output.points[y*input_->width + x].z = nan; } } output.header = input_->header; output.width = input_->width; output.height = input_->height; }