Skip to content

Commit

Permalink
style(pre-commit): autofix
Browse files Browse the repository at this point in the history
  • Loading branch information
@pre-commit-ci
pre-commit-ci[bot] committed Mar 6, 2023
1 parent db2d220 commit cb103ec
Show file tree
Hide file tree
Showing 3 changed files with 41 additions and 28 deletions.
18 changes: 11 additions & 7 deletions ...eprocessor/include/pointcloud_preprocessor/outlier_filter/ring_outlier_filter_nodelet.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -33,10 +33,11 @@ class RingOutlierFilterComponent : public pointcloud_preprocessor::Filter
virtual void filter(
const PointCloud2ConstPtr & input, const IndicesPtr & indices, PointCloud2 & output);

// TODO(sykwer): Temporary Implementation: Remove this interface when all the filter nodes conform to new API
// TODO(sykwer): Temporary Implementation: Remove this interface when all the filter nodes conform
// to new API
virtual void faster_filter(
const PointCloud2ConstPtr & input, const IndicesPtr & indices, PointCloud2 & output,
const TransformInfo & transform_info);
const PointCloud2ConstPtr & input, const IndicesPtr & indices, PointCloud2 & output,
const TransformInfo & transform_info);

private:
double distance_ratio_;
Expand All @@ -49,13 +50,16 @@ class RingOutlierFilterComponent : public pointcloud_preprocessor::Filter
/** \brief Parameter service callback */
rcl_interfaces::msg::SetParametersResult paramCallback(const std::vector<rclcpp::Parameter> & p);

bool isCluster(const PointCloud2ConstPtr & input, int first_data_idx, int last_data_idx, int walk_size) {
auto first_point = reinterpret_cast<const PointXYZI*>(&input->data[first_data_idx]);
auto last_point = reinterpret_cast<const PointXYZI*>(&input->data[last_data_idx]);
bool isCluster(
const PointCloud2ConstPtr & input, int first_data_idx, int last_data_idx, int walk_size)
{
auto first_point = reinterpret_cast<const PointXYZI *>(&input->data[first_data_idx]);
auto last_point = reinterpret_cast<const PointXYZI *>(&input->data[last_data_idx]);
const auto x = first_point->x - last_point->x;
const auto y = first_point->y - last_point->y;
const auto z = first_point->z - last_point->z;
return (walk_size > num_points_threshold_) || (x*x + y*y + z*z >= object_length_threshold_ * object_length_threshold_);
return (walk_size > num_points_threshold_) ||
(x * x + y * y + z * z >= object_length_threshold_ * object_length_threshold_);
}

public:
Expand Down
5 changes: 3 additions & 2 deletions sensing/pointcloud_preprocessor/src/filter.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -125,8 +125,9 @@ void pointcloud_preprocessor::Filter::subscribe(const std::string & filter_name)
// each time a child class supports the faster version.
// When all the child classes support the faster version, this workaround is deleted.
std::set<std::string> supported_nodes = {"CropBoxFilter", "RingOutlierFilter"};
auto callback = supported_nodes.find(filter_name) != supported_nodes.end() ? &Filter::faster_input_indices_callback
: &Filter::input_indices_callback;
auto callback = supported_nodes.find(filter_name) != supported_nodes.end()
? &Filter::faster_input_indices_callback
: &Filter::input_indices_callback;

if (use_indices_) {
// Subscribe to the input using a filter
Expand Down
46 changes: 27 additions & 19 deletions sensing/pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_nodelet.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -71,11 +71,11 @@ void RingOutlierFilterComponent::faster_filter(

for (size_t i = 0; i < 128; i++) {
ring2indices.push_back(std::vector<size_t>());
ring2indices.back().reserve(2000); // TODO(sykwer): Make it parameter
ring2indices.back().reserve(2000); // TODO(sykwer): Make it parameter
}

for (size_t data_idx = 0; data_idx < input->data.size(); data_idx += input->point_step) {
const uint16_t ring = *reinterpret_cast<const uint16_t*>(&input->data[data_idx + ring_offset]);
const uint16_t ring = *reinterpret_cast<const uint16_t *>(&input->data[data_idx + ring_offset]);
ring2indices[ring].push_back(data_idx);
}

Expand All @@ -90,33 +90,37 @@ void RingOutlierFilterComponent::faster_filter(
walk_last_idx = -1;

for (size_t idx = 0U; idx < indices.size() - 1; ++idx) {
const size_t &current_data_idx = indices[idx];
const size_t &next_data_idx = indices[idx + 1];
const size_t & current_data_idx = indices[idx];
const size_t & next_data_idx = indices[idx + 1];
walk_last_idx = idx;

// if(std::abs(iter->distance - (iter+1)->distance) <= std::sqrt(iter->distance) * 0.08)

const float &current_azimuth =
const float & current_azimuth =
*reinterpret_cast<const float *>(&input->data[current_data_idx + azimuth_offset]);
const float &next_azimuth =
const float & next_azimuth =
*reinterpret_cast<const float *>(&input->data[next_data_idx + azimuth_offset]);
float azimuth_diff = next_azimuth - current_azimuth;
azimuth_diff = azimuth_diff < 0.f ? azimuth_diff + 36000.f : azimuth_diff;

const float &current_distance =
const float & current_distance =
*reinterpret_cast<const float *>(&input->data[current_data_idx + distance_offset]);
const float &next_distance =
const float & next_distance =
*reinterpret_cast<const float *>(&input->data[next_data_idx + distance_offset]);

if (std::max(current_distance, next_distance) <
std::min(current_distance, next_distance) * distance_ratio_ && azimuth_diff < 100.f) {
continue; // Determined to be included in the same walk
if (
std::max(current_distance, next_distance) <
std::min(current_distance, next_distance) * distance_ratio_ &&
azimuth_diff < 100.f) {
continue; // Determined to be included in the same walk
}

if (isCluster(input, indices[walk_first_idx], indices[walk_last_idx], walk_last_idx - walk_first_idx + 1)) {
if (isCluster(
input, indices[walk_first_idx], indices[walk_last_idx],
walk_last_idx - walk_first_idx + 1)) {
for (int i = walk_first_idx; i <= walk_last_idx; i++) {
auto output_ptr = reinterpret_cast<PointXYZI*>(&output.data[output_size]);
*output_ptr = *reinterpret_cast<const PointXYZI*>(&input->data[indices[i]]);
auto output_ptr = reinterpret_cast<PointXYZI *>(&output.data[output_size]);
*output_ptr = *reinterpret_cast<const PointXYZI *>(&input->data[indices[i]]);
output_size += output.point_step;
}
}
Expand All @@ -126,18 +130,21 @@ void RingOutlierFilterComponent::faster_filter(

if (walk_first_idx > walk_last_idx) continue;

if (isCluster(input, indices[walk_first_idx], indices[walk_last_idx], walk_last_idx - walk_first_idx + 1)) {
if (isCluster(
input, indices[walk_first_idx], indices[walk_last_idx],
walk_last_idx - walk_first_idx + 1)) {
for (int i = walk_first_idx; i <= walk_last_idx; i++) {
auto output_ptr = reinterpret_cast<PointXYZI*>(&output.data[output_size]);
*output_ptr = *reinterpret_cast<const PointXYZI*>(&input->data[indices[i]]);
auto output_ptr = reinterpret_cast<PointXYZI *>(&output.data[output_size]);
*output_ptr = *reinterpret_cast<const PointXYZI *>(&input->data[indices[i]]);
output_size += output.point_step;
}
}
}

output.data.resize(output_size);

// Note that `input->header.frame_id` is data before converted when `transform_info.need_transform == true`
// Note that `input->header.frame_id` is data before converted when `transform_info.need_transform
// == true`
output.header.frame_id = !tf_input_frame_.empty() ? tf_input_frame_ : tf_input_orig_frame_;

output.height = 1;
Expand All @@ -158,7 +165,8 @@ void RingOutlierFilterComponent::faster_filter(
}
}

// TODO(sykwer): Temporary Implementation: Delete this function definition when all the filter nodes conform to new API
// TODO(sykwer): Temporary Implementation: Delete this function definition when all the filter nodes
// conform to new API
void RingOutlierFilterComponent::filter(
const PointCloud2ConstPtr & input, [[maybe_unused]] const IndicesPtr & indices,
PointCloud2 & output)
Expand Down

0 comments on commit cb103ec

Please sign in to comment.