Point Cloud Publishing with sensor_msgs::PointCloudIterator

pc_publisher.h

#include <memory>
#include <tuple>

#include <ros/ros.h>

#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/point_cloud2_iterator.h>


template <bool RGB = false> class PCPublisher {
public:
  using pc_iter = sensor_msgs::PointCloud2Iterator<float>;

private:
  ros::Publisher _pub;
  sensor_msgs::PointCloud2Ptr pc;
  std::unique_ptr<sensor_msgs::PointCloud2Modifier> pcm = nullptr;

public:
  sensor_msgs::PointCloud2::_header_type *header{};
  explicit PCPublisher(const ros::Publisher &pub, bool clear = true) : _pub(pub), header(nullptr) {
    if (clear)
      initialize_cloud();
  }

  explicit operator bool() const { return header; }

  void initialize_cloud() {
    pc = boost::make_shared<sensor_msgs::PointCloud2>();
    pcm = std::make_unique<sensor_msgs::PointCloud2Modifier>(*pc);
    header = &pc->header;
    pc->height = 1;
    pc->is_bigendian = false;
    pc->is_dense = false;
    set_fields();
  }
  void set_fields() {
    if constexpr (RGB)
      pcm->setPointCloud2FieldsByString(2, "xyz", "rgb");
    else
      pcm->setPointCloud2FieldsByString(1, "xyz");
  }
  auto resize(size_t size) {
    pcm->resize(size);

    if constexpr (RGB) {
      return std::make_tuple(sensor_msgs::PointCloud2Iterator<float>(*pc, "x"),
                             sensor_msgs::PointCloud2Iterator<unsigned char>(*pc, "rgb"));
    } else {
      return std::make_tuple(sensor_msgs::PointCloud2Iterator<float>(*pc, "x"));
    }
  }

  auto size() { return pcm->size(); }

  void publish() {
    _pub.publish(pc);
    header = nullptr;
    pcm.reset();
  }
};

z_main.cpp

#include<pc_publisher.h>

extern msg;

void main() {
  PCPublisher<true> pc_pub{nh.advertise<sensor_msgs::PointCloud2>("/ba/debug/pc1", 1, true)};
  pc_pub.header->frame_id = "camera_0_optical";

  auto [xyz, rgb] = pc_pub.resize(msg->correspondences.size());
  
  for (const auto &cor : msg->correspondences) {
    const auto w_pt = c.transforms[prev_ind] * WorldPt{cor.landmark.x, cor.landmark.y, cor.landmark.z};
    xyz[0] = float(w_pt[0]);
    xyz[1] = float(w_pt[1]);
    xyz[2] = float(w_pt[2]);
    rgb[0] = di == 1 ? 255 : 0;
    rgb[1] = di == 2 ? 255 : 0;
    rgb[2] = di == 3 ? 255 : 0;

    ++xyz, ++rgb;
  }
  
  pc_pub.header->stamp = msg->header.stamp;
  pc_pub.publish();
}