Skip to content

Commit

Permalink
feat(probabilistic_occupancy_grid_map): enable to create partial occu…
Browse files Browse the repository at this point in the history 
…pancy grid map without filter obstacle pointcloud (autowarefoundation#6857)

* feat: crop ogm creation range by raw pointcloud

Signed-off-by: yoshiri <[email protected]>

* feat: add same filtering method to projective based ogm creation mode

Signed-off-by: yoshiri <[email protected]>

---------

Signed-off-by: yoshiri <[email protected]>
  • Loading branch information
@YoshiRi
YoshiRi authored Apr 22, 2024
1 parent fb5b20f commit 6a78774
Show file tree
Hide file tree
Showing 2 changed files with 33 additions and 39 deletions.
38 changes: 18 additions & 20 deletions ...c_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -71,7 +71,7 @@ void OccupancyGridMapFixedBlindSpot::updateWithPointCloud(
utils::transformPointcloud(map_raw_pointcloud, scan2map_pose, scan_raw_pointcloud);
utils::transformPointcloud(map_obstacle_pointcloud, scan2map_pose, scan_obstacle_pointcloud);

// Create angle bins
// Create angle bins and sort by distance
struct BinInfo
{
BinInfo() = default;
Expand All @@ -96,45 +96,43 @@ void OccupancyGridMapFixedBlindSpot::updateWithPointCloud(
raw_pointcloud_angle_bins.at(angle_bin_index)
.push_back(BinInfo(std::hypot(*iter_y, *iter_x), *iter_wx, *iter_wy));
}
for (auto & raw_pointcloud_angle_bin : raw_pointcloud_angle_bins) {
std::sort(raw_pointcloud_angle_bin.begin(), raw_pointcloud_angle_bin.end(), [](auto a, auto b) {
return a.range < b.range;
});
}
// create and sort bin for obstacle pointcloud
for (PointCloud2ConstIterator<float> iter_x(scan_obstacle_pointcloud, "x"),
iter_y(scan_obstacle_pointcloud, "y"), iter_wx(map_obstacle_pointcloud, "x"),
iter_wy(map_obstacle_pointcloud, "y");
iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_wx, ++iter_wy) {
const double angle = atan2(*iter_y, *iter_x);
int angle_bin_index = (angle - min_angle) / angle_increment;
const int angle_bin_index = (angle - min_angle) / angle_increment;
const double range = std::hypot(*iter_y, *iter_x);
// Ignore obstacle points exceed the range of the raw points
if (raw_pointcloud_angle_bins.at(angle_bin_index).empty()) {
continue; // No raw point in this angle bin
} else if (range > raw_pointcloud_angle_bins.at(angle_bin_index).back().range) {
continue; // Obstacle point exceeds the range of the raw points
}
obstacle_pointcloud_angle_bins.at(angle_bin_index)
.push_back(BinInfo(std::hypot(*iter_y, *iter_x), *iter_wx, *iter_wy));
.push_back(BinInfo(range, *iter_wx, *iter_wy));
}

// Sort by distance
for (auto & obstacle_pointcloud_angle_bin : obstacle_pointcloud_angle_bins) {
std::sort(
obstacle_pointcloud_angle_bin.begin(), obstacle_pointcloud_angle_bin.end(),
[](auto a, auto b) { return a.range < b.range; });
}
for (auto & raw_pointcloud_angle_bin : raw_pointcloud_angle_bins) {
std::sort(raw_pointcloud_angle_bin.begin(), raw_pointcloud_angle_bin.end(), [](auto a, auto b) {
return a.range < b.range;
});
}

// First step: Initialize cells to the final point with freespace
for (size_t bin_index = 0; bin_index < obstacle_pointcloud_angle_bins.size(); ++bin_index) {
auto & obstacle_pointcloud_angle_bin = obstacle_pointcloud_angle_bins.at(bin_index);
auto & raw_pointcloud_angle_bin = raw_pointcloud_angle_bins.at(bin_index);

BinInfo end_distance;
if (raw_pointcloud_angle_bin.empty() && obstacle_pointcloud_angle_bin.empty()) {
if (raw_pointcloud_angle_bin.empty()) {
continue;
} else if (raw_pointcloud_angle_bin.empty()) {
end_distance = obstacle_pointcloud_angle_bin.back();
} else if (obstacle_pointcloud_angle_bin.empty()) {
end_distance = raw_pointcloud_angle_bin.back();
} else {
end_distance = obstacle_pointcloud_angle_bin.back().range + distance_margin_ <
raw_pointcloud_angle_bin.back().range
? raw_pointcloud_angle_bin.back()
: obstacle_pointcloud_angle_bin.back();
end_distance = raw_pointcloud_angle_bin.back();
}
raytrace(
scan_origin.position.x, scan_origin.position.y, end_distance.wx, end_distance.wy,
Expand Down
34 changes: 15 additions & 19 deletions ...upancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -72,7 +72,7 @@ void OccupancyGridMapProjectiveBlindSpot::updateWithPointCloud(
utils::transformPointcloud(map_raw_pointcloud, scan2map_pose, scan_raw_pointcloud);
utils::transformPointcloud(map_obstacle_pointcloud, scan2map_pose, scan_obstacle_pointcloud);

// Create angle bins
// Create angle bins and sort points by range
struct BinInfo3D
{
BinInfo3D(
Expand Down Expand Up @@ -110,6 +110,12 @@ void OccupancyGridMapProjectiveBlindSpot::updateWithPointCloud(
raw_pointcloud_angle_bins.at(angle_bin_index)
.emplace_back(std::hypot(*iter_y, *iter_x), *iter_wx, *iter_wy, *iter_wz);
}
for (auto & raw_pointcloud_angle_bin : raw_pointcloud_angle_bins) {
std::sort(raw_pointcloud_angle_bin.begin(), raw_pointcloud_angle_bin.end(), [](auto a, auto b) {
return a.range < b.range;
});
}
// Create obstacle angle bins and sort points by range
for (PointCloud2ConstIterator<float> iter_x(scan_obstacle_pointcloud, "x"),
iter_y(scan_obstacle_pointcloud, "y"), iter_z(scan_obstacle_pointcloud, "z"),
iter_wx(map_obstacle_pointcloud, "x"), iter_wy(map_obstacle_pointcloud, "y"),
Expand All @@ -121,6 +127,12 @@ void OccupancyGridMapProjectiveBlindSpot::updateWithPointCloud(
const double angle = atan2(*iter_y, *iter_x);
const int angle_bin_index = (angle - min_angle) / angle_increment;
const double range = std::hypot(*iter_x, *iter_y);
// Ignore obstacle points exceed the range of the raw points
if (raw_pointcloud_angle_bins.at(angle_bin_index).empty()) {
continue; // No raw point in this angle bin
} else if (range > raw_pointcloud_angle_bins.at(angle_bin_index).back().range) {
continue; // Obstacle point exceeds the range of the raw points
}
if (dz > projection_dz_threshold_) {
const double ratio = obstacle_z / dz;
const double projection_length = range * ratio;
Expand All @@ -136,18 +148,11 @@ void OccupancyGridMapProjectiveBlindSpot::updateWithPointCloud(
std::numeric_limits<double>::infinity(), std::numeric_limits<double>::infinity());
}
}

// Sort by distance
for (auto & obstacle_pointcloud_angle_bin : obstacle_pointcloud_angle_bins) {
std::sort(
obstacle_pointcloud_angle_bin.begin(), obstacle_pointcloud_angle_bin.end(),
[](auto a, auto b) { return a.range < b.range; });
}
for (auto & raw_pointcloud_angle_bin : raw_pointcloud_angle_bins) {
std::sort(raw_pointcloud_angle_bin.begin(), raw_pointcloud_angle_bin.end(), [](auto a, auto b) {
return a.range < b.range;
});
}

grid_map::Costmap2DConverter<grid_map::GridMap> converter;
if (pub_debug_grid_) {
Expand Down Expand Up @@ -177,22 +182,13 @@ void OccupancyGridMapProjectiveBlindSpot::updateWithPointCloud(

// First step: Initialize cells to the final point with freespace
for (size_t bin_index = 0; bin_index < obstacle_pointcloud_angle_bins.size(); ++bin_index) {
const auto & obstacle_pointcloud_angle_bin = obstacle_pointcloud_angle_bins.at(bin_index);
const auto & raw_pointcloud_angle_bin = raw_pointcloud_angle_bins.at(bin_index);

BinInfo3D ray_end;
if (raw_pointcloud_angle_bin.empty() && obstacle_pointcloud_angle_bin.empty()) {
if (raw_pointcloud_angle_bin.empty()) {
continue;
} else if (raw_pointcloud_angle_bin.empty()) {
ray_end = obstacle_pointcloud_angle_bin.back();
} else if (obstacle_pointcloud_angle_bin.empty()) {
ray_end = raw_pointcloud_angle_bin.back();
} else {
const auto & farthest_obstacle_this_bin = obstacle_pointcloud_angle_bin.back();
const auto & farthest_raw_this_bin = raw_pointcloud_angle_bin.back();
ray_end = is_visible_beyond_obstacle(farthest_obstacle_this_bin, farthest_raw_this_bin)
? farthest_raw_this_bin
: farthest_obstacle_this_bin;
ray_end = raw_pointcloud_angle_bin.back();
}
raytrace(
scan_origin.position.x, scan_origin.position.y, ray_end.wx, ray_end.wy,
Expand Down

0 comments on commit 6a78774

Please sign in to comment.