Eigen::Matrix3Xd GraspSet::calculateShadow4(const PointList& point_list, double shadow_length) const { double voxel_grid_size = 0.003; // voxel size for points that fill occluded region double num_shadow_points = floor(shadow_length / voxel_grid_size); // number of points along each shadow vector const int num_cams = point_list.getCamSource().rows(); // Calculate the set of cameras which see the points. Eigen::VectorXi camera_set = point_list.getCamSource().rowwise().sum(); // Calculate the center point of the point neighborhood. Eigen::Vector3d center = point_list.getPoints().rowwise().sum(); center /= (double) point_list.size(); // Stores the list of all bins of the voxelized, occluded points. std::vector< Vector3iSet > shadows; shadows.resize(num_cams, Vector3iSet(num_shadow_points * 10000)); for (int i = 0; i < num_cams; i++) { if (camera_set(i) >= 1) { double t0_if = omp_get_wtime(); // Calculate the unit vector that points from the camera position to the center of the point neighborhood. Eigen::Vector3d shadow_vec = center - point_list.getViewPoints().col(i); // Scale that vector by the shadow length. shadow_vec = shadow_length * shadow_vec / shadow_vec.norm(); // Calculate occluded points. // shadows[i] = calculateVoxelizedShadowVectorized4(point_list, shadow_vec, num_shadow_points, voxel_grid_size); calculateVoxelizedShadowVectorized(point_list.getPoints(), shadow_vec, num_shadow_points, voxel_grid_size, shadows[i]); } } // Only one camera view point. if (num_cams == 1) { // Convert voxels back to points. // std::vector<Eigen::Vector3i> voxels(shadows[0].begin(), shadows[0].end()); // Eigen::Matrix3Xd shadow_out = shadowVoxelsToPoints(voxels, voxel_grid_size); // return shadow_out; return shadowVoxelsToPoints(std::vector<Eigen::Vector3i>(shadows[0].begin(), shadows[0].end()), voxel_grid_size); } // Multiple camera view points: find the intersection of all sets of occluded points. double t0_intersection = omp_get_wtime(); Vector3iSet bins_all = shadows[0]; for (int i = 1; i < num_cams; i++) { if (camera_set(i) >= 1) // check that there are points seen by this camera { bins_all = intersection(bins_all, shadows[i]); } } if (MEASURE_TIME) std::cout << "intersection runtime: " << omp_get_wtime() - t0_intersection << "s\n"; // Convert voxels back to points. std::vector<Eigen::Vector3i> voxels(bins_all.begin(), bins_all.end()); Eigen::Matrix3Xd shadow_out = shadowVoxelsToPoints(voxels, voxel_grid_size); return shadow_out; }