112 bool smooth,
int wsize)
115 cx =
static_cast<float> (cx_) / 2.f - 0.5f;
116 cy =
static_cast<float> (cy_) / 2.f - 0.5f;
122 float max_u, max_v, min_u, min_v;
123 max_u = max_v = std::numeric_limits<float>::max () * -1;
124 min_u = min_v = std::numeric_limits<float>::max ();
126 for (
const auto& point: *scene)
128 float b_x = point.x / point.z;
134 float b_y = point.y / point.z;
141 float maxC = std::max (std::max (std::abs (max_u), std::abs (max_v)), std::max (std::abs (min_u), std::abs (min_v)));
145 depth_ =
new float[cx_ * cy_];
146 std::fill_n(depth_, cx * cy, std::numeric_limits<float>::quiet_NaN());
148 for (
const auto& point: *scene)
150 const float& x = point.x;
151 const float& y = point.y;
152 const float& z = point.z;
153 const int u =
static_cast<int> (f_ * x / z + cx);
154 const int v =
static_cast<int> (f_ * y / z + cy);
156 if (u >= cx_ || v >= cy_ || u < 0 || v < 0)
159 if ((z < depth_[u * cy_ + v]) || (!std::isfinite(depth_[u * cy_ + v])))
160 depth_[u * cx_ + v] = z;
167 int ws2 = int (std::floor (
static_cast<float> (ws) / 2.f));
168 float * depth_smooth =
new float[cx_ * cy_];
169 for (
int i = 0; i < (cx_ * cy_); i++)
170 depth_smooth[i] = std::numeric_limits<float>::quiet_NaN ();
172 for (
int u = ws2; u < (cx_ - ws2); u++)
174 for (
int v = ws2; v < (cy_ - ws2); v++)
176 float min = std::numeric_limits<float>::max ();
177 for (
int j = (u - ws2); j <= (u + ws2); j++)
179 for (
int i = (v - ws2); i <= (v + ws2); i++)
181 if (std::isfinite(depth_[j * cx_ + i]) && (depth_[j * cx_ + i] < min))
183 min = depth_[j * cx_ + i];
188 if (min < (std::numeric_limits<float>::max () - 0.1))
190 depth_smooth[u * cx_ + v] = min;
195 std::copy(depth_smooth, depth_smooth + cx_ * cy_, depth_);
196 delete[] depth_smooth;
void copyPointCloud(const pcl::PointCloud< PointInT > &cloud_in, pcl::PointCloud< PointOutT > &cloud_out)
Copy all the fields from a given point cloud into a new point cloud.