diff --git a/depth_segmentation/cfg/DepthSegmenter.cfg b/depth_segmentation/cfg/DepthSegmenter.cfg
index 58db927..222798e 100755
--- a/depth_segmentation/cfg/DepthSegmenter.cfg
+++ b/depth_segmentation/cfg/DepthSegmenter.cfg
@@ -12,6 +12,12 @@ general_params.add("dilate_depth_image", bool_t, 0,
 general_params.add("dilation_size", int_t, 0,
                    "Size of the dilation for the registered depth image.", 1, 1,
                    15)
+general_params.add("median_filter_depth_image", bool_t, 0,
+                   "Apply median filter to the depth image.", False)
+general_params.add(
+    "median_filter_size", int_t, 0,
+    "Size of the median filter kernel for the registered depth image.", 1, 1,
+    5)
 general_params.add("visualize_segmented_scene", bool_t, 0,
                    "Publish segmented scene.", False)
 general_params.add("display_rgb_image", bool_t, 0, "Visualize RGB image.",
diff --git a/depth_segmentation/cfg/asl_office_floor_config.yaml b/depth_segmentation/cfg/asl_office_floor_config.yaml
index 7159c4f..decce42 100644
--- a/depth_segmentation/cfg/asl_office_floor_config.yaml
+++ b/depth_segmentation/cfg/asl_office_floor_config.yaml
@@ -32,6 +32,8 @@ max_distance_sensor_noise_param_3rd_order: 0.0001
 max_distance_use_max_distance: true
 max_distance_use_threshold: true
 max_distance_window_size: 1
+median_filter_depth_image: false
+median_filter_size: 1
 min_convexity_display: false
 min_convexity_mask_threshold: -0.0005
 min_convexity_morphological_opening_size: 1
diff --git a/depth_segmentation/cfg/primesense_config.yaml b/depth_segmentation/cfg/primesense_config.yaml
index 356a735..a3be826 100644
--- a/depth_segmentation/cfg/primesense_config.yaml
+++ b/depth_segmentation/cfg/primesense_config.yaml
@@ -26,6 +26,8 @@ max_distance_sensor_noise_param_2nd_order: 0.0019
 max_distance_sensor_noise_param_3rd_order: 0.0001
 max_distance_use_threshold: true
 max_distance_window_size: 1
+median_filter_depth_image: false
+median_filter_size: 1
 min_convexity_display: true
 min_convexity_mask_threshold: -0.0005
 min_convexity_morphological_opening_size: 1
diff --git a/depth_segmentation/cfg/scenenet_config.yaml b/depth_segmentation/cfg/scenenet_config.yaml
index d559a59..12009b2 100644
--- a/depth_segmentation/cfg/scenenet_config.yaml
+++ b/depth_segmentation/cfg/scenenet_config.yaml
@@ -31,6 +31,8 @@ max_distance_sensor_noise_param_2nd_order: 0.0019
 max_distance_sensor_noise_param_3rd_order: 0.0001
 max_distance_use_threshold: true
 max_distance_window_size: 1
+median_filter_depth_image: false
+median_filter_size: 1
 min_convexity_display: true
 min_convexity_mask_threshold: -0.0005
 min_convexity_morphological_opening_size: 1
diff --git a/depth_segmentation/cfg/scenenn_config.yaml b/depth_segmentation/cfg/scenenn_config.yaml
index e4d0bdc..8f34aac 100644
--- a/depth_segmentation/cfg/scenenn_config.yaml
+++ b/depth_segmentation/cfg/scenenn_config.yaml
@@ -32,6 +32,8 @@ max_distance_sensor_noise_param_3rd_order: 0.0001
 max_distance_use_max_distance: true
 max_distance_use_threshold: true
 max_distance_window_size: 1
+median_filter_depth_image: false
+median_filter_size: 1
 min_convexity_display: false
 min_convexity_mask_threshold: -0.0005
 min_convexity_morphological_opening_size: 1
diff --git a/depth_segmentation/include/depth_segmentation/common.h b/depth_segmentation/include/depth_segmentation/common.h
index 5143978..1d04948 100644
--- a/depth_segmentation/include/depth_segmentation/common.h
+++ b/depth_segmentation/include/depth_segmentation/common.h
@@ -132,6 +132,8 @@ struct IsNotNan {
 struct Params {
   bool dilate_depth_image = false;
   size_t dilation_size = 1u;
+  bool median_filter_depth_image = false;
+  size_t median_filter_size = 1u;
   FinalEdgeMapParams final_edge;
   LabelMapParams label;
   DepthDiscontinuityMapParams depth_discontinuity;
diff --git a/depth_segmentation/src/depth_segmentation.cpp b/depth_segmentation/src/depth_segmentation.cpp
index 2eecdb2..ca2519b 100644
--- a/depth_segmentation/src/depth_segmentation.cpp
+++ b/depth_segmentation/src/depth_segmentation.cpp
@@ -178,6 +178,8 @@ void DepthSegmenter::dynamicReconfigureCallback(
   // General params.
   params_.dilate_depth_image = config.dilate_depth_image;
   params_.dilation_size = config.dilation_size;
+  params_.median_filter_depth_image = config.median_filter_depth_image;
+  params_.median_filter_size = config.median_filter_size;
   params_.visualize_segmented_scene = config.visualize_segmented_scene;
   params_.display_rgb_image = config.display_rgb_image;
   params_.display_depth_image = config.display_depth_image;
diff --git a/depth_segmentation/src/depth_segmentation_node.cpp b/depth_segmentation/src/depth_segmentation_node.cpp
index 07a7a80..3fa41fb 100644
--- a/depth_segmentation/src/depth_segmentation_node.cpp
+++ b/depth_segmentation/src/depth_segmentation_node.cpp
@@ -388,6 +388,11 @@ class DepthSegmentationNode {
       *dilated_rescaled_depth = *rescaled_depth;
     }
 
+    if (params_.median_filter_depth_image) {
+      cv::medianBlur(*dilated_rescaled_depth, *dilated_rescaled_depth,
+                     params_.median_filter_size);
+    }
+
     *bw_image = cv::Mat::zeros(cv_rgb_image->image.size(), CV_8UC1);
 
     cvtColor(cv_rgb_image->image, *bw_image, cv::COLOR_RGB2GRAY);