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);
}
}