template <typename PointT, typename Scalar> void
pcl::transformPointCloud (const pcl::PointCloud<PointT> &cloud_in, 
                          const std::vector<int> &indices, 
                          pcl::PointCloud<PointT> &cloud_out,
                          const Eigen::Transform<Scalar, 3, Eigen::Affine> &transform)
{
  size_t npts = indices.size ();
  // In order to transform the data, we need to remove NaNs
  cloud_out.is_dense = cloud_in.is_dense;
  cloud_out.header   = cloud_in.header;
  cloud_out.width    = static_cast<int> (npts);
  cloud_out.height   = 1;
  cloud_out.points.resize (npts);

  if (cloud_in.is_dense)
  {
    // If the dataset is dense, simply transform it!
    for (size_t i = 0; i < npts; ++i)
    {
      // Copy fields first, then transform xyz data
      //cloud_out.points[i] = cloud_in.points[indices[i]]; 
      //cloud_out.points[i].getVector3fMap () = transform*cloud_out.points[i].getVector3fMap ();
      Eigen::Matrix<Scalar, 3, 1> pt (cloud_in[indices[i]].x, cloud_in[indices[i]].y, cloud_in[indices[i]].z);
      cloud_out[i].x = static_cast<float> (transform (0, 0) * pt.coeffRef (0) + transform (0, 1) * pt.coeffRef (1) + transform (0, 2) * pt.coeffRef (2) + transform (0, 3));
      cloud_out[i].y = static_cast<float> (transform (1, 0) * pt.coeffRef (0) + transform (1, 1) * pt.coeffRef (1) + transform (1, 2) * pt.coeffRef (2) + transform (1, 3));
      cloud_out[i].z = static_cast<float> (transform (2, 0) * pt.coeffRef (0) + transform (2, 1) * pt.coeffRef (1) + transform (2, 2) * pt.coeffRef (2) + transform (2, 3));
    }
  }
  else
  {
    // Dataset might contain NaNs and Infs, so check for them first,
    // otherwise we get errors during the multiplication (?)
    for (size_t i = 0; i < npts; ++i)
    {
      if (!pcl_isfinite (cloud_in.points[indices[i]].x) || 
          !pcl_isfinite (cloud_in.points[indices[i]].y) || 
          !pcl_isfinite (cloud_in.points[indices[i]].z))
        continue;
      //cloud_out.points[i] = cloud_in.points[indices[i]]; 
      //cloud_out.points[i].getVector3fMap () = transform*cloud_out.points[i].getVector3fMap ();
      Eigen::Matrix<Scalar, 3, 1> pt (cloud_in[indices[i]].x, cloud_in[indices[i]].y, cloud_in[indices[i]].z);
      cloud_out[i].x = static_cast<float> (transform (0, 0) * pt.coeffRef (0) + transform (0, 1) * pt.coeffRef (1) + transform (0, 2) * pt.coeffRef (2) + transform (0, 3));
      cloud_out[i].y = static_cast<float> (transform (1, 0) * pt.coeffRef (0) + transform (1, 1) * pt.coeffRef (1) + transform (1, 2) * pt.coeffRef (2) + transform (1, 3));
      cloud_out[i].z = static_cast<float> (transform (2, 0) * pt.coeffRef (0) + transform (2, 1) * pt.coeffRef (1) + transform (2, 2) * pt.coeffRef (2) + transform (2, 3));
    }
  }
}

template<typename PointT> inline void
pcl::getMaxDistance (const pcl::PointCloud<PointT> &cloud, const std::vector<int> &indices,
                     const Eigen::Vector4f &pivot_pt, Eigen::Vector4f &max_pt)
{
  float max_dist = -FLT_MAX;
  int max_idx = -1;
  float dist;

  // If the data is dense, we don't need to check for NaN
  if (cloud.is_dense)
  {
    for (size_t i = 0; i < indices.size (); ++i)
    {
      pcl::Vector4fMapConst pt = cloud.points[indices[i]].getVector4fMap ();
      dist = (pivot_pt - pt).norm ();
      if (dist > max_dist)
      {
        max_idx = static_cast<int> (i);
        max_dist = dist;
      }
    }
  }
  // NaN or Inf values could exist => check for them
  else
  {
    for (size_t i = 0; i < indices.size (); ++i)
    {
      // Check if the point is invalid
      if (!pcl_isfinite (cloud.points[indices[i]].x) || !pcl_isfinite (cloud.points[indices[i]].y)
          ||
          !pcl_isfinite (cloud.points[indices[i]].z))
        continue;

      pcl::Vector4fMapConst pt = cloud.points[indices[i]].getVector4fMap ();
      dist = (pivot_pt - pt).norm ();
      if (dist > max_dist)
      {
        max_idx = static_cast<int> (i);
        max_dist = dist;
      }
    }
  }

  max_pt = cloud.points[indices[max_idx]].getVector4fMap ();
}

double Registrator::computeCloudResolution(const pcl::PointCloud<TYPE_Point_Sensor>::ConstPtr &cloud)
{
      double res      = 0.0;
      int    n_points = 0;
      int    nres;

      pcl::search::KdTree<TYPE_Point_Sensor> tree;

      tree.setInputCloud (cloud);

      for (int iii=0; iii<cloud->size(); ++iii)
      {
          ///////////////////////////////////////////////////////
          ///////////////////////////////////////////////////////
          if (  !pcl_isfinite( (*cloud)[iii].x )  ||    /////////
                !pcl_isfinite( (*cloud)[iii].y )  ||    /////////
                !pcl_isfinite( (*cloud)[iii].z )  )     continue;
          ///////////////////////////////////////////////////////
          ///////////////////////////////////////////////////////

          //std::cout << "computeCloudResolution - TYPE_Point_Sensor" << "\n"
          //          << (*cloud)[iii].x                              << "\t\t"
          //          << (*cloud)[iii].y                              << "\t\t"
          //          << (*cloud)[iii].z                              << "\t\t"
          //          << std::endl;

          std::vector<int>   indices(       2 ); // dummy
          std::vector<float> sqr_distances( 2 );

          //Considering the second neighbor since the first is the point itself !!!
          nres = tree.nearestKSearch(iii, 2, indices, sqr_distances);
          if (nres == 2)
          {
            res += sqrt (sqr_distances[1]);
            ++n_points;
          }
      }

      //////////////////////////////////////
      if (n_points != 0)    res /= n_points;
      //////////////////////////////////////

      return res;
}

template<typename PointT> inline void
pcl::getMaxDistance (const pcl::PointCloud<PointT> &cloud, const Eigen::Vector4f &pivot_pt, Eigen::Vector4f &max_pt)
{
  float max_dist = -FLT_MAX;
  int max_idx = -1;
  float dist;

  // If the data is dense, we don't need to check for NaN
  if (cloud.is_dense)
  {
    for (size_t i = 0; i < cloud.points.size (); ++i)
    {
      pcl::Vector4fMapConst pt = cloud.points[i].getVector4fMap ();
      dist = (pivot_pt - pt).norm ();
      if (dist > max_dist)
      {
        max_idx = int (i);
        max_dist = dist;
      }
    }
  }
  // NaN or Inf values could exist => check for them
  else
  {
    for (size_t i = 0; i < cloud.points.size (); ++i)
    {
      // Check if the point is invalid
      if (!pcl_isfinite (cloud.points[i].x) || !pcl_isfinite (cloud.points[i].y) || !pcl_isfinite (cloud.points[i].z))
        continue;
      pcl::Vector4fMapConst pt = cloud.points[i].getVector4fMap ();
      dist = (pivot_pt - pt).norm ();
      if (dist > max_dist)
      {
        max_idx = int (i);
        max_dist = dist;
      }
    }
  }

  if(max_idx != -1)
    max_pt = cloud.points[max_idx].getVector4fMap ();
  else
    max_pt = Eigen::Vector4f(std::numeric_limits<float>::quiet_NaN(),std::numeric_limits<float>::quiet_NaN(),std::numeric_limits<float>::quiet_NaN(),std::numeric_limits<float>::quiet_NaN());
}

bool
af::isMatrixHealthy (const Eigen::MatrixXd &matrix)
{
  for (int i = 0; i < matrix.rows (); ++i)
    for (int j = 0; j < matrix.cols (); ++j)
      if (!pcl_isfinite (matrix (i, j)))
        return (false);

  return (true);
}

void
pcl::getMinMax3D (const sensor_msgs::PointCloud2ConstPtr &cloud, int x_idx, int y_idx, int z_idx,
                  Eigen::Vector4f &min_pt, Eigen::Vector4f &max_pt)
{
  // @todo fix this
  if (cloud->fields[x_idx].datatype != sensor_msgs::PointField::FLOAT32 || 
      cloud->fields[y_idx].datatype != sensor_msgs::PointField::FLOAT32 ||
      cloud->fields[z_idx].datatype != sensor_msgs::PointField::FLOAT32)
  {
    PCL_ERROR ("[pcl::getMinMax3D] XYZ dimensions are not float type!\n");
    return;
  }

  Eigen::Array4f min_p, max_p;
  min_p.setConstant (FLT_MAX);
  max_p.setConstant (-FLT_MAX);

  int nr_points = cloud->width * cloud->height;

  Eigen::Array4f pt = Eigen::Array4f::Zero ();
  Eigen::Array4i xyz_offset (cloud->fields[x_idx].offset, cloud->fields[y_idx].offset, cloud->fields[z_idx].offset, 0);

  for (int cp = 0; cp < nr_points; ++cp)
  {
    // Unoptimized memcpys: assume fields x, y, z are in random order
    memcpy (&pt[0], &cloud->data[xyz_offset[0]], sizeof (float));
    memcpy (&pt[1], &cloud->data[xyz_offset[1]], sizeof (float));
    memcpy (&pt[2], &cloud->data[xyz_offset[2]], sizeof (float));
    // Check if the point is invalid
    if (!pcl_isfinite (pt[0]) || 
        !pcl_isfinite (pt[1]) || 
        !pcl_isfinite (pt[2]))
    {
      xyz_offset += cloud->point_step;
      continue;
    }
    xyz_offset += cloud->point_step;
    min_p = (min_p.min) (pt);
    max_p = (max_p.max) (pt);
  }
  min_pt = min_p;
  max_pt = max_p;
}

// IMPLEMENTATIONS
void removePointsWithNanNormals(pcl::PointCloud<pcl::PointNormal>& cloud)
{
    pcl::PointCloud<pcl::PointNormal> tmp;
    for (size_t i = 0; i < cloud.points.size(); ++i)
    {
        if (pcl_isfinite(cloud.points[i].normal_x))
            tmp.push_back(cloud.points[i]);
    }
    cloud = tmp;
}

template <typename PointInT, typename PointOutT> void
pcl::MovingLeastSquares<PointInT, PointOutT>::projectPointToMLSSurface (float &u_disp, float &v_disp,
                                                                        Eigen::Vector3d &u, Eigen::Vector3d &v,
                                                                        Eigen::Vector3d &plane_normal,
                                                                        Eigen::Vector3d &mean,
                                                                        float &curvature,
                                                                        Eigen::VectorXd &c_vec,
                                                                        int num_neighbors,
                                                                        PointOutT &result_point,
                                                                        pcl::Normal &result_normal) const
{
  double n_disp = 0.0f;
  double d_u = 0.0f, d_v = 0.0f;

  // HARDCODED 5*nr_coeff_ to guarantee that the computed polynomial had a proper point set basis
  if (polynomial_fit_ && num_neighbors >= 5*nr_coeff_ && pcl_isfinite (c_vec[0]))
  {
    // Compute the displacement along the normal using the fitted polynomial
    // and compute the partial derivatives needed for estimating the normal
    int j = 0;
    float u_pow = 1.0f, v_pow = 1.0f, u_pow_prev = 1.0f, v_pow_prev = 1.0f;
    for (int ui = 0; ui <= order_; ++ui)
    {
      v_pow = 1;
      for (int vi = 0; vi <= order_ - ui; ++vi)
      {
        // Compute displacement along normal
        n_disp += u_pow * v_pow * c_vec[j++];

        // Compute partial derivatives
        if (ui >= 1)
          d_u += c_vec[j-1] * ui * u_pow_prev * v_pow;
        if (vi >= 1)
          d_v += c_vec[j-1] * vi * u_pow * v_pow_prev;

        v_pow_prev = v_pow;
        v_pow *= v_disp;
      }
      u_pow_prev = u_pow;
      u_pow *= u_disp;
    }
  }

  result_point.x = static_cast<float> (mean[0] + u[0] * u_disp + v[0] * v_disp + plane_normal[0] * n_disp);
  result_point.y = static_cast<float> (mean[1] + u[1] * u_disp + v[1] * v_disp + plane_normal[1] * n_disp);
  result_point.z = static_cast<float> (mean[2] + u[2] * u_disp + v[2] * v_disp + plane_normal[2] * n_disp);

  Eigen::Vector3d normal = plane_normal - d_u * u - d_v * v;
  normal.normalize ();

  result_normal.normal_x = static_cast<float> (normal[0]);
  result_normal.normal_y = static_cast<float> (normal[1]);
  result_normal.normal_z = static_cast<float> (normal[2]);
  result_normal.curvature = curvature;
}

template <typename PointNT> void
pcl::MarchingCubesGreedy<PointNT>::voxelizeData()
{
    for (size_t cp = 0; cp < input_->points.size(); ++cp)
    {
        // Check if the point is invalid
        if (!pcl_isfinite (input_->points[cp].x) ||
                !pcl_isfinite (input_->points[cp].y) ||
                !pcl_isfinite (input_->points[cp].z))
            continue;

        Eigen::Vector3i index_3d;
        MarchingCubes<PointNT>::getCellIndex (input_->points[cp].getVector4fMap (), index_3d);
        uint64_t index_1d = MarchingCubes<PointNT>::getIndexIn1D (index_3d);
        Leaf cell_data;
        for (int i = 0; i < 8; ++i)
            cell_data.vertex[i] = 1;

        cell_hash_map_[index_1d] = cell_data;

        // the vertices are shared by 8 voxels, so we need to update all 8 of them
        HashMap neighbor_list;
        this->getNeighborList1D (cell_data, index_3d, neighbor_list);
        BOOST_FOREACH (typename HashMap::value_type entry, neighbor_list)
        {
            Eigen::Vector3i i3d;
            this->getIndexIn3D(entry.first, i3d);
            // if the neighbor doesn't exist, add it, otherwise we need to do an OR operation on the vertices
            if (cell_hash_map_.find (entry.first) == cell_hash_map_.end ())
            {
                cell_hash_map_[entry.first] = entry.second;
            }
            else
            {
                for (int i = 0; i < 8; ++i)
                {
                    if (entry.second.vertex[i] > 0)
                        cell_hash_map_[entry.first].vertex[i] = entry.second.vertex[i];
                }
            }
        }
    }

template <typename PointInT, typename PointNT, typename PointOutT, typename PointRFT> void
pcl::SHOTEstimationBase<PointInT, PointNT, PointOutT, PointRFT>::createBinDistanceShape (
    int index,
    const std::vector<int> &indices,
    std::vector<double> &bin_distance_shape)
{
  bin_distance_shape.resize (indices.size ());

  const PointRFT& current_frame = frames_->points[index];
  //if (!pcl_isfinite (current_frame.rf[0]) || !pcl_isfinite (current_frame.rf[4]) || !pcl_isfinite (current_frame.rf[11]))
    //return;

  Eigen::Vector4f current_frame_z (current_frame.z_axis[0], current_frame.z_axis[1], current_frame.z_axis[2], 0);

  unsigned nan_counter = 0;
  for (size_t i_idx = 0; i_idx < indices.size (); ++i_idx)
  {
    // check NaN normal
    const Eigen::Vector4f& normal_vec = normals_->points[indices[i_idx]].getNormalVector4fMap ();
    if (!pcl_isfinite (normal_vec[0]) ||
        !pcl_isfinite (normal_vec[1]) ||
        !pcl_isfinite (normal_vec[2]))
    {
      bin_distance_shape[i_idx] = std::numeric_limits<double>::quiet_NaN ();
      ++nan_counter;
    } else
    {
      //double cosineDesc = feat[i].rf[6]*normal[0] + feat[i].rf[7]*normal[1] + feat[i].rf[8]*normal[2];
      double cosineDesc = normal_vec.dot (current_frame_z);

      if (cosineDesc > 1.0)
        cosineDesc = 1.0;
      if (cosineDesc < - 1.0)
        cosineDesc = - 1.0;

      bin_distance_shape[i_idx] = ((1.0 + cosineDesc) * nr_shape_bins_) / 2;
    }
  }
  if (nan_counter > 0)
    PCL_WARN ("[pcl::%s::createBinDistanceShape] Point %d has %d (%f%%) NaN normals in its neighbourhood\n",
      getClassName ().c_str (), index, nan_counter, (static_cast<float>(nan_counter)*100.f/static_cast<float>(indices.size ())));
}

Eigen::Affine3d planarAlignmentTransform (const Eigen::Vector4d& target, const Eigen::Vector4d& to_align)
{
  Eigen::Vector3d target_norm (target[0], target[1], target[2]);
  Eigen::Vector3d align_norm (to_align[0], to_align[1], to_align[2]);
  double angle = acos (align_norm.dot (target_norm));
  Eigen::Vector3d axis = align_norm.cross (target_norm);
  axis.normalize ();
  Eigen::Affine3d transform;
  // If the normal is near identical, we'll get an invalid transform, and should instead use identity
  if ((pcl_isfinite (angle)) && (pcl_isfinite (axis[0])) && (pcl_isfinite (axis[1])) && (pcl_isfinite (axis[2])))
  {
    transform = Eigen::AngleAxisd (angle, axis);
  }
  else
  {
    transform = Eigen::Affine3d::Identity ();
  }
  //transform = Eigen::AngleAxisd (angle, axis);
  transform.translation () = target_norm * -1 * (target[3] - to_align[3]);
  return (transform);
}

void
  pcl::BivariatePolynomialT<real>::findCriticalPoints (std::vector<real>& x_values, std::vector<real>& y_values,
                                                       std::vector<int>& types) const
{
  x_values.clear ();
  y_values.clear ();
  types.clear ();
  
  if (degree == 2)
  {
    real x = (real(2)*parameters[2]*parameters[3] - parameters[1]*parameters[4]) /
             (parameters[1]*parameters[1] - real(4)*parameters[0]*parameters[3]),
         y = (real(-2)*parameters[0]*x - parameters[2]) / parameters[1];
    
    if (!pcl_isfinite(x) || !pcl_isfinite(y))
      return;
    
    int type = 2;
    real det_H = real(4)*parameters[0]*parameters[3] - parameters[1]*parameters[1];
    //std::cout << "det(H) = "<<det_H<<"\n";
    if (det_H > real(0))  // Check Hessian determinant
    {
      if (parameters[0]+parameters[3] < real(0))  // Check Hessian trace
        type = 0;
      else
        type = 1;
    }
    x_values.push_back(x);
    y_values.push_back(y);
    types.push_back(type);
    
    //real gx, gy;
    //((BivariatePolynomialT<real>*)this)->getValueOfGradient (x, y, gx, gy);
    //std::cout << "Check: p'("<<x<<", "<<y<<") = ("<<gx<<", "<<gy<<")\n";
  }
  else
  {
    std::cerr << __PRETTY_FUNCTION__ << " is not implemented for polynomials of degree "<<degree<<". Sorry.\n";
  }
}

void removeNaNs(const PointCloudNormal::Ptr& cloudInput, PointCloudNormal::Ptr& cloudOutput, std::vector<int>& removedPoints)
{
	removedPoints.clear();
	cloudOutput->header   = cloudInput->header;

	for(size_t i=0; i < cloudInput->points.size(); i++)
	{
		if(!pcl_isfinite(cloudInput->points[i].x) ||
				!pcl_isfinite(cloudInput->points[i].y) ||
				!pcl_isfinite(cloudInput->points[i].z))
		{
//			ROS_INFO_STREAM("NAN FOUND IN POINT COORDINATE. " << i);
			removedPoints.push_back((int)i);
		}
		else if(!pcl_isfinite(cloudInput->points[i].normal_x) ||
				!pcl_isfinite(cloudInput->points[i].normal_y) ||
				!pcl_isfinite(cloudInput->points[i].normal_z))
		{
//			ROS_INFO_STREAM("NAN FOUND IN NORMAL. Specify larger K or search radius to calculate normals " << i);
			removedPoints.push_back(int(i));
		}

		else
			cloudOutput->points.push_back(cloudInput->points[i]);
	}
	cloudOutput->height = 1;
	cloudOutput->width  = static_cast<uint32_t>(cloudOutput->points.size());
	ROS_INFO_STREAM("Size of removed indices" << removedPoints.size());
}

template <typename PointT> void
pcl::transformPointCloud (const pcl::PointCloud<PointT> &cloud_in, 
                          pcl::PointCloud<PointT> &cloud_out,
                          const Eigen::Matrix4f &transform)
{
  if (&cloud_in != &cloud_out)
  {
    // Note: could be replaced by cloud_out = cloud_in
    cloud_out.header   = cloud_in.header;
    cloud_out.width    = cloud_in.width;
    cloud_out.height   = cloud_in.height;
    cloud_out.is_dense = cloud_in.is_dense;
    cloud_out.points.reserve (cloud_out.points.size ());
    cloud_out.points.assign (cloud_in.points.begin (), cloud_in.points.end ());
  }

  Eigen::Matrix3f rot   = transform.block<3, 3> (0, 0);
  Eigen::Vector3f trans = transform.block<3, 1> (0, 3);
  // If the data is dense, we don't need to check for NaN
  if (cloud_in.is_dense)
  {
    for (size_t i = 0; i < cloud_out.points.size (); ++i)
      cloud_out.points[i].getVector3fMap () = rot * 
                                              cloud_in.points[i].getVector3fMap () + trans;
  }
  // Dataset might contain NaNs and Infs, so check for them first.
  else
  {
    for (size_t i = 0; i < cloud_out.points.size (); ++i)
    {
      if (!pcl_isfinite (cloud_in.points[i].x) || 
          !pcl_isfinite (cloud_in.points[i].y) || 
          !pcl_isfinite (cloud_in.points[i].z))
        continue;
      cloud_out.points[i].getVector3fMap () = rot * 
                                              cloud_in.points[i].getVector3fMap () + trans;
    }
  }
}

template <typename PointInT, typename PointOutT, typename PointRFT> void
pcl::UniqueShapeContext<PointInT, PointOutT, PointRFT>::computeFeature (PointCloudOut &output)
{
  assert (descriptor_length_ == 1960);

  output.is_dense = true;

  for (size_t point_index = 0; point_index < indices_->size (); ++point_index)
  {
    //output[point_index].descriptor.resize (descriptor_length_);

    // If the point is not finite, set the descriptor to NaN and continue
    const PointRFT& current_frame = (*frames_)[point_index];
    if (!isFinite ((*input_)[(*indices_)[point_index]]) ||
        !pcl_isfinite (current_frame.x_axis[0]) ||
        !pcl_isfinite (current_frame.y_axis[0]) ||
        !pcl_isfinite (current_frame.z_axis[0])  )
    {
      for (size_t i = 0; i < descriptor_length_; ++i)
        output[point_index].descriptor[i] = std::numeric_limits<float>::quiet_NaN ();

      memset (output[point_index].rf, 0, sizeof (output[point_index].rf[0]) * 9);
      output.is_dense = false;
      continue;
    }

    for (int d = 0; d < 3; ++d)
    {
      output.points[point_index].rf[0 + d] = current_frame.x_axis[d];
      output.points[point_index].rf[3 + d] = current_frame.y_axis[d];
      output.points[point_index].rf[6 + d] = current_frame.z_axis[d];
    }

    std::vector<float> descriptor (descriptor_length_);
    computePointDescriptor (point_index, descriptor);
    for (size_t j = 0; j < descriptor_length_; ++j)
      output [point_index].descriptor[j] = descriptor[j];
  }
}

pcl::PointCloud<PointT>::Ptr FilterBoundary(const pcl::PointCloud<PointT>::Ptr cloud)
{
    float threshold = 0.04;
    int BLen = 5;
    
    int w = cloud->width;
    int h = cloud->height;
    int num = cloud->size();
    cv::Mat depthmap = cv::Mat::ones(h+2*BLen, w+2*BLen, CV_32FC1)*-1000;
    
    for(int i = 0 ; i < num; i++ )
    {
        if( pcl_isfinite(cloud->at(i).z) == true )
        {
            int r_idx = i / w + BLen;
            int c_idx = i % w + BLen;
            depthmap.at<float>(r_idx, c_idx) = cloud->at(i).z;
        }
    }
    
    pcl::PointCloud<PointT>::Ptr cloud_f(new pcl::PointCloud<PointT>());
    for(int i=0 ; i<num; i++ ){
        
        int row = i / w + BLen;
        int col = i % w + BLen;
        if( pcl_isfinite(cloud->at(i).z) == true )
        {
        
            float zCur = depthmap.at<float>(row, col);

            if( fabs(depthmap.at<float>(row-BLen, col)-zCur) > threshold || fabs(depthmap.at<float>(row+BLen, col)-zCur) > threshold
                || fabs(zCur-depthmap.at<float>(row, col-BLen)) > threshold || fabs(zCur-depthmap.at<float>(row, col+BLen)) > threshold)
                        ;//Boundary->push_back(cloud_rgb->points[i]);
            else
                cloud_f->push_back(cloud->at(i));
        }
    }
    return cloud_f;
}

template <typename PointT> inline void
pcl::getMinMax3D (const pcl::PointCloud<PointT> &cloud, PointT &min_pt, PointT &max_pt)
{
    Eigen::Array4f min_p, max_p;
    min_p.setConstant (FLT_MAX);
    max_p.setConstant (-FLT_MAX);

    // If the data is dense, we don't need to check for NaN
    if (cloud.is_dense)
    {
        for (size_t i = 0; i < cloud.points.size (); ++i)
        {
            pcl::Array4fMapConst pt = cloud.points[i].getArray4fMap ();
            min_p = min_p.min (pt);
            max_p = max_p.max (pt);
        }
    }
    // NaN or Inf values could exist => check for them
    else
    {
        for (size_t i = 0; i < cloud.points.size (); ++i)
        {
            // Check if the point is invalid
            if (!pcl_isfinite (cloud.points[i].x) ||
                    !pcl_isfinite (cloud.points[i].y) ||
                    !pcl_isfinite (cloud.points[i].z))
                continue;
            pcl::Array4fMapConst pt = cloud.points[i].getArray4fMap ();
            min_p = min_p.min (pt);
            max_p = max_p.max (pt);
        }
    }
    min_pt.x = min_p[0];
    min_pt.y = min_p[1];
    min_pt.z = min_p[2];
    max_pt.x = max_p[0];
    max_pt.y = max_p[1];
    max_pt.z = max_p[2];
}

void
computeCloudResolution (const pcl::PointCloud<PointType>::ConstPtr &cloud)
{
  double res = 0.0;
  int n_points = 0;
  int nres;
  std::vector<int> indices (2);
  std::vector<float> sqr_distances (2);
  pcl::search::KdTree<PointType> tree;
  tree.setInputCloud (cloud);

  for (size_t i = 0; i < cloud->size (); ++i)
  {
    if (! pcl_isfinite ((*cloud)[i].x))
    {
      continue;
    }
    //Considering the second neighbor since the first is the point itself.
    nres = tree.nearestKSearch (i, 2, indices, sqr_distances);
    if (nres == 2)
    {
      res += sqrt (sqr_distances[1]);
      ++n_points;
    }
  }
  if (n_points != 0)
  {
    res /= n_points;
  }
  //  Set up resolution invariance
  //
  if (use_cloud_resolution_)
  {
    float resolution = static_cast<float>(res);
    if (resolution != 0.0f)
    {
      model_ss_   *= resolution;
      scene_ss_   *= resolution;
      rf_rad_     *= resolution;
      descr_rad_  *= resolution;
      cg_size_    *= resolution;
    }

    std::cout << "Model resolution:       " << resolution << std::endl;
    std::cout << "Model sampling size:    " << model_ss_ << std::endl;
    std::cout << "Scene sampling size:    " << scene_ss_ << std::endl;
    std::cout << "LRF support radius:     " << rf_rad_ << std::endl;
    std::cout << "SHOT descriptor radius: " << descr_rad_ << std::endl;
    std::cout << "Clustering bin size:    " << cg_size_ << std::endl << std::endl;
  }
}

void
UniformSamplingExtractor<PointT>::filterPlanar (const PointInTPtr & input, std::vector<int> &kp_idx)
{
    //create a search object
    typename pcl::search::Search<PointT>::Ptr tree;

    if (input->isOrganized () && !force_unorganized_)
        tree.reset (new pcl::search::OrganizedNeighbor<PointT> ());
    else
        tree.reset (new pcl::search::KdTree<PointT> (false));
    tree->setInputCloud (input);

    size_t kept = 0;
    for (size_t i = 0; i < kp_idx.size (); i++)
    {
        std::vector<int>  neighborhood_indices;
        std::vector<float> neighborhood_dist;

        if (tree->radiusSearch (kp_idx[i], radius_, neighborhood_indices, neighborhood_dist))
        {
            EIGEN_ALIGN16 Eigen::Matrix3f covariance_matrix;
            Eigen::Vector4f xyz_centroid;
            EIGEN_ALIGN16 Eigen::Vector3f eigenValues;
            EIGEN_ALIGN16 Eigen::Matrix3f eigenVectors;

            //compute planarity of the region
            computeMeanAndCovarianceMatrix (*input, neighborhood_indices, covariance_matrix, xyz_centroid);
            pcl::eigen33 (covariance_matrix, eigenVectors, eigenValues);

            float eigsum = eigenValues.sum ();
            if (!pcl_isfinite(eigsum))
                PCL_ERROR("Eigen sum is not finite\n");

            float t_planar = threshold_planar_;

            if(z_adaptative_)
                t_planar *= (1 + (std::max(input->points[kp_idx[i]].z,1.f) - 1.f));


            //if ((fabs (eigenValues[0] - eigenValues[1]) < 1.5e-4) || (eigsum != 0 && fabs (eigenValues[0] / eigsum) > 1.e-2))
            if ((fabs (eigenValues[0] - eigenValues[1]) < 1.5e-4) || (eigsum != 0 && fabs (eigenValues[0] / eigsum) > t_planar))
            {
                //region is not planar, add to filtered keypoint
                kp_idx[kept] = kp_idx[i];
                kept++;
            }
        }
    }
    kp_idx.resize (kept);
}

 bool compPCA(pcl::PCA<PointCloud::PointType> &pca, const PointCloud &pc, const float w, const Eigen::Vector2i &o, const Eigen::Vector2f &d) {
   PointCloud tmp;
   for(int x=0; x<w; x++) {
     Eigen::Vector2i p = o + (d*x).cast<int>();
     if(pcl_isfinite(pc(p(0),p(1)).getVector3fMap().sum()))
       tmp.push_back( pc(p(0),p(1)) );
   }
   if(tmp.size()<2) {
     ROS_WARN("no valid points");
     return false;
   }
   pca.compute(tmp);
   return true;
 }