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

jian-dong · jian-dong · commit e1b106c626c6 · 2024-06-21T19:56:57.000+08:00
diff --git a/orbbec_camera/SDK/include/libobsensor/hpp/Utils.hpp b/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
diff --git a/orbbec_camera/include/orbbec_camera/constants.h b/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;
diff --git a/orbbec_camera/include/orbbec_camera/ob_camera_node.h b/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
diff --git a/orbbec_camera/src/ob_camera_node.cpp b/orbbec_camera/src/ob_camera_node.cpp
