void TSDFVolumeOctree::getFrustumCulledVoxels (const Eigen::Affine3d& trans,
                                             std::vector<OctreeNode::Ptr> &voxels) const {
     std::vector<OctreeNode::Ptr> voxels_all;
     octree_->getLeaves (voxels_all, max_cell_size_x_, max_cell_size_y_, max_cell_size_z_);
     pcl::PointCloud<pcl::PointXYZ>::Ptr voxel_cloud(new pcl::PointCloud<pcl::PointXYZ> (voxels_all.size(), 1));
     std::vector<int> indices;
     for (size_t i = 0; i < voxel_cloud->size(); ++i) {
         pcl::PointXYZ &pt = voxel_cloud->at (i);
         voxels_all[i]->getCenter (pt.x, pt.y, pt.z);
     }
     pcl::FrustumCulling<pcl::PointXYZ> fc (false);
     Eigen::Matrix4f cam2robot;
     cam2robot << 0, 0, 1, 0,
                  0, -1, 0, 0,
                  1, 0, 0, 0,
                  0, 0, 0, 1;
     Eigen::Matrix4f trans_robot = trans.matrix().cast<float> () * cam2robot;
     fc.setCameraPose (trans_robot);
     fc.setHorizontalFOV (70);
     fc.setVerticalFOV (70);
     fc.setNearPlaneDistance (min_sensor_dist_);
     fc.setFarPlaneDistance (max_sensor_dist_);
     fc.setInputCloud (voxel_cloud);
     fc.filter (indices);
     voxels.resize (indices.size());
     for (size_t i = 0; i < indices.size(); ++i) {
         voxels[i] = voxels_all[indices[i]];
     }
 }
 void VoxelGridLargeScale::filter(const sensor_msgs::PointCloud2::ConstPtr& msg)
 {
   boost::mutex::scoped_lock lock(mutex_);
   if (leaf_size_ == 0.0) {
     pub_.publish(msg);
   }
   else {
     pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
     pcl::PointCloud<pcl::PointXYZ>::Ptr output(new pcl::PointCloud<pcl::PointXYZ>);
     pcl::fromROSMsg(*msg, *cloud);
     // check size
     Eigen::Vector4f min_pt, max_pt;
     pcl::getMinMax3D(*cloud, min_pt, max_pt);
     Eigen::Vector4f diff_pt = max_pt - min_pt;
     const int32_t int_max = std::numeric_limits<int32_t>::max();
     const int64_t dx = static_cast<int64_t>(diff_pt[0] / leaf_size_) + 1;
     const int64_t dy = static_cast<int64_t>(diff_pt[1] / leaf_size_) + 1;
     const int64_t dz = static_cast<int64_t>(diff_pt[2] / leaf_size_) + 1;
     const int min_dx = int_max / (dy * dz);
     const int num_x = dx / min_dx + 1;
     const double box_size = min_dx * leaf_size_;
     // ROS_INFO("num_x: %d", num_x);
     // ROS_INFO("%d, %d, %d", dx, dy, dz);
     // ROS_INFO("%d, %d, %d", min_dx, dx, dy*dz);
     for (int xi = 0; xi < num_x; xi++) {
       Eigen::Vector4f min_box = min_pt + Eigen::Vector4f(box_size * xi,
                                                          0,
                                                          0,
                                                          0);
       Eigen::Vector4f max_box = min_pt + Eigen::Vector4f(box_size * (xi + 1),
                                                          diff_pt[1],
                                                          diff_pt[2],
                                                          0);
       pcl::CropBox<pcl::PointXYZ> crop_box;
       crop_box.setMin(min_box);
       crop_box.setMax(max_box);
       crop_box.setInputCloud(cloud);
       pcl::PointCloud<pcl::PointXYZ>::Ptr box_cloud(new pcl::PointCloud<pcl::PointXYZ>);
       crop_box.filter(*box_cloud);
       pcl::VoxelGrid<pcl::PointXYZ> voxel;
       voxel.setLeafSize(leaf_size_, leaf_size_, leaf_size_);
       voxel.setInputCloud(box_cloud);
       pcl::PointCloud<pcl::PointXYZ>::Ptr voxel_cloud(new pcl::PointCloud<pcl::PointXYZ>);
       voxel.filter(*voxel_cloud);
       *output += *voxel_cloud;
     }
     sensor_msgs::PointCloud2 ros_cloud;
     pcl::toROSMsg(*output, ros_cloud);
     ros_cloud.header = msg->header;
     pub_.publish(ros_cloud);
   }
 }