Skip to content

Commit

Permalink
feat: add option for interval coefficients
Browse files Browse the repository at this point in the history 
Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp>
  • Loading branch information
@technolojin
technolojin committed Jan 15, 2024
1 parent 5d74286 commit 70008e9
Show file tree
Hide file tree
Showing 3 changed files with 58 additions and 18 deletions.
12 changes: 8 additions & 4 deletions ...n/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_detail.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -114,12 +114,14 @@ get_uuid_marker_ptr(
const std_msgs::msg::ColorRGBA & color_rgba);

AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr
get_pose_covariance_marker_ptr(const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance);
get_pose_covariance_marker_ptr(
const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance,
const double & confidence_interval_coefficient);

AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr
get_yaw_covariance_marker_ptr(
const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance, const double & length,
const double & line_width);
const double & confidence_interval_coefficient, const double & line_width);

AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr
get_velocity_text_marker_ptr(
Expand All @@ -139,7 +141,8 @@ get_twist_marker_ptr(
AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr
get_twist_covariance_marker_ptr(
const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance,
const geometry_msgs::msg::TwistWithCovariance & twist_with_covariance);
const geometry_msgs::msg::TwistWithCovariance & twist_with_covariance,
const double & confidence_interval_coefficient);

AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr
get_yaw_rate_marker_ptr(
Expand All @@ -149,7 +152,8 @@ get_yaw_rate_marker_ptr(
AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr
get_yaw_rate_covariance_marker_ptr(
const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance,
const geometry_msgs::msg::TwistWithCovariance & twist_with_covariance, const double & line_width);
const geometry_msgs::msg::TwistWithCovariance & twist_with_covariance,
const double & confidence_interval_coefficient, const double & line_width);

AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr
get_predicted_path_marker_ptr(
Expand Down
40 changes: 36 additions & 4 deletions ...ware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -104,6 +104,13 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase
"Visualization Type", "Normal", "Simplicity of the polygon to display object.", this);
m_simple_visualize_mode_property->addOption("Normal", 0);
m_simple_visualize_mode_property->addOption("Simple", 1);
// Confidence interval property
m_confidence_interval_property = new rviz_common::properties::EnumProperty(
"Confidence Interval", "95.45%, 2 sigma", "Confidence interval of state estimations.", this);
m_confidence_interval_property->addOption("68.27%, 1 sigma", 0);
m_confidence_interval_property->addOption("86.64%, 1.5 sigma", 1);
m_confidence_interval_property->addOption("95.45%, 2 sigma", 2);
m_confidence_interval_property->addOption("99.73%, 3 sigma", 3);

// iterate over default values to create and initialize the properties.
for (const auto & map_property_it : detail::kDefaultObjectPropertyValues) {
Expand Down Expand Up @@ -250,7 +257,8 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase
const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance) const
{
if (m_display_pose_covariance_property.getBool()) {
return detail::get_pose_covariance_marker_ptr(pose_with_covariance);
return detail::get_pose_covariance_marker_ptr(
pose_with_covariance, get_confidence_interval());
} else {
return std::nullopt;
}
Expand All @@ -261,7 +269,8 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase
const double & line_width) const
{
if (m_display_yaw_covariance_property.getBool()) {
return detail::get_yaw_covariance_marker_ptr(pose_with_covariance, length, line_width);
return detail::get_yaw_covariance_marker_ptr(
pose_with_covariance, length, get_confidence_interval(), line_width);
} else {
return std::nullopt;
}
Expand Down Expand Up @@ -310,7 +319,8 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase
const geometry_msgs::msg::TwistWithCovariance & twist_with_covariance) const
{
if (m_display_twist_covariance_property.getBool()) {
return detail::get_twist_covariance_marker_ptr(pose_with_covariance, twist_with_covariance);
return detail::get_twist_covariance_marker_ptr(
pose_with_covariance, twist_with_covariance, get_confidence_interval());
} else {
return std::nullopt;
}
Expand All @@ -336,7 +346,7 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase
{
if (m_display_yaw_rate_covariance_property.getBool()) {
return detail::get_yaw_rate_covariance_marker_ptr(
pose_with_covariance, twist_with_covariance, line_width);
pose_with_covariance, twist_with_covariance, get_confidence_interval(), line_width);
} else {
return std::nullopt;
}
Expand Down Expand Up @@ -464,6 +474,26 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase

double get_line_width() { return m_line_width_property.getFloat(); }

double get_confidence_interval() const
{
switch (m_confidence_interval_property->getOptionInt()) {
case 0:
// 68.27%, 1 sigma
return 1.0;
case 1:
// 86.64%, 1.5 sigma
return 1.5;
case 2:
// 95.45%, 2 sigma
return 2.0;
case 3:
// 99.73%, 3 sigma
return 3.0;
default:
return 2.0;
}
}

private:
// All rviz plugins should have this. Should be initialized with pointer to this class
MarkerCommon m_marker_common;
Expand All @@ -475,6 +505,8 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase
rviz_common::properties::EnumProperty * m_display_type_property;
// Property to choose simplicity of visualization polygon
rviz_common::properties::EnumProperty * m_simple_visualize_mode_property;
// Property to set confidence interval of state estimations
rviz_common::properties::EnumProperty * m_confidence_interval_property;
// Property to enable/disable label visualization
rviz_common::properties::BoolProperty m_display_label_property;
// Property to enable/disable uuid visualization
Expand Down
24 changes: 14 additions & 10 deletions common/autoware_auto_perception_rviz_plugin/src/object_detection/object_polygon_detail.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -120,7 +120,8 @@ visualization_msgs::msg::Marker::SharedPtr get_twist_marker_ptr(

visualization_msgs::msg::Marker::SharedPtr get_twist_covariance_marker_ptr(
const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance,
const geometry_msgs::msg::TwistWithCovariance & twist_with_covariance)
const geometry_msgs::msg::TwistWithCovariance & twist_with_covariance,
const double & confidence_interval_coefficient)
{
auto marker_ptr = std::make_shared<Marker>();
marker_ptr->type = visualization_msgs::msg::Marker::CYLINDER;
Expand Down Expand Up @@ -160,8 +161,8 @@ visualization_msgs::msg::Marker::SharedPtr get_twist_covariance_marker_ptr(
marker_ptr->pose.orientation.w = std::cos(phi / 2.0);

// ellipse size
marker_ptr->scale.x = sigma1 * 2.448; // 2.448 sigma is 95%
marker_ptr->scale.y = sigma2 * 2.448; // 2.448 sigma is 95%
marker_ptr->scale.x = sigma1 * confidence_interval_coefficient;
marker_ptr->scale.y = sigma2 * confidence_interval_coefficient;
marker_ptr->scale.z = 0.05;

marker_ptr->lifetime = rclcpp::Duration::from_seconds(0.5);
Expand Down Expand Up @@ -220,7 +221,8 @@ visualization_msgs::msg::Marker::SharedPtr get_yaw_rate_marker_ptr(

visualization_msgs::msg::Marker::SharedPtr get_yaw_rate_covariance_marker_ptr(
const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance,
const geometry_msgs::msg::TwistWithCovariance & twist_with_covariance, const double & line_width)
const geometry_msgs::msg::TwistWithCovariance & twist_with_covariance,
const double & confidence_interval_coefficient, const double & line_width)
{
auto marker_ptr = std::make_shared<Marker>();
marker_ptr->type = visualization_msgs::msg::Marker::LINE_LIST;
Expand All @@ -231,7 +233,7 @@ visualization_msgs::msg::Marker::SharedPtr get_yaw_rate_covariance_marker_ptr(

// yaw rate covariance
const double yaw_rate_covariance = twist_with_covariance.covariance[35];
const double yaw_rate_sigma = std::sqrt(yaw_rate_covariance) * 2.448; // 2.448 sigma is 95%
const double yaw_rate_sigma = std::sqrt(yaw_rate_covariance) * confidence_interval_coefficient;
const double yaw_rate = twist_with_covariance.twist.angular.z;
const double velocity = std::sqrt(
twist_with_covariance.twist.linear.x * twist_with_covariance.twist.linear.x +
Expand Down Expand Up @@ -353,7 +355,8 @@ void calc_covariance_eigen_vectors(
}

visualization_msgs::msg::Marker::SharedPtr get_pose_covariance_marker_ptr(
const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance)
const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance,
const double & confidence_interval_coefficient)
{
auto marker_ptr = std::make_shared<Marker>();
marker_ptr->type = visualization_msgs::msg::Marker::CYLINDER;
Expand All @@ -376,8 +379,8 @@ visualization_msgs::msg::Marker::SharedPtr get_pose_covariance_marker_ptr(
marker_ptr->pose.orientation.w = std::cos(yaw / 2.0);

// ellipse size
marker_ptr->scale.x = sigma1 * 2.448; // 2.448 sigma is 95%
marker_ptr->scale.y = sigma2 * 2.448; // 2.448 sigma is 95%
marker_ptr->scale.x = sigma1 * confidence_interval_coefficient;
marker_ptr->scale.y = sigma2 * confidence_interval_coefficient;
marker_ptr->scale.z = 0.05;

// ellipse color density
Expand All @@ -396,7 +399,7 @@ visualization_msgs::msg::Marker::SharedPtr get_pose_covariance_marker_ptr(

visualization_msgs::msg::Marker::SharedPtr get_yaw_covariance_marker_ptr(
const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance, const double & length,
const double & line_width)
const double & confidence_interval_coefficient, const double & line_width)
{
auto marker_ptr = std::make_shared<Marker>();
marker_ptr->type = visualization_msgs::msg::Marker::LINE_STRIP;
Expand All @@ -408,7 +411,8 @@ visualization_msgs::msg::Marker::SharedPtr get_yaw_covariance_marker_ptr(

// orientation covariance
double yaw_vector_length = std::max(length, 1.0);
double yaw_sigma = std::sqrt(pose_with_covariance.covariance[35]) * 2.448; // 2.448 sigma is 95%
double yaw_sigma =
std::sqrt(pose_with_covariance.covariance[35]) * confidence_interval_coefficient;
// get arc points
if (yaw_sigma > M_PI) {
yaw_vector_length = 1.0;
Expand Down

0 comments on commit 70008e9

Please sign in to comment.