Skip to content

Commit

Permalink
[refactor] tmp
Browse files Browse the repository at this point in the history
  • Loading branch information
@HiroIshida
HiroIshida committed Mar 20, 2022
1 parent aafc5ce commit afee3e1
Show file tree
Hide file tree
Showing 2 changed files with 61 additions and 53 deletions.
22 changes: 11 additions & 11 deletions simulator/dummy_perception_publisher/include/dummy_perception_publisher/node.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -39,7 +39,9 @@
struct ObjectInfo
{
ObjectInfo(
const dummy_perception_publisher::msg::Object & object, const rclcpp::Time & current_time);
const dummy_perception_publisher::msg::Object & object, size_t idx,
const rclcpp::Time & current_time);
size_t idx;
double length;
double width;
double height;
Expand All @@ -50,6 +52,14 @@ struct ObjectInfo
tf2::Transform tf_map2moved_object;
};

void createObjectPointcloudObjectCentric(
const ObjectInfo & obj_info, const tf2::Transform & tf_base_link2map,
std::mt19937 & random_generator, pcl::PointCloud<pcl::PointXYZ>::Ptr pointcloud);

void createObjectPointcloudVehicleCentric(
const ObjectInfo & obj_info, const tf2::Transform & tf_base_link2map,
std::mt19937 & random_generator, pcl::PointCloud<pcl::PointXYZ>::Ptr pointcloud);

class DummyPerceptionPublisherNode : public rclcpp::Node
{
private:
Expand All @@ -72,16 +82,6 @@ class DummyPerceptionPublisherNode : public rclcpp::Node

std::mt19937 random_generator_;
void timerCallback();
void createObjectPointcloudObjectCentric(
const double length, const double width, const double height, const double std_dev_x,
const double std_dev_y, const double std_dev_z,
const tf2::Transform & tf_base_link2moved_object,
pcl::PointCloud<pcl::PointXYZ>::Ptr & pointcloud_ptr);
void createObjectPointcloudVehicleCentric(
const double length, const double width, const double height, const double std_dev_x,
const double std_dev_y, const double std_dev_z,
const tf2::Transform & tf_base_link2moved_object,
pcl::PointCloud<pcl::PointXYZ>::Ptr & pointcloud_ptr);
void objectCallback(const dummy_perception_publisher::msg::Object::ConstSharedPtr msg);

public:
Expand Down
92 changes: 50 additions & 42 deletions simulator/dummy_perception_publisher/src/node.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -16,6 +16,7 @@

#include <pcl/filters/voxel_grid_occlusion_estimation.h>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2/LinearMath/Transform.h>
#include <tf2/LinearMath/Vector3.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>

Expand Down Expand Up @@ -49,8 +50,10 @@ pcl::PointXYZ getBaseLinkToPoint(
};

ObjectInfo::ObjectInfo(
const dummy_perception_publisher::msg::Object & object, const rclcpp::Time & current_time)
: length(object.shape.dimensions.x),
const dummy_perception_publisher::msg::Object & object, size_t idx_,
const rclcpp::Time & current_time)
: idx(idx_),
length(object.shape.dimensions.x),
width(object.shape.dimensions.y),
height(object.shape.dimensions.z),
std_dev_x(std::sqrt(object.initial_state.pose_covariance.covariance[0])),
Expand Down Expand Up @@ -170,18 +173,16 @@ void DummyPerceptionPublisherNode::timerCallback()
tf_map2moved_object = tf_map2object_origin * tf_object_origin2moved_object;
tf2::toMsg(tf_map2moved_object, output_moved_object_pose.pose);

const auto obj_info = ObjectInfo(objects_.at(i), i, current_time);

// pointcloud
pcl::PointCloud<pcl::PointXYZ>::Ptr pointcloud_ptr(new pcl::PointCloud<pcl::PointXYZ>);
if (object_centric_pointcloud_) {
createObjectPointcloudObjectCentric(
objects_.at(i).shape.dimensions.x, objects_.at(i).shape.dimensions.y,
objects_.at(i).shape.dimensions.z, std_dev_x, std_dev_y, std_dev_z,
tf_base_link2map * tf_map2moved_object, pointcloud_ptr);
obj_info, tf_base_link2map, random_generator_, pointcloud_ptr);
} else {
createObjectPointcloudVehicleCentric(
objects_.at(i).shape.dimensions.x, objects_.at(i).shape.dimensions.y,
objects_.at(i).shape.dimensions.z, std_dev_x, std_dev_y, std_dev_z,
tf_base_link2map * tf_map2moved_object, pointcloud_ptr);
obj_info, tf_base_link2map, random_generator_, pointcloud_ptr);
}
v_pointcloud.push_back(pointcloud_ptr);

Expand Down Expand Up @@ -285,14 +286,13 @@ void DummyPerceptionPublisherNode::timerCallback()
}
}

void DummyPerceptionPublisherNode::createObjectPointcloudObjectCentric(
const double length, const double width, const double height, const double std_dev_x,
const double std_dev_y, const double std_dev_z, const tf2::Transform & tf_base_link2moved_object,
pcl::PointCloud<pcl::PointXYZ>::Ptr & pointcloud_ptr)
void createObjectPointcloudObjectCentric(
const ObjectInfo & obj_info, const tf2::Transform & tf_base_link2map,
std::mt19937 & random_generator, pcl::PointCloud<pcl::PointXYZ>::Ptr pointcloud)
{
std::normal_distribution<> x_random(0.0, std_dev_x);
std::normal_distribution<> y_random(0.0, std_dev_y);
std::normal_distribution<> z_random(0.0, std_dev_z);
std::normal_distribution<> x_random(0.0, obj_info.std_dev_x);
std::normal_distribution<> y_random(0.0, obj_info.std_dev_y);
std::normal_distribution<> z_random(0.0, obj_info.std_dev_z);
const double epsilon = 0.001;
const double step = 0.05;
const double vertical_theta_step = (1.0 / 180.0) * M_PI;
Expand All @@ -302,34 +302,40 @@ void DummyPerceptionPublisherNode::createObjectPointcloudObjectCentric(
const double horizontal_min_theta = (-180.0 / 180.0) * M_PI;
const double horizontal_max_theta = (180.0 / 180.0) * M_PI;

const double min_z = -1.0 * (height / 2.0) + tf_base_link2moved_object.getOrigin().z();
const double max_z = 1.0 * (height / 2.0) + tf_base_link2moved_object.getOrigin().z();
const auto tf_base_link2moved_object = tf_base_link2map * obj_info.tf_map2moved_object;

const double min_z = -1.0 * (obj_info.height / 2.0) + tf_base_link2moved_object.getOrigin().z();
const double max_z = 1.0 * (obj_info.height / 2.0) + tf_base_link2moved_object.getOrigin().z();
pcl::PointCloud<pcl::PointXYZ> horizontal_candidate_pointcloud;
pcl::PointCloud<pcl::PointXYZ> horizontal_pointcloud;
{
const double y = -1.0 * (width / 2.0);
for (double x = -1.0 * (length / 2.0); x <= ((length / 2.0) + epsilon); x += step) {
const double y = -1.0 * (obj_info.height / 2.0);
for (double x = -1.0 * (obj_info.length / 2.0); x <= ((obj_info.length / 2.0) + epsilon);
x += step) {
horizontal_candidate_pointcloud.push_back(
getBaseLinkToPoint(tf_base_link2moved_object, x, y, 0.0));
}
}
{
const double y = 1.0 * (width / 2.0);
for (double x = -1.0 * (length / 2.0); x <= ((length / 2.0) + epsilon); x += step) {
const double y = 1.0 * (obj_info.height / 2.0);
for (double x = -1.0 * (obj_info.length / 2.0); x <= ((obj_info.length / 2.0) + epsilon);
x += step) {
horizontal_candidate_pointcloud.push_back(
getBaseLinkToPoint(tf_base_link2moved_object, x, y, 0.0));
}
}
{
const double x = -1.0 * (length / 2.0);
for (double y = -1.0 * (width / 2.0); y <= ((width / 2.0) + epsilon); y += step) {
const double x = -1.0 * (obj_info.length / 2.0);
for (double y = -1.0 * (obj_info.height / 2.0); y <= ((obj_info.height / 2.0) + epsilon);
y += step) {
horizontal_candidate_pointcloud.push_back(
getBaseLinkToPoint(tf_base_link2moved_object, x, y, 0.0));
}
}
{
const double x = 1.0 * (length / 2.0);
for (double y = -1.0 * (width / 2.0); y <= ((width / 2.0) + epsilon); y += step) {
const double x = 1.0 * (obj_info.length / 2.0);
for (double y = -1.0 * (obj_info.height / 2.0); y <= ((obj_info.height / 2.0) + epsilon);
y += step) {
horizontal_candidate_pointcloud.push_back(
getBaseLinkToPoint(tf_base_link2moved_object, x, y, 0.0));
}
Expand All @@ -356,6 +362,7 @@ void DummyPerceptionPublisherNode::createObjectPointcloudObjectCentric(
horizontal_ray_traced_pointcloud_indices.at(index) = i;
}
}

for (const auto & pointcloud_index : horizontal_ray_traced_pointcloud_indices) {
if (pointcloud_index != no_data) {
// generate vertical point
Expand All @@ -369,30 +376,31 @@ void DummyPerceptionPublisherNode::createObjectPointcloudObjectCentric(
if (min_z <= z && z <= max_z + epsilon) {
pcl::PointXYZ point;
point.x =
horizontal_candidate_pointcloud.at(pointcloud_index).x + x_random(random_generator_);
horizontal_candidate_pointcloud.at(pointcloud_index).x + x_random(random_generator);
point.y =
horizontal_candidate_pointcloud.at(pointcloud_index).y + y_random(random_generator_);
point.z = z + z_random(random_generator_);
pointcloud_ptr->push_back(point);
horizontal_candidate_pointcloud.at(pointcloud_index).y + y_random(random_generator);
point.z = z + z_random(random_generator);
pointcloud->push_back(point);
}
}
}
}
}

void DummyPerceptionPublisherNode::createObjectPointcloudVehicleCentric(
const double length, const double width, const double height, const double std_dev_x,
const double std_dev_y, const double std_dev_z, const tf2::Transform & tf_base_link2moved_object,
pcl::PointCloud<pcl::PointXYZ>::Ptr & pointcloud_ptr)
void createObjectPointcloudVehicleCentric(
const ObjectInfo & obj_info, const tf2::Transform & tf_base_link2map,
std::mt19937 & random_generator, pcl::PointCloud<pcl::PointXYZ>::Ptr pointcloud)
{
const double horizontal_theta_step = 0.25 * M_PI / 180.0;
const auto tf_base_link2moved_object = tf_base_link2map * obj_info.tf_map2moved_object;
const auto tf_moved_object2base_link = tf_base_link2moved_object.inverse();
const tf2::Transform tf_rotonly(tf_moved_object2base_link.getRotation()); // For vector rotation

const auto sdf_box = [&](tf2::Vector3 p) {
// As for signd distance field for a box, please refere:
// https://www.iquilezles.org/www/articles/distfunctions/distfunctions.htm
const auto sd_val_x = std::abs(p.getX()) - 0.5 * length;
const auto sd_val_y = std::abs(p.getY()) - 0.5 * width;
const auto sd_val_x = std::abs(p.getX()) - 0.5 * obj_info.length;
const auto sd_val_y = std::abs(p.getY()) - 0.5 * obj_info.height;
const auto positive_dist_x = std::max(sd_val_x, 0.0);
const auto positive_dist_y = std::max(sd_val_y, 0.0);
const auto positive_dist = std::hypot(positive_dist_x, positive_dist_y);
Expand Down Expand Up @@ -420,27 +428,27 @@ void DummyPerceptionPublisherNode::createObjectPointcloudVehicleCentric(
return std::numeric_limits<double>::infinity();
};

std::normal_distribution<> x_random(0.0, std_dev_x);
std::normal_distribution<> y_random(0.0, std_dev_y);
std::normal_distribution<> z_random(0.0, std_dev_z);
std::normal_distribution<> x_random(0.0, obj_info.std_dev_x);
std::normal_distribution<> y_random(0.0, obj_info.std_dev_y);
std::normal_distribution<> z_random(0.0, obj_info.std_dev_z);

double angle = 0.0;
const size_t n_scan = static_cast<size_t>(std::floor(2 * M_PI / angle_increment_));
const size_t n_scan = static_cast<size_t>(std::floor(2 * M_PI / horizontal_theta_step));
for (size_t i = 0; i < n_scan; ++i) {
angle += angle_increment_;
angle += horizontal_theta_step;

const tf2::Vector3 dir_wrt_car(cos(angle), sin(angle), 0.0);
const tf2::Vector3 dir_wrt_obj = tf_rotonly * dir_wrt_car;
const auto start_ray_pos = tf_moved_object2base_link.getOrigin();
const tf2::Vector3 pos_noise(
-x_random(random_generator_), -y_random(random_generator_), -z_random(random_generator_));
-x_random(random_generator), -y_random(random_generator), -z_random(random_generator));
const auto start_ray_pos_with_noise = start_ray_pos + pos_noise;
const float ray_dist = compute_dist_by_spheretrace(start_ray_pos_with_noise, dir_wrt_obj);
if (std::isfinite(ray_dist)) {
const auto ray_tip = start_ray_pos_with_noise + ray_dist * dir_wrt_obj;
const auto point = getBaseLinkToPoint(
tf_base_link2moved_object, ray_tip.getX(), ray_tip.getY(), ray_tip.getZ());
pointcloud_ptr->push_back(point);
pointcloud->push_back(point);
}
}
}
Expand Down

0 comments on commit afee3e1

Please sign in to comment.