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;
}
