X-Git-Url: https://git.realraum.at/?a=blobdiff_plain;f=cpp%2FsampleOpenCV%2FsampleOpenCV.cpp;h=8a085fea94a00012e6968c58af5a5e296f025861;hb=5b0c366392979987048dd7e8838048726c2502b2;hp=ca4c208807662142a71876470d7f7b2e6e23f5a1;hpb=a1a44e4fb655e6ba7eef56e83526bd51e92f1ba4;p=201903hackathon.git

diff --git a/cpp/sampleOpenCV/sampleOpenCV.cpp b/cpp/sampleOpenCV/sampleOpenCV.cpp
index ca4c208..8a085fe 100644
--- a/cpp/sampleOpenCV/sampleOpenCV.cpp
+++ b/cpp/sampleOpenCV/sampleOpenCV.cpp
@@ -50,6 +50,7 @@ class MyListener : public IDepthDataListener
     bool normblurImage = true;
 
     const uint8_t confidence_threshold_ = 0;
+    const double distance_threshold_ = 50;
 
 public :
 
@@ -70,6 +71,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);
@@ -80,9 +82,11 @@ 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);
+                confRowPtr[x] = curPoint.depthConfidence;
                 if (curPoint.depthConfidence > confidence_threshold_)
                 {
                     // if the point is valid, map the pixel from 3D world
@@ -143,6 +147,9 @@ public :
             std::cout << "col" << col << "   min:" << min << "("<<latest_min_distance_diff_[col]<<")"<< "   max:" << max << std::endl;
         }
 
+        //naive obstacle avoidance demo
+        naiveObstacleAvoidanceDemo();
+
         // scale and display the depth image
         scaledZImage.create (Size (data->width * 4, data->height * 4), CV_8UC1);
         resize (zImage8, scaledZImage, scaledZImage.size());
@@ -157,10 +164,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)
@@ -214,10 +222,46 @@ private:
         return newGrayValue;
     }
 
+    bool naiveIsObstaclePresent(bool free_paths[num_dist_columns_])
+    {
+        bool rv=false;
+        for (uint32_t col=0; col<num_dist_columns_; col++)
+        {
+            free_paths[col] = latest_min_distance_[col] > distance_threshold_;
+            if (!free_paths[col])
+            {
+                rv=true;
+            }
+        }
+        return rv;
+    }
+
+    void naiveObstacleAvoidanceDemo()
+    {
+        bool free_paths[num_dist_columns_];
+        if (naiveIsObstaclePresent(free_paths))
+        {
+            assert(num_dist_columns_ == 4);
+            if (free_paths[1] && free_paths[2])
+            {
+                std::cout << "GOSTRAIGHT" << std::endl;
+            } else if (free_paths[0])
+            {
+                std::cout << "GOLEFT" << std::endl;
+            } else if (free_paths[3])
+            {
+                std::cout << "GORIGHT" << std::endl;
+            } else {
+                std::cout << "STOP" << std::endl;
+            }
+        }
+    }
+
     // define images for depth and gray
     // 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
@@ -319,7 +363,8 @@ 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