Skip to content

Commit

Permalink
chore(compare_map_segmentation): rename defined type (autowarefoundat…
Browse files Browse the repository at this point in the history
…ion#9181)

Signed-off-by: badai-nguyen <[email protected]>
  • Loading branch information
badai-nguyen authored Dec 5, 2024
1 parent 0992f1d commit c1d3478
Show file tree
Hide file tree
Showing 4 changed files with 24 additions and 22 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -28,14 +28,14 @@
namespace autoware::compare_map_segmentation
{

using PointCloud = typename pcl::Filter<pcl::PointXYZ>::PointCloud;
using PointCloudPtr = typename PointCloud::Ptr;
using PointCloudConstPtr = typename PointCloud::ConstPtr;
using FilteredPointCloud = typename pcl::Filter<pcl::PointXYZ>::PointCloud;
using FilteredPointCloudPtr = typename FilteredPointCloud::Ptr;
using FilteredPointCloudConstPtr = typename FilteredPointCloud::ConstPtr;

class DistanceBasedStaticMapLoader : public VoxelGridStaticMapLoader
{
private:
PointCloudConstPtr map_ptr_;
FilteredPointCloudConstPtr map_ptr_;
pcl::search::Search<pcl::PointXYZ>::Ptr tree_;

public:
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -29,14 +29,14 @@
namespace autoware::compare_map_segmentation
{

using PointCloud = typename pcl::Filter<pcl::PointXYZ>::PointCloud;
using PointCloudPtr = typename PointCloud::Ptr;
using PointCloudConstPtr = typename PointCloud::ConstPtr;
using FilteredPointCloud = typename pcl::Filter<pcl::PointXYZ>::PointCloud;
using FilteredPointCloudPtr = typename FilteredPointCloud::Ptr;
using FilteredPointCloudConstPtr = typename FilteredPointCloud::ConstPtr;

class VoxelDistanceBasedStaticMapLoader : public VoxelGridStaticMapLoader
{
private:
PointCloudConstPtr map_ptr_;
FilteredPointCloudConstPtr map_ptr_;
pcl::search::Search<pcl::PointXYZ>::Ptr tree_;

public:
Expand All @@ -55,7 +55,7 @@ class VoxelDistanceBasedDynamicMapLoader : public VoxelGridDynamicMapLoader
{
protected:
private:
PointCloudConstPtr map_ptr_;
FilteredPointCloudConstPtr map_ptr_;
/* data */
public:
explicit VoxelDistanceBasedDynamicMapLoader(
Expand All @@ -78,7 +78,7 @@ class VoxelDistanceBasedDynamicMapLoader : public VoxelGridDynamicMapLoader
pcl::fromROSMsg(map_cell_to_add.pointcloud, map_cell_pc_tmp);

VoxelGridPointXYZ map_cell_voxel_grid_tmp;
PointCloudPtr map_cell_downsampled_pc_ptr_tmp;
FilteredPointCloudPtr map_cell_downsampled_pc_ptr_tmp;

auto map_cell_voxel_input_tmp_ptr =
std::make_shared<pcl::PointCloud<pcl::PointXYZ>>(map_cell_pc_tmp);
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -63,7 +63,7 @@ bool VoxelGridMapLoader::is_close_to_neighbor_voxels(
}

bool VoxelGridMapLoader::is_close_to_neighbor_voxels(
const pcl::PointXYZ & point, const double distance_threshold, const PointCloudPtr & map,
const pcl::PointXYZ & point, const double distance_threshold, const FilteredPointCloudPtr & map,
VoxelGridPointXYZ & voxel) const
{
// check map downsampled pc
Expand Down Expand Up @@ -228,7 +228,8 @@ bool VoxelGridMapLoader::is_close_to_neighbor_voxels(

bool VoxelGridMapLoader::is_in_voxel(
const pcl::PointXYZ & src_point, const pcl::PointXYZ & target_point,
const double distance_threshold, const PointCloudPtr & map, VoxelGridPointXYZ & voxel) const
const double distance_threshold, const FilteredPointCloudPtr & map,
VoxelGridPointXYZ & voxel) const
{
int voxel_index =
voxel.getCentroidIndexAt(voxel.getGridCoordinates(src_point.x, src_point.y, src_point.z));
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -54,9 +54,9 @@ class VoxelGridEx : public pcl::VoxelGrid<PointT>
using pcl::VoxelGrid<PointT>::div_b_;
using pcl::VoxelGrid<PointT>::inverse_leaf_size_;

using PointCloud = typename pcl::Filter<PointT>::PointCloud;
using PointCloudPtr = typename PointCloud::Ptr;
using PointCloudConstPtr = typename PointCloud::ConstPtr;
using FilteredPointCloud = typename pcl::Filter<PointT>::PointCloud;
using FilteredPointCloudPtr = typename FilteredPointCloud::Ptr;
using FilteredPointCloudConstPtr = typename FilteredPointCloud::ConstPtr;

public:
using pcl::VoxelGrid<PointT>::leaf_layout_;
Expand Down Expand Up @@ -93,8 +93,8 @@ class VoxelGridMapLoader

public:
using VoxelGridPointXYZ = VoxelGridEx<pcl::PointXYZ>;
using PointCloud = typename pcl::Filter<pcl::PointXYZ>::PointCloud;
using PointCloudPtr = typename PointCloud::Ptr;
using FilteredPointCloud = typename pcl::Filter<pcl::PointXYZ>::PointCloud;
using FilteredPointCloudPtr = typename FilteredPointCloud::Ptr;
explicit VoxelGridMapLoader(
rclcpp::Node * node, double leaf_size, double downsize_ratio_z_axis,
std::string * tf_map_input_frame);
Expand All @@ -106,11 +106,12 @@ class VoxelGridMapLoader
const pcl::PointXYZ & point, const double distance_threshold, VoxelGridPointXYZ & voxel,
pcl::search::Search<pcl::PointXYZ>::Ptr tree);
bool is_close_to_neighbor_voxels(
const pcl::PointXYZ & point, const double distance_threshold, const PointCloudPtr & map,
const pcl::PointXYZ & point, const double distance_threshold, const FilteredPointCloudPtr & map,
VoxelGridPointXYZ & voxel) const;
bool is_in_voxel(
const pcl::PointXYZ & src_point, const pcl::PointXYZ & target_point,
const double distance_threshold, const PointCloudPtr & map, VoxelGridPointXYZ & voxel) const;
const double distance_threshold, const FilteredPointCloudPtr & map,
VoxelGridPointXYZ & voxel) const;

void publish_downsampled_map(const pcl::PointCloud<pcl::PointXYZ> & downsampled_pc);
std::string * tf_map_input_frame_;
Expand All @@ -121,7 +122,7 @@ class VoxelGridStaticMapLoader : public VoxelGridMapLoader
protected:
rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr sub_map_;
VoxelGridPointXYZ voxel_grid_;
PointCloudPtr voxel_map_ptr_;
FilteredPointCloudPtr voxel_map_ptr_;
std::atomic_bool is_initialized_{false};

public:
Expand All @@ -138,7 +139,7 @@ class VoxelGridDynamicMapLoader : public VoxelGridMapLoader
struct MapGridVoxelInfo
{
VoxelGridPointXYZ map_cell_voxel_grid;
PointCloudPtr map_cell_pc_ptr;
FilteredPointCloudPtr map_cell_pc_ptr;
float min_b_x, min_b_y, max_b_x, max_b_y;
pcl::search::Search<pcl::PointXYZ>::Ptr map_cell_kdtree;
};
Expand Down Expand Up @@ -291,7 +292,7 @@ class VoxelGridDynamicMapLoader : public VoxelGridMapLoader
pcl::fromROSMsg(map_cell_to_add.pointcloud, map_cell_pc_tmp);

VoxelGridPointXYZ map_cell_voxel_grid_tmp;
PointCloudPtr map_cell_downsampled_pc_ptr_tmp;
FilteredPointCloudPtr map_cell_downsampled_pc_ptr_tmp;

auto map_cell_voxel_input_tmp_ptr =
std::make_shared<pcl::PointCloud<pcl::PointXYZ>>(map_cell_pc_tmp);
Expand Down

0 comments on commit c1d3478

Please sign in to comment.