point_cloud.hpp

Go to the documentation of this file.

// Copyright 2024 Ekumen, Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
 
#ifndef BELUGA_ROS_POINT_CLOUD_HPP
#define BELUGA_ROS_POINT_CLOUD_HPP
 
#include <range/v3/view/iota.hpp>
 
#include <beluga/sensor/data/point_cloud.hpp>
#include <beluga/views/take_evenly.hpp>
#include <beluga_ros/messages.hpp>
 
#include <sophus/se3.hpp>
 
#include <Eigen/Dense>
 
#include "beluga/eigen_compatibility.hpp"
 
namespace beluga_ros {
 
template <typename T>
class PointCloud3 : public beluga::BasePointCloud<PointCloud3<T>> {
 public:
  using Scalar = T;
 
  static_assert(
      std::is_same_v<Scalar, float> || std::is_same_v<Scalar, double>,
      "Pointcloud3 only supports float or double datatype");
 
  explicit PointCloud3(beluga_ros::msg::PointCloud2ConstSharedPtr cloud, Sophus::SE3d origin = Sophus::SE3d())
      : cloud_(std::move(cloud)), origin_(std::move(origin)) {
    assert(cloud_ != nullptr);
    constexpr uint8_t fieldType = sensor_msgs::typeAsPointFieldType<T>::value;
    // Check if point cloud is 3D
    if (cloud_->fields.size() < 3) {
      throw std::invalid_argument("PointCloud is not 3D");
    }
    // Check point cloud is XYZ... type
    if (cloud_->fields.at(0).name != "x" || cloud_->fields.at(1).name != "y" || cloud_->fields.at(2).name != "z") {
      throw std::invalid_argument("PointCloud not XYZ...");
    }
    // Check XYZ datatype is the same
    if (cloud_->fields.at(0).datatype != fieldType || cloud_->fields.at(1).datatype != fieldType ||
        cloud_->fields.at(2).datatype != fieldType) {
      throw std::invalid_argument("XYZ datatype are not same");
    }
    // Check stride is divisible
    if (cloud_->point_step % sizeof(Scalar) != 0) {
      throw std::invalid_argument("Data is not memory-aligned");
    }
    stride_ = static_cast<int>(cloud_->point_step / sizeof(Scalar));
  }
 
  [[nodiscard]] const auto& origin() const { return origin_; }
 
  [[nodiscard]] auto points() const {
    const beluga_ros::msg::PointCloud2ConstIterator<Scalar> iter_points(*cloud_, "x");
    return Eigen::Map<const Eigen::Matrix3X<Scalar>, 0, Eigen::OuterStride<>>(
        &iter_points[0], 3, cloud_->width * cloud_->height, stride_);
  }
 
 private:
  beluga_ros::msg::PointCloud2ConstSharedPtr cloud_;
  int stride_;
  Sophus::SE3d origin_;
};
 
}  // namespace beluga_ros
 
#endif  // BELUGA_ROS_POINT_CLOUD_HPP