kinect_smoothing.h
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2011, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* $Id$
*
*/
#pragma once
#include <pcl/io/openni_camera/openni_image.h>
#include <thrust/tuple.h>
#include <pcl/pcl_exports.h>
namespace pcl
{
namespace cuda
{
struct DisparityBoundSmoothing
{
DisparityBoundSmoothing (int width, int height, int window_size, float focal_length, float baseline, float disparity_threshold, float *data, float *raw_data)
: width_ (width), height_ (height), window_size_ (window_size)
, focal_length_(focal_length), baseline_(baseline), disparity_threshold_(disparity_threshold)
, data_(data), raw_data_(raw_data)
{}
int width_, height_;
int window_size_;
float focal_length_;
float baseline_;
float disparity_threshold_;
float *data_;
float *raw_data_;
// helper function
inline __host__ __device__
float disparity2depth (float disparity)
{
return baseline_ * focal_length_ / disparity;
}
// helper function
inline __host__ __device__
float depth2disparity (float depth)
{
return baseline_ * focal_length_ / depth;
}
// clampToDisparityBounds
inline __host__ __device__
float clampToDisparityBounds (float avg_depth, float depth)
{
float disparity = depth2disparity (depth);
float avg_disparity = depth2disparity (avg_depth);
float min_disparity = disparity - disparity_threshold_;
float max_disparity = disparity + disparity_threshold_;
if (avg_disparity > max_disparity)
return disparity2depth (max_disparity);
if (avg_disparity < min_disparity)
return disparity2depth (min_disparity);
return avg_depth;
}
// actual kernel operator
inline __host__ __device__
float operator () (int idx)
{
float depth = data_ [idx];
#ifdef __CUDACC__
if (depth == 0 | isnan(depth) | isinf(depth))
return 0;
#else
if (depth == 0 | std::isnan(depth) | std::isinf(depth))
return 0;
#endif
int xIdx = idx % width_;
int yIdx = idx / width_;
// TODO: test median
// This implements a fixed window size in image coordinates (pixels)
int4 bounds = make_int4 (
xIdx - window_size_,
xIdx + window_size_,
yIdx - window_size_,
yIdx + window_size_
);
// clamp the coordinates to fit to depth image size
bounds.x = clamp (bounds.x, 0, width_-1);
bounds.y = clamp (bounds.y, 0, width_-1);
bounds.z = clamp (bounds.z, 0, height_-1);
bounds.w = clamp (bounds.w, 0, height_-1);
float average_depth = depth;
int counter = 1;
// iterate over all pixels in the rectangular region
for (int y = bounds.z; y <= bounds.w; ++y)
{
for (int x = bounds.x; x <= bounds.y; ++x)
{
// find index in point cloud from x,y pixel positions
int otherIdx = ((int)y) * width_ + ((int)x);
float otherDepth = data_[otherIdx];
// ignore invalid points
if (otherDepth == 0)
continue;
if (std::abs(otherDepth - depth) > 200)
continue;
++counter;
average_depth += otherDepth;
}
}
return clampToDisparityBounds (average_depth / counter, raw_data_[idx]);
}
};
// This version requires a pre-computed map of float3 (nr_valid_points, min_allowable_depth, max_allowable_depth);
struct DisparityClampedSmoothing
{
DisparityClampedSmoothing (float* data, float3* disparity_helper_map, int width, int height, int window_size)
: data_(data), disparity_helper_map_(disparity_helper_map), width_(width), height_(height), window_size_(window_size)
{}
float* data_;
float3* disparity_helper_map_;
int width_;
int height_;
int window_size_;
template <typename Tuple>
inline __host__ __device__
float operator () (Tuple t)
{
float depth = thrust::get<0> (t);
int idx = thrust::get<1> (t);
float3 dhel = disparity_helper_map_[idx];
int nr = (int) dhel.x;
float min_d = dhel.y;
float max_d = dhel.z;
#ifdef __CUDACC__
if (depth == 0 | isnan(depth) | isinf(depth))
return 0.0f;
#else
if (depth == 0 | std::isnan(depth) | std::isinf(depth))
return 0.0f;
#endif
int xIdx = idx % width_;
int yIdx = idx / width_;
// This implements a fixed window size in image coordinates (pixels)
int4 bounds = make_int4 (
xIdx - window_size_,
xIdx + window_size_,
yIdx - window_size_,
yIdx + window_size_
);
// clamp the coordinates to fit to disparity image size
bounds.x = clamp (bounds.x, 0, width_-1);
bounds.y = clamp (bounds.y, 0, width_-1);
bounds.z = clamp (bounds.z, 0, height_-1);
bounds.w = clamp (bounds.w, 0, height_-1);
// iterate over all pixels in the rectangular region
for (int y = bounds.z; y <= bounds.w; ++y)
{
for (int x = bounds.x; x <= bounds.y; ++x)
{
// find index in point cloud from x,y pixel positions
int otherIdx = ((int)y) * width_ + ((int)x);
depth += data_[otherIdx];
}
}
return clamp (depth / nr, min_d, max_d);
}
};
struct DisparityHelperMap
{
DisparityHelperMap (float* data, int width, int height, int window_size, float baseline, float focal_length, float disp_thresh)
: data_(data), width_(width), height_(height), window_size_(window_size), baseline_(baseline), focal_length_(focal_length), disp_thresh_(disp_thresh)
{}
float* data_;
int width_;
int height_;
int window_size_;
float baseline_;
float focal_length_;
float disp_thresh_;
// helper function
inline __host__ __device__
float disparity2depth (float disparity)
{
return baseline_ * focal_length_ / disparity;
}
// helper function
inline __host__ __device__
float depth2disparity (float depth)
{
return baseline_ * focal_length_ / depth;
}
inline __host__ __device__
float3 operator () (int idx)
{
float disparity = depth2disparity (data_ [idx]);
#ifdef __CUDACC__
if (disparity == 0 | isnan(disparity) | isinf(disparity))
return make_float3 (0,0,0);
#else
if (disparity == 0 | std::isnan(disparity) | std::isinf(disparity))
return make_float3 (0,0,0);
#endif
int xIdx = idx % width_;
int yIdx = idx / width_;
// This implements a fixed window size in image coordinates (pixels)
int4 bounds = make_int4 (
xIdx - window_size_,
xIdx + window_size_,
yIdx - window_size_,
yIdx + window_size_
);
// clamp the coordinates to fit to disparity image size
bounds.x = clamp (bounds.x, 0, width_-1);
bounds.y = clamp (bounds.y, 0, width_-1);
bounds.z = clamp (bounds.z, 0, height_-1);
bounds.w = clamp (bounds.w, 0, height_-1);
int counter = 1;
// iterate over all pixels in the rectangular region
for (int y = bounds.z; y <= bounds.w; ++y)
{
for (int x = bounds.x; x <= bounds.y; ++x)
{
// find index in point cloud from x,y pixel positions
int otherIdx = ((int)y) * width_ + ((int)x);
float otherDepth = data_[otherIdx];
// ignore invalid points
if (otherDepth > 0)
++counter;
}
}
return make_float3 ((float) counter,
disparity2depth (disparity + disp_thresh_),
disparity2depth (disparity - disp_thresh_));
}
};
} // namespace
} // namespace
pcl::cuda::DisparityHelperMap::disparity2depth
__host__ __device__ float disparity2depth(float disparity)
Definition: kinect_smoothing.h:230
pcl::cuda::DisparityBoundSmoothing::focal_length_
float focal_length_
Definition: kinect_smoothing.h:59
pcl::cuda::DisparityHelperMap::disp_thresh_
float disp_thresh_
Definition: kinect_smoothing.h:226
pcl
This file defines compatibility wrappers for low level I/O functions.
Definition: convolution.h:45
pcl::cuda::DisparityClampedSmoothing::height_
int height_
Definition: kinect_smoothing.h:162
pcl::cuda::DisparityClampedSmoothing::window_size_
int window_size_
Definition: kinect_smoothing.h:163
pcl::cuda::DisparityHelperMap::operator()
__host__ __device__ float3 operator()(int idx)
Definition: kinect_smoothing.h:243
pcl::cuda::DisparityBoundSmoothing::disparity2depth
__host__ __device__ float disparity2depth(float disparity)
Definition: kinect_smoothing.h:67
pcl::cuda::DisparityClampedSmoothing::data_
float * data_
Definition: kinect_smoothing.h:159
pcl::cuda::DisparityHelperMap::depth2disparity
__host__ __device__ float depth2disparity(float depth)
Definition: kinect_smoothing.h:237
pcl::cuda::DisparityClampedSmoothing::DisparityClampedSmoothing
DisparityClampedSmoothing(float *data, float3 *disparity_helper_map, int width, int height, int window_size)
Definition: kinect_smoothing.h:155
pcl::cuda::DisparityBoundSmoothing::raw_data_
float * raw_data_
Definition: kinect_smoothing.h:63
pcl::cuda::DisparityHelperMap::DisparityHelperMap
DisparityHelperMap(float *data, int width, int height, int window_size, float baseline, float focal_length, float disp_thresh)
Definition: kinect_smoothing.h:216
pcl::cuda::DisparityHelperMap::window_size_
int window_size_
Definition: kinect_smoothing.h:223
pcl::cuda::DisparityBoundSmoothing
Definition: kinect_smoothing.h:49
pcl::cuda::DisparityBoundSmoothing::data_
float * data_
Definition: kinect_smoothing.h:62
pcl::cuda::DisparityBoundSmoothing::depth2disparity
__host__ __device__ float depth2disparity(float depth)
Definition: kinect_smoothing.h:74
pcl::cuda::DisparityClampedSmoothing
Definition: kinect_smoothing.h:153
pcl::cuda::DisparityBoundSmoothing::clampToDisparityBounds
__host__ __device__ float clampToDisparityBounds(float avg_depth, float depth)
Definition: kinect_smoothing.h:81
pcl::cuda::DisparityHelperMap::data_
float * data_
Definition: kinect_smoothing.h:220
pcl::cuda::DisparityBoundSmoothing::baseline_
float baseline_
Definition: kinect_smoothing.h:60
pcl::cuda::DisparityBoundSmoothing::DisparityBoundSmoothing
DisparityBoundSmoothing(int width, int height, int window_size, float focal_length, float baseline, float disparity_threshold, float *data, float *raw_data)
Definition: kinect_smoothing.h:51
pcl::cuda::DisparityBoundSmoothing::width_
int width_
Definition: kinect_smoothing.h:57
pcl::cuda::DisparityHelperMap
Definition: kinect_smoothing.h:214
pcl::cuda::DisparityHelperMap::height_
int height_
Definition: kinect_smoothing.h:222
pcl::cuda::DisparityHelperMap::focal_length_
float focal_length_
Definition: kinect_smoothing.h:225
pcl::cuda::DisparityClampedSmoothing::width_
int width_
Definition: kinect_smoothing.h:161
pcl::cuda::DisparityClampedSmoothing::disparity_helper_map_
float3 * disparity_helper_map_
Definition: kinect_smoothing.h:160
pcl::cuda::DisparityClampedSmoothing::operator()
__host__ __device__ float operator()(Tuple t)
Definition: kinect_smoothing.h:167
pcl::cuda::DisparityBoundSmoothing::disparity_threshold_
float disparity_threshold_
Definition: kinect_smoothing.h:61
pcl::cuda::DisparityBoundSmoothing::window_size_
int window_size_
Definition: kinect_smoothing.h:58
pcl::cuda::DisparityBoundSmoothing::height_
int height_
Definition: kinect_smoothing.h:57
pcl::cuda::DisparityHelperMap::width_
int width_
Definition: kinect_smoothing.h:221
pcl::cuda::DisparityHelperMap::baseline_
float baseline_
Definition: kinect_smoothing.h:224
pcl::cuda::DisparityBoundSmoothing::operator()
__host__ __device__ float operator()(int idx)
Definition: kinect_smoothing.h:98