Skip to content

Commit

Permalink
feat(radar_fusion_to_detected_object): add yaw gate (autowarefoundati…
Browse files Browse the repository at this point in the history 
…on#1271)

* feat(radar_fusion_to_detected_object): add yaw gate

Signed-off-by: scepter914 <scepter914@gmail.com>

* fix setting paraper

Signed-off-by: scepter914 <scepter914@gmail.com>

* enable config file

Signed-off-by: scepter914 <scepter914@gmail.com>

* update README

Signed-off-by: scepter914 <scepter914@gmail.com>

* ci(pre-commit): autofix

Signed-off-by: scepter914 <scepter914@gmail.com>

* fix bug

Signed-off-by: scepter914 <scepter914@gmail.com>

* add const

Signed-off-by: scepter914 <scepter914@gmail.com>

* ci(pre-commit): autofix

Signed-off-by: scepter914 <scepter914@gmail.com>

* Update perception/radar_fusion_to_detected_object/src/radar_fusion_to_detected_object.cpp

Co-authored-by: Yukihiro Saito <yukky.saito@gmail.com>
Signed-off-by: scepter914 <scepter914@gmail.com>

* ci(pre-commit): autofix

Signed-off-by: scepter914 <scepter914@gmail.com>

Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
Co-authored-by: Yukihiro Saito <yukky.saito@gmail.com>
  • Loading branch information
@scepter914 @pre-commit-ci @yukkysaito
3 people authored and yukke42 committed Oct 14, 2022
1 parent c8c1d76 commit 8a61084
Show file tree
Hide file tree
Showing 6 changed files with 38 additions and 9 deletions.
9 changes: 5 additions & 4 deletions perception/radar_fusion_to_detected_object/README.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -13,10 +13,11 @@ The document of core algorithm is [here](docs/algorithm.md)

### Parameters for sensor fusion

| Name | Type | Description | Default value |
| :----------------------- | :----- | :------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :------------ |
| bounding_box_margin | double | The distance to extend the 2D bird's-eye view Bounding Box on each side. This distance is used as a threshold to find radar centroids falling inside the extended box. [m] | 2.0 |
| split_threshold_velocity | double | The object's velocity threshold to decide to split for two objects from radar information (currently not implemented) [m/s] | 5.0 |
| Name | Type | Description | Default value |
| :----------------------- | :----- | :------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :------------ |
| bounding_box_margin | double | The distance to extend the 2D bird's-eye view Bounding Box on each side. This distance is used as a threshold to find radar centroids falling inside the extended box. [m] | 2.0 |
| split_threshold_velocity | double | The object's velocity threshold to decide to split for two objects from radar information (currently not implemented) [m/s] | 5.0 |
| threshold_yaw_diff | double | The yaw orientation threshold. If $ \vert \theta _{ob} - \theta_ {ra} \vert < threshold*yaw_diff $ attached to radar information include estimated velocity, where $ \theta*{ob} $ is yaw angle from 3d detected object, $ \theta\_ {ra} $ is yaw angle from radar object. [rad] | 0.35 |

### Weight parameters for velocity estimation

Expand Down
1 change: 1 addition & 0 deletions .../radar_fusion_to_detected_object/config/radar_object_fusion_to_detected_object.param.yaml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -6,6 +6,7 @@
core_params:
bounding_box_margin: 2.0
split_threshold_velocity: 5.0
threshold_yaw_diff: 0.35
velocity_weight_average: 0.0
velocity_weight_median: 0.0
velocity_weight_min_distance: 1.0
Expand Down
2 changes: 2 additions & 0 deletions perception/radar_fusion_to_detected_object/include/radar_fusion_to_detected_object.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -48,6 +48,7 @@ class RadarFusionToDetectedObject
// Radar fusion param
double bounding_box_margin{};
double split_threshold_velocity{};
double threshold_yaw_diff{};

// Weight param for velocity estimation
double velocity_weight_average{};
Expand Down Expand Up @@ -97,6 +98,7 @@ class RadarFusionToDetectedObject
const DetectedObject & object, std::shared_ptr<std::vector<RadarInput>> & radars);
TwistWithCovariance convertDopplerToTwist(
const DetectedObject & object, const TwistWithCovariance & twist_with_covariance);
bool isYawCorrect(const double & yaw, const double & yaw_threshold);
Twist addTwist(const Twist & twist_1, const Twist & twist_2);
Twist scaleTwist(const Twist & twist, const double scale);
double getTwistNorm(const Twist & twist);
Expand Down
2 changes: 1 addition & 1 deletion .../radar_fusion_to_detected_object/launch/radar_object_fusion_to_detected_object.launch.xml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -12,6 +12,6 @@
<remap from="~/input/objects" to="$(var input/objects)"/>
<remap from="~/input/radars" to="$(var input/radars)"/>
<remap from="~/output/objects" to="$(var output/objects)"/>
<!--<param from="$(var config_file)" /> -->
<param from="$(var config_file)"/>
</node>
</launch>
30 changes: 26 additions & 4 deletions perception/radar_fusion_to_detected_object/src/radar_fusion_to_detected_object.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -40,6 +40,7 @@ void RadarFusionToDetectedObject::setParam(const Param & param)
// Radar fusion param
param_.bounding_box_margin = param.bounding_box_margin;
param_.split_threshold_velocity = param.split_threshold_velocity;
param_.threshold_yaw_diff = param.split_threshold_velocity;

// normalize weight param
double sum_weight = param.velocity_weight_median + param.velocity_weight_min_distance +
Expand Down Expand Up @@ -97,12 +98,19 @@ RadarFusionToDetectedObject::Output RadarFusionToDetectedObject::update(
// If object is split, then filter radar again
radars_within_split_object = filterRadarWithinObject(object, radars_within_object);
}

// Estimate twist of object
if (!radars_within_split_object || !(*radars_within_split_object).empty()) {
split_object.kinematics.has_twist = true;
split_object.kinematics.twist_with_covariance =
TwistWithCovariance twist_with_covariance =
estimateTwist(split_object, radars_within_split_object);
const double twist_yaw = tier4_autoware_utils::normalizeRadian(
std::atan2(twist_with_covariance.twist.linear.y, twist_with_covariance.twist.linear.x));
const double object_yaw = tier4_autoware_utils::normalizeRadian(
tf2::getYaw(split_object.kinematics.pose_with_covariance.pose.orientation));
const double diff_yaw = tier4_autoware_utils::normalizeRadian(twist_yaw - object_yaw);
if (isYawCorrect(diff_yaw, param_.threshold_yaw_diff)) {
split_object.kinematics.twist_with_covariance = twist_with_covariance;
split_object.kinematics.has_twist = true;
}
}

// Delete objects with low probability
Expand All @@ -116,6 +124,20 @@ RadarFusionToDetectedObject::Output RadarFusionToDetectedObject::update(
return output;
}

// Judge whether object's yaw is same direction with twist's yaw.
// This function improve multi object tracking with observed speed.
bool RadarFusionToDetectedObject::isYawCorrect(const double & yaw, const double & yaw_threshold)
{
double normalized_yaw = tier4_autoware_utils::normalizeRadian(yaw);
if (std::abs(normalized_yaw) < yaw_threshold) {
return true;
} else if (M_PI - yaw_threshold < std::abs(normalized_yaw)) {
return true;
} else {
return false;
}
}

// Choose radar pointcloud/objects within 3D bounding box from lidar-base detection with margin
// space from bird's-eye view.
std::shared_ptr<std::vector<RadarFusionToDetectedObject::RadarInput>>
Expand Down Expand Up @@ -249,7 +271,7 @@ TwistWithCovariance RadarFusionToDetectedObject::estimateTwist(
return twist_with_covariance;
}

// Jugde wether low confidence objects that do not have some radar points/objects or not.
// Judge whether low confidence objects that do not have some radar points/objects or not.
bool RadarFusionToDetectedObject::isQualified(
const DetectedObject & object, std::shared_ptr<std::vector<RadarInput>> & radars)
{
Expand Down
3 changes: 3 additions & 0 deletions ...dar_object_fusion_to_detected_object_node/radar_object_fusion_to_detected_object_node.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -69,6 +69,8 @@ RadarObjectFusionToDetectedObjectNode::RadarObjectFusionToDetectedObjectNode(
declare_parameter<double>("core_params.bounding_box_margin", 0.5);
core_param_.split_threshold_velocity =
declare_parameter<double>("core_params.split_threshold_velocity", 0.0);
core_param_.threshold_yaw_diff =
declare_parameter<double>("core_params.threshold_yaw_diff", 0.35);
core_param_.velocity_weight_average =
declare_parameter<double>("core_params.velocity_weight_average", 0.0);
core_param_.velocity_weight_median =
Expand Down Expand Up @@ -136,6 +138,7 @@ rcl_interfaces::msg::SetParametersResult RadarObjectFusionToDetectedObjectNode::
// Update params
update_param(params, "core_params.bounding_box_margin", p.bounding_box_margin);
update_param(params, "core_params.split_threshold_velocity", p.split_threshold_velocity);
update_param(params, "core_params.threshold_yaw_diff", p.threshold_yaw_diff);
update_param(params, "core_params.velocity_weight_average", p.velocity_weight_average);
update_param(params, "core_params.velocity_weight_median", p.velocity_weight_median);
update_param(
Expand Down

0 comments on commit 8a61084

Please sign in to comment.