39 #ifndef PCL_SURFACE_IMPL_BILATERAL_UPSAMPLING_H_
40 #define PCL_SURFACE_IMPL_BILATERAL_UPSAMPLING_H_
42 #include <pcl/surface/bilateral_upsampling.h>
44 #include <pcl/console/print.h>
47 template <
typename Po
intInT,
typename Po
intOutT>
void
51 output.
header = input_->header;
60 if (input_->isOrganized () ==
false)
62 PCL_ERROR (
"Input cloud is not organized.\n");
67 unprojection_matrix_ = projection_matrix_.inverse ();
69 for (
int i = 0; i < 3; ++i)
71 for (
int j = 0; j < 3; ++j)
72 printf (
"%f ", unprojection_matrix_(i, j));
79 performProcessing (output);
85 template <
typename Po
intInT,
typename Po
intOutT>
void
88 output.
resize (input_->size ());
89 float nan = std::numeric_limits<float>::quiet_NaN ();
91 Eigen::MatrixXf val_exp_depth_matrix;
92 Eigen::VectorXf val_exp_rgb_vector;
93 computeDistances (val_exp_depth_matrix, val_exp_rgb_vector);
95 for (
int x = 0; x < static_cast<int> (input_->width); ++x)
96 for (
int y = 0; y < static_cast<int> (input_->height); ++y)
98 int start_window_x = std::max (x - window_size_, 0),
99 start_window_y = std::max (y - window_size_, 0),
100 end_window_x = std::min (x + window_size_,
static_cast<int> (input_->width)),
101 end_window_y = std::min (y + window_size_,
static_cast<int> (input_->height));
106 for (
int x_w = start_window_x; x_w < end_window_x; ++ x_w)
107 for (
int y_w = start_window_y; y_w < end_window_y; ++ y_w)
109 float val_exp_depth = val_exp_depth_matrix (
static_cast<Eigen::MatrixXf::Index
> (x - x_w + window_size_),
110 static_cast<Eigen::MatrixXf::Index
> (y - y_w + window_size_));
112 Eigen::VectorXf::Index d_color =
static_cast<Eigen::VectorXf::Index
> (
113 abs (input_->points[y_w * input_->width + x_w].r - input_->points[y * input_->width + x].r) +
114 abs (input_->points[y_w * input_->width + x_w].g - input_->points[y * input_->width + x].g) +
115 abs (input_->points[y_w * input_->width + x_w].b - input_->points[y * input_->width + x].b));
117 float val_exp_rgb = val_exp_rgb_vector (d_color);
119 if (pcl_isfinite (input_->points[y_w*input_->width + x_w].z))
121 sum += val_exp_depth * val_exp_rgb * input_->points[y_w*input_->width + x_w].z;
122 norm_sum += val_exp_depth * val_exp_rgb;
126 output.
points[y*input_->width + x].r = input_->points[y*input_->width + x].r;
127 output.
points[y*input_->width + x].g = input_->points[y*input_->width + x].g;
128 output.
points[y*input_->width + x].b = input_->points[y*input_->width + x].b;
130 if (norm_sum != 0.0f)
132 float depth = sum / norm_sum;
133 Eigen::Vector3f pc (
static_cast<float> (x) * depth,
static_cast<float> (y) * depth, depth);
134 Eigen::Vector3f pw (unprojection_matrix_ * pc);
135 output.
points[y*input_->width + x].x = pw[0];
136 output.
points[y*input_->width + x].y = pw[1];
137 output.
points[y*input_->width + x].z = pw[2];
141 output.
points[y*input_->width + x].x = nan;
142 output.
points[y*input_->width + x].y = nan;
143 output.
points[y*input_->width + x].z = nan;
147 output.
header = input_->header;
148 output.
width = input_->width;
149 output.
height = input_->height;
153 template <
typename Po
intInT,
typename Po
intOutT>
void
156 val_exp_depth.resize (2*window_size_+1,2*window_size_+1);
157 val_exp_rgb.resize (3*255);
160 for (
int dx = -window_size_; dx < window_size_+1; ++dx)
163 for (
int dy = -window_size_; dy < window_size_+1; ++dy)
165 float val_exp = expf (- (dx*dx + dy*dy) / (2.0f *
static_cast<float> (sigma_depth_ * sigma_depth_)));
166 val_exp_depth(i,j) = val_exp;
172 for (
int d_color = 0; d_color < 3*255; d_color++)
174 float val_exp = expf (- d_color * d_color / (2.0f * sigma_color_ * sigma_color_));
175 val_exp_rgb(d_color) = val_exp;
180 #define PCL_INSTANTIATE_BilateralUpsampling(T,OutT) template class PCL_EXPORTS pcl::BilateralUpsampling<T,OutT>;