X-Git-Url: https://git.realraum.at/?a=blobdiff_plain;f=cpp%2FsampleOpenCV%2FsampleOpenCV.cpp;h=93384eafad16967fe25d22f954e582b1b92d434c;hb=90ae6e0b723e1573f75522b966437beb48b08dd0;hp=dd94565696ccf80457416cb3f90ff5aa4f4f78f3;hpb=b640687357b256462f75c7a08286907825b5a5a1;p=201903hackathon.git

diff --git a/cpp/sampleOpenCV/sampleOpenCV.cpp b/cpp/sampleOpenCV/sampleOpenCV.cpp
index dd94565..93384ea 100644
--- a/cpp/sampleOpenCV/sampleOpenCV.cpp
+++ b/cpp/sampleOpenCV/sampleOpenCV.cpp
@@ -39,8 +39,21 @@ using namespace cv;
 class MyListener : public IDepthDataListener
 {
 
+    static uint32_t const num_dist_columns_=4;
+    double latest_min_distance_[num_dist_columns_];
+    double latest_min_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_ = 0;
+
 public :
 
+
     MyListener() :
         undistortImage (false)
     {
@@ -57,6 +70,7 @@ public :
         // each image containing one 32Bit channel
         zImage.create (Size (data->width, data->height), CV_32FC1);
         grayImage.create (Size (data->width, data->height), CV_32FC1);
+        confidenceImage.create (Size (data->width, data->height), CV_8UC1);
 
         // set the image to zero
         zImage = Scalar::all (0);
@@ -67,15 +81,21 @@ public :
         {
             float *zRowPtr = zImage.ptr<float> (y);
             float *grayRowPtr = grayImage.ptr<float> (y);
+            uint8_t *confRowPtr = confidenceImage.ptr<uint8_t> (y);
             for (int x = 0; x < zImage.cols; x++, k++)
             {
                 auto curPoint = data->points.at (k);
-                if (curPoint.depthConfidence > 0)
+                confRowPtr[x] = curPoint.depthConfidence;
+                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;
                 }
             }
         }
@@ -100,6 +120,32 @@ public :
             undistort (temp, zImage8, cameraMatrix, distortionCoefficients);
         }
 
+        if (normblurImage)
+        {
+            auto temp = zImage8.clone();
+            for ( int i = 1; i < MAX_KERNEL_LENGTH; i = i + 2 )
+            {
+                blur( temp, zImage8, Size( i, i ), Point(-1,-1) );
+            }
+        }
+
+        //// Debug: show column part of image
+        // Mat subimg = zImage8(Rect((zImage.cols/num_dist_columns_)*3,0, zImage.cols/num_dist_columns_ , zImage8.rows));
+        // imshow ("column", subimg);
+ 
+        //detect column distance
+        auto col_width = zImage.cols/num_dist_columns_;
+        for (uint32_t col=0; col<num_dist_columns_; col++)
+        {
+            auto col_y_start = col*col_width;
+            Mat subimg = zImage8(Rect(col_y_start, 0, col_width , zImage8.rows));
+            double min, max;
+            minMaxLoc(subimg,&min,&max);
+            latest_min_distance_diff_[col]=min-latest_min_distance_[col];
+            latest_min_distance_[col]=min;
+            std::cout << "col" << col << "   min:" << min << "("<<latest_min_distance_diff_[col]<<")"<< "   max:" << max << std::endl;
+        }
+
         // scale and display the depth image
         scaledZImage.create (Size (data->width * 4, data->height * 4), CV_8UC1);
         resize (zImage8, scaledZImage, scaledZImage.size());
@@ -114,10 +160,11 @@ public :
         }
 
         // scale and display the gray image
-        scaledGrayImage.create (Size (data->width * 4, data->height * 4), CV_8UC1);
-        resize (grayImage8, scaledGrayImage, scaledGrayImage.size());
+        // scaledGrayImage.create (Size (data->width * 4, data->height * 4), CV_8UC1);
+        // resize (grayImage8, scaledGrayImage, scaledGrayImage.size());
+        // imshow ("Gray", scaledGrayImage);
 
-        imshow ("Gray", scaledGrayImage);
+        imshow ("Confidence", confidenceImage);
     }
 
     void setLensParameters (const LensParameters &lensParameters)
@@ -145,6 +192,12 @@ public :
         undistortImage = !undistortImage;
     }
 
+    void toggleNormBlur()
+    {
+        std::lock_guard<std::mutex> lock (flagMutex);
+        normblurImage = !normblurImage;
+    }
+
 private:
 
     // adjust z value to fit fixed scaling, here max dist is 2.5m
@@ -169,6 +222,7 @@ private:
     // and for their 8Bit and scaled versions
     Mat zImage, zImage8, scaledZImage;
     Mat grayImage, grayImage8, scaledGrayImage;
+    Mat confidenceImage;
 
     // lens matrices used for the undistortion of
     // the image
@@ -270,7 +324,9 @@ int main (int argc, char *argv[])
 
     // create two windows
     namedWindow ("Depth", WINDOW_AUTOSIZE);
-    namedWindow ("Gray", WINDOW_AUTOSIZE);
+    // namedWindow ("Gray", WINDOW_AUTOSIZE);
+    namedWindow ("Confidence", WINDOW_AUTOSIZE);
+    // namedWindow ("column", WINDOW_AUTOSIZE);
 
     // start capture mode
     if (cameraDevice->startCapture() != CameraStatus::SUCCESS)
@@ -291,6 +347,12 @@ int main (int argc, char *argv[])
             // toggle the undistortion of the image
             listener.toggleUndistort();
         }
+
+        if (currentKey == 'b')
+        {
+            // toggle the undistortion of the image
+            listener.toggleNormBlur();
+        }
     }
 
     // stop capture mode