Changeset 56 in pacpussensors for trunk/StereoVisionDisparity

Timestamp:

Jun 27, 2014, 2:16:15 PM (11 years ago)

Author:

phudelai

Message:

Vision component updated for StereoVision for PointGrey Flea3

Location:

trunk/StereoVisionDisparity

Files:

• DisparityMap.cpp (modified) (5 diffs)
• DisparityMap.h (modified) (2 diffs)
• ObstacleDetectionComponent.cpp (modified) (7 diffs)
• ObstacleDetectionComponent.h (modified) (2 diffs)

trunk/StereoVisionDisparity/DisparityMap.cpp

@@ -144 +144 @@
         THREAD_ALIVE = true;
         start();
+
+        emit minimumDisparityChanged(this->min_disp);
+        emit numberOfDisparityChanged(this->num_disp);
+        emit SADWindowSizeChanged(this->sbm_SADWindowSize);
+        emit textureTresholdChanged(this->sbm_textureThreshold); 
+        emit uniquenessRatioChanged(this->sbm_uniquenessRatio);
 }
 
@@ -577 +583 @@
 }
 
+
+////////////////////////////////////////////////////////////////////////////////
+void DisparityMap::setMinimumDisparity(int minimum_disp)
+{
+        this->min_disp = minimum_disp;
+
+        emit minimumDisparityChanged(this->min_disp);
+}
+
+
+////////////////////////////////////////////////////////////////////////////////
+void DisparityMap::setNumberOfDisparity(int numberOfDisparity)
+{
+        // Must be a multiple of 16
+        this->num_disp = numberOfDisparity & ~15;
+        if (this->num_disp < 16)
+                this->num_disp = 16;
+
+        emit numberOfDisparityChanged(this->num_disp);
+}
+
+
+////////////////////////////////////////////////////////////////////////////////
+void DisparityMap::setSADWindowSize(int SADWindowSize)
+{
+        // Must be an odd number
+        this->sbm_SADWindowSize = SADWindowSize | 1;
+        
+        emit SADWindowSizeChanged(this->sbm_SADWindowSize);
+}
+
+
+////////////////////////////////////////////////////////////////////////////////
+void DisparityMap::setTextureTreshold(int textureTreshold)
+{
+        this->sbm_textureThreshold = textureTreshold;
+
+        emit textureTresholdChanged(this->sbm_textureThreshold); 
+}
+
+
+////////////////////////////////////////////////////////////////////////////////
+void DisparityMap::setUniquenessRatio(int uniquenessRatio)
+{
+        this->sbm_uniquenessRatio = uniquenessRatio;
+
+        emit uniquenessRatioChanged(this->sbm_uniquenessRatio);
+}
+
+
 ////////////////////////////////////////////////////////////////////////////////
 ///     CalcDisparityMap
@@ -640 +696 @@
                         Right_img_gray = Right_img_exp;
                 }
+
+                /*cv::namedWindow("Disparity trackbar", CV_WINDOW_NORMAL);
+                cv::createTrackbar("sbm_preFilterCap", "Disparity trackbar", &sbm_preFilterCap, 63);
+                cv::createTrackbar("sbm_preFilterSize", "Disparity trackbar", &sbm_preFilterSize, 250);
+                cv::createTrackbar("sbm_SADWindowSize", "Disparity trackbar", &sbm_SADWindowSize, 250);
+                cv::createTrackbar("min_disp", "Disparity trackbar", &min_disp, 250);
+                cv::createTrackbar("num_disp", "Disparity trackbar", &num_disp, 250);
+                cv::createTrackbar("sbm_textureThreshold", "Disparity trackbar", &sbm_textureThreshold, 250);
+                cv::createTrackbar("sbm_uniquenessRatio", "Disparity trackbar", &sbm_uniquenessRatio, 250);
+                cv::createTrackbar("sbm_speckleWindowSize", "Disparity trackbar", &sbm_speckleWindowSize, 250);
+                cv::createTrackbar("sbm_speckleRange", "Disparity trackbar", &sbm_speckleRange, 250);
+                this->sbm_SADWindowSize = (this->sbm_SADWindowSize % 2 > 0) ? this->sbm_SADWindowSize : this->sbm_SADWindowSize + 1;
+                this->num_disp = (this->num_disp % 16 == 0) ? this->num_disp : this->num_disp - (this->num_disp % 16);*/
 
                 // Configuration
@@ -666 +735 @@
                                                                                                                                         // the parameter to a positive value, it will be implicitly multiplied by 16. Normally, 1 or 2 is 
                                                                                                                                         // good enough.
-                
+
                 // Calculate the disparity image (16S = scaled by 16, must be divided by 16; 32F = original value)
                 sbm(Left_img_gray, Right_img_gray, Disp_map_exp, CV_16S);
@@ -701 +770 @@
                 sgbm.disp12MaxDiff = this->sgbm_disp12MaxDiff;                          // Maximum allowed difference (in integer pixel units) in the left-right disparity check. Set it to 
                                                                                                                                         // a non-positive value to disable the check.
-                sgbm.fullDP = type == SGBM;                                                     // Set it to true to run the full-scale two-pass dynamic programming algorithm. 
+                sgbm.fullDP = type == SGBM;                                                                     // Set it to true to run the full-scale two-pass dynamic programming algorithm. 
                                                                                                                                         // It will consume O(W*H*numDisparities) bytes, which is large for 640x480 stereo 
                                                                                                                                         // and huge for HD-size pictures.

trunk/StereoVisionDisparity/DisparityMap.h

@@ -39 +39 @@
                                                                                                 public ComponentBase
 {
+        Q_OBJECT
 public:
         //============================= DEFAULT ELEMENTS ===============================================
@@ -62 +63 @@
 
         void InitDefault();
+
+public Q_SLOTS:
+        void setMinimumDisparity(int minimum_disp);
+        void setNumberOfDisparity(int numberOfDisparity);
+        void setSADWindowSize(int SADWindowSize);
+        void setTextureTreshold(int textureTreshold);
+        void setUniquenessRatio(int uniquenessRatio);
+
+Q_SIGNALS:
+        void minimumDisparityChanged(int minDisp);
+        void numberOfDisparityChanged(int numOfDisp);
+        void SADWindowSizeChanged(int SADWinSize);
+        void textureTresholdChanged(int textTresh);
+        void uniquenessRatioChanged(int uniqRatio);
 
 private:

trunk/StereoVisionDisparity/ObstacleDetectionComponent.cpp

@@ -43 +43 @@
 static const string ObstacleDetectionMemoryName_dispout = "ObstacleDetection-disp";
 
+static const string ObstacleDetectionMemoryName_dispMapNorm = "ObstacleDetection-dispMapNorm";
+static const string ObstacleDetectionMemoryName_result = "ObstacleDetection-result";
+
 //////////////////////////////////////////////////////////////////////////
 /*      ComparePoints1
@@ -134 +137 @@
         this->shmem_dispout = NULL;
 
+        if(this->shmem_dispMapNormalized)
+                delete shmem_dispMapNormalized;
+
+        this->shmem_dispMapNormalized = NULL;
+
+        if(this->shmem_result)
+                delete shmem_result;
+
+        this->shmem_result = NULL;
 }
 
@@ -169 +181 @@
         this->shmem_dispout = new ShMem(ObstacleDetectionMemoryName_dispout.c_str(), this->dispout_mem_size);
 
+        this->shmem_dispMapNormalized = new ShMem(ObstacleDetectionMemoryName_dispMapNorm.c_str(), (this->cam_width * this->cam_height));
+
+        this->shmem_result = new ShMem(ObstacleDetectionMemoryName_result.c_str(), (this->cam_width * this->cam_height * 3));
+
         // Run thread
         THREAD_ALIVE = true;
@@ -214 +230 @@
 
                 this->shmem_dispout = NULL;
+
+                if(this->shmem_dispMapNormalized)
+                        delete shmem_dispMapNormalized;
+
+                this->shmem_dispMapNormalized = NULL;
+
+                if(this->shmem_result)
+                        delete shmem_result;
+
+                this->shmem_result = NULL;
 
                 // Free the malloc memories
@@ -221 +247 @@
                 free(this->mask2_mem);
                 free(this->dispout_mem);
-
         }
 
@@ -452 +477 @@
                         }
 
+                        //memcpy(this->dispMapNorm_mem, &this->dispMapNormImageHeader, sizeof(TimestampedStructImage)); 
+                        //memcpy((void*)((TimestampedStructImage*)this->dispMapNorm_mem + 1), (void*)Disp_map.data, this->dispMapNormImageHeader.image.data_size); 
+                        this->shmem_dispMapNormalized->write((void*)Disp_map.data, (this->cam_width * this->destiny_roi_height));
+
                         // Image to detect near obstacles
                         cv::Mat v_disp_map2 = v_disp_map.clone();
@@ -540 +569 @@
 
                         // ----------------- Apply the mask in the reference image ------------------
+                
+                        std::vector<cv::Mat> channels(3);
+                        cv::Mat reference;
+                        if(this->cam_channels == 1)
+                        {
+                                cv::cvtColor((this->CurrentReferenceFrame(cv::Rect(this->destiny_roi_x, this->destiny_roi_y, this->destiny_roi_width, this->destiny_roi_height))).clone(), reference, CV_GRAY2BGR);
+                        }
+                        else
+                        {
+                                reference = (this->CurrentReferenceFrame(cv::Rect(this->destiny_roi_x, this->destiny_roi_y, this->destiny_roi_width, this->destiny_roi_height))).clone();
+                        }
+
+                        cv::split(reference, channels);
+                        
+                        masks.second = 1 - masks.second;
+                        channels[1] = masks.second.mul(channels[0]); // Activate the red color as obstacles
+                        channels[2] = masks.second.mul(channels[1]); // Activate the red color as obstacles
+
+                        //masks.second = masks.second - masks.first;
+                        //channels[0] = (1 - masks.second).mul(channels[0]); // Activate the yellow color for unclassified area
+                        
+                        cv::merge(channels, reference);
+
+                        this->shmem_result->write((void*)reference.data, (this->destiny_roi_width * this->destiny_roi_height * 3));
+
                         if(this->showdebug)
                         {
-                                std::vector<cv::Mat> channels(3);
-                                cv::Mat reference;
-                                if(this->cam_channels == 1)
-                                {
-                                        cv::Mat img_gray = (this->CurrentReferenceFrame(cv::Rect(this->destiny_roi_x, this->destiny_roi_y, this->destiny_roi_width, this->destiny_roi_height))).clone();
-                                        cv::cvtColor(img_gray, reference, CV_GRAY2BGR);
-                                }
-                                else
-                                {
-                                        reference = (this->CurrentReferenceFrame(cv::Rect(this->destiny_roi_x, this->destiny_roi_y, this->destiny_roi_width, this->destiny_roi_height))).clone();
-                                }
-
-                                cv::split(reference, channels);
-                        
-                                masks.second = 1 - masks.second;
-                                channels[0] = masks.second.mul(channels[0]); // Activate the red color as obstacles
-                                channels[1] = masks.second.mul(channels[1]); // Activate the red color as obstacles
-
-                                masks.second = masks.second - masks.first;
-                                channels[0] = (1 - masks.second).mul(channels[0]); // Activate the yellow color for unclassified area
-
-                                cv::merge(channels, reference);
-
-                                cv::namedWindow("ObstacleDetectionComponent - Final Classification",CV_WINDOW_AUTOSIZE);
+                                cv::namedWindow("ObstacleDetectionComponent - Final Classification", CV_WINDOW_AUTOSIZE);
                                 cv::imshow("ObstacleDetectionComponent - Final Classification", reference);
                         }

trunk/StereoVisionDisparity/ObstacleDetectionComponent.h

@@ -94 +94 @@
         TimestampedStructImage DispOutImageHeader;              // Header for the output disp image
 
-        void* ref_mem;
-        void* dispin_mem;
-        void* mask1_mem;
-        void* mask2_mem;
-        void* dispout_mem;
+        void * ref_mem;
+        void * dispin_mem;
+        void * mask1_mem;
+        void * mask2_mem;
+        void * dispout_mem;
 
         size_t ref_mem_size;            // Image shared memory position size
@@ -114 +114 @@
         ShMem * shmem_mask2;            // Shared memory control access to the image data (obstacles mask)
         ShMem * shmem_dispout;          // Shared memory control access to the image data (disparity map 16)
+
+        ShMem * shmem_dispMapNormalized;
+        ShMem * shmem_result;
 
         cv::Mat CurrentReferenceFrame;  // Reference image