Point Cloud Library (PCL) 1.13.0
Loading...
Searching...
No Matches
bilateral_upsampling.hpp
1/*
2 * Software License Agreement (BSD License)
3 *
4 * Point Cloud Library (PCL) - www.pointclouds.org
5 * Copyright (c) 2009-2012, Willow Garage, Inc.
6 *
7 * All rights reserved.
8 *
9 * Redistribution and use in source and binary forms, with or without
10 * modification, are permitted provided that the following conditions
11 * are met:
12 *
13 * * Redistributions of source code must retain the above copyright
14 * notice, this list of conditions and the following disclaimer.
15 * * Redistributions in binary form must reproduce the above
16 * copyright notice, this list of conditions and the following
17 * disclaimer in the documentation and/or other materials provided
18 * with the distribution.
19 * * Neither the name of Willow Garage, Inc. nor the names of its
20 * contributors may be used to endorse or promote products derived
21 * from this software without specific prior written permission.
22 *
23 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34 * POSSIBILITY OF SUCH DAMAGE.
35 *
36 */
37
38
39#ifndef PCL_SURFACE_IMPL_BILATERAL_UPSAMPLING_H_
40#define PCL_SURFACE_IMPL_BILATERAL_UPSAMPLING_H_
41
42#include <pcl/surface/bilateral_upsampling.h>
43#include <algorithm>
44#include <pcl/console/print.h>
45
46#include <Eigen/LU> // for inverse
47
48//////////////////////////////////////////////////////////////////////////////////////////////
49template <typename PointInT, typename PointOutT> void
51{
52 // Copy the header
53 output.header = input_->header;
54
55 if (!initCompute ())
56 {
57 output.width = output.height = 0;
58 output.clear ();
59 return;
60 }
61
62 if (input_->isOrganized () == false)
63 {
64 PCL_ERROR ("Input cloud is not organized.\n");
65 return;
66 }
67
68 // Invert projection matrix
69 unprojection_matrix_ = projection_matrix_.inverse ();
70
71 for (int i = 0; i < 3; ++i)
72 {
73 for (int j = 0; j < 3; ++j)
74 printf ("%f ", unprojection_matrix_(i, j));
75
76 printf ("\n");
77 }
78
79
80 // Perform the actual surface reconstruction
81 performProcessing (output);
82
83 deinitCompute ();
84}
85
86//////////////////////////////////////////////////////////////////////////////////////////////
87template <typename PointInT, typename PointOutT> void
89{
90 output.resize (input_->size ());
91 float nan = std::numeric_limits<float>::quiet_NaN ();
92
93 Eigen::MatrixXf val_exp_depth_matrix;
94 Eigen::VectorXf val_exp_rgb_vector;
95 computeDistances (val_exp_depth_matrix, val_exp_rgb_vector);
96
97 for (int x = 0; x < static_cast<int> (input_->width); ++x)
98 for (int y = 0; y < static_cast<int> (input_->height); ++y)
99 {
100 int start_window_x = std::max (x - window_size_, 0),
101 start_window_y = std::max (y - window_size_, 0),
102 end_window_x = std::min (x + window_size_, static_cast<int> (input_->width)),
103 end_window_y = std::min (y + window_size_, static_cast<int> (input_->height));
104
105 float sum = 0.0f,
106 norm_sum = 0.0f;
107
108 for (int x_w = start_window_x; x_w < end_window_x; ++ x_w)
109 for (int y_w = start_window_y; y_w < end_window_y; ++ y_w)
110 {
111 float val_exp_depth = val_exp_depth_matrix (static_cast<Eigen::MatrixXf::Index> (x - x_w + window_size_),
112 static_cast<Eigen::MatrixXf::Index> (y - y_w + window_size_));
113
114 auto d_color = static_cast<Eigen::VectorXf::Index> (
115 std::abs ((*input_)[y_w * input_->width + x_w].r - (*input_)[y * input_->width + x].r) +
116 std::abs ((*input_)[y_w * input_->width + x_w].g - (*input_)[y * input_->width + x].g) +
117 std::abs ((*input_)[y_w * input_->width + x_w].b - (*input_)[y * input_->width + x].b));
118
119 float val_exp_rgb = val_exp_rgb_vector (d_color);
120
121 if (std::isfinite ((*input_)[y_w*input_->width + x_w].z))
122 {
123 sum += val_exp_depth * val_exp_rgb * (*input_)[y_w*input_->width + x_w].z;
124 norm_sum += val_exp_depth * val_exp_rgb;
125 }
126 }
127
128 output[y*input_->width + x].r = (*input_)[y*input_->width + x].r;
129 output[y*input_->width + x].g = (*input_)[y*input_->width + x].g;
130 output[y*input_->width + x].b = (*input_)[y*input_->width + x].b;
131
132 if (norm_sum != 0.0f)
133 {
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[y*input_->width + x].x = pw[0];
138 output[y*input_->width + x].y = pw[1];
139 output[y*input_->width + x].z = pw[2];
140 }
141 else
142 {
143 output[y*input_->width + x].x = nan;
144 output[y*input_->width + x].y = nan;
145 output[y*input_->width + x].z = nan;
146 }
147 }
148
149 output.header = input_->header;
150 output.width = input_->width;
151 output.height = input_->height;
152}
153
154
155template <typename PointInT, typename PointOutT> void
156pcl::BilateralUpsampling<PointInT, PointOutT>::computeDistances (Eigen::MatrixXf &val_exp_depth, Eigen::VectorXf &val_exp_rgb)
157{
158 val_exp_depth.resize (2*window_size_+1,2*window_size_+1);
159 val_exp_rgb.resize (3*255+1);
160
161 int j = 0;
162 for (int dx = -window_size_; dx < window_size_+1; ++dx)
163 {
164 int i = 0;
165 for (int dy = -window_size_; dy < window_size_+1; ++dy)
166 {
167 float val_exp = std::exp (- (dx*dx + dy*dy) / (2.0f * static_cast<float> (sigma_depth_ * sigma_depth_)));
168 val_exp_depth(i,j) = val_exp;
169 i++;
170 }
171 j++;
172 }
173
174 for (int d_color = 0; d_color < 3*255+1; d_color++)
175 {
176 float val_exp = std::exp (- d_color * d_color / (2.0f * sigma_color_ * sigma_color_));
177 val_exp_rgb(d_color) = val_exp;
178 }
179}
180
181
182#define PCL_INSTANTIATE_BilateralUpsampling(T,OutT) template class PCL_EXPORTS pcl::BilateralUpsampling<T,OutT>;
183
184
185#endif /* PCL_SURFACE_IMPL_BILATERAL_UPSAMPLING_H_ */
void computeDistances(Eigen::MatrixXf &val_exp_depth, Eigen::VectorXf &val_exp_rgb)
Computes the distance for depth and RGB.
void process(pcl::PointCloud< PointOutT > &output) override
Method that does the actual processing on the input cloud.
void performProcessing(pcl::PointCloud< PointOutT > &output) override
Abstract cloud processing method.
PointCloud represents the base class in PCL for storing collections of 3D points.
void resize(std::size_t count)
Resizes the container to contain count elements.
std::uint32_t width
The point cloud width (if organized as an image-structure).
pcl::PCLHeader header
The point cloud header.
std::uint32_t height
The point cloud height (if organized as an image-structure).
void clear()
Removes all points in a cloud and sets the width and height to 0.