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 dx = float (x - x_w),
110 dy = float (y - y_w);
112 float val_exp_depth = val_exp_depth_matrix(dx+window_size_, dy+window_size_);
114 float d_color =
static_cast<float> (
115 abs (input_->points[y_w * input_->width + x_w].r - input_->points[y * input_->width + x].r) +
116 abs (input_->points[y_w * input_->width + x_w].g - input_->points[y * input_->width + x].g) +
117 abs (input_->points[y_w * input_->width + x_w].b - input_->points[y * input_->width + x].b));
119 float val_exp_rgb = val_exp_rgb_vector(d_color);
121 if (pcl_isfinite (input_->points[y_w*input_->width + x_w].z))
123 sum += val_exp_depth * val_exp_rgb * input_->points[y_w*input_->width + x_w].z;
124 norm_sum += val_exp_depth * val_exp_rgb;
128 output.
points[y*input_->width + x].r = input_->points[y*input_->width + x].r;
129 output.
points[y*input_->width + x].g = input_->points[y*input_->width + x].g;
130 output.
points[y*input_->width + x].b = input_->points[y*input_->width + x].b;
132 if (norm_sum != 0.0f)
134 float depth = sum / norm_sum;
135 Eigen::Vector3f pc (static_cast<float> (x) * depth, static_cast<float> (y) * depth, depth);
136 Eigen::Vector3f pw (unprojection_matrix_ * pc);
137 output.
points[y*input_->width + x].x = pw[0];
138 output.
points[y*input_->width + x].y = pw[1];
139 output.
points[y*input_->width + x].z = pw[2];
143 output.
points[y*input_->width + x].x = nan;
144 output.
points[y*input_->width + x].y = nan;
145 output.
points[y*input_->width + x].z = nan;
149 output.
header = input_->header;
150 output.
width = input_->width;
151 output.
height = input_->height;
155 template <
typename Po
intInT,
typename Po
intOutT>
void
158 val_exp_depth.resize (2*window_size_+1,2*window_size_+1);
159 val_exp_rgb.resize (3*255);
162 for (
int dx = -window_size_; dx < window_size_+1; ++dx)
165 for (
int dy = -window_size_; dy < window_size_+1; ++dy)
167 float val_exp = expf (- (dx*dx + dy*dy) / (2.0f * static_cast<float> (sigma_depth_ * sigma_depth_)));
168 val_exp_depth(i,j) = val_exp;
174 for (
int d_color = 0; d_color < 3*255; d_color++)
176 float val_exp = expf (- d_color * d_color / (2.0f * sigma_color_ * sigma_color_));
177 val_exp_rgb(d_color) = val_exp;
182 #define PCL_INSTANTIATE_BilateralUpsampling(T,OutT) template class PCL_EXPORTS pcl::BilateralUpsampling<T,OutT>;
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
uint32_t height
The point cloud height (if organized as an image-structure).
uint32_t width
The point cloud width (if organized as an image-structure).
void performProcessing(pcl::PointCloud< PointOutT > &output)
Abstract cloud processing method.
PointCloud represents the base class in PCL for storing collections of 3D points. ...
pcl::PCLHeader header
The point cloud header.
void resize(size_t n)
Resize the cloud.
void process(pcl::PointCloud< PointOutT > &output)
Method that does the actual processing on the input cloud.
void computeDistances(Eigen::MatrixXf &val_exp_depth, Eigen::VectorXf &val_exp_rgb)
Computes the distance for depth and RGB.