15 #ifndef BELUGA_ROS_POINT_CLOUD_HPP
16 #define BELUGA_ROS_POINT_CLOUD_HPP
18 #include <range/v3/view/iota.hpp>
22 #include <beluga_ros/messages.hpp>
24 #include <sophus/se3.hpp>
26 #include <Eigen/Dense>
28 #include "beluga/eigen_compatibility.hpp"
55 template <
typename T,
bool Strict = true>
63 std::is_same_v<Scalar, float> || std::is_same_v<Scalar, double>,
64 "Pointcloud3 only supports floating point types");
71 explicit PointCloud3(beluga_ros::msg::PointCloud2ConstSharedPtr cloud, Sophus::SE3d
origin = Sophus::SE3d())
73 assert(cloud_ !=
nullptr);
74 if (cloud_->fields.size() != 3) {
75 throw std::invalid_argument(
"point cloud must have exactly 3 fields");
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");
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");
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");
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");
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");
104 [[nodiscard]]
const auto&
origin()
const {
return origin_; }
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>());
114 assert(datatype == sensor_msgs::typeAsPointFieldType<double>::value);
115 return Eigen::Matrix3X<Scalar>(points_matrix<double>(*cloud_).template cast<Scalar>());
117 return Eigen::Matrix3X<Scalar>(points_matrix<Scalar>(*cloud_));
119 return points_matrix<Scalar>(*cloud_);
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);
132 beluga_ros::msg::PointCloud2ConstSharedPtr cloud_;
133 Sophus::SE3d origin_;
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