pyramid.hpp

Go to the documentation of this file.

/*
 * Software License Agreement (BSD License)
 *
 *  Point Cloud Library (PCL) - www.pointclouds.org
 *  Copyright (c) 2012-, Open Perception, 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 the copyright holder(s) 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$
 *
 */

#ifndef PCL_FILTERS_IMPL_PYRAMID_HPP
#define PCL_FILTERS_IMPL_PYRAMID_HPP

template <typename PointT> bool
pcl::filters::Pyramid<PointT>::initCompute ()
{
  if (!input_->isOrganized ())
  {
    PCL_ERROR ("[pcl::fileters::%s::initCompute] Number of levels should be at least 2!", getClassName ().c_str ());
    return (false);
  }
  
  if (levels_ < 2)
  {
    PCL_ERROR ("[pcl::fileters::%s::initCompute] Number of levels should be at least 2!", getClassName ().c_str ());
    return (false);
  }

  // size_t ratio (std::pow (2, levels_));
  // size_t last_width = input_->width / ratio;
  // size_t last_height = input_->height / ratio;
  
  if (levels_ > 4)
  {
    PCL_ERROR ("[pcl::fileters::%s::initCompute] Number of levels should not exceed 4!", getClassName ().c_str ());
    return (false);
  }
  
  if (large_)
  {
    Eigen::VectorXf k (5);
    k << 1.f/16.f, 1.f/4.f, 3.f/8.f, 1.f/4.f, 1.f/16.f;
    kernel_ = k * k.transpose ();
    if (threshold_ != std::numeric_limits<float>::infinity ())
      threshold_ *= 2 * threshold_;

  }
  else
  {
    Eigen::VectorXf k (3);
    k << 1.f/4.f, 1.f/2.f, 1.f/4.f;
    kernel_ = k * k.transpose ();
    if (threshold_ != std::numeric_limits<float>::infinity ())
      threshold_ *= threshold_;
  }
  
  return (true);
}

template <typename PointT> void
pcl::filters::Pyramid<PointT>::compute (std::vector<PointCloudPtr>& output)
{
  std::cout << "compute" << std::endl;
  if (!initCompute ())
  {
    PCL_ERROR ("[pcl::%s::compute] initCompute failed!\n", getClassName ().c_str ());
    return;
  }

  int kernel_rows = static_cast<int> (kernel_.rows ());
  int kernel_cols = static_cast<int> (kernel_.cols ());
  int kernel_center_x = kernel_cols / 2;
  int kernel_center_y = kernel_rows / 2;

  output.resize (levels_ + 1);
  output[0].reset (new pcl::PointCloud<PointT>);
  *(output[0]) = *input_;

  if (input_->is_dense)
  {
    for (int l = 1; l <= levels_; ++l)
    {
      output[l].reset (new pcl::PointCloud<PointT> (output[l-1]->width/2, output[l-1]->height/2));
      const PointCloud<PointT> &previous = *output[l-1];
      PointCloud<PointT> &next = *output[l];
#ifdef _OPENMP
#pragma omp parallel for shared (next) num_threads(threads_)
#endif
      for(int i=0; i < next.height; ++i)
      {
        for(int j=0; j < next.width; ++j)
        {
          for(int m=0; m < kernel_rows; ++m)
          {
            int mm = kernel_rows - 1 - m;      
            for(int n=0; n < kernel_cols; ++n) 
            {
              int nn = kernel_cols - 1 - n;

              int ii = 2*i + (m - kernel_center_y);
              int jj = 2*j + (n - kernel_center_x);
              
              if (ii < 0) ii = 0;
              if (ii >= previous.height) ii = previous.height - 1;
              if (jj < 0) jj = 0;
              if (jj >= previous.width) jj = previous.width - 1;
              next.at (j,i) += previous.at (jj,ii) * kernel_ (mm,nn);
            }
          }
        }
      }
    }
  }
  else
  {
    for (int l = 1; l <= levels_; ++l)
    {
      output[l].reset (new pcl::PointCloud<PointT> (output[l-1]->width/2, output[l-1]->height/2));
      const PointCloud<PointT> &previous = *output[l-1];
      PointCloud<PointT> &next = *output[l];
#ifdef _OPENMP
#pragma omp parallel for shared (next) num_threads(threads_)
#endif
      for(int i=0; i < next.height; ++i)
      {
        for(int j=0; j < next.width; ++j)
        {
          float weight = 0;
          for(int m=0; m < kernel_rows; ++m)
          {
            int mm = kernel_rows - 1 - m;
            for(int n=0; n < kernel_cols; ++n)
            {
              int nn = kernel_cols - 1 - n;
              int ii = 2*i + (m - kernel_center_y);
              int jj = 2*j + (n - kernel_center_x);
              if (ii < 0) ii = 0;
              if (ii >= previous.height) ii = previous.height - 1;
              if (jj < 0) jj = 0;
              if (jj >= previous.width) jj = previous.width - 1;
              if (!isFinite (previous.at (jj,ii)))
                continue;
              if (pcl::squaredEuclideanDistance (previous.at (2*j,2*i), previous.at (jj,ii)) < threshold_)
              {
                next.at (j,i) += previous.at (jj,ii).x * kernel_ (mm,nn);
                weight+= kernel_ (mm,nn);
              }
            }
          }
          if (weight == 0)
            nullify (next.at (j,i));
          else
          {
            weight = 1.f/weight;
            next.at (j,i)*= weight;
          }
        }
      }
    }
  }    
}

namespace pcl
{
  namespace filters
  {
    template <> void 
    Pyramid<pcl::PointXYZRGB>::compute (std::vector<Pyramid<pcl::PointXYZRGB>::PointCloudPtr> &output)
    {
      std::cout << "PointXYZRGB" << std::endl;
      if (!initCompute ())
      {
        PCL_ERROR ("[pcl::%s::compute] initCompute failed!\n", getClassName ().c_str ());
        return;
      }
      
      int kernel_rows = static_cast<int> (kernel_.rows ());
      int kernel_cols = static_cast<int> (kernel_.cols ());
      int kernel_center_x = kernel_cols / 2;
      int kernel_center_y = kernel_rows / 2;

      output.resize (levels_ + 1);
      output[0].reset (new pcl::PointCloud<pcl::PointXYZRGB>);
      *(output[0]) = *input_;

      if (input_->is_dense)
      {
        for (int l = 1; l <= levels_; ++l)
        {
          output[l].reset (new pcl::PointCloud<pcl::PointXYZRGB> (output[l-1]->width/2, output[l-1]->height/2));
          const PointCloud<pcl::PointXYZRGB> &previous = *output[l-1];
          PointCloud<pcl::PointXYZRGB> &next = *output[l];
#ifdef _OPENMP
#pragma omp parallel for shared (next) num_threads(threads_)
#endif
          for(int i=0; i < next.height; ++i)              // rows
          {
            for(int j=0; j < next.width; ++j)          // columns
            {
              float r = 0, g = 0, b = 0;
              for(int m=0; m < kernel_rows; ++m)     // kernel rows
              {
                int mm = kernel_rows - 1 - m;      // row index of flipped kernel
                for(int n=0; n < kernel_cols; ++n) // kernel columns
                {
                  int nn = kernel_cols - 1 - n;  // column index of flipped kernel
                  // index of input signal, used for checking boundary
                  int ii = 2*i + (m - kernel_center_y);
                  int jj = 2*j + (n - kernel_center_x);
                  
                  // ignore input samples which are out of bound
                  if (ii < 0) ii = 0;
                  if (ii >= previous.height) ii = previous.height - 1;
                  if (jj < 0) jj = 0;
                  if (jj >= previous.width) jj = previous.width - 1;
                  next.at (j,i).x += previous.at (jj,ii).x * kernel_ (mm,nn);
                  next.at (j,i).y += previous.at (jj,ii).y * kernel_ (mm,nn);
                  next.at (j,i).z += previous.at (jj,ii).z * kernel_ (mm,nn);
                  b += previous.at (jj,ii).b * kernel_ (mm,nn);
                  g += previous.at (jj,ii).g * kernel_ (mm,nn);
                  r += previous.at (jj,ii).r * kernel_ (mm,nn);
                }
              }
              next.at (j,i).b = static_cast<pcl::uint8_t> (b);
              next.at (j,i).g = static_cast<pcl::uint8_t> (g);
              next.at (j,i).r = static_cast<pcl::uint8_t> (r);
            }
          }
        }
      }
      else
      {
        for (int l = 1; l <= levels_; ++l)
        {
          output[l].reset (new pcl::PointCloud<pcl::PointXYZRGB> (output[l-1]->width/2, output[l-1]->height/2));
          const PointCloud<pcl::PointXYZRGB> &previous = *output[l-1];
          PointCloud<pcl::PointXYZRGB> &next = *output[l];
#ifdef _OPENMP
#pragma omp parallel for shared (next) num_threads(threads_)
#endif
          for(int i=0; i < next.height; ++i)
          {
            for(int j=0; j < next.width; ++j)
            {
              float weight = 0;
              float r = 0, g = 0, b = 0;
              for(int m=0; m < kernel_rows; ++m)
              {
                int mm = kernel_rows - 1 - m;
                for(int n=0; n < kernel_cols; ++n)
                {
                  int nn = kernel_cols - 1 - n;
                  int ii = 2*i + (m - kernel_center_y);
                  int jj = 2*j + (n - kernel_center_x);
                  if (ii < 0) ii = 0;
                  if (ii >= previous.height) ii = previous.height - 1;
                  if (jj < 0) jj = 0;
                  if (jj >= previous.width) jj = previous.width - 1;
                  if (!isFinite (previous.at (jj,ii)))
                    continue;
                  if (pcl::squaredEuclideanDistance (previous.at (2*j,2*i), previous.at (jj,ii)) < threshold_)
                  {
                    next.at (j,i).x += previous.at (jj,ii).x * kernel_ (mm,nn);
                    next.at (j,i).y += previous.at (jj,ii).y * kernel_ (mm,nn);
                    next.at (j,i).z += previous.at (jj,ii).z * kernel_ (mm,nn);
                    b += previous.at (jj,ii).b * kernel_ (mm,nn);
                    g += previous.at (jj,ii).g * kernel_ (mm,nn);
                    r += previous.at (jj,ii).r * kernel_ (mm,nn);
                    weight+= kernel_ (mm,nn);
                  }
                }
              }
              if (weight == 0)
                nullify (next.at (j,i));
              else
              {
                weight = 1.f/weight;
                r*= weight; g*= weight; b*= weight;
                next.at (j,i).x*= weight; next.at (j,i).y*= weight; next.at (j,i).z*= weight;
                next.at (j,i).b = static_cast<pcl::uint8_t> (b);
                next.at (j,i).g = static_cast<pcl::uint8_t> (g);
                next.at (j,i).r = static_cast<pcl::uint8_t> (r);
              }
            }
          }
        }
      }    
    }
    
    template <> void 
    Pyramid<pcl::PointXYZRGBA>::compute (std::vector<Pyramid<pcl::PointXYZRGBA>::PointCloudPtr> &output)
    {
      std::cout << "PointXYZRGBA" << std::endl;
      if (!initCompute ())
      {
        PCL_ERROR ("[pcl::%s::compute] initCompute failed!\n", getClassName ().c_str ());
        return;
      }
      
      int kernel_rows = static_cast<int> (kernel_.rows ());
      int kernel_cols = static_cast<int> (kernel_.cols ());
      int kernel_center_x = kernel_cols / 2;
      int kernel_center_y = kernel_rows / 2;

      output.resize (levels_ + 1);
      output[0].reset (new pcl::PointCloud<pcl::PointXYZRGBA>);
      *(output[0]) = *input_;

      if (input_->is_dense)
      {
        for (int l = 1; l <= levels_; ++l)
        {
          output[l].reset (new pcl::PointCloud<pcl::PointXYZRGBA> (output[l-1]->width/2, output[l-1]->height/2));
          const PointCloud<pcl::PointXYZRGBA> &previous = *output[l-1];
          PointCloud<pcl::PointXYZRGBA> &next = *output[l];
#ifdef _OPENMP
#pragma omp parallel for shared (next) num_threads(threads_)
#endif
          for(int i=0; i < next.height; ++i)              // rows
          {
            for(int j=0; j < next.width; ++j)          // columns
            {
              float r = 0, g = 0, b = 0, a = 0;
              for(int m=0; m < kernel_rows; ++m)     // kernel rows
              {
                int mm = kernel_rows - 1 - m;      // row index of flipped kernel
                for(int n=0; n < kernel_cols; ++n) // kernel columns
                {
                  int nn = kernel_cols - 1 - n;  // column index of flipped kernel
                  // index of input signal, used for checking boundary
                  int ii = 2*i + (m - kernel_center_y);
                  int jj = 2*j + (n - kernel_center_x);
                  
                  // ignore input samples which are out of bound
                  if (ii < 0) ii = 0;
                  if (ii >= previous.height) ii = previous.height - 1;
                  if (jj < 0) jj = 0;
                  if (jj >= previous.width) jj = previous.width - 1;
                  next.at (j,i).x += previous.at (jj,ii).x * kernel_ (mm,nn);
                  next.at (j,i).y += previous.at (jj,ii).y * kernel_ (mm,nn);
                  next.at (j,i).z += previous.at (jj,ii).z * kernel_ (mm,nn);
                  b += previous.at (jj,ii).b * kernel_ (mm,nn);
                  g += previous.at (jj,ii).g * kernel_ (mm,nn);
                  r += previous.at (jj,ii).r * kernel_ (mm,nn);
                  a += previous.at (jj,ii).a * kernel_ (mm,nn);
                }
              }
              next.at (j,i).b = static_cast<pcl::uint8_t> (b);
              next.at (j,i).g = static_cast<pcl::uint8_t> (g);
              next.at (j,i).r = static_cast<pcl::uint8_t> (r);
              next.at (j,i).a = static_cast<pcl::uint8_t> (a);
            }
          }
        }
      }
      else
      {
        for (int l = 1; l <= levels_; ++l)
        {
          output[l].reset (new pcl::PointCloud<pcl::PointXYZRGBA> (output[l-1]->width/2, output[l-1]->height/2));
          const PointCloud<pcl::PointXYZRGBA> &previous = *output[l-1];
          PointCloud<pcl::PointXYZRGBA> &next = *output[l];
#ifdef _OPENMP
#pragma omp parallel for shared (next) num_threads(threads_)
#endif
          for(int i=0; i < next.height; ++i)
          {
            for(int j=0; j < next.width; ++j)
            {
              float weight = 0;
              float r = 0, g = 0, b = 0, a = 0;
              for(int m=0; m < kernel_rows; ++m)
              {
                int mm = kernel_rows - 1 - m;
                for(int n=0; n < kernel_cols; ++n)
                {
                  int nn = kernel_cols - 1 - n;
                  int ii = 2*i + (m - kernel_center_y);
                  int jj = 2*j + (n - kernel_center_x);
                  if (ii < 0) ii = 0;
                  if (ii >= previous.height) ii = previous.height - 1;
                  if (jj < 0) jj = 0;
                  if (jj >= previous.width) jj = previous.width - 1;
                  if (!isFinite (previous.at (jj,ii)))
                    continue;
                  if (pcl::squaredEuclideanDistance (previous.at (2*j,2*i), previous.at (jj,ii)) < threshold_)
                  {
                    next.at (j,i).x += previous.at (jj,ii).x * kernel_ (mm,nn);
                    next.at (j,i).y += previous.at (jj,ii).y * kernel_ (mm,nn);
                    next.at (j,i).z += previous.at (jj,ii).z * kernel_ (mm,nn);
                    b += previous.at (jj,ii).b * kernel_ (mm,nn);
                    g += previous.at (jj,ii).g * kernel_ (mm,nn);
                    r += previous.at (jj,ii).r * kernel_ (mm,nn);
                    a += previous.at (jj,ii).a * kernel_ (mm,nn);
                    weight+= kernel_ (mm,nn);
                  }
                }
              }
              if (weight == 0)
                nullify (next.at (j,i));
              else
              {
                weight = 1.f/weight;
                r*= weight; g*= weight; b*= weight; a*= weight;
                next.at (j,i).x*= weight; next.at (j,i).y*= weight; next.at (j,i).z*= weight;
                next.at (j,i).b = static_cast<pcl::uint8_t> (b);
                next.at (j,i).g = static_cast<pcl::uint8_t> (g);
                next.at (j,i).r = static_cast<pcl::uint8_t> (r);
                next.at (j,i).a = static_cast<pcl::uint8_t> (a);
              }
            }
          }
        }
      }    
    }

    template<> void
    Pyramid<pcl::RGB>::nullify (pcl::RGB& p)
    {
      p.r = 0; p.g = 0; p.b = 0;
    }    

    template <> void 
    Pyramid<pcl::RGB>::compute (std::vector<Pyramid<pcl::RGB>::PointCloudPtr> &output)
    {
      std::cout << "RGB" << std::endl;
      if (!initCompute ())
      {
        PCL_ERROR ("[pcl::%s::compute] initCompute failed!\n", getClassName ().c_str ());
        return;
      }
      
      int kernel_rows = static_cast<int> (kernel_.rows ());
      int kernel_cols = static_cast<int> (kernel_.cols ());
      int kernel_center_x = kernel_cols / 2;
      int kernel_center_y = kernel_rows / 2;

      output.resize (levels_ + 1);
      output[0].reset (new pcl::PointCloud<pcl::RGB>);
      *(output[0]) = *input_;

      if (input_->is_dense)
      {
        for (int l = 1; l <= levels_; ++l)
        {
          output[l].reset (new pcl::PointCloud<pcl::RGB> (output[l-1]->width/2, output[l-1]->height/2));
          const PointCloud<pcl::RGB> &previous = *output[l-1];
          PointCloud<pcl::RGB> &next = *output[l];
#ifdef _OPENMP
#pragma omp parallel for shared (next) num_threads(threads_)
#endif
          for(int i=0; i < next.height; ++i)
          {
            for(int j=0; j < next.width; ++j)
            {
              float r = 0, g = 0, b = 0;
              for(int m=0; m < kernel_rows; ++m)
              {
                int mm = kernel_rows - 1 - m;
                for(int n=0; n < kernel_cols; ++n)
                {
                  int nn = kernel_cols - 1 - n;
                  int ii = 2*i + (m - kernel_center_y);
                  int jj = 2*j + (n - kernel_center_x);
                  if (ii < 0) ii = 0;
                  if (ii >= previous.height) ii = previous.height - 1;
                  if (jj < 0) jj = 0;
                  if (jj >= previous.width) jj = previous.width - 1;
                  b += previous.at (jj,ii).b * kernel_ (mm,nn);
                  g += previous.at (jj,ii).g * kernel_ (mm,nn);
                  r += previous.at (jj,ii).r * kernel_ (mm,nn);
                }
              }
              next.at (j,i).b = static_cast<pcl::uint8_t> (b);
              next.at (j,i).g = static_cast<pcl::uint8_t> (g);
              next.at (j,i).r = static_cast<pcl::uint8_t> (r);
            }
          }
        }
      }
      else
      {
        for (int l = 1; l <= levels_; ++l)
        {
          output[l].reset (new pcl::PointCloud<pcl::RGB> (output[l-1]->width/2, output[l-1]->height/2));
          const PointCloud<pcl::RGB> &previous = *output[l-1];
          PointCloud<pcl::RGB> &next = *output[l];
#ifdef _OPENMP
#pragma omp parallel for shared (next) num_threads(threads_)
#endif
          for(int i=0; i < next.height; ++i)
          {
            for(int j=0; j < next.width; ++j)
            {
              float weight = 0;
              float r = 0, g = 0, b = 0;
              for(int m=0; m < kernel_rows; ++m)
              {
                int mm = kernel_rows - 1 - m;
                for(int n=0; n < kernel_cols; ++n)
                {
                  int nn = kernel_cols - 1 - n;
                  int ii = 2*i + (m - kernel_center_y);
                  int jj = 2*j + (n - kernel_center_x);
                  if (ii < 0) ii = 0;
                  if (ii >= previous.height) ii = previous.height - 1;
                  if (jj < 0) jj = 0;
                  if (jj >= previous.width) jj = previous.width - 1;
                  if (!isFinite (previous.at (jj,ii)))
                    continue;
                  if (pcl::squaredEuclideanDistance (previous.at (2*j,2*i), previous.at (jj,ii)) < threshold_)
                  {
                    b += previous.at (jj,ii).b * kernel_ (mm,nn);
                    g += previous.at (jj,ii).g * kernel_ (mm,nn);
                    r += previous.at (jj,ii).r * kernel_ (mm,nn);
                    weight+= kernel_ (mm,nn);
                  }
                }
              }
              if (weight == 0)
                nullify (next.at (j,i));
              else
              {
                weight = 1.f/weight;
                r*= weight; g*= weight; b*= weight;
                next.at (j,i).b = static_cast<pcl::uint8_t> (b);
                next.at (j,i).g = static_cast<pcl::uint8_t> (g);
                next.at (j,i).r = static_cast<pcl::uint8_t> (r);
              }
            }
          }
        }
      }    
    }

  }
}

#endif