Changeset 56 in pacpussensors for trunk/StereoVisionDisparity/ObstacleDetectionComponent.cpp

Timestamp:

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

Author:

phudelai

Message:

Vision component updated for StereoVision for PointGrey Flea3

File:

• trunk/StereoVisionDisparity/ObstacleDetectionComponent.cpp (modified) (7 diffs)

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);
                         }