pointcloud_densification.cpp 2.9 KB

12345678910111213141516171819202122232425262728293031323334353637383940414243444546474849505152535455565758596061626364656667686970717273747576777879808182838485868788899091929394959697
  1. // Copyright 2021 TIER IV, Inc.
  2. //
  3. // Licensed under the Apache License, Version 2.0 (the "License");
  4. // you may not use this file except in compliance with the License.
  5. // You may obtain a copy of the License at
  6. //
  7. // http://www.apache.org/licenses/LICENSE-2.0
  8. //
  9. // Unless required by applicable law or agreed to in writing, software
  10. // distributed under the License is distributed on an "AS IS" BASIS,
  11. // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  12. // See the License for the specific language governing permissions and
  13. // limitations under the License.
  14. #include "lidar_centerpoint/preprocess/pointcloud_densification.hpp"
  15. #include <pcl_ros/transforms.hpp>
  16. #include <boost/optional.hpp>
  17. #include <pcl_conversions/pcl_conversions.h>
  18. #ifdef ROS_DISTRO_GALACTIC
  19. #include <tf2_eigen/tf2_eigen.h>
  20. #else
  21. #include <tf2_eigen/tf2_eigen.hpp>
  22. #endif
  23. #include <string>
  24. #include <utility>
  25. namespace
  26. {
  27. boost::optional<geometry_msgs::msg::Transform> getTransform(
  28. const tf2_ros::Buffer & tf_buffer, const std::string & target_frame_id,
  29. const std::string & source_frame_id, const rclcpp::Time & time)
  30. {
  31. try {
  32. geometry_msgs::msg::TransformStamped transform_stamped;
  33. transform_stamped = tf_buffer.lookupTransform(
  34. target_frame_id, source_frame_id, time, rclcpp::Duration::from_seconds(0.5));
  35. return transform_stamped.transform;
  36. } catch (tf2::TransformException & ex) {
  37. RCLCPP_WARN_STREAM(rclcpp::get_logger("lidar_centerpoint"), ex.what());
  38. return boost::none;
  39. }
  40. }
  41. Eigen::Affine3f transformToEigen(const geometry_msgs::msg::Transform & t)
  42. {
  43. Eigen::Affine3f a;
  44. a.matrix() = tf2::transformToEigen(t).matrix().cast<float>();
  45. return a;
  46. }
  47. } // namespace
  48. namespace centerpoint
  49. {
  50. PointCloudDensification::PointCloudDensification(const DensificationParam & param) : param_(param)
  51. {
  52. }
  53. bool PointCloudDensification::enqueuePointCloud(
  54. const sensor_msgs::msg::PointCloud2 & pointcloud_msg, const tf2_ros::Buffer & tf_buffer)
  55. {
  56. const auto header = pointcloud_msg.header;
  57. auto transform_world2current =
  58. getTransform(tf_buffer, header.frame_id, param_.world_frame_id(), header.stamp);
  59. if (!transform_world2current) {
  60. return false;
  61. }
  62. auto affine_world2current = transformToEigen(transform_world2current.get());
  63. enqueue(pointcloud_msg, affine_world2current);
  64. dequeue();
  65. return true;
  66. }
  67. void PointCloudDensification::enqueue(
  68. const sensor_msgs::msg::PointCloud2 & msg, const Eigen::Affine3f & affine_world2current)
  69. {
  70. affine_world2current_ = affine_world2current;
  71. current_timestamp_ = rclcpp::Time(msg.header.stamp).seconds();
  72. PointCloudWithTransform pointcloud = {msg, affine_world2current.inverse()};
  73. pointcloud_cache_.push_front(pointcloud);
  74. }
  75. void PointCloudDensification::dequeue()
  76. {
  77. if (pointcloud_cache_.size() > param_.pointcloud_cache_size()) {
  78. pointcloud_cache_.pop_back();
  79. }
  80. }
  81. } // namespace centerpoint