From 562296742342225eb5b0d73fdbf5925854435146 Mon Sep 17 00:00:00 2001 From: Bernhard Tittelbach Date: Tue, 26 Mar 2019 02:59:09 +0100 Subject: [PATCH] custom threshold and assume invalid points are far away instead of super close --- cpp/sampleOpenCV/sampleOpenCV.cpp | 18 +++++++++++++++++- 1 file changed, 17 insertions(+), 1 deletion(-) diff --git a/cpp/sampleOpenCV/sampleOpenCV.cpp b/cpp/sampleOpenCV/sampleOpenCV.cpp index dd94565..cd81a3d 100644 --- 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; } } } -- 1.7.10.4