Skip to content

Commit

Permalink
fix: remove unnecessary neighbor voxels calculation
Browse files Browse the repository at this point in the history 
Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp>
  • Loading branch information
@badai-nguyen
badai-nguyen committed Mar 20, 2023
1 parent 7126fbd commit 9c6b6a9
Show file tree
Hide file tree
Showing 2 changed files with 189 additions and 84 deletions.
17 changes: 9 additions & 8 deletions ...ption/compare_map_segmentation/include/compare_map_segmentation/voxel_grid_map_loader.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -23,7 +23,6 @@
#include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
#include <sensor_msgs/msg/point_cloud2.hpp>

#include <pcl/filters/voxel_grid.h>
#include <pcl_conversions/pcl_conversions.h>

#include <map>
Expand Down Expand Up @@ -59,17 +58,20 @@ class VoxelGridMapLoader
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr downsampled_map_pub_;

public:
typedef compare_map_segmentation::MultiVoxelGrid<pcl::PointXYZ> MultiVoxelGrid;
typedef typename pcl::Filter<pcl::PointXYZ>::PointCloud PointCloud;
typedef typename PointCloud::Ptr PointCloudPtr;
explicit VoxelGridMapLoader(
rclcpp::Node * node, double leaf_size, std::string * tf_map_input_frame, std::mutex * mutex);
virtual bool is_close_to_map(const pcl::PointXYZ & point, const double distance_threshold) = 0;
void publish_downsampled_map(const pcl::PointCloud<pcl::PointXYZ> & downsampled_pc);

/** \brief Get representative points of 27 neighboor voxels*/
pcl::PointCloud<pcl::PointXYZ> getNeighborVoxelPoints(
pcl::PointXYZ point, double voxel_size);
bool is_close_to_neighbor_voxels(
const pcl::PointXYZ & point, const double distance_threshold, const PointCloudPtr & map,
MultiVoxelGrid & voxel) const;
bool is_in_voxel(
const pcl::PointXYZ & src_point, const pcl::PointXYZ & target_point,
const double distance_threshold, const PointCloudPtr & map, MultiVoxelGrid & voxel) const;

void publish_downsampled_map(const pcl::PointCloud<pcl::PointXYZ> & downsampled_pc);
bool is_close_points(
const pcl::PointXYZ point, const pcl::PointXYZ target_point,
const double distance_threshold) const;
Expand All @@ -81,7 +83,7 @@ class VoxelGridStaticMapLoader : public VoxelGridMapLoader
{
private:
rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr sub_map_;
pcl::VoxelGrid<pcl::PointXYZ> voxel_grid_;
MultiVoxelGrid voxel_grid_;
PointCloudPtr voxel_map_ptr_;

public:
Expand All @@ -94,7 +96,6 @@ class VoxelGridStaticMapLoader : public VoxelGridMapLoader
// *************** for Dynamic and Differential Map loader Voxel Grid Filter *************
class VoxelGridDynamicMapLoader : public VoxelGridMapLoader
{
typedef compare_map_segmentation::MultiVoxelGrid<pcl::PointXYZ> MultiVoxelGrid;
struct MapGridVoxelInfo
{
MultiVoxelGrid map_cell_voxel_grid;
Expand Down
256 changes: 180 additions & 76 deletions perception/compare_map_segmentation/src/voxel_grid_map_loader.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -25,42 +25,6 @@ VoxelGridMapLoader::VoxelGridMapLoader(
"debug/downsampled_map/pointcloud", rclcpp::QoS{1}.transient_local());
}

pcl::PointCloud<pcl::PointXYZ> VoxelGridMapLoader::getNeighborVoxelPoints(
pcl::PointXYZ point, double voxel_size)
{
pcl::PointCloud<pcl::PointXYZ> output;
output.push_back(pcl::PointXYZ(point.x, point.y, point.z));
output.push_back(pcl::PointXYZ(point.x, point.y - voxel_size, point.z - voxel_size));
output.push_back(pcl::PointXYZ(point.x, point.y - voxel_size, point.z));
output.push_back(pcl::PointXYZ(point.x, point.y - voxel_size, point.z + voxel_size));
output.push_back(pcl::PointXYZ(point.x, point.y, point.z - voxel_size));
output.push_back(pcl::PointXYZ(point.x, point.y, point.z + voxel_size));
output.push_back(pcl::PointXYZ(point.x, point.y + voxel_size, point.z - voxel_size));
output.push_back(pcl::PointXYZ(point.x, point.y + voxel_size, point.z));
output.push_back(pcl::PointXYZ(point.x, point.y + voxel_size, point.z + voxel_size));

output.push_back(pcl::PointXYZ(point.x - voxel_size, point.y - voxel_size, point.z - voxel_size));
output.push_back(pcl::PointXYZ(point.x - voxel_size, point.y - voxel_size, point.z));
output.push_back(pcl::PointXYZ(point.x - voxel_size, point.y - voxel_size, point.z + voxel_size));
output.push_back(pcl::PointXYZ(point.x - voxel_size, point.y, point.z - voxel_size));
output.push_back(pcl::PointXYZ(point.x - voxel_size, point.y, point.z));
output.push_back(pcl::PointXYZ(point.x - voxel_size, point.y, point.z + voxel_size));
output.push_back(pcl::PointXYZ(point.x - voxel_size, point.y + voxel_size, point.z - voxel_size));
output.push_back(pcl::PointXYZ(point.x - voxel_size, point.y + voxel_size, point.z));
output.push_back(pcl::PointXYZ(point.x - voxel_size, point.y + voxel_size, point.z + voxel_size));

output.push_back(pcl::PointXYZ(point.x + voxel_size, point.y - voxel_size, point.z - voxel_size));
output.push_back(pcl::PointXYZ(point.x + voxel_size, point.y - voxel_size, point.z));
output.push_back(pcl::PointXYZ(point.x + voxel_size, point.y - voxel_size, point.z + voxel_size));
output.push_back(pcl::PointXYZ(point.x + voxel_size, point.y, point.z - voxel_size));
output.push_back(pcl::PointXYZ(point.x + voxel_size, point.y, point.z));
output.push_back(pcl::PointXYZ(point.x + voxel_size, point.y, point.z + voxel_size));
output.push_back(pcl::PointXYZ(point.x + voxel_size, point.y + voxel_size, point.z - voxel_size));
output.push_back(pcl::PointXYZ(point.x + voxel_size, point.y + voxel_size, point.z));
output.push_back(pcl::PointXYZ(point.x + voxel_size, point.y + voxel_size, point.z + voxel_size));
return output;
}

bool VoxelGridMapLoader::is_close_points(
const pcl::PointXYZ point, const pcl::PointXYZ target_point,
const double distance_threshold) const
Expand All @@ -80,6 +44,180 @@ void VoxelGridMapLoader::publish_downsampled_map(
downsampled_map_pub_->publish(downsampled_map_msg);
}

bool VoxelGridMapLoader::is_close_to_neighbor_voxels(
const pcl::PointXYZ & point, const double distance_threshold, const PointCloudPtr & map,
MultiVoxelGrid & voxel) const
{
// check map downsampled pc
if (map == NULL) {
return false;
}
if (is_in_voxel(
pcl::PointXYZ(point.x, point.y, point.z), point, distance_threshold, map, voxel)) {
return true;
}
if (is_in_voxel(
pcl::PointXYZ(point.x, point.y - distance_threshold, point.z - distance_threshold), point,
distance_threshold, map, voxel)) {
return true;
}
if (is_in_voxel(
pcl::PointXYZ(point.x, point.y - distance_threshold, point.z), point, distance_threshold,
map, voxel)) {
return true;
}
if (is_in_voxel(
pcl::PointXYZ(point.x, point.y - distance_threshold, point.z + distance_threshold), point,
distance_threshold, map, voxel)) {
return true;
}
if (is_in_voxel(
pcl::PointXYZ(point.x, point.y, point.z - distance_threshold), point, distance_threshold,
map, voxel)) {
return true;
}
if (is_in_voxel(
pcl::PointXYZ(point.x, point.y, point.z + distance_threshold), point, distance_threshold,
map, voxel)) {
return true;
}
if (is_in_voxel(
pcl::PointXYZ(point.x, point.y + distance_threshold, point.z - distance_threshold), point,
distance_threshold, map, voxel)) {
return true;
}
if (is_in_voxel(
pcl::PointXYZ(point.x, point.y + distance_threshold, point.z), point, distance_threshold,
map, voxel)) {
return true;
}
if (is_in_voxel(
pcl::PointXYZ(point.x, point.y + distance_threshold, point.z + distance_threshold), point,
distance_threshold, map, voxel)) {
return true;
}

if (is_in_voxel(
pcl::PointXYZ(
point.x - distance_threshold, point.y - distance_threshold, point.z - distance_threshold),
point, distance_threshold, map, voxel)) {
return true;
}
if (is_in_voxel(
pcl::PointXYZ(point.x - distance_threshold, point.y - distance_threshold, point.z), point,
distance_threshold, map, voxel)) {
return true;
}
if (is_in_voxel(
pcl::PointXYZ(
point.x - distance_threshold, point.y - distance_threshold, point.z + distance_threshold),
point, distance_threshold, map, voxel)) {
return true;
}
if (is_in_voxel(
pcl::PointXYZ(point.x - distance_threshold, point.y, point.z - distance_threshold), point,
distance_threshold, map, voxel)) {
return true;
}
if (is_in_voxel(
pcl::PointXYZ(point.x - distance_threshold, point.y, point.z), point, distance_threshold,
map, voxel)) {
return true;
}
if (is_in_voxel(
pcl::PointXYZ(point.x - distance_threshold, point.y, point.z + distance_threshold), point,
distance_threshold, map, voxel)) {
return true;
}
if (is_in_voxel(
pcl::PointXYZ(
point.x - distance_threshold, point.y + distance_threshold, point.z - distance_threshold),
point, distance_threshold, map, voxel)) {
return true;
}
if (is_in_voxel(
pcl::PointXYZ(point.x - distance_threshold, point.y + distance_threshold, point.z), point,
distance_threshold, map, voxel)) {
return true;
}
if (is_in_voxel(
pcl::PointXYZ(
point.x - distance_threshold, point.y + distance_threshold, point.z + distance_threshold),
point, distance_threshold, map, voxel)) {
return true;
}

if (is_in_voxel(
pcl::PointXYZ(
point.x + distance_threshold, point.y - distance_threshold, point.z - distance_threshold),
point, distance_threshold, map, voxel)) {
return true;
}
if (is_in_voxel(
pcl::PointXYZ(point.x + distance_threshold, point.y - distance_threshold, point.z), point,
distance_threshold, map, voxel)) {
return true;
}
if (is_in_voxel(
pcl::PointXYZ(
point.x + distance_threshold, point.y - distance_threshold, point.z + distance_threshold),
point, distance_threshold, map, voxel)) {
return true;
}
if (is_in_voxel(
pcl::PointXYZ(point.x + distance_threshold, point.y, point.z - distance_threshold), point,
distance_threshold, map, voxel)) {
return true;
}
if (is_in_voxel(
pcl::PointXYZ(point.x + distance_threshold, point.y, point.z), point, distance_threshold,
map, voxel)) {
return true;
}
if (is_in_voxel(
pcl::PointXYZ(point.x + distance_threshold, point.y, point.z + distance_threshold), point,
distance_threshold, map, voxel)) {
return true;
}
if (is_in_voxel(
pcl::PointXYZ(
point.x + distance_threshold, point.y + distance_threshold, point.z - distance_threshold),
point, distance_threshold, map, voxel)) {
return true;
}
if (is_in_voxel(
pcl::PointXYZ(point.x + distance_threshold, point.y + distance_threshold, point.z), point,
distance_threshold, map, voxel)) {
return true;
}
if (is_in_voxel(
pcl::PointXYZ(
point.x + distance_threshold, point.y + distance_threshold, point.z + distance_threshold),
point, distance_threshold, map, voxel)) {
return true;
}

return false;
}

bool VoxelGridMapLoader::is_in_voxel(
const pcl::PointXYZ & src_point, const pcl::PointXYZ & target_point,
const double distance_threshold, const PointCloudPtr & map, MultiVoxelGrid & voxel) const
{
int voxel_index =
voxel.getCentroidIndexAt(voxel.getGridCoordinates(src_point.x, src_point.y, src_point.z));
if (voxel_index != -1) { // not empty voxel
const double dist_x = map->points.at(voxel_index).x - target_point.x;
const double dist_y = map->points.at(voxel_index).y - target_point.y;
const double dist_z = map->points.at(voxel_index).z - target_point.z;
const double sqr_distance = dist_x * dist_x + dist_y * dist_y + dist_z * dist_z;
if (sqr_distance < distance_threshold * distance_threshold) {
return true;
}
}
return false;
}

//*************************** for Static Map loader Voxel Grid Filter *************

VoxelGridStaticMapLoader::VoxelGridStaticMapLoader(
Expand Down Expand Up @@ -111,22 +249,8 @@ void VoxelGridStaticMapLoader::onMapCallback(
bool VoxelGridStaticMapLoader::is_close_to_map(
const pcl::PointXYZ & point, const double distance_threshold)
{
// check map downsampled pc
if (voxel_map_ptr_ == NULL) {
return false;
}
auto neighbor_voxel_points = getNeighborVoxelPoints(point, distance_threshold);
int voxel_index;
for (size_t j = 0; j < neighbor_voxel_points.size(); ++j) {
voxel_index = voxel_grid_.getCentroidIndexAt(voxel_grid_.getGridCoordinates(
neighbor_voxel_points[j].x, neighbor_voxel_points[j].y, neighbor_voxel_points[j].z));

if (voxel_index == -1) {
continue;
}
if (is_close_points(voxel_map_ptr_->points.at(voxel_index), point, distance_threshold)) {
return true;
}
if (is_close_to_neighbor_voxels(point, distance_threshold, voxel_map_ptr_, voxel_grid_)) {
return true;
}
return false;
}
Expand Down Expand Up @@ -176,7 +300,7 @@ bool VoxelGridDynamicMapLoader::is_close_to_map(
if (current_voxel_grid_dict_.size() == 0) {
return false;
}
for (const auto & kv : current_voxel_grid_dict_) {
for (auto & kv : current_voxel_grid_dict_) {
// TODO(badai-nguyen): add neighboor map cells checking for points on boundary when map loader
// I/F is updated
if (point.x < kv.second.min_b_x) {
Expand All @@ -195,21 +319,10 @@ bool VoxelGridDynamicMapLoader::is_close_to_map(
if (kv.second.map_cell_pc_ptr == NULL) {
return false;
}
auto neighbor_voxel_points = getNeighborVoxelPoints(point, distance_threshold);
int voxel_index;
for (size_t j = 0; j < neighbor_voxel_points.size(); ++j) {
voxel_index = kv.second.map_cell_voxel_grid.getCentroidIndexAt(
kv.second.map_cell_voxel_grid.getGridCoordinates(
neighbor_voxel_points[j].x, neighbor_voxel_points[j].y, neighbor_voxel_points[j].z));
if (voxel_index == -1) {
continue;
}
if (is_close_points(
kv.second.map_cell_pc_ptr->points.at(voxel_index), point, distance_threshold)) {
return true;
}
if (is_close_to_neighbor_voxels(
point, distance_threshold, kv.second.map_cell_pc_ptr, kv.second.map_cell_voxel_grid)) {
return true;
}
return false;
}
return false;
}
Expand All @@ -225,7 +338,6 @@ void VoxelGridDynamicMapLoader::timer_callback()
}

if (should_update_map()) {
RCLCPP_INFO(logger_, "Vehicle moved. Update current map!.\n");
last_updated_position_ = current_position_;
request_update_map((current_position_.value()));
last_updated_position_ = current_position_;
Expand All @@ -244,7 +356,6 @@ bool VoxelGridDynamicMapLoader::should_update_map() const

void VoxelGridDynamicMapLoader::request_update_map(const geometry_msgs::msg::Point & position)
{
auto exe_start_time = std::chrono::system_clock::now();
auto request = std::make_shared<autoware_map_msgs::srv::GetDifferentialPointCloudMap::Request>();
request->area.center = position;
request->area.radius = map_loader_radius_;
Expand All @@ -271,12 +382,5 @@ void VoxelGridDynamicMapLoader::request_update_map(const geometry_msgs::msg::Poi
result.get()->new_pointcloud_with_ids, result.get()->ids_to_remove);
}
}

auto exe_stop_time = std::chrono::system_clock::now();
const double exe_run_time =
std::chrono::duration_cast<std::chrono::microseconds>(exe_stop_time - exe_start_time).count() /
1000.0;
RCLCPP_INFO(logger_, "Current map updating time: %lf [ms]", exe_run_time);

publish_downsampled_map(getCurrentDownsampledMapPc());
}

0 comments on commit 9c6b6a9

Please sign in to comment.