Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

refactor(radar_fusion_to_detected_object): refactor radar_fusion_to_detected_object #1440

Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
15 changes: 7 additions & 8 deletions perception/radar_fusion_to_detected_object/CMakeLists.txt
Original file line number Diff line number Diff line change
@@ -1,7 +1,7 @@
cmake_minimum_required(VERSION 3.5)
project(radar_fusion_to_detected_object)

## Compile options
# Compile options
if(NOT CMAKE_CXX_STANDARD)
set(CMAKE_CXX_STANDARD 17)
set(CMAKE_CXX_STANDARD_REQUIRED ON)
Expand All @@ -12,11 +12,11 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
add_compile_options(-Wall -Wextra -Wpedantic)
endif()

## Dependencies
# Dependencies
find_package(autoware_cmake REQUIRED)
autoware_package()

## Targets
# Targets
ament_auto_add_library(radar_object_fusion_to_detected_object_node_component SHARED
src/radar_object_fusion_to_detected_object_node/radar_object_fusion_to_detected_object_node.cpp
src/radar_fusion_to_detected_object.cpp
Expand All @@ -27,17 +27,16 @@ rclcpp_components_register_node(radar_object_fusion_to_detected_object_node_comp
EXECUTABLE radar_object_fusion_to_detected_object_node
)

## Tests
# Tests
if(BUILD_TESTING)
list(APPEND AMENT_LINT_AUTO_EXCLUDE ament_cmake_uncrustify)

find_package(ament_lint_auto REQUIRED)
ament_lint_auto_find_test_dependencies()
endif()

## Package
# Package
ament_auto_package(
INSTALL_TO_SHARE
config
launch
config
launch
)
Original file line number Diff line number Diff line change
Expand Up @@ -18,6 +18,10 @@
#include "rclcpp/logger.hpp"
#include "tier4_autoware_utils/tier4_autoware_utils.hpp"

#define EIGEN_MPL2_ONLY
#include <Eigen/Core>
#include <Eigen/Geometry>

#include "autoware_auto_perception_msgs/msg/detected_objects.hpp"
#include "geometry_msgs/msg/pose_with_covariance.hpp"
#include "geometry_msgs/msg/twist_with_covariance.hpp"
Expand Down Expand Up @@ -98,11 +102,13 @@ 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);
bool isYawCorrect(
const DetectedObject & object, const TwistWithCovariance & twist_with_covariance,
const double & yaw_threshold);
Eigen::Vector2d toVector2d(const TwistWithCovariance & twist_with_covariance);
TwistWithCovariance toTwistWithCovariance(const Eigen::Vector2d & vector2d);

double getTwistNorm(const Twist & twist);
Twist sumTwist(const std::vector<Twist> & twists);
LinearRing2d createObject2dWithMargin(const Point2d object_size, const double margin);
};
} // namespace radar_fusion_to_detected_object
Expand Down
1 change: 1 addition & 0 deletions perception/radar_fusion_to_detected_object/package.xml
Original file line number Diff line number Diff line change
Expand Up @@ -11,6 +11,7 @@
<build_depend>autoware_cmake</build_depend>

<depend>autoware_auto_perception_msgs</depend>
<depend>eigen</depend>
<depend>geometry_msgs</depend>
<depend>rclcpp</depend>
<depend>rclcpp_components</depend>
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -42,7 +42,7 @@ void RadarFusionToDetectedObject::setParam(const Param & param)
param_.split_threshold_velocity = param.split_threshold_velocity;
param_.threshold_yaw_diff = param.split_threshold_velocity;

// normalize weight param
// Normalize weight param
double sum_weight = param.velocity_weight_median + param.velocity_weight_min_distance +
param.velocity_weight_average + param.velocity_weight_target_value_average +
param.velocity_weight_target_value_top;
Expand Down Expand Up @@ -71,7 +71,6 @@ RadarFusionToDetectedObject::Output RadarFusionToDetectedObject::update(
const RadarFusionToDetectedObject::Input & input)
{
RadarFusionToDetectedObject::Output output{};

output.objects.header = input.objects->header;

if (!input.objects || input.objects->objects.empty()) {
Expand All @@ -90,24 +89,22 @@ RadarFusionToDetectedObject::Output RadarFusionToDetectedObject::update(
split_objects.emplace_back(object);

for (auto & split_object : split_objects) {
// set radars within objects
std::shared_ptr<std::vector<RadarInput>> radars_within_split_object;
if (split_objects.size() == 1) {
// If object is not split, radar data within object is same
radars_within_split_object = radars_within_object;
} else {
// If object is split, then filter radar again
radars_within_split_object = filterRadarWithinObject(object, radars_within_object);
radars_within_split_object = filterRadarWithinObject(split_object, radars_within_object);
}

// Estimate twist of object
if (!radars_within_split_object || !(*radars_within_split_object).empty()) {
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)) {

if (isYawCorrect(split_object, twist_with_covariance, param_.threshold_yaw_diff)) {
split_object.kinematics.twist_with_covariance = twist_with_covariance;
split_object.kinematics.has_twist = true;
}
Expand All @@ -126,12 +123,18 @@ RadarFusionToDetectedObject::Output RadarFusionToDetectedObject::update(

// 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)
bool RadarFusionToDetectedObject::isYawCorrect(
const DetectedObject & object, const TwistWithCovariance & twist_with_covariance,
const double & yaw_threshold)
{
double normalized_yaw = tier4_autoware_utils::normalizeRadian(yaw);
if (std::abs(normalized_yaw) < yaw_threshold) {
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(object.kinematics.pose_with_covariance.pose.orientation));
const double diff_yaw = tier4_autoware_utils::normalizeRadian(twist_yaw - object_yaw);
if (std::abs(diff_yaw) < yaw_threshold) {
return true;
} else if (M_PI - yaw_threshold < std::abs(normalized_yaw)) {
} else if (M_PI - yaw_threshold < std::abs(diff_yaw)) {
return true;
} else {
return false;
Expand Down Expand Up @@ -176,13 +179,13 @@ RadarFusionToDetectedObject::filterRadarWithinObject(
TwistWithCovariance RadarFusionToDetectedObject::estimateTwist(
const DetectedObject & object, std::shared_ptr<std::vector<RadarInput>> & radars)
{
TwistWithCovariance twist_with_covariance{};
if (!radars || (*radars).empty()) {
return twist_with_covariance;
TwistWithCovariance output{};
return output;
}

// calculate twist for radar data with min distance
Twist twist_min_distance{};
Eigen::Vector2d vec_min_distance(0.0, 0.0);
if (param_.velocity_weight_min_distance > 0.0) {
auto comp_func = [&](const RadarInput & a, const RadarInput & b) {
return tier4_autoware_utils::calcSquaredDistance2d(
Expand All @@ -193,11 +196,12 @@ TwistWithCovariance RadarFusionToDetectedObject::estimateTwist(
object.kinematics.pose_with_covariance.pose.position);
};
auto iter = std::min_element(std::begin(*radars), std::end(*radars), comp_func);
twist_min_distance = iter->twist_with_covariance.twist;
TwistWithCovariance twist_min_distance = iter->twist_with_covariance;
vec_min_distance = toVector2d(twist_min_distance);
}

// calculate twist for radar data with median twist
Twist twist_median{};
Eigen::Vector2d vec_median(0.0, 0.0);
if (param_.velocity_weight_median > 0.0) {
auto ascending_func = [&](const RadarInput & a, const RadarInput & b) {
return getTwistNorm(a.twist_with_covariance.twist) <
Expand All @@ -207,68 +211,58 @@ TwistWithCovariance RadarFusionToDetectedObject::estimateTwist(

if ((*radars).size() % 2 == 1) {
int median_index = ((*radars).size() - 1) / 2;
twist_median = (*radars).at(median_index).twist_with_covariance.twist;
vec_median = toVector2d((*radars).at(median_index).twist_with_covariance);
} else {
int median_index = (*radars).size() / 2;

twist_median = scaleTwist(
addTwist(
(*radars).at(median_index - 1).twist_with_covariance.twist,
(*radars).at(median_index).twist_with_covariance.twist),
0.5);
Eigen::Vector2d v1 = toVector2d((*radars).at(median_index - 1).twist_with_covariance);
Eigen::Vector2d v2 = toVector2d((*radars).at(median_index).twist_with_covariance);
vec_median = (v1 + v2) / 2.0;
}
}

// calculate twist for radar data with average twist
Twist twist_average{};
Eigen::Vector2d vec_average(0.0, 0.0);
if (param_.velocity_weight_average > 0.0) {
for (const auto & radar : (*radars)) {
twist_average = addTwist(twist_average, radar.twist_with_covariance.twist);
vec_average += toVector2d(radar.twist_with_covariance);
}
twist_average = scaleTwist(twist_average, (1.0 / (*radars).size()));
vec_average /= (*radars).size();
}

// calculate twist for radar data with top target value
Twist twist_top_target_value{};
Eigen::Vector2d vec_top_target_value(0.0, 0.0);
if (param_.velocity_weight_target_value_top > 0.0) {
auto comp_func = [](const RadarInput & a, const RadarInput & b) {
return a.target_value < b.target_value;
};
auto iter = std::max_element(std::begin((*radars)), std::end((*radars)), comp_func);
twist_top_target_value = iter->twist_with_covariance.twist;
vec_top_target_value = toVector2d(iter->twist_with_covariance);
}

// calculate twist for radar data with target_value * average
Twist twist_target_value_average{};
Eigen::Vector2d vec_target_value_average(0.0, 0.0);
double sum_target_value = 0.0;
if (param_.velocity_weight_target_value_average > 0.0) {
for (const auto & radar : (*radars)) {
twist_target_value_average = scaleTwist(
addTwist(twist_target_value_average, radar.twist_with_covariance.twist),
radar.target_value);
vec_target_value_average += (toVector2d(radar.twist_with_covariance) * radar.target_value);
sum_target_value += radar.target_value;
}
twist_target_value_average = scaleTwist(twist_target_value_average, 1.0 / sum_target_value);
vec_target_value_average /= sum_target_value;
}

// estimate doppler velocity with cost weight
std::vector<Twist> weight_twists{};
weight_twists.emplace_back(scaleTwist(twist_min_distance, param_.velocity_weight_min_distance));
weight_twists.emplace_back(scaleTwist(twist_median, param_.velocity_weight_median));
weight_twists.emplace_back(scaleTwist(twist_average, param_.velocity_weight_average));
weight_twists.emplace_back(
scaleTwist(twist_top_target_value, param_.velocity_weight_target_value_top));
weight_twists.emplace_back(
scaleTwist(twist_target_value_average, param_.velocity_weight_target_value_average));

twist_with_covariance.twist = sumTwist(weight_twists);
Eigen::Vector2d sum_vec = vec_min_distance * param_.velocity_weight_min_distance +
vec_median * param_.velocity_weight_median +
vec_average * param_.velocity_weight_average +
vec_top_target_value * param_.velocity_weight_target_value_top +
vec_target_value_average * param_.velocity_weight_target_value_average;
TwistWithCovariance estimated_twist_with_covariance = toTwistWithCovariance(sum_vec);

// [TODO] (Satoshi Tanaka) Implement
// Convert doppler velocity to twist
// if (param_.convert_doppler_to_twist) {
// twist_with_covariance = convertDopplerToTwist(object, twist_with_covariance);
// }
return twist_with_covariance;
return estimated_twist_with_covariance;
}

// Judge whether low confidence objects that do not have some radar points/objects or not.
Expand All @@ -293,27 +287,20 @@ bool RadarFusionToDetectedObject::isQualified(
// return twist_with_covariance;
// }

Twist RadarFusionToDetectedObject::addTwist(const Twist & twist_1, const Twist & twist_2)
Eigen::Vector2d RadarFusionToDetectedObject::toVector2d(
const TwistWithCovariance & twist_with_covariance)
{
Twist output{};
output.linear.x = twist_1.linear.x + twist_2.linear.x;
output.linear.y = twist_1.linear.y + twist_2.linear.y;
output.linear.z = twist_1.linear.z + twist_2.linear.z;
output.angular.x = twist_1.angular.x + twist_2.angular.x;
output.angular.y = twist_1.angular.y + twist_2.angular.y;
output.angular.z = twist_1.angular.z + twist_2.angular.z;
auto vec = twist_with_covariance.twist.linear;
Eigen::Vector2d output(vec.x, vec.y);
return output;
}

Twist RadarFusionToDetectedObject::scaleTwist(const Twist & twist, const double scale)
TwistWithCovariance RadarFusionToDetectedObject::toTwistWithCovariance(
const Eigen::Vector2d & vector2d)
{
Twist output{};
output.linear.x = twist.linear.x * scale;
output.linear.y = twist.linear.y * scale;
output.linear.z = twist.linear.z * scale;
output.angular.x = twist.angular.x * scale;
output.angular.y = twist.angular.y * scale;
output.angular.z = twist.angular.z * scale;
TwistWithCovariance output{};
output.twist.linear.x = vector2d(0);
output.twist.linear.y = vector2d(1);
return output;
}

Expand All @@ -325,15 +312,6 @@ double RadarFusionToDetectedObject::getTwistNorm(const Twist & twist)
return output;
}

Twist RadarFusionToDetectedObject::sumTwist(const std::vector<Twist> & twists)
{
Twist output{};
for (const auto & twist : twists) {
output = addTwist(output, twist);
}
return output;
}

LinearRing2d RadarFusionToDetectedObject::createObject2dWithMargin(
const Point2d object_size, const double margin)
{
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -71,6 +71,8 @@ RadarObjectFusionToDetectedObjectNode::RadarObjectFusionToDetectedObjectNode(
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_min_distance =
declare_parameter<double>("core_params.velocity_weight_min_distance", 1.0);
core_param_.velocity_weight_average =
declare_parameter<double>("core_params.velocity_weight_average", 0.0);
core_param_.velocity_weight_median =
Expand Down