| 47 | : lower_bound(lower_bound), resolution(resolution), grid(num_rows, num_cols, CV_8UC1, 0.0) {} |
| 48 | |
| 49 | DensityMap GenerateDensityMap(const std::vector<Eigen::Vector3d> &pcd, |
| 50 | const Eigen::Matrix4d &T_ground, |
| 51 | const float density_map_resolution, |
| 52 | const float density_threshold) { |
| 53 | double max_points = std::numeric_limits<double>::min(); |
| 54 | double min_points = std::numeric_limits<double>::max(); |
| 55 | Eigen::Array2i lower_bound_coordinates = Eigen::Array2i::Constant(max_int); |
| 56 | Eigen::Array2i upper_bound_coordinates = Eigen::Array2i::Constant(min_int); |
| 57 | |
| 58 | auto Discretize2D = [&](const Eigen::Vector3d &p) -> Eigen::Array2i { |
| 59 | return ((T_ground.block<2, 3>(0, 0) * p + T_ground.block<2, 1>(0, 3)) / |
| 60 | density_map_resolution) |
| 61 | .array() |
| 62 | .floor() |
| 63 | .cast<int>(); |
| 64 | }; |
| 65 | std::vector<Eigen::Array2i> pixels(pcd.size()); |
| 66 | std::transform(pcd.cbegin(), pcd.cend(), pixels.begin(), [&](const Eigen::Vector3d &point) { |
| 67 | const Eigen::Array2i pixel = Discretize2D(point); |
| 68 | lower_bound_coordinates = lower_bound_coordinates.min(pixel); |
| 69 | upper_bound_coordinates = upper_bound_coordinates.max(pixel); |
| 70 | return pixel; |
| 71 | }); |
| 72 | const Eigen::Array2i rows_and_columns = upper_bound_coordinates - lower_bound_coordinates; |
| 73 | const int n_rows = rows_and_columns.x() + 1; |
| 74 | const int n_cols = rows_and_columns.y() + 1; |
| 75 | |
| 76 | cv::Mat counting_grid(n_rows, n_cols, CV_64FC1, 0.0); |
| 77 | std::for_each(pixels.cbegin(), pixels.cend(), [&](const Eigen::Array2i &pixel) { |
| 78 | const Eigen::Array2i px = pixel - lower_bound_coordinates; |
| 79 | double &count = counting_grid.at<double>(px.x(), px.y()); |
| 80 | count += 1.0; |
| 81 | max_points = std::max(max_points, count); |
| 82 | min_points = std::min(min_points, count); |
| 83 | }); |
| 84 | |
| 85 | DensityMap density_map(n_rows, n_cols, density_map_resolution, lower_bound_coordinates); |
| 86 | counting_grid.forEach<double>([&](const double count, const int pos[]) { |
| 87 | double density = (count - min_points) / (max_points - min_points); |
| 88 | density = density > density_threshold ? density : 0.0; |
| 89 | density_map(pos[0], pos[1]) = static_cast<uint8_t>(255 * density); |
| 90 | }); |
| 91 | |
| 92 | return density_map; |
| 93 | } |
| 94 | } // namespace map_closures |
no test coverage detected