Skip to content

Commit

Permalink
fix: using empty padding field
Browse files Browse the repository at this point in the history 
Signed-off-by: badai-nguyen <[email protected]>
  • Loading branch information
@badai-nguyen
badai-nguyen committed Apr 12, 2024
1 parent d7bd69f commit eeab1b2
Show file tree
Hide file tree
Showing 2 changed files with 188 additions and 60 deletions.
17 changes: 10 additions & 7 deletions ...r/include/occupancy_grid_map_outlier_filter/occupancy_grid_map_outlier_filter_nodelet.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -56,8 +56,8 @@ class RadiusSearch2dFilter
const PclPointCloud & input, const Pose & pose, PclPointCloud & output,
PclPointCloud & outlier);
void filter(
const PclPointCloud & high_conf_input, const PclPointCloud & low_conf_input, const Pose & pose,
PclPointCloud & output, PclPointCloud & outlier);
const PointCloud2 & high_conf_input, const PointCloud2 & low_conf_input, const Pose & pose,
PointCloud2 & output, PointCloud2 & outlier);

private:
float search_radius_;
Expand All @@ -79,22 +79,25 @@ class OccupancyGridMapOutlierFilterComponent : public rclcpp::Node
const PointCloud2::ConstSharedPtr & input_pointcloud);
void filterByOccupancyGridMap(
const OccupancyGrid & occupancy_grid_map, const PointCloud2 & pointcloud,
PclPointCloud & high_confidence, PclPointCloud & low_confidence, PclPointCloud & out_ogm);
PointCloud2 & high_confidence, PointCloud2 & low_confidence, PointCloud2 & out_ogm);
void splitPointCloudFrontBack(
const PointCloud2::ConstSharedPtr & input_pc, PointCloud2 & front_pc, PointCloud2 & behind_pc);
void initializerPointCloud2(const PointCloud2 & input, PointCloud2 & output);
void finalizePointCloud2(const PointCloud2 & input, PointCloud2 & output);
void concatPointCloud2(PointCloud2 & output, const PointCloud2 & input);

private:
class Debugger
{
public:
explicit Debugger(OccupancyGridMapOutlierFilterComponent & node);
void publishOutlier(const PclPointCloud & input, const Header & header);
void publishHighConfidence(const PclPointCloud & input, const Header & header);
void publishLowConfidence(const PclPointCloud & input, const Header & header);
void publishOutlier(const PointCloud2 & input, const Header & header);
void publishHighConfidence(const PointCloud2 & input, const Header & header);
void publishLowConfidence(const PointCloud2 & input, const Header & header);

private:
void transformToBaseLink(
const PclPointCloud & input, const Header & header, PointCloud2 & output);
const PointCloud2 & input, const Header & header, PointCloud2 & output);
rclcpp::Publisher<PointCloud2>::SharedPtr outlier_pointcloud_pub_;
rclcpp::Publisher<PointCloud2>::SharedPtr low_confidence_pointcloud_pub_;
rclcpp::Publisher<PointCloud2>::SharedPtr high_confidence_pointcloud_pub_;
Expand Down
231 changes: 178 additions & 53 deletions ...ption/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -147,34 +147,45 @@ void RadiusSearch2dFilter::filter(
}

void RadiusSearch2dFilter::filter(
const PclPointCloud & high_conf_input, const PclPointCloud & low_conf_input, const Pose & pose,
PclPointCloud & output, PclPointCloud & outlier)
const PointCloud2 & high_conf_xyz_cloud, const PointCloud2 & low_conf_xyz_cloud,
const Pose & pose, PointCloud2 & output, PointCloud2 & outlier)
{
const auto & high_conf_xyz_cloud = high_conf_input;
const auto & low_conf_xyz_cloud = low_conf_input;
// check the limit points number
if (low_conf_xyz_cloud.points.size() > max_filter_points_nb_) {
if (low_conf_xyz_cloud.width > max_filter_points_nb_) {
RCLCPP_WARN(
rclcpp::get_logger("OccupancyGridMapOutlierFilterComponent"),
"Skip outlier filter since too much low_confidence pointcloud!");
return;
}

int x_offset = low_conf_xyz_cloud.fields[pcl::getFieldIndex(low_conf_xyz_cloud, "x")].offset;
int y_offset = low_conf_xyz_cloud.fields[pcl::getFieldIndex(low_conf_xyz_cloud, "y")].offset;
int point_step = low_conf_xyz_cloud.point_step;
pcl::PointCloud<pcl::PointXY>::Ptr xy_cloud(new pcl::PointCloud<pcl::PointXY>);
xy_cloud->points.resize(low_conf_xyz_cloud.points.size() + high_conf_xyz_cloud.points.size());
for (size_t i = 0; i < low_conf_xyz_cloud.points.size(); ++i) {
xy_cloud->points[i].x = low_conf_xyz_cloud.points[i].x;
xy_cloud->points[i].y = low_conf_xyz_cloud.points[i].y;
xy_cloud->points.resize(low_conf_xyz_cloud.width + high_conf_xyz_cloud.width);
for (size_t i = 0; i < low_conf_xyz_cloud.width; ++i) {
std::memcpy(
&xy_cloud->points[i].x, &low_conf_xyz_cloud.data[i * point_step + x_offset], sizeof(float));
std::memcpy(
&xy_cloud->points[i].y, &low_conf_xyz_cloud.data[i * point_step + y_offset], sizeof(float));
}
for (size_t i = low_conf_xyz_cloud.points.size(); i < xy_cloud->points.size(); ++i) {
xy_cloud->points[i].x = high_conf_xyz_cloud.points[i - low_conf_xyz_cloud.points.size()].x;
xy_cloud->points[i].y = high_conf_xyz_cloud.points[i - low_conf_xyz_cloud.points.size()].y;

for (size_t i = low_conf_xyz_cloud.width; i < xy_cloud->points.size(); ++i) {
size_t high_conf_xyz_cloud_index = i - low_conf_xyz_cloud.width;
std::memcpy(
&xy_cloud->points[i].x,
&high_conf_xyz_cloud.data[high_conf_xyz_cloud_index * point_step + x_offset], sizeof(float));
std::memcpy(
&xy_cloud->points[i].y,
&high_conf_xyz_cloud.data[high_conf_xyz_cloud_index * point_step + y_offset], sizeof(float));
}

std::vector<int> k_indices(xy_cloud->points.size());
std::vector<float> k_distances(xy_cloud->points.size());
kd_tree_->setInputCloud(xy_cloud);
for (size_t i = 0; i < low_conf_xyz_cloud.points.size(); ++i) {

size_t output_size = 0;
size_t outlier_size = 0;
for (size_t i = 0; i < low_conf_xyz_cloud.data.size() / low_conf_xyz_cloud.point_step; ++i) {
const float distance =
std::hypot(xy_cloud->points[i].x - pose.position.x, xy_cloud->points[i].y - pose.position.y);
const int min_points_threshold = std::min(
Expand All @@ -184,11 +195,20 @@ void RadiusSearch2dFilter::filter(
kd_tree_->radiusSearch(i, search_radius_, k_indices, k_distances, min_points_threshold);

if (min_points_threshold <= points_num) {
output.points.push_back(low_conf_xyz_cloud.points.at(i));
std::memcpy(
&output.data[output_size], &low_conf_xyz_cloud.data[i * low_conf_xyz_cloud.point_step],
low_conf_xyz_cloud.point_step);
output_size += low_conf_xyz_cloud.point_step;
} else {
outlier.points.push_back(low_conf_xyz_cloud.points.at(i));
std::memcpy(
&outlier.data[outlier_size], &low_conf_xyz_cloud.data[i * low_conf_xyz_cloud.point_step],
low_conf_xyz_cloud.point_step);
outlier_size += low_conf_xyz_cloud.point_step;
}
}

output.data.resize(output_size);
outlier.data.resize(outlier_size);
}

OccupancyGridMapOutlierFilterComponent::OccupancyGridMapOutlierFilterComponent(
Expand Down Expand Up @@ -239,21 +259,50 @@ OccupancyGridMapOutlierFilterComponent::OccupancyGridMapOutlierFilterComponent(
void OccupancyGridMapOutlierFilterComponent::splitPointCloudFrontBack(
const PointCloud2::ConstSharedPtr & input_pc, PointCloud2 & front_pc, PointCloud2 & behind_pc)
{
PclPointCloud tmp_behind_pc;
PclPointCloud tmp_front_pc;
for (sensor_msgs::PointCloud2ConstIterator<float> x(*input_pc, "x"), y(*input_pc, "y"),
z(*input_pc, "z"), intensity(*input_pc, "intensity");
x != x.end(); ++x, ++y, ++z, ++intensity) {
if (*x < 0.0) {
tmp_behind_pc.push_back(pcl::PointXYZI(*x, *y, *z, *intensity));
int x_offset = input_pc->fields[pcl::getFieldIndex(*input_pc, "x")].offset;
int point_step = input_pc->point_step;

front_pc.data.resize(input_pc->data.size());
behind_pc.data.resize(input_pc->data.size());
front_pc.point_step = input_pc->point_step;
behind_pc.point_step = input_pc->point_step;
front_pc.fields = input_pc->fields;
behind_pc.fields = input_pc->fields;
front_pc.height = input_pc->height;
behind_pc.height = input_pc->height;

size_t front_count = 0;
size_t behind_count = 0;

for (size_t global_offset = 0; global_offset < input_pc->data.size();
global_offset += point_step) {
float x;
std::memcpy(&x, &input_pc->data[global_offset + x_offset], sizeof(float));
if (x < 0.0) {
std::memcpy(
&behind_pc.data[behind_count * point_step], &input_pc->data[global_offset],
input_pc->point_step);
behind_count++;
} else {
tmp_front_pc.push_back(pcl::PointXYZI(*x, *y, *z, *intensity));
std::memcpy(
&front_pc.data[front_count * point_step], &input_pc->data[global_offset],
input_pc->point_step);
front_count++;
}
}
pcl::toROSMsg(tmp_front_pc, front_pc);
pcl::toROSMsg(tmp_behind_pc, behind_pc);
front_pc.data.resize(front_count * point_step);
front_pc.width = front_count;
front_pc.row_step = front_count * front_pc.point_step;
front_pc.header = input_pc->header;
front_pc.is_bigendian = input_pc->is_bigendian;
front_pc.is_dense = input_pc->is_dense;

behind_pc.data.resize(behind_count * point_step);
behind_pc.width = behind_count;
behind_pc.row_step = behind_count * behind_pc.point_step;
behind_pc.header = input_pc->header;
behind_pc.is_bigendian = input_pc->is_bigendian;
behind_pc.is_dense = input_pc->is_dense;
}
void OccupancyGridMapOutlierFilterComponent::onOccupancyGridMapAndPointCloud2(
const OccupancyGrid::ConstSharedPtr & input_ogm, const PointCloud2::ConstSharedPtr & input_pc)
Expand All @@ -264,6 +313,10 @@ void OccupancyGridMapOutlierFilterComponent::onOccupancyGridMapAndPointCloud2(
PointCloud2 input_front_pc{};
PointCloud2 input_behind_pc{};
PointCloud2 ogm_frame_input_behind_pc{};
PointCloud2 ogm_frame_pc;
PointCloud2 input_front_pc;
PointCloud2 input_behind_pc;
PointCloud2 ogm_frame_input_behind_pc;
splitPointCloudFrontBack(input_pc, input_front_pc, input_behind_pc);
if (
!transformPointcloud(input_front_pc, *tf2_, input_ogm->header.frame_id, ogm_frame_pc) ||
Expand All @@ -272,33 +325,50 @@ void OccupancyGridMapOutlierFilterComponent::onOccupancyGridMapAndPointCloud2(
return;
}
// Occupancy grid map based filter
PclPointCloud high_confidence_pc{};
PclPointCloud low_confidence_pc{};
PclPointCloud out_ogm_pc{};
PclPointCloud ogm_frame_behind_pc;
pcl::fromROSMsg(ogm_frame_input_behind_pc, ogm_frame_behind_pc);
PointCloud2 high_confidence_pc{};
PointCloud2 low_confidence_pc{};
PointCloud2 out_ogm_pc{};
initializerPointCloud2(ogm_frame_pc, high_confidence_pc);
initializerPointCloud2(ogm_frame_pc, low_confidence_pc);
initializerPointCloud2(ogm_frame_pc, out_ogm_pc);
// PclPointCloud ogm_frame_behind_pc;
// pcl::fromROSMsg(ogm_frame_input_behind_pc, ogm_frame_behind_pc);
filterByOccupancyGridMap(
*input_ogm, ogm_frame_pc, high_confidence_pc, low_confidence_pc, out_ogm_pc);
// Apply Radius search 2d filter for low confidence pointcloud
PclPointCloud filtered_low_confidence_pc{};
PclPointCloud outlier_pc{};
PointCloud2 filtered_low_confidence_pc{};
PointCloud2 outlier_pc{};
initializerPointCloud2(low_confidence_pc, outlier_pc);
initializerPointCloud2(low_confidence_pc, filtered_low_confidence_pc);
PointCloud2 outlier_pc;
outlier_pc.data.resize(low_confidence_pc.data.size());
outlier_pc.point_step = low_confidence_pc.point_step;
outlier_pc.fields = low_confidence_pc.fields;
outlier_pc.height = 1;
if (radius_search_2d_filter_ptr_) {
auto pc_frame_pose_stamped = getPoseStamped(
*tf2_, input_ogm->header.frame_id, input_pc->header.frame_id, input_ogm->header.stamp);
radius_search_2d_filter_ptr_->filter(
high_confidence_pc, low_confidence_pc, pc_frame_pose_stamped.pose, filtered_low_confidence_pc,
outlier_pc);
} else {
outlier_pc = low_confidence_pc;
std::memcpy(&outlier_pc.data[0], &low_confidence_pc.data[0], low_confidence_pc.data.size());
outlier_pc.data.resize(low_confidence_pc.data.size());
}
// Concatenate high confidence pointcloud from occupancy grid map and non-outlier pointcloud
PclPointCloud concat_pc =
high_confidence_pc + filtered_low_confidence_pc + out_ogm_pc + ogm_frame_behind_pc;
PointCloud2 ogm_frame_filtered_pc{};
concatPointCloud2(ogm_frame_filtered_pc, high_confidence_pc);
concatPointCloud2(ogm_frame_filtered_pc, filtered_low_confidence_pc);
concatPointCloud2(ogm_frame_filtered_pc, out_ogm_pc);
concatPointCloud2(ogm_frame_filtered_pc, ogm_frame_input_behind_pc);

finalizePointCloud2(ogm_frame_pc, ogm_frame_filtered_pc);
finalizePointCloud2(ogm_frame_pc, filtered_low_confidence_pc);
finalizePointCloud2(ogm_frame_pc, outlier_pc);
finalizePointCloud2(ogm_frame_pc, high_confidence_pc);
// Convert to ros msg
{
PointCloud2 ogm_frame_filtered_pc{};
auto base_link_frame_filtered_pc_ptr = std::make_unique<PointCloud2>();
pcl::toROSMsg(concat_pc, ogm_frame_filtered_pc);
ogm_frame_filtered_pc.header = ogm_frame_pc.header;
if (!transformPointcloud(
ogm_frame_filtered_pc, *tf2_, base_link_frame_, *base_link_frame_filtered_pc_ptr)) {
Expand All @@ -323,24 +393,82 @@ void OccupancyGridMapOutlierFilterComponent::onOccupancyGridMapAndPointCloud2(
}
}

void OccupancyGridMapOutlierFilterComponent::initializerPointCloud2(
const PointCloud2 & input, PointCloud2 & output)
{
output.header = input.header;
output.point_step = input.point_step;
output.fields = input.fields;
output.height = input.height;
output.is_bigendian = input.is_bigendian;
output.is_dense = input.is_dense;
output.data.resize(input.data.size());
}

void OccupancyGridMapOutlierFilterComponent::finalizePointCloud2(
const PointCloud2 & input, PointCloud2 & output)
{
output.header = input.header;
output.point_step = input.point_step;
output.fields = input.fields;
output.height = input.height;
output.is_bigendian = input.is_bigendian;
output.is_dense = input.is_dense;
output.width = output.data.size() / output.point_step / output.height;
output.row_step = output.data.size() / output.height;
}

void OccupancyGridMapOutlierFilterComponent::concatPointCloud2(
PointCloud2 & output, const PointCloud2 & input)
{
size_t output_size = output.data.size();
output.data.resize(output.data.size() + input.data.size());
std::memcpy(&output.data[output_size], &input.data[0], input.data.size());
}
void OccupancyGridMapOutlierFilterComponent::filterByOccupancyGridMap(
const OccupancyGrid & occupancy_grid_map, const PointCloud2 & pointcloud,
PclPointCloud & high_confidence, PclPointCloud & low_confidence, PclPointCloud & out_ogm)
PointCloud2 & high_confidence, PointCloud2 & low_confidence, PointCloud2 & out_ogm)
{
for (sensor_msgs::PointCloud2ConstIterator<float> x(pointcloud, "x"), y(pointcloud, "y"),
z(pointcloud, "z"), intensity(pointcloud, "intensity");
x != x.end(); ++x, ++y, ++z, ++intensity) {
const auto cost = getCost(occupancy_grid_map, *x, *y);
int x_offset = pointcloud.fields[pcl::getFieldIndex(pointcloud, "x")].offset;
int y_offset = pointcloud.fields[pcl::getFieldIndex(pointcloud, "y")].offset;
// int z_offset = pointcloud.fields[pcl::getFieldIndex(pointcloud, "z")].offset;
// int intensity_offset = pointcloud.fields[pcl::getFieldIndex(pointcloud, "intensity")].offset;
size_t high_confidence_size = 0;
size_t low_confidence_size = 0;
size_t out_ogm_size = 0;

for (size_t global_offset = 0; global_offset < pointcloud.data.size();
global_offset += pointcloud.point_step) {
float x;
float y;
std::memcpy(&x, &pointcloud.data[global_offset + x_offset], sizeof(float));
std::memcpy(&y, &pointcloud.data[global_offset + y_offset], sizeof(float));

const auto cost = getCost(occupancy_grid_map, x, y);
if (cost) {
if (cost_threshold_ < *cost) {
high_confidence.push_back(pcl::PointXYZI(*x, *y, *z, *intensity));
std::memcpy(
&high_confidence.data[high_confidence_size], &pointcloud.data[global_offset],
pointcloud.point_step);
high_confidence_size += pointcloud.point_step;
} else {
low_confidence.push_back(pcl::PointXYZI(*x, *y, *z, *intensity));
std::memcpy(
&low_confidence.data[low_confidence_size], &pointcloud.data[global_offset],
pointcloud.point_step);
low_confidence_size += pointcloud.point_step;
}
} else {
out_ogm.push_back(pcl::PointXYZI(*x, *y, *z, *intensity));
std::memcpy(
&out_ogm.data[out_ogm_size], &pointcloud.data[global_offset], pointcloud.point_step);
out_ogm_size += pointcloud.point_step;
}
}
high_confidence.data.resize(high_confidence_size);
low_confidence.data.resize(low_confidence_size);
out_ogm.data.resize(out_ogm_size);
finalizePointCloud2(pointcloud, high_confidence);
finalizePointCloud2(pointcloud, low_confidence);
finalizePointCloud2(pointcloud, out_ogm);
}

OccupancyGridMapOutlierFilterComponent::Debugger::Debugger(
Expand All @@ -356,34 +484,31 @@ OccupancyGridMapOutlierFilterComponent::Debugger::Debugger(
}

void OccupancyGridMapOutlierFilterComponent::Debugger::publishOutlier(
const PclPointCloud & input, const Header & header)
const PointCloud2 & input, const Header & header)
{
auto output_ptr = std::make_unique<PointCloud2>();
transformToBaseLink(input, header, *output_ptr);
outlier_pointcloud_pub_->publish(std::move(output_ptr));
}
void OccupancyGridMapOutlierFilterComponent::Debugger::publishHighConfidence(
const PclPointCloud & input, const Header & header)
const PointCloud2 & input, const Header & header)
{
auto output_ptr = std::make_unique<PointCloud2>();
transformToBaseLink(input, header, *output_ptr);
high_confidence_pointcloud_pub_->publish(std::move(output_ptr));
}

void OccupancyGridMapOutlierFilterComponent::Debugger::publishLowConfidence(
const PclPointCloud & input, const Header & header)
const PointCloud2 & input, const Header & header)
{
auto output_ptr = std::make_unique<PointCloud2>();
transformToBaseLink(input, header, *output_ptr);
low_confidence_pointcloud_pub_->publish(std::move(output_ptr));
}

void OccupancyGridMapOutlierFilterComponent::Debugger::transformToBaseLink(
const PclPointCloud & pcl_input, const Header & header, PointCloud2 & output)
const PointCloud2 & ros_input, [[maybe_unused]] const Header & header, PointCloud2 & output)
{
PointCloud2 ros_input{};
pcl::toROSMsg(pcl_input, ros_input);
ros_input.header = header;
transformPointcloud(ros_input, *(node_.tf2_), node_.base_link_frame_, output);
}

Expand Down

0 comments on commit eeab1b2

Please sign in to comment.