custom threshold and assume invalid points are far away instead of super close

diff --git a/cpp/sampleOpenCV/sampleOpenCV.cpp b/cpp/sampleOpenCV/sampleOpenCV.cpp
index dd94565..cd81a3d 100644 (file)
--- a/cpp/sampleOpenCV/sampleOpenCV.cpp
+++ b/cpp/sampleOpenCV/sampleOpenCV.cpp
@@ -39,6 +39,18 @@ using namespace cv;
 class MyListener : public IDepthDataListener
 {
 
+    static uint32_t const num_dist_columns_=5;
+    uint32_t latest_distance_[num_dist_columns_];
+    uint32_t latest_distance_diff_[num_dist_columns_];
+
+    const int DELAY_BLUR = 100;
+    // const int MAX_KERNEL_LENGTH = 31;
+    const int MAX_KERNEL_LENGTH = 23;
+
+    bool normblurImage = true;
+
+    const uint8_t confidence_threshold_ = 5;
+
 public :
 
     MyListener() :
@@ -70,12 +82,16 @@ public :
             for (int x = 0; x < zImage.cols; x++, k++)
             {
                 auto curPoint = data->points.at (k);
-                if (curPoint.depthConfidence > 0)
+                if (curPoint.depthConfidence > confidence_threshold_)
                 {
                     // if the point is valid, map the pixel from 3D world
                     // coordinates to a 2D plane (this will distort the image)
                     zRowPtr[x] = adjustZValue (curPoint.z);
                     grayRowPtr[x] = adjustGrayValue (curPoint.grayValue);
+                } else {
+                    //asume point is as far away as possible and thus "SAFE" for obstacle avoidance
+                    zRowPtr[x] = 255;
+                    grayRowPtr[x] = 255;
                 }
             }
         }