Update: Fixed the issue of point cloud distortion in femto bolt

orbbec_camera/SDK/include/libobsensor/hpp/Utils.hpp

@@ -6,133 +6,155 @@
 #pragma once
 
 #include "Types.hpp"
+#include "Frame.hpp"
 
 namespace ob {
 class Device;
 
 class OB_EXTENSION_API CoordinateTransformHelper {
-public:
-    /**
-     * @brief Transform a 3d point of a source coordinate system into a 3d point of the target coordinate system.
-     *
-     * @param calibrationParam Device calibration param,see pipeline::getCalibrationParam
-     * @param sourcePoint3f Source 3d point value
-     * @param sourceSensorType Source sensor type
-     * @param targetSensorType Target sensor type
-     * @param targetPoint3f Target 3d point value
-     *
-     * @return bool  Transform result
-     */
-    static bool calibration3dTo3d(const OBCalibrationParam calibrationParam, const OBPoint3f sourcePoint3f, const OBSensorType sourceSensorType,
-                                  const OBSensorType targetSensorType, OBPoint3f *targetPoint3f);
+ public:
+  /**
+   * @brief Transform a 3d point of a source coordinate system into a 3d point of the target
+   * coordinate system.
+   *
+   * @param calibrationParam Device calibration param,see pipeline::getCalibrationParam
+   * @param sourcePoint3f Source 3d point value
+   * @param sourceSensorType Source sensor type
+   * @param targetSensorType Target sensor type
+   * @param targetPoint3f Target 3d point value
+   *
+   * @return bool  Transform result
+   */
+  static bool calibration3dTo3d(const OBCalibrationParam calibrationParam,
+                                const OBPoint3f sourcePoint3f, const OBSensorType sourceSensorType,
+                                const OBSensorType targetSensorType, OBPoint3f *targetPoint3f);
 
-    /**
-     * @brief Transform a 2d pixel coordinate with an associated depth value of the source camera into a 3d point of the target coordinate system.
-     *
-     * @param calibrationParam Device calibration param,see pipeline::getCalibrationParam
-     * @param sourcePoint2f Source 2d point value
-     * @param sourceDepthPixelValue The depth of sourcePoint2f in millimeters
-     * @param sourceSensorType Source sensor type
-     * @param targetSensorType Target sensor type
-     * @param targetPoint3f Target 3d point value
-     *
-     * @return bool Transform result
-     */
-    static bool calibration2dTo3d(const OBCalibrationParam calibrationParam, const OBPoint2f sourcePoint2f, const float sourceDepthPixelValue,
-                                  const OBSensorType sourceSensorType, const OBSensorType targetSensorType, OBPoint3f *targetPoint3f);
+  /**
+   * @brief Transform a 2d pixel coordinate with an associated depth value of the source camera into
+   * a 3d point of the target coordinate system.
+   *
+   * @param calibrationParam Device calibration param,see pipeline::getCalibrationParam
+   * @param sourcePoint2f Source 2d point value
+   * @param sourceDepthPixelValue The depth of sourcePoint2f in millimeters
+   * @param sourceSensorType Source sensor type
+   * @param targetSensorType Target sensor type
+   * @param targetPoint3f Target 3d point value
+   *
+   * @return bool Transform result
+   */
+  static bool calibration2dTo3d(const OBCalibrationParam calibrationParam,
+                                const OBPoint2f sourcePoint2f, const float sourceDepthPixelValue,
+                                const OBSensorType sourceSensorType,
+                                const OBSensorType targetSensorType, OBPoint3f *targetPoint3f);
 
-    /**
-     * @brief Transform a 2d pixel coordinate with an associated depth value of the source camera into a 3d point of the target coordinate system.
-     * @brief This function uses undistortion, which may result in longer processing time.
-     *
-     * @param calibrationParam Device calibration param,see pipeline::getCalibrationParam
-     * @param sourcePoint2f Source 2d point value
-     * @param sourceDepthPixelValue The depth of sourcePoint2f in millimeters
-     * @param sourceSensorType Source sensor type
-     * @param targetSensorType Target sensor type
-     * @param targetPoint3f Target 3d point value
-     *
-     * @return bool Transform result
-     */
-    static bool calibration2dTo3dUndistortion(const OBCalibrationParam calibrationParam, const OBPoint2f sourcePoint2f, const float sourceDepthPixelValue,
-                                              const OBSensorType sourceSensorType, const OBSensorType targetSensorType, OBPoint3f *targetPoint3f);
+  /**
+   * @brief Transform a 2d pixel coordinate with an associated depth value of the source camera into
+   * a 3d point of the target coordinate system.
+   * @brief This function uses undistortion, which may result in longer processing time.
+   *
+   * @param calibrationParam Device calibration param,see pipeline::getCalibrationParam
+   * @param sourcePoint2f Source 2d point value
+   * @param sourceDepthPixelValue The depth of sourcePoint2f in millimeters
+   * @param sourceSensorType Source sensor type
+   * @param targetSensorType Target sensor type
+   * @param targetPoint3f Target 3d point value
+   *
+   * @return bool Transform result
+   */
+  static bool calibration2dTo3dUndistortion(const OBCalibrationParam calibrationParam,
+                                            const OBPoint2f sourcePoint2f,
+                                            const float sourceDepthPixelValue,
+                                            const OBSensorType sourceSensorType,
+                                            const OBSensorType targetSensorType,
+                                            OBPoint3f *targetPoint3f);
 
-    /**
-     * @brief Transform a 3d point of a source coordinate system into a 2d pixel coordinate of the target camera.
-     *
-     * @param calibrationParam Device calibration param,see pipeline::getCalibrationParam
-     * @param sourcePoint3f Source 3d point value
-     * @param sourceSensorType Source sensor type
-     * @param targetSensorType Target sensor type
-     * @param targetPoint2f Target 2d point value
-     *
-     * @return bool Transform result
-     */
-    static bool calibration3dTo2d(const OBCalibrationParam calibrationParam, const OBPoint3f sourcePoint3f, const OBSensorType sourceSensorType,
-                                  const OBSensorType targetSensorType, OBPoint2f *targetPoint2f);
+  /**
+   * @brief Transform a 3d point of a source coordinate system into a 2d pixel coordinate of the
+   * target camera.
+   *
+   * @param calibrationParam Device calibration param,see pipeline::getCalibrationParam
+   * @param sourcePoint3f Source 3d point value
+   * @param sourceSensorType Source sensor type
+   * @param targetSensorType Target sensor type
+   * @param targetPoint2f Target 2d point value
+   *
+   * @return bool Transform result
+   */
+  static bool calibration3dTo2d(const OBCalibrationParam calibrationParam,
+                                const OBPoint3f sourcePoint3f, const OBSensorType sourceSensorType,
+                                const OBSensorType targetSensorType, OBPoint2f *targetPoint2f);
 
-    /**
-     * @brief Transform a 2d pixel coordinate with an associated depth value of the source camera into a 2d pixel coordinate of the target camera
-     *
-     * @param calibrationParam Device calibration param,see pipeline::getCalibrationParam
-     * @param sourcePoint2f Source 2d point value
-     * @param sourceDepthPixelValue The depth of sourcePoint2f in millimeters
-     * @param sourceSensorType Source sensor type
-     * @param targetSensorType Target sensor type
-     * @param targetPoint2f Target 2d point value
-     *
-     * @return bool Transform result
-     */
-    static bool calibration2dTo2d(const OBCalibrationParam calibrationParam, const OBPoint2f sourcePoint2f, const float sourceDepthPixelValue,
-                                  const OBSensorType sourceSensorType, const OBSensorType targetSensorType, OBPoint2f *targetPoint2f);
+  /**
+   * @brief Transform a 2d pixel coordinate with an associated depth value of the source camera into
+   * a 2d pixel coordinate of the target camera
+   *
+   * @param calibrationParam Device calibration param,see pipeline::getCalibrationParam
+   * @param sourcePoint2f Source 2d point value
+   * @param sourceDepthPixelValue The depth of sourcePoint2f in millimeters
+   * @param sourceSensorType Source sensor type
+   * @param targetSensorType Target sensor type
+   * @param targetPoint2f Target 2d point value
+   *
+   * @return bool Transform result
+   */
+  static bool calibration2dTo2d(const OBCalibrationParam calibrationParam,
+                                const OBPoint2f sourcePoint2f, const float sourceDepthPixelValue,
+                                const OBSensorType sourceSensorType,
+                                const OBSensorType targetSensorType, OBPoint2f *targetPoint2f);
 
-    /**
-     * @brief Transforms the depth frame into the geometry of the color camera.
-     *
-     * @param device Device handle
-     * @param depthFrame Input depth frame
-     * @param targetColorCameraWidth Target color camera width
-     * @param targetColorCameraHeight Target color camera height
-     *
-     * @return std::shared_ptr<ob::Frame> Transformed depth frame
-     */
-    static std::shared_ptr<ob::Frame> transformationDepthFrameToColorCamera(std::shared_ptr<ob::Device> device, std::shared_ptr<ob::Frame> depthFrame,
-                                                                            uint32_t targetColorCameraWidth, uint32_t targetColorCameraHeight);
+  /**
+   * @brief Transforms the depth frame into the geometry of the color camera.
+   *
+   * @param device Device handle
+   * @param depthFrame Input depth frame
+   * @param targetColorCameraWidth Target color camera width
+   * @param targetColorCameraHeight Target color camera height
+   *
+   * @return std::shared_ptr<ob::Frame> Transformed depth frame
+   */
+  static std::shared_ptr<ob::Frame> transformationDepthFrameToColorCamera(
+      std::shared_ptr<ob::Device> device, std::shared_ptr<ob::Frame> depthFrame,
+      uint32_t targetColorCameraWidth, uint32_t targetColorCameraHeight);
 
-    /**
-     * @brief Init transformation tables
-     *
-     * @param calibrationParam Device calibration param,see pipeline::getCalibrationParam
-     * @param sensorType sensor type
-     * @param data input data,needs to be allocated externally.During initialization, the external allocation size is 'dataSize', for example, dataSize = 1920 *
-     * 1080 * 2*sizeof(float) (1920 * 1080 represents the image resolution, and 2 represents two LUTs, one for x-coordinate and one for y-coordinate).
-     * @param dataSize input data size
-     * @param xyTables output xy tables
-     *
-     * @return bool Transform result
-     */
-    static bool transformationInitXYTables(const OBCalibrationParam calibrationParam, const OBSensorType sensorType, float *data, uint32_t *dataSize,
-                                           OBXYTables *xyTables);
+  /**
+   * @brief Init transformation tables
+   *
+   * @param calibrationParam Device calibration param,see pipeline::getCalibrationParam
+   * @param sensorType sensor type
+   * @param data input data,needs to be allocated externally.During initialization, the external
+   * allocation size is 'dataSize', for example, dataSize = 1920 * 1080 * 2*sizeof(float) (1920 *
+   * 1080 represents the image resolution, and 2 represents two LUTs, one for x-coordinate and one
+   * for y-coordinate).
+   * @param dataSize input data size
+   * @param xyTables output xy tables
+   *
+   * @return bool Transform result
+   */
+  static bool transformationInitXYTables(const OBCalibrationParam calibrationParam,
+                                         const OBSensorType sensorType, float *data,
+                                         uint32_t *dataSize, OBXYTables *xyTables);
 
-    /**
-     * @brief Transform depth image to point cloud data
-     *
-     * @param xyTables input xy tables,see CoordinateTransformHelper::transformationInitXYTables
-     * @param depthImageData input depth image data
-     * @param pointCloudData output point cloud data
-     *
-     */
-    static void transformationDepthToPointCloud(OBXYTables *xyTables, const void *depthImageData, void *pointCloudData);
+  /**
+   * @brief Transform depth image to point cloud data
+   *
+   * @param xyTables input xy tables,see CoordinateTransformHelper::transformationInitXYTables
+   * @param depthImageData input depth image data
+   * @param pointCloudData output point cloud data
+   *
+   */
+  static void transformationDepthToPointCloud(OBXYTables *xyTables, const void *depthImageData,
+                                              void *pointCloudData);
 
-    /**
-     * @brief Transform depth image to RGBD point cloud data
-     *
-     * @param xyTables input xy tables,see CoordinateTransformHelper::transformationInitXYTables
-     * @param depthImageData input depth image data
-     * @param colorImageData input color image data (only RGB888 support)
-     * @param pointCloudData output RGBD point cloud data
-     *
-     */
-    static void transformationDepthToRGBDPointCloud(OBXYTables *xyTables, const void *depthImageData, const void *colorImageData, void *pointCloudData);
+  /**
+   * @brief Transform depth image to RGBD point cloud data
+   *
+   * @param xyTables input xy tables,see CoordinateTransformHelper::transformationInitXYTables
+   * @param depthImageData input depth image data
+   * @param colorImageData input color image data (only RGB888 support)
+   * @param pointCloudData output RGBD point cloud data
+   *
+   */
+  static void transformationDepthToRGBDPointCloud(OBXYTables *xyTables, const void *depthImageData,
+                                                  const void *colorImageData, void *pointCloudData);
 };
 }  // namespace ob

orbbec_camera/include/orbbec_camera/constants.h

@@ -103,6 +103,7 @@ const float ROS_DEPTH_SCALE = 0.001;
 const int32_t FEMTO_OW_PID = 0x0638;
 const int32_t FEMTO_BOLT_PID = 0x066b;
 const int32_t FEMTO_LIVE_PID = 0x0668;
+const uint32_t FEMTO_MEGA_PID = 0x0669;
 const int32_t FEMTO_PID = 0x0635;
 const int32_t ASTRA_PLUS_PID = 0x0636;
 const int32_t ASTRA_PLUS_S_PID = 0x0637;

orbbec_camera/include/orbbec_camera/ob_camera_node.h

@@ -139,9 +139,9 @@ class OBCameraNode {
       const rcl_interfaces::msg::ParameterDescriptor& parameter_descriptor =
           rcl_interfaces::msg::ParameterDescriptor());  // set and get parameter
 
-  ~OBCameraNode();
+  ~OBCameraNode() noexcept;
 
-  void clean();
+  void clean() noexcept;
 
   void startStreams();
 
@@ -538,5 +538,13 @@ class OBCameraNode {
   bool retry_on_usb3_detection_failure_ = false;
   std::atomic_bool is_camera_node_initialized_{false};
   int laser_energy_level_ = -1;
+  ob::PointCloudFilter depth_point_cloud_filter_;
+  ob::PointCloudFilter colored_point_cloud_filter_;
+  std::optional<OBCalibrationParam> calibration_param_;
+  std::optional<OBXYTables> xy_tables_;
+  float* xy_table_data_ = nullptr;
+  uint32_t xy_table_data_size_ = 0;
+  uint8_t* rgb_pint_cloud_buffer_ = nullptr;
+  uint32_t rgb_pint_cloud_buffer_size_ = 0;
 };
 }  // namespace orbbec_camera

orbbec_camera/src/ob_camera_node.cpp

@@ -23,6 +23,7 @@
 #include <filesystem>
 #include <fstream>
 #include "diagnostic_msgs/msg/diagnostic_status.hpp"
+#include "libobsensor/hpp/Utils.hpp"
 
 #if defined(USE_RK_HW_DECODER)
 #include "orbbec_camera/rk_mpp_decoder.h"
@@ -67,6 +68,12 @@ OBCameraNode::OBCameraNode(rclcpp::Node *node, std::shared_ptr<ob::Device> devic
   if (enable_stream_[COLOR]) {
     rgb_buffer_ = new uint8_t[width_[COLOR] * height_[COLOR] * 3];
   }
+  if (enable_colored_point_cloud_ && enable_stream_[DEPTH] && enable_stream_[COLOR]) {
+    rgb_pint_cloud_buffer_size_ = width_[COLOR] * height_[COLOR] * sizeof(OBColorPoint);
+    rgb_pint_cloud_buffer_ = new uint8_t[rgb_pint_cloud_buffer_size_];
+    xy_table_data_size_ = width_[DEPTH] * height_[DEPTH] * 2;
+    xy_table_data_ = new float[xy_table_data_size_];
+  }
   is_camera_node_initialized_ = true;
 }
 
@@ -85,9 +92,9 @@ void OBCameraNode::setAndGetNodeParameter(
   }
 }
 
-OBCameraNode::~OBCameraNode() { clean(); }
+OBCameraNode::~OBCameraNode() noexcept { clean(); }
 
-void OBCameraNode::clean() {
+void OBCameraNode::clean() noexcept {
   std::lock_guard<decltype(device_lock_)> lock(device_lock_);
   RCLCPP_WARN_STREAM(logger_, "Do destroy ~OBCameraNode");
   is_running_.store(false);
@@ -108,6 +115,14 @@ void OBCameraNode::clean() {
     delete[] rgb_buffer_;
     rgb_buffer_ = nullptr;
   }
+  if (rgb_pint_cloud_buffer_) {
+    delete[] rgb_pint_cloud_buffer_;
+    rgb_pint_cloud_buffer_ = nullptr;
+  }
+  if (xy_table_data_) {
+    delete[] xy_table_data_;
+    xy_table_data_ = nullptr;
+  }
 }
 
 void OBCameraNode::setupDevices() {
@@ -1189,13 +1204,8 @@ void OBCameraNode::publishPointCloud(const std::shared_ptr<ob::FrameSet> &frame_
 
 void OBCameraNode::publishDepthPointCloud(const std::shared_ptr<ob::FrameSet> &frame_set) {
   (void)frame_set;
-  if (!depth_cloud_pub_) {
-    return;
-  }
-  if (depth_cloud_pub_->get_subscription_count() == 0 || !enable_point_cloud_) {
-    return;
-  }
-  if (!depth_frame_) {
+  if (!depth_cloud_pub_ || depth_cloud_pub_->get_subscription_count() == 0 ||
+      !enable_point_cloud_ || !depth_frame_) {
     return;
   }
   std::lock_guard<decltype(point_cloud_mutex_)> point_cloud_msg_lock(point_cloud_mutex_);
@@ -1204,19 +1214,21 @@ void OBCameraNode::publishDepthPointCloud(const std::shared_ptr<ob::FrameSet> &f
     RCLCPP_ERROR_STREAM(logger_, "depth frame is null");
     return;
   }
+  CHECK_NOTNULL(pipeline_);
+  auto camera_params = pipeline_->getCameraParam();
+  depth_point_cloud_filter_.setCameraParam(camera_params);
+  float depth_scale = depth_frame->getValueScale();
+  depth_point_cloud_filter_.setPositionDataScaled(depth_scale);
+  depth_point_cloud_filter_.setCreatePointFormat(OB_FORMAT_POINT);
+  auto result_frame = depth_point_cloud_filter_.process(depth_frame);
+  if (!result_frame) {
+    RCLCPP_ERROR_STREAM(logger_, "Failed to process depth frame");
+    return;
+  }
+  auto point_size = result_frame->dataSize() / sizeof(OBPoint);
+  auto *points = static_cast<OBPoint *>(result_frame->data());
   auto width = depth_frame->width();
   auto height = depth_frame->height();
-  auto depth_profile = stream_profile_[DEPTH]->as<ob::VideoStreamProfile>();
-  CHECK_NOTNULL(depth_profile.get());
-  auto depth_intrinsics = depth_profile->getIntrinsic();
-  float fdx = depth_intrinsics.fx * ((float)(width) / depth_intrinsics.width);
-  float fdy = depth_intrinsics.fy * ((float)(height) / depth_intrinsics.height);
-  fdx = 1 / fdx;
-  fdy = 1 / fdy;
-  float u0 = depth_intrinsics.cx * ((float)(width) / depth_intrinsics.width);
-  float v0 = depth_intrinsics.cy * ((float)(height) / depth_intrinsics.height);
-
-  const auto *depth_data = (uint16_t *)depth_frame->data();
   auto point_cloud_msg = std::make_unique<sensor_msgs::msg::PointCloud2>();
   sensor_msgs::PointCloud2Modifier modifier(*point_cloud_msg);
   modifier.setPointCloud2FieldsByString(1, "xyz");
@@ -1228,28 +1240,19 @@ void OBCameraNode::publishDepthPointCloud(const std::shared_ptr<ob::FrameSet> &f
   sensor_msgs::PointCloud2Iterator<float> iter_x(*point_cloud_msg, "x");
   sensor_msgs::PointCloud2Iterator<float> iter_y(*point_cloud_msg, "y");
   sensor_msgs::PointCloud2Iterator<float> iter_z(*point_cloud_msg, "z");
-  size_t valid_count = 0;
-  const static float MIN_DISTANCE = 20.0;
-  const static float MAX_DISTANCE = 10000.0;
-  double depth_scale = depth_frame->getValueScale();
+  const static float MIN_DISTANCE = 20.0;     // 2cm
+  const static float MAX_DISTANCE = 10000.0;  // 10m
   const static float min_depth = MIN_DISTANCE / depth_scale;
   const static float max_depth = MAX_DISTANCE / depth_scale;
-  for (uint32_t y = 0; y < height; y++) {
-    for (uint32_t x = 0; x < width; x++) {
-      bool valid_point = true;
-      if (depth_data[y * width + x] < min_depth || depth_data[y * width + x] > max_depth) {
-        valid_point = false;
-      }
-      if (valid_point || ordered_pc_) {
-        float xf = (x - u0) * fdx;
-        float yf = (y - v0) * fdy;
-        float zf = depth_data[y * width + x] * depth_scale;
-        *iter_x = zf * xf / 1000.0;
-        *iter_y = zf * yf / 1000.0;
-        *iter_z = zf / 1000.0;
-        ++iter_x, ++iter_y, ++iter_z;
-        valid_count++;
-      }
+  size_t valid_count = 0;
+  for (size_t i = 0; i < point_size; i++) {
+    bool valid_point = points[i].z >= min_depth && points[i].z <= max_depth;
+    if (valid_point || ordered_pc_) {
+      *iter_x = static_cast<float>(points[i].x / 1000.0);
+      *iter_y = static_cast<float>(points[i].y / 1000.0);
+      *iter_z = static_cast<float>(points[i].z / 1000.0);
+      ++iter_x, ++iter_y, ++iter_z;
+      valid_count++;
     }
   }
   if (valid_count == 0) {
@@ -1288,16 +1291,12 @@ void OBCameraNode::publishDepthPointCloud(const std::shared_ptr<ob::FrameSet> &f
 }
 
 void OBCameraNode::publishColoredPointCloud(const std::shared_ptr<ob::FrameSet> &frame_set) {
-  if (!depth_registration_cloud_pub_) {
-    return;
-  }
-  if (depth_registration_cloud_pub_->get_subscription_count() == 0 ||
-      !enable_colored_point_cloud_) {
-    return;
-  }
-  if (!depth_frame_) {
+  if (!depth_registration_cloud_pub_ ||
+      depth_registration_cloud_pub_->get_subscription_count() == 0 ||
+      !enable_colored_point_cloud_ || !depth_frame_) {
     return;
   }
+
   CHECK_NOTNULL(depth_frame_.get());
   std::lock_guard<decltype(point_cloud_mutex_)> point_cloud_msg_lock(point_cloud_mutex_);
   auto depth_frame = depth_frame_->as<ob::DepthFrame>();
@@ -1314,16 +1313,40 @@ void OBCameraNode::publishColoredPointCloud(const std::shared_ptr<ob::FrameSet>
                  depth_height, color_width, color_height);
     return;
   }
-  auto camera_params = pipeline_->getCameraParam();
-  auto intrinsics = camera_params.rgbIntrinsic;
-  float fdx = intrinsics.fx * ((float)(color_width) / intrinsics.width);
-  float fdy = intrinsics.fy * ((float)(color_height) / intrinsics.height);
-  fdx = 1 / fdx;
-  fdy = 1 / fdy;
-  float u0 = intrinsics.cx * ((float)(color_width) / intrinsics.width);
-  float v0 = intrinsics.cy * ((float)(color_height) / intrinsics.height);
-  const auto *depth_data = (uint16_t *)depth_frame->data();
+  if (!xy_tables_.has_value()) {
+    calibration_param_ = pipeline_->getCalibrationParam(pipeline_config_);
+
+    uint32_t table_size =
+        color_width * color_height * 2;  // one for x-coordinate and one for y-coordinate LUT
+    if (xy_table_data_size_ != table_size) {
+      RCLCPP_INFO_STREAM(logger_, "Init xy tables with size " << table_size);
+      xy_table_data_size_ = table_size;
+      delete[] xy_table_data_;
+      xy_table_data_ = new float[table_size];
+    }
+
+    xy_tables_ = OBXYTables();
+    CHECK_NOTNULL(xy_table_data_);
+    if (!ob::CoordinateTransformHelper::transformationInitXYTables(
+            *calibration_param_, OB_SENSOR_COLOR, xy_table_data_, &table_size, &(*xy_tables_))) {
+      RCLCPP_ERROR_STREAM(logger_, "Failed to init xy tables");
+      return;
+    }
+  }
+
+  const auto *depth_data = (uint8_t *)depth_frame->data();
   const auto *color_data = (uint8_t *)(rgb_buffer_);
+  CHECK_NOTNULL(rgb_pint_cloud_buffer_);
+  uint32_t point_cloud_buffer_size = color_width * color_height * sizeof(OBColorPoint);
+  if (point_cloud_buffer_size > rgb_pint_cloud_buffer_size_) {
+    delete[] rgb_pint_cloud_buffer_;
+    rgb_pint_cloud_buffer_ = new uint8_t[point_cloud_buffer_size];
+    rgb_pint_cloud_buffer_size_ = point_cloud_buffer_size;
+  }
+  memset(rgb_pint_cloud_buffer_, 0, rgb_pint_cloud_buffer_size_);
+  auto *point_cloud = (OBColorPoint *)rgb_pint_cloud_buffer_;
+  ob::CoordinateTransformHelper::transformationDepthToRGBDPointCloud(&(*xy_tables_), depth_data,
+                                                                     color_data, point_cloud);
   auto point_cloud_msg = std::make_unique<sensor_msgs::msg::PointCloud2>();
   sensor_msgs::PointCloud2Modifier modifier(*point_cloud_msg);
   modifier.setPointCloud2FieldsByString(1, "xyz");
@@ -1347,31 +1370,17 @@ void OBCameraNode::publishColoredPointCloud(const std::shared_ptr<ob::FrameSet>
   double depth_scale = depth_frame->getValueScale();
   static float min_depth = MIN_DISTANCE / depth_scale;
   static float max_depth = MAX_DISTANCE / depth_scale;
-  for (uint32_t y = 0; y < color_height; y++) {
-    for (uint32_t x = 0; x < color_width; x++) {
-      bool valid_point = true;
-      float depth = depth_data[y * depth_width + x];
-      if (depth < min_depth || depth > max_depth) {
-        valid_point = false;
-      }
-      if (valid_point || ordered_pc_) {
-        float xf = (x - u0) * fdx;
-        float yf = (y - v0) * fdy;
-        float zf = depth * depth_scale;
-        *iter_x = zf * xf / 1000.0;
-        *iter_y = zf * yf / 1000.0;
-        *iter_z = zf / 1000.0;
-        *iter_r = color_data[(y * color_width + x) * 3];
-        *iter_g = color_data[(y * color_width + x) * 3 + 1];
-        *iter_b = color_data[(y * color_width + x) * 3 + 2];
-        ++iter_x;
-        ++iter_y;
-        ++iter_z;
-        ++iter_r;
-        ++iter_g;
-        ++iter_b;
-        ++valid_count;
-      }
+  for (size_t i = 0; i < color_width * color_height; i++) {
+    bool valid_point = point_cloud[i].z >= min_depth && point_cloud[i].z <= max_depth;
+    if (valid_point || ordered_pc_) {
+      *iter_x = static_cast<float>(point_cloud[i].x / 1000.0);
+      *iter_y = static_cast<float>(point_cloud[i].y / 1000.0);
+      *iter_z = static_cast<float>(point_cloud[i].z / 1000.0);
+      *iter_r = static_cast<uint8_t>(point_cloud[i].r);
+      *iter_g = static_cast<uint8_t>(point_cloud[i].g);
+      *iter_b = static_cast<uint8_t>(point_cloud[i].b);
+      ++iter_x, ++iter_y, ++iter_z, ++iter_r, ++iter_g, ++iter_b;
+      ++valid_count;
     }
   }
   if (valid_count == 0) {
@@ -1576,7 +1585,8 @@ bool OBCameraNode::decodeColorFrameToBuffer(const std::shared_ptr<ob::Frame> &fr
       metadata_publishers_[COLOR]->get_subscription_count() > 0) {
     has_subscriber = true;
   }
-  if (camera_info_publishers_[COLOR]->get_subscription_count() > 0) {
+  if (camera_info_publishers_.count(COLOR) &&
+      camera_info_publishers_[COLOR]->get_subscription_count() > 0) {
     has_subscriber = true;
   }
   bool is_decoded = false;