text: #include "ndt_cpu/VoxelGrid.h"
  #include "ndt_cpu/debug.h"
  #include <math.h>
  #include <limits>
  #include <inttypes.h>
  
  #include <vector>
  #include <cmath>
  
  #include <stdio.h>
  #include <sys/time.h>
  
  #include "ndt_cpu/SymmetricEigenSolver.h"
  
  namespace cpu {
  
  template <typename PointSourceType>
  VoxelGrid<PointSourceType>::VoxelGrid():
    voxel_num_(0),
    max_x_(FLT_MIN),
    max_y_(FLT_MIN),
    max_z_(FLT_MIN),
    min_x_(FLT_MAX),
    min_y_(FLT_MAX),
    min_z_(FLT_MAX),
    voxel_x_(0),
    voxel_y_(0),
    voxel_z_(0),
    max_b_x_(0),
    max_b_y_(0),
    max_b_z_(0),
    min_b_x_(0),
    min_b_y_(0),
    min_b_z_(0),
    vgrid_x_(0),
    vgrid_y_(0),
    vgrid_z_(0),
    min_points_per_voxel_(6),
    real_max_bx_(INT_MIN),
    real_max_by_(INT_MIN),
    real_max_bz_(INT_MIN),
    real_min_bx_(INT_MAX),
    real_min_by_(INT_MAX),
    real_min_bz_(INT_MAX)
  {
    centroid_.reset();
    icovariance_.reset();
    points_id_.reset();
    points_per_voxel_.reset();
    tmp_centroid_.reset();
    tmp_cov_.reset();
  };
  
  template <typename PointSourceType>
  int VoxelGrid<PointSourceType>::roundUp(int input, int factor)
  {
    return (input < 0) ? -((-input) / factor) * factor : ((input + factor - 1) / factor) * factor;
  }
  
  template <typename PointSourceType>
  int VoxelGrid<PointSourceType>::roundDown(int input, int factor)
  {
    return (input < 0) ? -((-input + factor - 1) / factor) * factor : (input / factor) * factor;
  }
  
  template <typename PointSourceType>
  int VoxelGrid<PointSourceType>::div(int input, int divisor)
  {
    return (input < 0) ? -((-input + divisor - 1) / divisor) : input / divisor;
  }
  
  template <typename PointSourceType>
  void VoxelGrid<PointSourceType>::initialize()
  {
    centroid_.reset();
    centroid_ = boost::make_shared<std::vector<Eigen::Vector3d> >(voxel_num_);
  
    icovariance_.reset();
    icovariance_ = boost::make_shared<std::vector<Eigen::Matrix3d> >(voxel_num_);
  
    points_id_.reset();
    points_id_ = boost::make_shared<std::vector<std::vector<int> > >(voxel_num_);
  
    points_per_voxel_.reset();
    points_per_voxel_ = boost::make_shared<std::vector<int> >(voxel_num_, 0);
  
    tmp_centroid_.reset();
    tmp_centroid_ = boost::make_shared<std::vector<Eigen::Vector3d> >(voxel_num_);
  
    tmp_cov_.reset();
    tmp_cov_ = boost::make_shared<std::vector<Eigen::Matrix3d> >(voxel_num_);
  }
  
  template <typename PointSourceType>
  int VoxelGrid<PointSourceType>::getVoxelNum() const
  {
    return voxel_num_;
  }
  
  template <typename PointSourceType>
  float VoxelGrid<PointSourceType>::getMaxX() const
  {
    return max_x_;
  }
  
  template <typename PointSourceType>
  float VoxelGrid<PointSourceType>::getMaxY() const
  {
    return max_y_;
  }
  
  template <typename PointSourceType>
  float VoxelGrid<PointSourceType>::getMaxZ() const
  {
    return max_z_;
  }
  
  template <typename PointSourceType>
  float VoxelGrid<PointSourceType>::getMinX() const
  {
    return min_x_;
  }
  
  template <typename PointSourceType>
  float VoxelGrid<PointSourceType>::getMinY() const
  {
    return min_y_;
  }
  
  template <typename PointSourceType>
  float VoxelGrid<PointSourceType>::getMinZ() const
  {
    return min_z_;
  }
  
  template <typename PointSourceType>
  float VoxelGrid<PointSourceType>::getVoxelX() const
  {
    return voxel_x_;
  }
  
  template <typename PointSourceType>
  float VoxelGrid<PointSourceType>::getVoxelY() const
  {
    return voxel_y_;
  }
  
  template <typename PointSourceType>
  float VoxelGrid<PointSourceType>::getVoxelZ() const
  {
    return voxel_z_;
  }
  
  template <typename PointSourceType>
  int VoxelGrid<PointSourceType>::getMaxBX() const
  {
    return max_b_x_;
  }
  
  template <typename PointSourceType>
  int VoxelGrid<PointSourceType>::getMaxBY() const
  {
    return max_b_y_;
  }
  
  template <typename PointSourceType>
  int VoxelGrid<PointSourceType>::getMaxBZ() const
  {
    return max_b_z_;
  }
  
  template <typename PointSourceType>
  int VoxelGrid<PointSourceType>::getMinBX() const
  {
    return min_b_x_;
  }
  
  template <typename PointSourceType>
  int VoxelGrid<PointSourceType>::getMinBY() const
  {
    return min_b_y_;
  }
  
  template <typename PointSourceType>
  int VoxelGrid<PointSourceType>::getMinBZ() const
  {
    return min_b_z_;
  }
  
  template <typename PointSourceType>
  int VoxelGrid<PointSourceType>::getVgridX() const
  {
    return vgrid_x_;
  }
  
  template <typename PointSourceType>
  int VoxelGrid<PointSourceType>::getVgridY() const
  {
    return vgrid_y_;
  }
  
  template <typename PointSourceType>
  int VoxelGrid<PointSourceType>::getVgridZ() const
  {
    return vgrid_z_;
  }
  
  template <typename PointSourceType>
  Eigen::Vector3d VoxelGrid<PointSourceType>::getCentroid(int voxel_id) const
  {
    return (*centroid_)[voxel_id];
  }
  
  template <typename PointSourceType>
  Eigen::Matrix3d VoxelGrid<PointSourceType>::getInverseCovariance(int voxel_id) const
  {
    return (*icovariance_)[voxel_id];
  }
  
  template <typename PointSourceType>
  void VoxelGrid<PointSourceType>::setLeafSize(float voxel_x, float voxel_y, float voxel_z)
  {
    voxel_x_ = voxel_x;
    voxel_y_ = voxel_y;
    voxel_z_ = voxel_z;
  }
  
  template <typename PointSourceType>
  int VoxelGrid<PointSourceType>::voxelId(PointSourceType p)
  {
    int idx = static_cast<int>(floor(p.x / voxel_x_)) - min_b_x_;
    int idy = static_cast<int>(floor(p.y / voxel_y_)) - min_b_y_;
    int idz = static_cast<int>(floor(p.z / voxel_z_)) - min_b_z_;
  
    return (idx + idy * vgrid_x_ + idz * vgrid_x_ * vgrid_y_);
  }
  
  template <typename PointSourceType>
  int VoxelGrid<PointSourceType>::voxelId(PointSourceType p,
                        float voxel_x, float voxel_y, float voxel_z,
                        int min_b_x, int min_b_y, int min_b_z,
                        int vgrid_x, int vgrid_y, int vgrid_z)
  {
    int idx = static_cast<int>(floor(p.x / voxel_x)) - min_b_x;
    int idy = static_cast<int>(floor(p.y / voxel_y)) - min_b_y;
    int idz = static_cast<int>(floor(p.z / voxel_z)) - min_b_z;
  
    return (idx + idy * vgrid_x + idz * vgrid_x * vgrid_y);
  }
  
  template <typename PointSourceType>
  int VoxelGrid<PointSourceType>::voxelId(int idx, int idy, int idz,
                      int min_b_x, int min_b_y, int min_b_z,
                      int size_x, int size_y, int size_z)
  {
    return (idx - min_b_x) + (idy - min_b_y) * size_x + (idz - min_b_z) * size_x * size_y;
  }
  
  template <typename PointSourceType>
  void VoxelGrid<PointSourceType>::computeCentroidAndCovariance()
  {
    for (int idx = real_min_bx_; idx <= real_max_bx_; idx++)
      for (int idy = real_min_by_; idy <= real_max_by_; idy++)
        for (int idz = real_min_bz_; idz <= real_max_bz_; idz++) {
          int i = voxelId(idx, idy, idz, min_b_x_, min_b_y_, min_b_z_, vgrid_x_, vgrid_y_, vgrid_z_);
          int ipoint_num = (*points_id_)[i].size();
          double point_num = static_cast<double>(ipoint_num);
          Eigen::Vector3d pt_sum = (*tmp_centroid_)[i];
  
          if (ipoint_num > 0) {
            (*centroid_)[i] = pt_sum / point_num;
          }
  
          Eigen::Matrix3d covariance;
  
          if (ipoint_num >= min_points_per_voxel_) {
            covariance = ((*tmp_cov_)[i] - 2.0 * (pt_sum * (*centroid_)[i].transpose())) / point_num + (*centroid_)[i] * (*centroid_)[i].transpose();
            covariance *= (point_num - 1.0) / point_num;
  
            SymmetricEigensolver3x3 sv(covariance);
  
            sv.compute();
            Eigen::Matrix3d evecs = sv.eigenvectors();
            Eigen::Matrix3d evals = sv.eigenvalues().asDiagonal();
  
            if (evals(0, 0) < 0 || evals(1, 1) < 0 || evals(2, 2) <= 0) {
              (*points_per_voxel_)[i] = -1;
              continue;
            }
  
            double min_cov_eigvalue = evals(2, 2) * 0.01;
  
            if (evals(0, 0) < min_cov_eigvalue) {
              evals(0, 0) = min_cov_eigvalue;
  
              if (evals(1, 1) < min_cov_eigvalue) {
                evals(1, 1) = min_cov_eigvalue;
              }
  
              covariance = evecs * evals * evecs.inverse();
            }
  
            (*icovariance_)[i] = covariance.inverse();
          }
        }
  }
  
  //Input are supposed to be in device memory
  template <typename PointSourceType>
  void VoxelGrid<PointSourceType>::setInput(typename pcl::PointCloud<PointSourceType>::Ptr input_cloud)
  {
    if (input_cloud->points.size() > 0) {
      /* If no voxel grid was created, then
       * build the initial voxel grid and octree
       */
      source_cloud_ = input_cloud;
  
      findBoundaries();
  
      std::vector<Eigen::Vector3i> voxel_ids(input_cloud->points.size());
  
      for (int i = 0; i < input_cloud->points.size(); i++) {
        Eigen::Vector3i &vid = voxel_ids[i];
        PointSourceType p = input_cloud->points[i];
  
        vid(0) = static_cast<int>(floor(p.x / voxel_x_));
        vid(1) = static_cast<int>(floor(p.y / voxel_y_));
        vid(2) = static_cast<int>(floor(p.z / voxel_z_));
      }
  
      octree_.setInput(voxel_ids, input_cloud);
  
      voxel_ids.clear();
  
      initialize();
  
      scatterPointsToVoxelGrid();
  
      computeCentroidAndCovariance();
    }
  }
  
  template <typename PointSourceType>
  void VoxelGrid<PointSourceType>::findBoundaries()
  {
  
    findBoundaries(source_cloud_, max_x_, max_y_, max_z_, min_x_, min_y_, min_z_);
  
    real_max_bx_ = max_b_x_ = static_cast<int> (floor(max_x_ / voxel_x_));
    real_max_by_ = max_b_y_ = static_cast<int> (floor(max_y_ / voxel_y_));
    real_max_bz_ = max_b_z_ = static_cast<int> (floor(max_z_ / voxel_z_));
  
    real_min_bx_ = min_b_x_ = static_cast<int> (floor(min_x_ / voxel_x_));
    real_min_by_ = min_b_y_ = static_cast<int> (floor(min_y_ / voxel_y_));
    real_min_bz_ = min_b_z_ = static_cast<int> (floor(min_z_ / voxel_z_));
  
    /* Allocate a poll of memory that is larger than the requested memory so
     * we do not have to reallocate buffers when the target cloud is set
     */
    /* Max bounds round toward plus infinity */
    max_b_x_ = roundUp(max_b_x_, MAX_BX_);
    max_b_y_ = roundUp(max_b_y_, MAX_BY_);
    max_b_z_ = roundUp(max_b_z_, MAX_BZ_);
  
    /* Min bounds round toward minus infinity */
    min_b_x_ = roundDown(min_b_x_, MAX_BX_);
    min_b_y_ = roundDown(min_b_y_, MAX_BY_);
    min_b_z_ = roundDown(min_b_z_, MAX_BZ_);
  
    vgrid_x_ = max_b_x_ - min_b_x_ + 1;
    vgrid_y_ = max_b_y_ - min_b_y_ + 1;
    vgrid_z_ = max_b_z_ - min_b_z_ + 1;
  
    if (vgrid_x_ > 0 && vgrid_y_ > 0 && vgrid_z_ > 0) {
      voxel_num_ = vgrid_x_ * vgrid_y_ * vgrid_z_;
    } else {
      voxel_num_ = 0;
    }
  }
  
  template <typename PointSourceType>
  void VoxelGrid<PointSourceType>::findBoundaries(typename pcl::PointCloud<PointSourceType>::Ptr input_cloud,
                            float &max_x, float &max_y, float &max_z,
                            float &min_x, float &min_y, float &min_z)
  {
  
    max_x = max_y = max_z = -FLT_MAX;
    min_x = min_y = min_z = FLT_MAX;
  
    for (int i = 0; i < input_cloud->points.size(); i++) {
      float x = input_cloud->points[i].x;
      float y = input_cloud->points[i].y;
      float z = input_cloud->points[i].z;
  
      max_x = (max_x > x) ? max_x : x;
      max_y = (max_y > y) ? max_y : y;
      max_z = (max_z > z) ? max_z : z;
  
      min_x = (min_x < x) ? min_x : x;
      min_y = (min_y < y) ? min_y : y;
      min_z = (min_z < z) ? min_z : z;
    }
  }
  
  template <typename PointSourceType>
  void VoxelGrid<PointSourceType>::radiusSearch(PointSourceType p, float radius, std::vector<int> &voxel_ids, int max_nn)
  {
    float t_x = p.x;
    float t_y = p.y;
    float t_z = p.z;
  
    int max_id_x = static_cast<int>(floor((t_x + radius) / voxel_x_));
    int max_id_y = static_cast<int>(floor((t_y + radius) / voxel_y_));
    int max_id_z = static_cast<int>(floor((t_z + radius) / voxel_z_));
  
    int min_id_x = static_cast<int>(floor((t_x - radius) / voxel_x_));
    int min_id_y = static_cast<int>(floor((t_y - radius) / voxel_y_));
    int min_id_z = static_cast<int>(floor((t_z - radius) / voxel_z_));
  
    /* Find intersection of the cube containing
     * the NN sphere of the point and the voxel grid
     */
    max_id_x = (max_id_x > real_max_bx_) ? real_max_bx_ : max_id_x;
    max_id_y = (max_id_y > real_max_by_) ? real_max_by_ : max_id_y;
    max_id_z = (max_id_z > real_max_bz_) ? real_max_bz_ : max_id_z;
  
    min_id_x = (min_id_x < real_min_bx_) ? real_min_bx_ : min_id_x;
    min_id_y = (min_id_y < real_min_by_) ? real_min_by_ : min_id_y;
    min_id_z = (min_id_z < real_min_bz_) ? real_min_bz_ : min_id_z;
    int nn = 0;
  
    for (int idx = min_id_x; idx <= max_id_x && nn < max_nn; idx++) {
      for (int idy = min_id_y; idy <= max_id_y && nn < max_nn; idy++) {
        for (int idz = min_id_z; idz <= max_id_z && nn < max_nn; idz++) {
          int vid = voxelId(idx, idy, idz,
                    min_b_x_, min_b_y_, min_b_z_,
                    vgrid_x_, vgrid_y_, vgrid_z_);
  
          if ((*points_per_voxel_)[vid] >= min_points_per_voxel_) {
            double cx = (*centroid_)[vid](0) - static_cast<double>(t_x);
            double cy = (*centroid_)[vid](1) - static_cast<double>(t_y);
            double cz = (*centroid_)[vid](2) - static_cast<double>(t_z);
  
            double distance = sqrt(cx * cx + cy * cy + cz * cz);
  
            if (distance < radius) {
              nn++;
              voxel_ids.push_back(vid);
            }
          }
        }
      }
    }
  }
  
  template <typename PointSourceType>
  void VoxelGrid<PointSourceType>::scatterPointsToVoxelGrid()
  {
  
    for (int pid = 0; pid < source_cloud_->points.size(); pid++) {
      int vid = voxelId(source_cloud_->points[pid]);
      PointSourceType p = source_cloud_->points[pid];
  
      Eigen::Vector3d p3d(p.x, p.y, p.z);
  
      if ((*points_id_)[vid].size() == 0) {
        (*centroid_)[vid].setZero();
        (*points_per_voxel_)[vid] = 0;
        (*tmp_centroid_)[vid].setZero();
        (*tmp_cov_)[vid].setIdentity();
      }
  
      (*tmp_centroid_)[vid] += p3d;
      (*tmp_cov_)[vid] += p3d * p3d.transpose();
      (*points_id_)[vid].push_back(pid);
      (*points_per_voxel_)[vid]++;
    }
  }
  
  template <typename PointSourceType>
  int VoxelGrid<PointSourceType>::nearestVoxel(PointSourceType query_point, Eigen::Matrix<float, 6, 1> boundaries, float max_range)
  {
    // Index of the origin of the circle (query point)
    float qx = query_point.x;
    float qy = query_point.y;
    float qz = query_point.z;
  
    int lower_x = static_cast<int>(floor(boundaries(0) / voxel_x_));
    int lower_y = static_cast<int>(floor(boundaries(1) / voxel_y_));
    int lower_z = static_cast<int>(floor(boundaries(2) / voxel_z_));
  
    int upper_x = static_cast<int>(floor(boundaries(3) / voxel_x_));
    int upper_y = static_cast<int>(floor(boundaries(4) / voxel_y_));
    int upper_z = static_cast<int>(floor(boundaries(5) / voxel_z_));
  
    double min_dist = DBL_MAX;
    int nn_vid = -1;
  
    for (int i = lower_x; i <= upper_x; i++) {
      for (int j = lower_y; j <= upper_y; j++) {
        for (int k = lower_z; k <= upper_z; k++) {
          int vid = voxelId(i, j, k, min_b_x_, min_b_y_, min_b_z_, vgrid_x_, vgrid_y_, vgrid_z_);
          Eigen::Vector3d c = (*centroid_)[vid];
  
          if ((*points_id_)[vid].size() > 0) {
            double cur_dist = sqrt((qx - c(0)) * (qx - c(0)) + (qy - c(1)) * (qy - c(1)) + (qz - c(2)) * (qz - c(2)));
  
            if (cur_dist < min_dist) {
              min_dist = cur_dist;
              nn_vid = vid;
            }
          }
        }
      }
    }
  
    return nn_vid;
  }
  
  template <typename PointSourceType>
  double VoxelGrid<PointSourceType>::nearestNeighborDistance(PointSourceType q, float max_range)
  {
    Eigen::Matrix<float, 6, 1> nn_node_bounds;
  
    nn_node_bounds = octree_.nearestOctreeNode(q);
  
    int nn_vid = nearestVoxel(q, nn_node_bounds, max_range);
  
    Eigen::Vector3d c = (*centroid_)[nn_vid];
    double min_dist = sqrt((q.x - c(0)) * (q.x - c(0)) + (q.y - c(1)) * (q.y - c(1)) + (q.z - c(2)) * (q.z - c(2)));
  
    if (min_dist >= max_range) {
      return DBL_MAX;
    }
  
    return min_dist;
  
  }
  
  template <typename PointSourceType>
  void VoxelGrid<PointSourceType>::updateBoundaries(float max_x, float max_y, float max_z,
                            float min_x, float min_y, float min_z)
  {
  
    float new_max_x, new_max_y, new_max_z;
    float new_min_x, new_min_y, new_min_z;
  
    new_max_x = (max_x_ >= max_x) ? max_x_ : max_x;
    new_max_y = (max_y_ >= max_y) ? max_y_ : max_y;
    new_max_z = (max_z_ >= max_z) ? max_z_ : max_z;
  
    new_min_x = (min_x_ <= min_x) ? min_x_ : min_x;
    new_min_y = (min_y_ <= min_y) ? min_y_ : min_y;
    new_min_z = (min_z_ <= min_z) ? min_z_ : min_z;
  
    /* If the boundaries change, then we need to extend the list of voxels */
    if (new_max_x > max_x_ || new_max_y > max_y_ || new_max_z > max_z_ ||
        new_min_x < min_x_ || new_min_y < min_y_ || new_min_z < min_z_) {
  
      int max_b_x = static_cast<int> (floor(new_max_x / voxel_x_));
      int max_b_y = static_cast<int> (floor(new_max_y / voxel_y_));
      int max_b_z = static_cast<int> (floor(new_max_z / voxel_z_));
  
      int min_b_x = static_cast<int> (floor(new_min_x / voxel_x_));
      int min_b_y = static_cast<int> (floor(new_min_y / voxel_y_));
      int min_b_z = static_cast<int> (floor(new_min_z / voxel_z_));
  
      int real_max_bx = max_b_x;
      int real_max_by = max_b_y;
      int real_max_bz = max_b_z;
  
      int real_min_bx = min_b_x;
      int real_min_by = min_b_y;
      int real_min_bz = min_b_z;
  
      /* Max bounds round toward plus infinity */
      max_b_x = roundUp(max_b_x, MAX_BX_);
      max_b_y = roundUp(max_b_y, MAX_BY_);
      max_b_z = roundUp(max_b_z, MAX_BZ_);
  
      /* Min bounds round toward minus infinity */
      min_b_x = roundDown(min_b_x, MAX_BX_);
      min_b_y = roundDown(min_b_y, MAX_BY_);
      min_b_z = roundDown(min_b_z, MAX_BZ_);
  
      if (max_b_x > max_b_x_ || max_b_y > max_b_y_ || max_b_z > max_b_z_ ||
          min_b_x < min_b_x_ || min_b_y < min_b_y_ || min_b_z < min_b_z_) {
        int vgrid_x = max_b_x - min_b_x + 1;
        int vgrid_y = max_b_y - min_b_y + 1;
        int vgrid_z = max_b_z - min_b_z + 1;
  
        int voxel_num = vgrid_x * vgrid_y * vgrid_z;
  
        boost::shared_ptr<std::vector<Eigen::Vector3d> > old_centroid = centroid_;
        boost::shared_ptr<std::vector<Eigen::Matrix3d> > old_icovariance = icovariance_;
        boost::shared_ptr<std::vector<std::vector<int> > > old_points_id = points_id_;
        boost::shared_ptr<std::vector<int> > old_points_per_voxel = points_per_voxel_;
  
        boost::shared_ptr<std::vector<Eigen::Vector3d> > old_tmp_centroid = tmp_centroid_;
        boost::shared_ptr<std::vector<Eigen::Matrix3d> > old_tmp_cov = tmp_cov_;
  
        centroid_ = boost::make_shared<std::vector<Eigen::Vector3d> >(voxel_num);
        icovariance_ = boost::make_shared<std::vector<Eigen::Matrix3d> >(voxel_num);
        points_id_ = boost::make_shared<std::vector<std::vector<int> > >(voxel_num);
        points_per_voxel_ = boost::make_shared<std::vector<int> >(voxel_num, 0);
        tmp_centroid_ = boost::make_shared<std::vector<Eigen::Vector3d> >(voxel_num);
        tmp_cov_ = boost::make_shared<std::vector<Eigen::Matrix3d> >(voxel_num);
  
        // Move the old non-empty voxels to the new list of voxels
  
        int idx, idy, idz;
        int old_id, new_id;
  
        for (int idx = real_min_bx_; idx <= real_max_bx_; idx++) {
          for (int idy = real_min_by_; idy <= real_max_by_; idy++) {
            for (int idz = real_min_bz_; idz <= real_max_bz_; idz++) {
              old_id = voxelId(idx, idy, idz,
                        min_b_x_, min_b_y_, min_b_z_,
                        vgrid_x_, vgrid_y_, vgrid_z_);
              new_id = voxelId(idx, idy, idz,
                        min_b_x, min_b_y, min_b_z,
                        vgrid_x, vgrid_y, vgrid_z);
  
              if ((*old_points_id)[old_id].size() > 0) {
                (*points_per_voxel_)[new_id] = (*old_points_per_voxel)[old_id];
                (*centroid_)[new_id] = (*old_centroid)[old_id];
                (*icovariance_)[new_id] = (*old_icovariance)[old_id];
                (*points_id_)[new_id] = (*old_points_id)[old_id];
  
                (*tmp_centroid_)[new_id] = (*old_tmp_centroid)[old_id];
                (*tmp_cov_)[new_id] = (*old_tmp_cov)[old_id];
              }
            }
          }
        }
  
        // Update boundaries of voxels
        max_b_x_ = max_b_x;
        max_b_y_ = max_b_y;
        max_b_z_ = max_b_z;
  
        min_b_x_ = min_b_x;
        min_b_y_ = min_b_y;
        min_b_z_ = min_b_z;
  
        vgrid_x_ = vgrid_x;
        vgrid_y_ = vgrid_y;
        vgrid_z_ = vgrid_z;
  
        voxel_num_ = voxel_num;
  
      }
      // Update actual voxel boundaries
      real_min_bx_ = real_min_bx;
      real_min_by_ = real_min_by;
      real_min_bz_ = real_min_bz;
  
      real_max_bx_ = real_max_bx;
      real_max_by_ = real_max_by;
      real_max_bz_ = real_max_bz;
  
  
      //Update boundaries of points
      max_x_ = new_max_x;
      max_y_ = new_max_y;
      max_z_ = new_max_z;
  
      min_x_ = new_min_x;
      min_y_ = new_min_y;
      min_z_ = new_min_z;
  
    }
  }
  
  
  template <typename PointSourceType>
  void VoxelGrid<PointSourceType>::update(typename pcl::PointCloud<PointSourceType>::Ptr new_cloud)
  {
    if (new_cloud->points.size() <= 0) {
      return;
    }
  
    float new_max_x, new_max_y, new_max_z;
    float new_min_x, new_min_y, new_min_z;
  
    // Find boundaries of the new point cloud
    findBoundaries(new_cloud, new_max_x, new_max_y, new_max_z, new_min_x, new_min_y, new_min_z);
  
    /* Update current boundaries of the voxel grid
     * Also allocate buffer for new voxel grid and
     * octree and move the current voxel grid and
     * octree to the new buffer if necessary
     */
    updateBoundaries(new_max_x, new_max_y, new_max_z, new_min_x, new_min_y, new_min_z);
  
    /* Update changed voxels (voxels that contains new points).
     * Update centroids of voxels and their covariance matrixes
     * as well as inverse covariance matrixes */
    updateVoxelContent(new_cloud);
  
    /* Update octree */
    std::vector<Eigen::Vector3i> new_voxel_id(new_cloud->points.size());
  
    for (int i = 0; i < new_cloud->points.size(); i++) {
      Eigen::Vector3i &vid = new_voxel_id[i];
      PointSourceType p = new_cloud->points[i];
  
      vid(0) = static_cast<int>(floor(p.x / voxel_x_));
      vid(1) = static_cast<int>(floor(p.y / voxel_y_));
      vid(2) = static_cast<int>(floor(p.z / voxel_z_));
    }
  
    octree_.update(new_voxel_id, new_cloud);
  
    *source_cloud_ += *new_cloud;
  }
  
  template <typename PointSourceType>
  void VoxelGrid<PointSourceType>::updateVoxelContent(typename pcl::PointCloud<PointSourceType>::Ptr new_cloud)
  {
    int total_points_num = source_cloud_->points.size();
  
    for (int i = 0; i < new_cloud->points.size(); i++) {
      PointSourceType p = new_cloud->points[i];
      Eigen::Vector3d p3d(p.x, p.y, p.z);
      int vx = static_cast<int>(floor(p.x / voxel_x_));
      int vy = static_cast<int>(floor(p.y / voxel_y_));
      int vz = static_cast<int>(floor(p.z / voxel_z_));
      int vid = voxelId(vx, vy, vz, min_b_x_, min_b_y_, min_b_z_, vgrid_x_, vgrid_y_, vgrid_z_);
      std::vector<int> &tmp_pid = (*points_id_)[vid];
  
      if (tmp_pid.size() == 0) {
        (*centroid_)[vid].setZero();
        (*tmp_cov_)[vid].setIdentity();
        (*tmp_centroid_)[vid].setZero();
      }
  
      (*tmp_centroid_)[vid] += p3d;
      (*tmp_cov_)[vid] += p3d * p3d.transpose();
      (*points_id_)[vid].push_back(total_points_num + i);
  
      // Update centroids
      int ipoint_num = (*points_id_)[vid].size();
      (*centroid_)[vid] = (*tmp_centroid_)[vid] / static_cast<double>(ipoint_num);
      (*points_per_voxel_)[vid] = ipoint_num;
  
  
      // Update covariance
      double point_num = static_cast<double>(ipoint_num);
      Eigen::Vector3d pt_sum = (*tmp_centroid_)[vid];
  
      // Update  centroids
      (*centroid_)[vid] = (*tmp_centroid_)[vid] / point_num;
  
      if (ipoint_num >= min_points_per_voxel_) {
        Eigen::Matrix3d covariance;
        covariance = ((*tmp_cov_)[vid] - 2.0 * (pt_sum * (*centroid_)[vid].transpose())) / point_num + (*centroid_)[vid] * (*centroid_)[vid].transpose();
        covariance *= (point_num - 1.0) / point_num;
  
        SymmetricEigensolver3x3 sv(covariance);
  
        sv.compute();
        Eigen::Matrix3d evecs = sv.eigenvectors();
        Eigen::Matrix3d evals = sv.eigenvalues().asDiagonal();
  
        if (evals(0, 0) < 0 || evals(1, 1) < 0 || evals(2, 2) <= 0) {
          (*points_per_voxel_)[vid] = -1;
          continue;
        }
  
        double min_cov_eigvalue = evals(2, 2) * 0.01;
  
        if (evals(0, 0) < min_cov_eigvalue) {
          evals(0, 0) = min_cov_eigvalue;
  
          if (evals(1, 1) < min_cov_eigvalue) {
            evals(1, 1) = min_cov_eigvalue;
          }
  
          covariance = evecs * evals * evecs.inverse();
        }
  
        (*icovariance_)[vid] = covariance.inverse();
      }
    }
  }
  
  template class VoxelGrid<pcl::PointXYZI>;
  template class VoxelGrid<pcl::PointXYZ>;
  
  }
  