add fillter test cam intel 14:33

include/robot_depth_image_proc/depth_frame_filter.h

@@ -0,0 +1,62 @@
+#ifndef ROBOT_DEPTH_IMAGE_PROC_DEPTH_FRAME_FILTER_H
+#define ROBOT_DEPTH_IMAGE_PROC_DEPTH_FRAME_FILTER_H
+
+#include <cstdint>
+#include <vector>
+
+#include <robot_sensor_msgs/Image.h>
+
+#include <robot_depth_image_proc/point_cloud_xyz.h>
+
+namespace depth_image_proc
+{
+
+/**
+ * Stateful per-camera depth image pre-filter, applied before the depth ->
+ * point cloud conversion:
+ *
+ *  - Edge filter: invalidates pixels sitting on depth discontinuities plus a
+ *    dilated halo around them. Border pixels mix foreground and background
+ *    light, producing "flying pixels" floating between the object and the
+ *    background. Besides adjacent-pixel jumps it also catches the smooth
+ *    ramps flying pixels smear into (edge_window baseline) and pixels
+ *    hugging no-data holes at occlusion boundaries (edge_invalid_border).
+ *  - Temporal filter: keeps a pixel only after its depth stayed within
+ *    temporal_max_delta for temporal_min_frames consecutive frames. Rejects
+ *    the transient speckle left by objects moving through the field of view.
+ *
+ * Rejected pixels are invalidated in place (0 for 16UC1/mono16, NaN for
+ * 32FC1). History is never used to fill pixels back in, so the filter cannot
+ * create ghost obstacles of objects that already left the scene.
+ *
+ * One instance per camera stream. Not thread-safe: call apply() from a
+ * single thread (e.g. a single-threaded ros::spin()).
+ */
+class DepthFrameFilter
+{
+public:
+  explicit DepthFrameFilter(const DepthFilterConfig& config);
+
+  /// Invalidates edge-halo and temporally unstable pixels in place.
+  /// Unsupported encodings are left untouched.
+  void apply(robot_sensor_msgs::Image& depth_msg);
+
+private:
+  template<typename T>
+  void applyImpl(robot_sensor_msgs::Image& depth_msg);
+
+  void resetHistory(uint32_t width, uint32_t height);
+
+  const DepthFilterConfig config_;
+
+  uint32_t width_{0};
+  uint32_t height_{0};
+  std::vector<float> prev_depth_m_;     ///< <= 0 means no valid history
+  std::vector<uint16_t> stable_frames_; ///< consecutive stable frame count
+  std::vector<uint8_t> edge_mask_;
+  std::vector<uint8_t> dilate_scratch_;
+};
+
+}  // namespace depth_image_proc
+
+#endif

include/robot_depth_image_proc/point_cloud_xyz.h

@@ -27,11 +27,40 @@ struct DepthFilterConfig
   /// the speckle ("flying pixel") filter.
   int speckle_min_neighbors = 3;
 
+  /// Depth jump [m] between adjacent pixels marking an object edge. Pixels on
+  /// and around edges carry mixed foreground/background depth (flying
+  /// pixels). 0 disables the edge filter.
+  double edge_max_delta = 0.1;
+  /// Halo radius [px] removed around detected edges (Chebyshev distance).
+  int edge_dilation = 2;
+  /// Edge detection also compares pixels this many px apart and marks the
+  /// whole span in between. Flying pixels smear into smooth foreground-to-
+  /// background ramps whose per-pixel step stays below edge_max_delta; the
+  /// wider baseline still sees the full jump. 1 keeps only the
+  /// adjacent-pixel test.
+  int edge_window = 4;
+  /// Treat valid pixels bordering invalid (no-data) pixels as edge pixels.
+  /// Mixed pixels hug the no-data band stereo matching leaves at occlusion
+  /// boundaries, where no valid-to-valid depth jump exists to detect.
+  bool edge_invalid_border = true;
+
+  /// Per-pixel frame-to-frame depth tolerance [m] to count as stable.
+  double temporal_max_delta = 0.06;
+  /// Keep a pixel only after it has been stable for this many consecutive
+  /// frames. Rejects transient speckle from objects moving through the view
+  /// at the cost of (frames - 1) camera periods of detection latency.
+  /// 0 or 1 disables the temporal filter.
+  int temporal_min_frames = 2;
+
   bool valid() const
   {
     return decimation >= 1 && range_min >= 0.0 && range_max > range_min &&
            speckle_max_delta > 0.0 && speckle_min_neighbors >= 0 &&
-           speckle_min_neighbors <= 8;
+           speckle_min_neighbors <= 8 && edge_max_delta >= 0.0 &&
+           edge_dilation >= 0 && edge_dilation <= 10 &&
+           edge_window >= 1 && edge_window <= 32 &&
+           temporal_max_delta > 0.0 && temporal_min_frames >= 0 &&
+           temporal_min_frames <= 100;
   }
 };