尝试将pointcloud2从rosbag转换为google draco点云（编码）

Question

我正在尝试将点云从 rosbag 转换并编码为另一个用 draco 编码的点云https://github.com/google/draco/

这是我的代码：

#include <chrono> // Per misurare il tempo
#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/point_cloud2.hpp>
#include <sensor_msgs/point_cloud2_iterator.hpp>
#include <draco/compression/encode.h>
#include <draco/point_cloud/point_cloud.h>
#include <draco/point_cloud/point_cloud_builder.h>
#include <draco/core/encoder_buffer.h>
#include <draco/core/decoder_buffer.h>
#include <draco/compression/decode.h>

class PointCloudCompressorNode : public rclcpp::Node {
public:
  PointCloudCompressorNode()
    : Node("pointcloud_compressor_node"),
      compression_speed_(declare_parameter<int>("compression_speed", 5)),
      quantization_bits_(declare_parameter<int>("quantization_bits", 10)) {

    RCLCPP_INFO(this->get_logger(), "Initializing PointCloudCompressorNode...");
    
    sub_ = this->create_subscription<sensor_msgs::msg::PointCloud2>(
      "/zed/zed_node/point_cloud/cloud_registered", rclcpp::SensorDataQoS(),
      std::bind(&PointCloudCompressorNode::callback, this, std::placeholders::_1));

    pub_ = this->create_publisher<sensor_msgs::msg::PointCloud2>(
      "/compressed_pointcloud", rclcpp::QoS(10));
  }

private:
  void callback(const sensor_msgs::msg::PointCloud2::SharedPtr msg) {
    // Inizia il timer
    auto start_time = std::chrono::steady_clock::now();

    RCLCPP_INFO(this->get_logger(), "Received PointCloud2 message.");
    RCLCPP_INFO(this->get_logger(), "PointCloud2 message: width=%d, height=%d, fields=%ld, is_dense=%d",
                msg->width, msg->height, msg->fields.size(), msg->is_dense);

    std::unique_ptr<draco::PointCloud> draco_cloud;
    if (!convertToDracoPointCloud(msg, draco_cloud)) {
      RCLCPP_ERROR(this->get_logger(), "Failed to convert PointCloud2 to Draco.");
      return;
    }

    std::vector<uint8_t> compressed_data;
    if (!compressPointCloud(*draco_cloud, compressed_data)) {
      RCLCPP_ERROR(this->get_logger(), "Failed to compress Draco PointCloud.");
      return;
    }

    auto compressed_msg = sensor_msgs::msg::PointCloud2();
    compressed_msg.header = msg->header;
    compressed_msg.data = compressed_data;
    pub_->publish(compressed_msg);

    // Ferma il timer e calcola il tempo totale
    auto end_time = std::chrono::steady_clock::now();
    auto elapsed_time = std::chrono::duration_cast<std::chrono::milliseconds>(end_time - start_time).count();

    RCLCPP_INFO(this->get_logger(), "Published compressed PointCloud2 message of size %zu bytes.", compressed_data.size());
    RCLCPP_INFO(this->get_logger(), "Total processing time: %ld ms.", elapsed_time);
  }

bool convertToDracoPointCloud(const sensor_msgs::msg::PointCloud2::SharedPtr &msg,
                              std::unique_ptr<draco::PointCloud> &draco_cloud) {
    draco::PointCloudBuilder builder;

    // Calcola il numero di punti validi
    size_t total_points = msg->width * msg->height;
    draco::PointIndex::ValueType valid_points = 0;

    sensor_msgs::PointCloud2ConstIterator<float> iter_x(*msg, "x");
    sensor_msgs::PointCloud2ConstIterator<float> iter_y(*msg, "y");
    sensor_msgs::PointCloud2ConstIterator<float> iter_z(*msg, "z");

    // Conta i punti validi cioè quelli che hanno almeno x y z:
    for (size_t i = 0; i < total_points; ++i, ++iter_x, ++iter_y, ++iter_z) {
        const float position[3] = {*iter_x, *iter_y, *iter_z};
        if (std::isfinite(position[0]) && std::isfinite(position[1]) && std::isfinite(position[2])) {
            valid_points++;
        }
    }

    RCLCPP_INFO(this->get_logger(), "Processed %zu valid points out of %zu total points.", valid_points, total_points);

    if (valid_points == 0) {
        RCLCPP_ERROR(this->get_logger(), "No valid points found in PointCloud2.");
        return false;
    }

    // Inizia il builder con il numero di punti validi
    builder.Start(valid_points);

    // Aggiungi attributi: POSITION
    const int pos_att_id = builder.AddAttribute(draco::GeometryAttribute::POSITION, 3, draco::DataType::DT_FLOAT32);
    if (pos_att_id == -1) {
        RCLCPP_ERROR(this->get_logger(), "Failed to add POSITION attribute to Draco PointCloud.");
        return false;
    }

    // Itera di nuovo e aggiungi i punti validi
    iter_x = sensor_msgs::PointCloud2ConstIterator<float>(*msg, "x");
    iter_y = sensor_msgs::PointCloud2ConstIterator<float>(*msg, "y");
    iter_z = sensor_msgs::PointCloud2ConstIterator<float>(*msg, "z");

    size_t point_index = 0;
    for (size_t i = 0; i < valid_points; ++i, ++iter_x, ++iter_y, ++iter_z) {
        const float position[3] = {*iter_x, *iter_y, *iter_z};
        if (std::isfinite(position[0]) && std::isfinite(position[1]) && std::isfinite(position[2])) {
            builder.SetAttributeValueForPoint(pos_att_id, draco::PointIndex(point_index), position);
            point_index++;
        }
    }



    // Finalizza il PointCloud Draco
    draco_cloud = builder.Finalize(false);
    if (!draco_cloud || draco_cloud->num_points() != valid_points) {
        RCLCPP_ERROR(this->get_logger(), "Mismatch in point count after Finalize: expected %zu, got %zu.",
                    valid_points, draco_cloud ? draco_cloud->num_points() : 0);
        return false;
    }

    if (!draco_cloud) {
        RCLCPP_ERROR(this->get_logger(), "Failed to finalize Draco PointCloud.");
        return false;
    }

    RCLCPP_INFO(this->get_logger(), "Draco PointCloud created with %zu points.", draco_cloud->num_points());
    return true;
}

  bool compressPointCloud(const draco::PointCloud &draco_cloud, std::vector<uint8_t> &compressed_data) {
    draco::EncoderBuffer buffer;
    draco::Encoder encoder;

    // Impostazioni di compressione
    // Questa è la classe base:  Base::SetSpeedOptions(encoding_speed, decoding_speed); devo capire a cosa fa riferimento questa velocità..
    encoder.SetSpeedOptions(compression_speed_, compression_speed_);
    encoder.SetAttributeQuantization(draco::GeometryAttribute::POSITION, quantization_bits_);
    RCLCPP_INFO(this->get_logger(), "Encoder settings: compression_speed=%d, quantization_bits=%d.",
                compression_speed_, quantization_bits_);

    // Comprime il PointCloud
    draco::Status status = encoder.EncodePointCloudToBuffer(draco_cloud, &buffer);
    if (!status.ok()) {
      RCLCPP_ERROR(this->get_logger(), "Failed to encode Draco PointCloud: %s", status.error_msg());
      return false;
    }

    // Assegna i dati compressi
    compressed_data.assign(buffer.data(), buffer.data() + buffer.size());
    RCLCPP_INFO(this->get_logger(), "PointCloud compressed to %zu bytes.", compressed_data.size());
    return true;
  }

  rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr sub_;
  rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub_;

  int compression_speed_;
  int quantization_bits_;
};

int main(int argc, char **argv) {
  rclcpp::init(argc, argv);
  rclcpp::spin(std::make_shared<PointCloudCompressorNode>());
  rclcpp::shutdown();
  return 0;
}

当我在 ros2 humility 中运行该包时，我收到了这样的消息：

pc:~/teleoperation_ws$ ros2 run point_cloud_relay point_cloud_relay --ros-args -p compression_speed:=7 -p quantization_bits:=0
[INFO] [1732786049.343657818] [pointcloud_compressor_node]: Initializing PointCloudCompressorNode...
[INFO] [1732786071.338711408] [pointcloud_compressor_node]: Received PointCloud2 message.
[INFO] [1732786071.338824018] [pointcloud_compressor_node]: PointCloud2 message: width=960, height=540, fields=4, is_dense=0
[INFO] [1732786071.371806544] [pointcloud_compressor_node]: Processed 410328 valid points out of 518400 total points.
[ERROR] [1732786071.418566633] [pointcloud_compressor_node]: Mismatch in point count after Finalize: expected 410328, got 1600938866.
[ERROR] [1732786071.418736009] [pointcloud_compressor_node]: Failed to convert PointCloud2 to Draco.

我从错误中看到我得到了 41000 个有效点，当我尝试最终确定构建器时，我得到了 1600938866，我不知道为什么它会这样。 谁能解释一下如何解决它？

Answer 1

https://support.google.com/assistant/thread/310248669 /𝗪𝗵𝗮𝘁-𝗶𝘀-𝘁𝗵𝗲-𝗰𝗮𝗻𝗰𝗲𝗹𝗹𝗮𝘁𝗶𝗼𝗻-𝗽 𝗼𝗹𝗶𝗰𝘆-𝘄𝗶𝘁𝗵-𝗨𝗻𝗶𝘁𝗲𝗱-𝗔𝗶𝗿𝗹𝗶𝗻𝗲𝘀