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