kinect_error.hpp

Go to the documentation of this file.

//##################
//#### includes ####
#include <boost/random.hpp>
#include <boost/random/normal_distribution.hpp>


template<typename PointT>
void
preprocessing::KinectErrorGenerator<PointT>::applyFilter (PointCloud &pc_out)
{
  //normal distribution
  boost::mt19937 rng;
  rng.seed(clock());

  // set the parameters for output poincloud (pc_out)
  if(&*input_ == &pc_out)
    input_.reset(new PointCloud(pc_out));
  else
    pc_out = *input_;

  //Go through all points and discard points with amplitude outside filter limits
  for(int x=0; x<(int)input_->width; x++) {
    for(int y=0; y<(int)input_->height; y++) {

      //step 1: deviation

      float diff=0;
      int num=0;
      for(int dx=-2; dx<2; dx++) {
        for(int dy=-2; dy<2; dy++) {
          if(dx+x<0||dx+x>=(int)input_->width
              || dy+y<0||dy+y>=(int)input_->height)
            continue;

          diff += input_->points[x+y*input_->width].z -
              input_->points[x+dx+(y+dy)*input_->width].z;
          ++num;
        }
      }
      diff/=num;

      float mean=diff;

      PointT &p = pc_out.points[x+y*input_->width];

      diff = std::max(0.005f, std::abs(diff));

      diff = 0.01;
      mean = 0;

      diff *= p.z*p.z;

      diff = std::min(diff, 2.f);

      boost::normal_distribution<> nd(0, diff*standard_deviation_);

      boost::variate_generator<boost::mt19937&,
      boost::normal_distribution<> > var_nor(rng, nd);

      float add=0;
      do {
        add = var_nor();
      } while(std::abs(add)>std::max(p.z*p.z*0.005f,std::abs(mean)));

      p.x /= p.z;
      p.y /= p.z;

      p.z += add-mean;

      //step 2: quantization error
      int raw_depth = ((1./p.z)-3.3309495161)/-0.0030711016;

      p.z = 1.0 / (raw_depth * -0.0030711016 + 3.3309495161);

      p.x *= p.z;
      p.y *= p.z;

    }
  }

}


#define PCL_INSTANTIATE_KinectErrorGenerator(T) template class preprocessing::KinectErrorGenerator<T>;