Skip to content

Commit

Permalink
refactor: apply tier4_autoware_tuils::toPolygon2d()
Browse files Browse the repository at this point in the history 
Signed-off-by: ktro2828 <kotaro.uetake@tier4.jp>
  • Loading branch information
@ktro2828
ktro2828 committed Oct 18, 2022
1 parent b595ae2 commit ebf40b8
Show file tree
Hide file tree
Showing 4 changed files with 23 additions and 98 deletions.
3 changes: 0 additions & 3 deletions ...ation/include/obstacle_pointcloud_based_validator/obstacle_pointcloud_based_validator.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -63,9 +63,6 @@ class ObstaclePointCloudBasedValidator : public rclcpp::Node
std::optional<size_t> getPointCloudNumWithinPolygon(
const autoware_auto_perception_msgs::msg::DetectedObject & object,
const pcl::PointCloud<pcl::PointXY>::Ptr pointcloud);
void toPolygon2d(
const autoware_auto_perception_msgs::msg::DetectedObject & object,
const pcl::PointCloud<pcl::PointXY>::Ptr & polygon);
std::optional<float> getMaxRadius(
const autoware_auto_perception_msgs::msg::DetectedObject & object);
};
Expand Down
3 changes: 0 additions & 3 deletions ...ject_validation/include/occupancy_grid_based_validator/occupancy_grid_based_validator.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -62,9 +62,6 @@ class OccupancyGridBasedValidator : public rclcpp::Node
std::optional<cv::Mat> getMask(
const nav_msgs::msg::OccupancyGrid & occupancy_grid,
const autoware_auto_perception_msgs::msg::DetectedObject & object, cv::Mat mask);
void toPolygon2d(
const autoware_auto_perception_msgs::msg::DetectedObject & object,
std::vector<cv::Point2f> & vertices);
void showDebugImage(
const nav_msgs::msg::OccupancyGrid & ros_occ_grid,
const autoware_auto_perception_msgs::msg::DetectedObjects & objects, const cv::Mat & occ_grid);
Expand Down
73 changes: 17 additions & 56 deletions perception/detected_object_validation/src/obstacle_pointcloud_based_validator.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -17,6 +17,8 @@
#include <perception_utils/perception_utils.hpp>
#include <tier4_autoware_utils/tier4_autoware_utils.hpp>

#include <boost/geometry.hpp>

#include <pcl/filters/crop_hull.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/search/kdtree.h>
Expand Down Expand Up @@ -68,7 +70,9 @@ inline pcl::PointCloud<pcl::PointXYZ>::Ptr toXYZ(

namespace obstacle_pointcloud_based_validator
{
namespace bg = boost::geometry;
using Shape = autoware_auto_perception_msgs::msg::Shape;
using Polygon2d = tier4_autoware_utils::Polygon2d;

ObstaclePointCloudBasedValidator::ObstaclePointCloudBasedValidator(
const rclcpp::NodeOptions & node_options)
Expand Down Expand Up @@ -170,79 +174,36 @@ std::optional<size_t> ObstaclePointCloudBasedValidator::getPointCloudNumWithinPo
const autoware_auto_perception_msgs::msg::DetectedObject & object,
const pcl::PointCloud<pcl::PointXY>::Ptr pointcloud)
{
pcl::PointCloud<pcl::PointXY>::Ptr polygon(new pcl::PointCloud<pcl::PointXY>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cropped_pointcloud(new pcl::PointCloud<pcl::PointXYZ>);
std::vector<pcl::Vertices> vertices_array;
pcl::Vertices vertices;

toPolygon2d(object, polygon);
if (polygon->empty()) return std::nullopt;
Polygon2d poly2d =
tier4_autoware_utils::toPolygon2d(object.kinematics.pose_with_covariance.pose, object.shape);
if (bg::is_empty(poly2d)) return std::nullopt;

pcl::PointCloud<pcl::PointXYZ>::Ptr poly3d(new pcl::PointCloud<pcl::PointXYZ>);

for (size_t i = 0; i < polygon->size(); ++i) vertices.vertices.push_back(i);
vertices_array.push_back(vertices);
size_t idx = 0;
for (const auto & p : poly2d.outer()) {
vertices.vertices.emplace_back(idx);
vertices_array.emplace_back(vertices);
poly3d->emplace_back(p.x(), p.y(), 0.0);
idx++;
}

pcl::CropHull<pcl::PointXYZ> cropper; // don't be implemented PointXY by PCL
cropper.setInputCloud(toXYZ(pointcloud));
cropper.setDim(2);
cropper.setHullIndices(vertices_array);
cropper.setHullCloud(toXYZ(polygon));
cropper.setHullCloud(poly3d);
cropper.setCropOutside(true);
cropper.filter(*cropped_pointcloud);

if (debugger_) debugger_->addPointcloudWithinPolygon(cropped_pointcloud);
return cropped_pointcloud->size();
}

void ObstaclePointCloudBasedValidator::toPolygon2d(
const autoware_auto_perception_msgs::msg::DetectedObject & object,
const pcl::PointCloud<pcl::PointXY>::Ptr & polygon)
{
if (object.shape.type == Shape::BOUNDING_BOX) {
const auto & pose = object.kinematics.pose_with_covariance.pose;
double yaw = tf2::getYaw(pose.orientation);
Eigen::Matrix2d rotation;
rotation << std::cos(yaw), -std::sin(yaw), std::sin(yaw), std::cos(yaw);
Eigen::Vector2d offset0, offset1, offset2, offset3;
offset0 = rotation *
Eigen::Vector2d(object.shape.dimensions.x * 0.5f, object.shape.dimensions.y * 0.5f);
offset1 = rotation *
Eigen::Vector2d(object.shape.dimensions.x * 0.5f, -object.shape.dimensions.y * 0.5f);
offset2 = rotation *
Eigen::Vector2d(-object.shape.dimensions.x * 0.5f, -object.shape.dimensions.y * 0.5f);
offset3 = rotation *
Eigen::Vector2d(-object.shape.dimensions.x * 0.5f, object.shape.dimensions.y * 0.5f);
polygon->push_back(
pcl::PointXY(toPCL(pose.position.x + offset0.x(), pose.position.y + offset0.y())));
polygon->push_back(
pcl::PointXY(toPCL(pose.position.x + offset1.x(), pose.position.y + offset1.y())));
polygon->push_back(
pcl::PointXY(toPCL(pose.position.x + offset2.x(), pose.position.y + offset2.y())));
polygon->push_back(
pcl::PointXY(toPCL(pose.position.x + offset3.x(), pose.position.y + offset3.y())));
} else if (object.shape.type == Shape::CYLINDER) {
const auto & center = object.kinematics.pose_with_covariance.pose.position;
const auto & radius = object.shape.dimensions.x * 0.5;
constexpr int n = 6;
for (int i = 0; i < n; ++i) {
Eigen::Vector2d point;
point.x() = std::cos(
(static_cast<double>(i) / static_cast<double>(n)) * 2.0 * M_PI +
M_PI / static_cast<double>(n)) *
radius +
center.x;
point.y() = std::sin(
(static_cast<double>(i) / static_cast<double>(n)) * 2.0 * M_PI +
M_PI / static_cast<double>(n)) *
radius +
center.y;
polygon->push_back(toPCL(point.x(), point.y()));
}
} else if (object.shape.type == Shape::POLYGON) {
RCLCPP_WARN_THROTTLE(
this->get_logger(), *this->get_clock(), 5000, "POLYGON type is not supported");
}
}

std::optional<float> ObstaclePointCloudBasedValidator::getMaxRadius(
const autoware_auto_perception_msgs::msg::DetectedObject & object)
{
Expand Down
42 changes: 6 additions & 36 deletions perception/detected_object_validation/src/occupancy_grid_based_validator.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -33,6 +33,7 @@
namespace occupancy_grid_based_validator
{
using Shape = autoware_auto_perception_msgs::msg::Shape;
using Polygon2d = tier4_autoware_utils::Polygon2d;

OccupancyGridBasedValidator::OccupancyGridBasedValidator(const rclcpp::NodeOptions & node_options)
: rclcpp::Node("occupancy_grid_based_validator", node_options),
Expand Down Expand Up @@ -103,14 +104,14 @@ std::optional<cv::Mat> OccupancyGridBasedValidator::getMask(
{
const auto & resolution = occupancy_grid.info.resolution;
const auto & origin = occupancy_grid.info.origin;
std::vector<cv::Point2f> vertices;
std::vector<cv::Point> pixel_vertices;
toPolygon2d(object, vertices);
Polygon2d poly2d =
tier4_autoware_utils::toPolygon2d(object.kinematics.pose_with_covariance.pose, object.shape);

bool is_polygon_within_image = true;
for (const auto & vertex : vertices) {
const float px = (vertex.x - origin.position.x) / resolution;
const float py = (vertex.y - origin.position.y) / resolution;
for (const auto & p : poly2d.outer()) {
const float px = (p.x() - origin.position.x) / resolution;
const float py = (p.y() - origin.position.y) / resolution;
const bool is_point_within_image = (0 <= px && px < mask.cols && 0 <= py && py < mask.rows);

if (!is_point_within_image) is_polygon_within_image = false;
Expand Down Expand Up @@ -141,37 +142,6 @@ cv::Mat OccupancyGridBasedValidator::fromOccupancyGrid(
return cv_occ_grid;
}

void OccupancyGridBasedValidator::toPolygon2d(
const autoware_auto_perception_msgs::msg::DetectedObject & object,
std::vector<cv::Point2f> & vertices)
{
if (object.shape.type == Shape::BOUNDING_BOX) {
const auto & pose = object.kinematics.pose_with_covariance.pose;
double yaw = tier4_autoware_utils::normalizeRadian(tf2::getYaw(pose.orientation));
Eigen::Matrix2d rotation;
rotation << std::cos(yaw), -std::sin(yaw), std::sin(yaw), std::cos(yaw);
Eigen::Vector2d offset0, offset1, offset2, offset3;
offset0 = rotation *
Eigen::Vector2d(object.shape.dimensions.x * 0.5f, object.shape.dimensions.y * 0.5f);
offset1 = rotation *
Eigen::Vector2d(object.shape.dimensions.x * 0.5f, -object.shape.dimensions.y * 0.5f);
offset2 = rotation *
Eigen::Vector2d(-object.shape.dimensions.x * 0.5f, -object.shape.dimensions.y * 0.5f);
offset3 = rotation *
Eigen::Vector2d(-object.shape.dimensions.x * 0.5f, object.shape.dimensions.y * 0.5f);
vertices.push_back(cv::Point2f(pose.position.x + offset0.x(), pose.position.y + offset0.y()));
vertices.push_back(cv::Point2f(pose.position.x + offset1.x(), pose.position.y + offset1.y()));
vertices.push_back(cv::Point2f(pose.position.x + offset2.x(), pose.position.y + offset2.y()));
vertices.push_back(cv::Point2f(pose.position.x + offset3.x(), pose.position.y + offset3.y()));
} else if (object.shape.type == Shape::CYLINDER) {
RCLCPP_WARN_THROTTLE(
this->get_logger(), *this->get_clock(), 5000, "CYLINDER type is not supported");
} else if (object.shape.type == Shape::POLYGON) {
RCLCPP_WARN_THROTTLE(
this->get_logger(), *this->get_clock(), 5000, "POLYGON type is not supported");
}
}

void OccupancyGridBasedValidator::showDebugImage(
const nav_msgs::msg::OccupancyGrid & ros_occ_grid,
const autoware_auto_perception_msgs::msg::DetectedObjects & objects, const cv::Mat & occ_grid)
Expand Down

0 comments on commit ebf40b8

Please sign in to comment.