| 10 | namespace py = pybind11; |
| 11 | |
| 12 | Eigen::MatrixXd run_raycasting_cpp( |
| 13 | py::array_t<float, py::array::c_style | py::array::forcecast> depth_image, |
| 14 | Eigen::Matrix<double, 4, 4, Eigen::RowMajor> T_cam_to_world, |
| 15 | std::vector<int> grid_shape, |
| 16 | double fx, double fy, double cx, double cy, |
| 17 | Eigen::Vector3d origin, |
| 18 | int step, |
| 19 | double resolution) { |
| 20 | |
| 21 | py::buffer_info buf = depth_image.request(); |
| 22 | auto* ptr = static_cast<float*>(buf.ptr); |
| 23 | int depth_height = buf.shape[0]; |
| 24 | int depth_width = buf.shape[1]; |
| 25 | |
| 26 | Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> occupancy_grid = Eigen::MatrixXd::Zero(grid_shape[0], grid_shape[1] * grid_shape[2]); |
| 27 | occupancy_grid.resize(grid_shape[0], grid_shape[1] * grid_shape[2]); |
| 28 | Eigen::Map<Eigen::VectorXd> occupancy_grid_flat(occupancy_grid.data(), occupancy_grid.size()); |
| 29 | |
| 30 | |
| 31 | Eigen::Vector3d camera_origin = T_cam_to_world.topRightCorner(3, 1); |
| 32 | Eigen::Vector3i start_voxel_base = ((camera_origin - origin) / resolution).array().floor().cast<int>(); |
| 33 | |
| 34 | for (int v = 0; v < depth_height; v += step) { |
| 35 | for (int u = 0; u < depth_width; u += step) { |
| 36 | float d = ptr[v * depth_width + u]; |
| 37 | if (d <= 0) { |
| 38 | continue; |
| 39 | } |
| 40 | |
| 41 | double x = (u - cx) * d / fx; |
| 42 | double y = (v - cy) * d / fy; |
| 43 | double z = d; |
| 44 | |
| 45 | Eigen::Vector4d point_cam(x, y, z, 1.0); |
| 46 | Eigen::Vector4d point_world_h = T_cam_to_world * point_cam; |
| 47 | Eigen::Vector3d point_world = point_world_h.head<3>(); |
| 48 | |
| 49 | Eigen::Vector3i start_voxel = start_voxel_base; |
| 50 | Eigen::Vector3i end_voxel = ((point_world - origin) / resolution).array().floor().cast<int>(); |
| 51 | Eigen::Vector3i diff = end_voxel - start_voxel; |
| 52 | int steps = diff.cwiseAbs().maxCoeff(); |
| 53 | |
| 54 | if (steps == 0) { |
| 55 | continue; |
| 56 | } |
| 57 | |
| 58 | for (int i = 0; i <= steps; ++i) { |
| 59 | double t = static_cast<double>(i) / steps; |
| 60 | Eigen::Vector3i interp = (start_voxel.cast<double>() + t * diff.cast<double>()).unaryExpr([](double v) { return std::nearbyint(v); }).cast<int>(); |
| 61 | |
| 62 | if ((interp.array() < 0).any() || (interp.array() >= Eigen::Map<Eigen::VectorXi>(grid_shape.data(), 3).array()).any()) { |
| 63 | continue; |
| 64 | } |
| 65 | int flat_idx = interp.x() * (grid_shape[1] * grid_shape[2]) + interp.y() * grid_shape[2] + interp.z(); |
| 66 | occupancy_grid_flat(flat_idx) -= 0.05; |
| 67 | } |
| 68 | |
| 69 | if ((end_voxel.array() >= 0).all() && (end_voxel.array() < Eigen::Map<Eigen::VectorXi>(grid_shape.data(), 3).array()).all()) { |
no outgoing calls