/home/runner/work/beluga/beluga/beluga_ros/include/beluga_ros/point_cloud.hpp Source File

/home/runner/work/beluga/beluga/beluga_ros/include/beluga_ros/point_cloud.hpp Source File#

Beluga ROS: /home/runner/work/beluga/beluga/beluga_ros/include/beluga_ros/point_cloud.hpp Source File
Beluga ROS
point_cloud.hpp
Go to the documentation of this file.
1 // Copyright 2024 Ekumen, 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 
15 #ifndef BELUGA_ROS_POINT_CLOUD_HPP
16 #define BELUGA_ROS_POINT_CLOUD_HPP
17 
18 #include <range/v3/view/iota.hpp>
19 
22 #include <beluga_ros/messages.hpp>
23 
24 #include <sophus/se3.hpp>
25 
26 #include <Eigen/Dense>
27 
28 #include "beluga/eigen_compatibility.hpp"
29 
44 namespace beluga_ros {
45 
47 
55 template <typename T, bool Strict = true>
56 class PointCloud3 : public beluga::BasePointCloud<PointCloud3<T>> {
57  public:
59  using Scalar = T;
60 
61  // Assert fields' type is floating point
62  static_assert(
63  std::is_same_v<Scalar, float> || std::is_same_v<Scalar, double>,
64  "Pointcloud3 only supports floating point types");
65 
71  explicit PointCloud3(beluga_ros::msg::PointCloud2ConstSharedPtr cloud, Sophus::SE3d origin = Sophus::SE3d())
72  : cloud_(std::move(cloud)), origin_(std::move(origin)) {
73  assert(cloud_ != nullptr);
74  if (cloud_->fields.size() != 3) {
75  throw std::invalid_argument("point cloud must have exactly 3 fields");
76  }
77  const auto& field_0 = cloud_->fields.at(0);
78  const auto& field_1 = cloud_->fields.at(1);
79  const auto& field_2 = cloud_->fields.at(2);
80  if (field_0.name != "x" || field_1.name != "y" || field_2.name != "z") {
81  throw std::invalid_argument("point cloud layout is not xyz");
82  }
83  if (field_0.datatype != field_1.datatype || field_1.datatype != field_2.datatype) {
84  throw std::invalid_argument("point cloud xyz datatypes are not all the same");
85  }
86  if constexpr (Strict) {
87  if (field_0.datatype != sensor_msgs::typeAsPointFieldType<Scalar>::value) {
88  throw std::invalid_argument("xyz datatype does not match the expected type");
89  }
90  } else {
91  if (field_0.datatype != sensor_msgs::typeAsPointFieldType<float>::value &&
92  field_0.datatype != sensor_msgs::typeAsPointFieldType<double>::value) {
93  throw std::invalid_argument("xyz datatype is not floating point");
94  }
95  }
96  constexpr auto float_datatype = sensor_msgs::typeAsPointFieldType<float>::value;
97  const auto size_of_datatype = field_0.datatype == float_datatype ? sizeof(float) : sizeof(double);
98  if (cloud_->point_step % size_of_datatype != 0) {
99  throw std::invalid_argument("point cloud is not dense");
100  }
101  }
102 
104  [[nodiscard]] const auto& origin() const { return origin_; }
105 
107  [[nodiscard]] auto points() const {
108  if constexpr (!Strict) {
109  const auto datatype = cloud_->fields.at(0).datatype;
110  if (datatype != sensor_msgs::typeAsPointFieldType<Scalar>::value) {
111  if (datatype == sensor_msgs::typeAsPointFieldType<float>::value) {
112  return Eigen::Matrix3X<Scalar>(points_matrix<float>(*cloud_).template cast<Scalar>());
113  }
114  assert(datatype == sensor_msgs::typeAsPointFieldType<double>::value);
115  return Eigen::Matrix3X<Scalar>(points_matrix<double>(*cloud_).template cast<Scalar>());
116  }
117  return Eigen::Matrix3X<Scalar>(points_matrix<Scalar>(*cloud_));
118  } else {
119  return points_matrix<Scalar>(*cloud_);
120  }
121  }
122 
123  private:
124  template <typename U>
125  static auto points_matrix(const beluga_ros::msg::PointCloud2& cloud) {
126  const auto stride = static_cast<int>(cloud.point_step / sizeof(U));
127  const beluga_ros::msg::PointCloud2ConstIterator<U> iter_points(cloud, "x");
128  return Eigen::Map<const Eigen::Matrix3X<U>, 0, Eigen::OuterStride<>>(
129  &iter_points[0], 3, cloud.width * cloud.height, stride);
130  }
131 
132  beluga_ros::msg::PointCloud2ConstSharedPtr cloud_;
133  Sophus::SE3d origin_;
134 };
135 
138 
141 
142 } // namespace beluga_ros
143 
144 #endif // BELUGA_ROS_POINT_CLOUD_HPP
Thin wrapper type for 3D sensor_msgs/PointCloud2 messages with dense layouts.
Definition: point_cloud.hpp:56
auto points() const
Get cartesian points in the point cloud as a matrix.
Definition: point_cloud.hpp:107
T Scalar
Expected PointCloud2 fields type.
Definition: point_cloud.hpp:59
PointCloud3(beluga_ros::msg::PointCloud2ConstSharedPtr cloud, Sophus::SE3d origin=Sophus::SE3d())
Definition: point_cloud.hpp:71
const auto & origin() const
Get the point cloud frame origin in the filter frame.
Definition: point_cloud.hpp:104
The main Beluga ROS namespace.
Definition: amcl.hpp:47