Skip to content

Commit

Permalink
Rework the state of implementation from VRichardJP's PR
Browse files Browse the repository at this point in the history 
The ring indices were not read correctly from point buffers.
The ring/azimuth/etc. offsets were set to indices of the respective
fields in the input buffer's fields vector.
The missing lookup of the offset in the respective index's field
definition was added.

As discussed in autowarefoundation#3014 the law of cosines approach for calculating
point distances in isCluster ields logically different results
and thus needs more disussion before being merged. The isCluster
implementation in this commit has been reverted to the previous
euclidean distance check.

LiDAR point representation in the ad-hoc WalkInfo struct has been
replaced by a more readable ad-hoc PointXYZAD struct representation.
Once the input data has an enforced, consistent format, existing
point types can be used instead.

Signed-off-by: Maximilian Schmeller <maximilian.schmeller@tier4.jp>
  • Loading branch information
@mojomex
mojomex committed Jul 6, 2023
1 parent de2344f commit 5f765a3
Showing 1 changed file with 47 additions and 43 deletions.
90 changes: 47 additions & 43 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 @@ -76,28 +76,28 @@ void RingOutlierFilterComponent::faster_filter(
RCLCPP_ERROR(get_logger(), "input cloud does not contain 'intensity' data field");
return;
}
const auto intensity_offset = *maybe_intensity_offset;
const auto intensity_offset = input->fields.at(*maybe_intensity_offset).offset;

auto maybe_ring_offset = utils::getRingOffset(*input);
if (!maybe_ring_offset) {
RCLCPP_ERROR(get_logger(), "input cloud does not contain 'ring' data field");
return;
}
const auto ring_offset = *maybe_ring_offset;
const auto ring_offset = input->fields.at(*maybe_ring_offset).offset;

auto maybe_azimuth_offset = utils::getAzimuthOffset(*input);
if (!maybe_azimuth_offset) {
RCLCPP_ERROR(get_logger(), "input cloud does not contain 'azimuth' data field");
return;
}
const auto azimuth_offset = *maybe_azimuth_offset;
const auto azimuth_offset = input->fields.at(*maybe_azimuth_offset).offset;

auto maybe_distance_offset = utils::getDistanceOffset(*input);
if (!maybe_distance_offset) {
RCLCPP_ERROR(get_logger(), "input cloud does not contain 'distance' data field");
return;
}
const auto distance_offset = *maybe_distance_offset;
const auto distance_offset = input->fields.at(*maybe_distance_offset).offset;

// extract intensity from point raw data
auto getPointIntensity = [=](const unsigned char * raw_p) {
Expand Down Expand Up @@ -141,14 +141,22 @@ void RingOutlierFilterComponent::faster_filter(
// order. To do so, all rings are being processing simultaneously instead of separately. The
// overall logic is still the same.

// ad-hoc struct to store finished walks information (for isCluster())
// ad-hoc struct to store needed point data
struct PointXYZAD
{
float x;
float y;
float z;
float azimuth;
float distance;
};

// ad-hoc struct to store finished walks information (for isCluster() and isSameWalk())
struct WalkInfo
{
int num_points;
float first_point_distance;
float first_point_azimuth;
float last_point_distance;
float last_point_azimuth;
PointXYZAD first_point;
PointXYZAD last_point;
};

// ad-hoc struct to keep track of each ring current walk
Expand All @@ -161,22 +169,18 @@ void RingOutlierFilterComponent::faster_filter(

// helper functions

// check if walk is a valid cluster
const float object_length_threshold2 = object_length_threshold_ * object_length_threshold_;
// check if walk is a valid cluster (= either a sufficient number of points or a sufficient distance)
auto isCluster = [=](const WalkInfo & walk_info) {
// A cluster is a walk which has many points or is long enough
if (walk_info.num_points > num_points_threshold_) return true;

// Using the law of cosines, the distance between 2 points can be written as:
// |p2-p1|^2 = d1^2 + d2^2 - 2*d1*d2*cos(a)
// where d1 and d2 are the distance attribute of p1 and p2, and 'a' the azimuth diff between
// the 2 points.
const float dist2 =
walk_info.first_point_distance * walk_info.first_point_distance +
walk_info.last_point_distance * walk_info.last_point_distance -
2 * walk_info.first_point_distance * walk_info.last_point_distance *
std::cos((walk_info.last_point_azimuth - walk_info.first_point_azimuth) * (M_PI / 18000.0));
return dist2 > object_length_threshold2;
auto & first_point = walk_info.first_point;
auto & last_point = walk_info.last_point;

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 x * x + y * y + z * z >= object_length_threshold_ * object_length_threshold_;
};

// check if 2 points belong to the same walk
Expand Down Expand Up @@ -208,8 +212,11 @@ void RingOutlierFilterComponent::faster_filter(
for (const auto & [raw_p, point_walk_idx] :
ranges::views::zip(input->data | ranges::views::chunk(input->point_step), points_walk_idx)) {
const uint16_t ring_idx = getPointRing(raw_p.data());
const float curr_azimuth = getPointAzimuth(raw_p.data());
const float curr_distance = getPointDistance(raw_p.data());
const PointXYZAD curr_point{*reinterpret_cast<const float *>(raw_p.data()),
*reinterpret_cast<const float *>(raw_p.data() + sizeof(float)),
*reinterpret_cast<const float *>(raw_p.data() + 2 * sizeof(float)),
getPointAzimuth(raw_p.data()),
getPointDistance(raw_p.data())};

//FIXME(mojomex) Not needed for actual operation, remove after troubleshooting
if (ring_idx >= max_rings_num_) {
Expand All @@ -222,38 +229,35 @@ void RingOutlierFilterComponent::faster_filter(
auto & ring = rings_curr_walk[ring_idx];
if (ring.walk_idx == -1U) {
// first walk ever for this ring
walks.push_back(WalkInfo{1, curr_distance, curr_azimuth, curr_distance, curr_azimuth});
walks.push_back(WalkInfo{1, curr_point, curr_point});
const auto walk_idx = walks.size() - 1;
point_walk_idx = walk_idx;
rings_first_walk_idx[ring_idx] = walk_idx;
ring.walk_idx = walk_idx;
ring.prev_azimuth = curr_azimuth;
ring.prev_distance = curr_distance;
continue;
}

auto & walk = walks[ring.walk_idx];
if (isSameWalk(curr_distance, curr_azimuth, ring.prev_distance, ring.prev_azimuth)) {
if (isSameWalk(curr_point.distance, curr_point.azimuth, ring.prev_distance, ring.prev_azimuth)) {
// current point is part of previous walk
walk.num_points += 1;
walk.last_point_distance = curr_distance;
walk.last_point_azimuth = curr_azimuth;
walk.last_point = curr_point;
point_walk_idx = ring.walk_idx;
} else {
// previous walk is finished, start a new one
walks.push_back(WalkInfo{1, curr_distance, curr_azimuth, curr_distance, curr_azimuth});
walks.push_back(WalkInfo{1, curr_point, curr_point});
point_walk_idx = walks.size() - 1;
ring.walk_idx = walks.size() - 1;
}

ring.prev_azimuth = curr_azimuth;
ring.prev_distance = curr_distance;
ring.prev_azimuth = curr_point.azimuth;
ring.prev_distance = curr_point.distance;
}

// So far, we have processed ring points as if rings were not circular. Of course, the last and
// first points of a ring could totally be part of the same walk. When such thing happens, we need
// to merge the two walks
for (const auto & [first_walk_idx, last_walk_idx] : ranges::views::zip(
/**for (const auto & [first_walk_idx, last_walk_idx] : ranges::views::zip(
rings_first_walk_idx, rings_curr_walk | ranges::views::transform([](const auto & ring) {
return ring.walk_idx;
}))) {
Expand All @@ -264,15 +268,15 @@ void RingOutlierFilterComponent::faster_filter(
auto & last_walk = walks[last_walk_idx];
// check if the two walks are connected
if (isSameWalk(
first_walk.first_point_distance, first_walk.first_point_azimuth,
last_walk.last_point_distance, last_walk.last_point_azimuth)) {
first_walk.first_point.distance, first_walk.first_point.azimuth,
last_walk.last_point.distance, last_walk.last_point.azimuth)) {
// merge
first_walk.first_point_distance = last_walk.first_point_distance;
first_walk.first_point_azimuth = last_walk.first_point_azimuth;
last_walk.last_point_distance = first_walk.last_point_distance;
last_walk.last_point_azimuth = first_walk.last_point_azimuth;
first_walk.first_point.distance = last_walk.first_point.distance;
first_walk.first_point.azimuth = last_walk.first_point.azimuth;
last_walk.last_point.distance = first_walk.last_point.distance;
last_walk.last_point.azimuth = first_walk.last_point.azimuth;
}
}
}*/

// segregate walks by cluster (for fast lookup)
std::vector<bool> walk_is_cluster;
Expand Down Expand Up @@ -349,8 +353,8 @@ void RingOutlierFilterComponent::faster_filter(

if (invalid_ring_count > 0) {
RCLCPP_WARN(
get_logger(), "%d points had ring index over max_rings_num (%d) and have been ignored.",
invalid_ring_count, max_rings_num_);
get_logger(), "%d of %d total points had ring index over max_rings_num (%d) and have been ignored.",
invalid_ring_count, input->width * input->height, max_rings_num_);
}

// add processing time for debug
Expand Down

0 comments on commit 5f765a3

Please sign in to comment.