source: pacpussensors/trunk/StereoVisionDisparity/ObstacleDetectionComponent.cpp@ 53

Last change on this file since 53 was 53, checked in by phudelai, 10 years ago

StereoVisionDisparity updated
File size: 60.2 KB
Line 
1/*********************************************************************
2// created: 2013/06/19 - 18:40
3// filename: ObstacleDetectionComponent.cpp
4//
5// author: Danilo Alves de Lima and Students of SY27
6// Copyright Heudiasyc UMR UTC/CNRS 6599
7//
8// version: $Id: $
9//
10// purpose: Obstacle detection from stereo image data
11//
12//
13*********************************************************************/
14//#include "GeneralDefinitions.h"
15
16#include "ObstacleDetectionComponent.h"
17
18#include <iostream>
19#include <string>
20#include "opencv2/calib3d/calib3d.hpp"
21#include "opencv2/core/core.hpp"
22
23#include "Pacpus/kernel/ComponentFactory.h"
24#include "Pacpus/kernel/DbiteException.h"
25#include "Pacpus/kernel/DbiteFileTypes.h"
26#include "Pacpus/kernel/Log.h"
27
28using namespace std;
29using namespace pacpus;
30
31DECLARE_STATIC_LOGGER("pacpus.base.ObstacleDetectionComponent");
32
33// Construct the factory
34static ComponentFactory<ObstacleDetectionComponent> sFactory("ObstacleDetectionComponent");
35
36const int kMaxFilepathLength = 512; // TODO: should be same for all images
37
38static const string ObstacleDetectionMemoryName_ref = "DisparityMap-ref";
39static const string ObstacleDetectionMemoryName_dispin = "DisparityMap-disp";
40
41static const string ObstacleDetectionMemoryName_mask1 = "ObstacleDetection-mask1";
42static const string ObstacleDetectionMemoryName_mask2 = "ObstacleDetection-mask2";
43static const string ObstacleDetectionMemoryName_dispout = "ObstacleDetection-disp";
44
45//////////////////////////////////////////////////////////////////////////
46/* ComparePoints1
47 Description:
48 Compare if the point 1 if less than 2 by the criteria of the higher y e higher x
49 Parameters:
50 pt1 = point 1
51 pt2 = point 2
52*/
53bool ComparePoints1( cv::Point pt1, cv::Point pt2)
54{
55 /*
56 · Strict: pred(X, X) is always false.
57
58 · Weak: If !pred(X, Y) && !pred(Y, X), X==Y.
59
60 · Ordering: If pred(X, Y) && pred(Y, Z), then pred(X, Z).
61 */
62 if(pt1.y > pt2.y)
63 {
64 return true;
65 }
66 else if((pt1.y == pt2.y)&&(pt1.x >= pt2.x))
67 {
68 return true;
69 }
70 else
71 {
72 return false;
73 }
74}
75
76/// Constructor.
77ObstacleDetectionComponent::ObstacleDetectionComponent(QString name)
78 : ComponentBase(name)
79{
80 LOG_TRACE(Q_FUNC_INFO);
81
82 setRecording(0);
83
84 ANG_VARIATION = 20.0;
85 ANG_VARIATION2 = 7.0;
86
87 this->cam_width = 1280; // Image width
88 this->cam_height = 960; // Image height
89 this->cam_channels = 3;
90 this->showdebug = false; // Show frame acquired
91
92 // Size of the image data sizeof(char)*width*height*channels
93 this->mMaxImageInputSize1 = sizeof(char) * this->cam_width * this->cam_height * this->cam_channels;
94
95 // Input data
96 this->shmem_ref = 0; // Shared memory control access to the image data
97 this->shmem_dispin = 0; // Shared memory control access to the image data
98
99 // Output data
100 this->shmem_mask1 = 0; // Shared memory control access to the image data (free space mask)
101 this->shmem_mask2 = 0; // Shared memory control access to the image data (obstacles mask)
102 this->shmem_dispout = 0; // Shared memory control access to the image data (disparity map)
103}
104
105//////////////////////////////////////////////////////////////////////////
106/// Destructor.
107ObstacleDetectionComponent::~ObstacleDetectionComponent()
108{
109 LOG_TRACE(Q_FUNC_INFO);
110
111 if(this->shmem_ref)
112 delete shmem_ref;
113
114 this->shmem_ref = NULL;
115
116 if(this->shmem_dispin)
117 delete shmem_dispin;
118
119 this->shmem_dispin = NULL;
120
121 if(this->shmem_mask1)
122 delete shmem_mask1;
123
124 this->shmem_mask1 = NULL;
125
126 if(this->shmem_mask2)
127 delete shmem_mask2;
128
129 this->shmem_mask2 = NULL;
130
131 if(this->shmem_dispout)
132 delete shmem_dispout;
133
134 this->shmem_dispout = NULL;
135
136}
137
138//////////////////////////////////////////////////////////////////////////
139/// Called by the ComponentManager to start the component
140void ObstacleDetectionComponent::startActivity()
141{
142 LOG_TRACE(Q_FUNC_INFO);
143
144 this->mMaxImageInputSize1 = sizeof(unsigned char) * this->cam_width * this->cam_height * this->cam_channels;
145 this->mMaxImageInputSize2 = sizeof(unsigned short) * this->cam_width * this->cam_height;
146 this->mMaxImageOutputSize = sizeof(unsigned char) * this->cam_width * this->cam_height;
147
148 this->ref_mem_size = sizeof(TimestampedStructImage) + this->mMaxImageInputSize1;
149 this->dispin_mem_size = sizeof(TimestampedStructImage) + this->mMaxImageInputSize2;
150 this->mask1_mem_size = sizeof(TimestampedStructImage) + this->mMaxImageOutputSize;
151 this->mask2_mem_size = sizeof(TimestampedStructImage) + this->mMaxImageOutputSize;
152 this->dispout_mem_size = sizeof(TimestampedStructImage) + this->mMaxImageInputSize2;
153
154 // Allocate memory position for the maximum expected data
155 this->ref_mem = malloc(this->ref_mem_size);
156 this->dispin_mem = malloc(this->dispin_mem_size);
157 this->mask1_mem = malloc(this->mask1_mem_size);
158 this->mask2_mem = malloc(this->mask2_mem_size);
159 this->dispout_mem = malloc(this->dispout_mem_size);
160
161 this->shmem_ref = new ShMem(ObstacleDetectionMemoryName_ref.c_str(), this->ref_mem_size);
162
163 this->shmem_dispin = new ShMem(ObstacleDetectionMemoryName_dispin.c_str(), this->dispin_mem_size);
164
165 this->shmem_mask1 = new ShMem(ObstacleDetectionMemoryName_mask1.c_str(), this->mask1_mem_size);
166
167 this->shmem_mask2 = new ShMem(ObstacleDetectionMemoryName_mask2.c_str(), this->mask2_mem_size);
168
169 this->shmem_dispout = new ShMem(ObstacleDetectionMemoryName_dispout.c_str(), this->dispout_mem_size);
170
171 // Run thread
172 THREAD_ALIVE = true;
173 start();
174}
175
176//////////////////////////////////////////////////////////////////////////
177/// Called by the ComponentManager to stop the component
178void ObstacleDetectionComponent::stopActivity()
179{
180 LOG_TRACE(Q_FUNC_INFO);
181
182 if(THREAD_ALIVE)
183 {
184 // Stop thread
185 THREAD_ALIVE = false;
186
187 while(is_running)
188 {
189 msleep(/*MS_DELAY*/10);
190 }
191
192 if(this->shmem_ref)
193 delete shmem_ref;
194
195 this->shmem_ref = NULL;
196
197 if(this->shmem_dispin)
198 delete shmem_dispin;
199
200 this->shmem_dispin = NULL;
201
202 if(this->shmem_mask1)
203 delete shmem_mask1;
204
205 this->shmem_mask1 = NULL;
206
207 if(this->shmem_mask2)
208 delete shmem_mask2;
209
210 this->shmem_mask2 = NULL;
211
212 if(this->shmem_dispout)
213 delete shmem_dispout;
214
215 this->shmem_dispout = NULL;
216
217 // Free the malloc memories
218 free(this->ref_mem);
219 free(this->dispin_mem);
220 free(this->mask1_mem);
221 free(this->mask2_mem);
222 free(this->dispout_mem);
223
224 }
225
226 LOG_INFO("stopped component '" << name() << "'");
227}
228
229//////////////////////////////////////////////////////////////////////////
230/// Called by the ComponentManager to pass the XML parameters to the
231/// component
232ComponentBase::COMPONENT_CONFIGURATION ObstacleDetectionComponent::configureComponent(XmlComponentConfig config)
233{
234 LOG_TRACE(Q_FUNC_INFO);
235
236 // Initialize with default values
237 InitDefault();
238
239 if (config.getProperty("recording") != QString::null)
240 setRecording(config.getProperty("recording").toInt());
241
242 if (config.getProperty("cam_width") != QString::null)
243 this->cam_width = config.getProperty("cam_width").toInt();
244
245 if (config.getProperty("cam_height") != QString::null)
246 this->cam_height = config.getProperty("cam_height").toInt();
247
248 if (config.getProperty("cam_channels") != QString::null)
249 this->cam_channels = config.getProperty("cam_channels").toInt();
250
251 if (config.getProperty("min_disp") != QString::null)
252 this->min_disp = config.getProperty("min_disp").toInt();
253
254 if (config.getProperty("max_disp") != QString::null)
255 this->max_disp = config.getProperty("max_disp").toInt();
256
257 if (config.getProperty("min_disp_norm") != QString::null)
258 this->min_disp_norm = config.getProperty("min_disp_norm").toInt();
259
260 if (config.getProperty("max_disp_norm") != QString::null)
261 this->max_disp_norm = config.getProperty("max_disp_norm").toInt();
262
263 if (config.getProperty("showdebug") != QString::null)
264 this->showdebug = (bool)config.getProperty("showdebug").toInt();
265
266 if (config.getProperty("destiny_roi_x") != QString::null)
267 this->destiny_roi_x = config.getProperty("destiny_roi_x").toInt();
268
269 if (config.getProperty("destiny_roi_y") != QString::null)
270 this->destiny_roi_y = config.getProperty("destiny_roi_y").toInt();
271
272 if (config.getProperty("destiny_roi_width") != QString::null)
273 this->destiny_roi_width = config.getProperty("destiny_roi_width").toInt();
274
275 if (config.getProperty("destiny_roi_height") != QString::null)
276 this->destiny_roi_height = config.getProperty("destiny_roi_height").toInt();
277
278 if( ((this->destiny_roi_height != this->cam_height)||(this->destiny_roi_width != this->cam_width))&&
279 ((this->destiny_roi_height <= this->cam_height)&&(this->destiny_roi_width <= this->cam_width)) )
280 this->use_roi = true;
281
282 LOG_INFO("configured component '" << name() << "'");
283 return ComponentBase::CONFIGURED_OK;
284}
285
286/**
287* Initialize default values
288*/
289void ObstacleDetectionComponent::InitDefault()
290{
291 // Default
292 this->cam_width = this->destiny_roi_width = 1280; // Image width
293 this->cam_height = this->destiny_roi_height = 960; // Image height
294
295 this->destiny_roi_x = this->destiny_roi_y = 0; // Destiny image roi x and y
296
297 this->use_roi = false;
298 this->min_disp = 0;
299 this->max_disp = 255;
300
301 this->min_disp_norm = 0; // Minimum disparity value to equalize the disp map 16
302 this->max_disp_norm = 255; // Maximum disparity value to equalize the disp map 16
303
304 this->showdebug = false; // Show frame acquired
305}
306
307// Thread loop
308void ObstacleDetectionComponent::run()
309{
310 LOG_TRACE(Q_FUNC_INFO);
311
312 this->is_running = true;
313
314
315 if(this->CurrentReferenceFrame.cols != this->cam_width)
316 {
317 this->CurrentReferenceFrame = cv::Mat(cv::Size(this->cam_width , this->cam_height), CV_MAKETYPE(CV_8U, this->cam_channels));
318 }
319
320 // Create the image in which will be read the disparities
321 if(this->CurrentDisparityMap16.cols != this->cam_width)
322 {
323 this->CurrentDisparityMap16 = cv::Mat( this->cam_height, this->cam_width, CV_16S, cv::Scalar(0) );
324 }
325
326 if(this->CurrentSurfaceMask.cols != this->cam_width)
327 {
328 this->CurrentSurfaceMask = cv::Mat(this->cam_height, this->cam_width, CV_MAKETYPE(CV_8U, 1), cv::Scalar(0) );
329 }
330
331 if(this->CurrentObstaclesMask.cols != this->cam_width)
332 {
333 this->CurrentObstaclesMask = cv::Mat(this->cam_height, this->cam_width, CV_MAKETYPE(CV_8U, 1), cv::Scalar(0) );
334 }
335
336 // Images for type convertion
337 cv::Mat Disp_map = cv::Mat( this->CurrentDisparityMap16.rows, this->CurrentDisparityMap16.cols, CV_8UC1 );
338 cv::Mat Disp_map16;
339
340 // Keeps the last image timestamp;
341 road_time_t last_reading = 0;
342
343 // Initialize the output images header
344 this->Mask1ImageHeader.image.width = this->cam_width;
345 this->Mask1ImageHeader.image.height = this->cam_height;
346 this->Mask1ImageHeader.image.channels = 1;
347 this->Mask1ImageHeader.image.width_step = (size_t)(this->Mask1ImageHeader.image.height * this->Mask1ImageHeader.image.channels);
348 this->Mask1ImageHeader.image.data_size = this->mMaxImageOutputSize;
349
350 this->Mask2ImageHeader.image.width = this->cam_width;
351 this->Mask2ImageHeader.image.height = this->cam_height;
352 this->Mask2ImageHeader.image.channels = 1;
353 this->Mask2ImageHeader.image.width_step = (size_t)(this->Mask2ImageHeader.image.height * this->Mask2ImageHeader.image.channels);
354 this->Mask2ImageHeader.image.data_size = this->mMaxImageOutputSize;
355
356 this->DispOutImageHeader.image.width = this->cam_width;
357 this->DispOutImageHeader.image.height = this->cam_height;
358 this->DispOutImageHeader.image.channels = 1;
359 this->DispOutImageHeader.image.width_step = (size_t)(sizeof(unsigned short)*this->DispOutImageHeader.image.height * this->DispOutImageHeader.image.channels);
360 this->DispOutImageHeader.image.data_size = this->mMaxImageInputSize2;
361
362 // Time measurement
363 road_time_t init_time = 0;
364
365 while (THREAD_ALIVE)
366 {
367 init_time = road_time();
368
369 //LOG_INFO("Grab new image");
370 // header + image
371 this->shmem_ref->read(this->ref_mem, this->ref_mem_size);
372 this->shmem_dispin->read(this->dispin_mem, this->dispin_mem_size);
373
374 // Header
375 memcpy( &this->RefImageHeader, this->ref_mem, sizeof(TimestampedStructImage));
376 memcpy( &this->DispInImageHeader, this->dispin_mem, sizeof(TimestampedStructImage));
377
378 // Check image header
379 bool is_ok = false;
380 if( (this->RefImageHeader.image.data_size == this->mMaxImageInputSize1) && (this->RefImageHeader.time != last_reading) &&
381 (this->DispInImageHeader.image.data_size == this->mMaxImageInputSize2) && (this->DispInImageHeader.time == this->RefImageHeader.time) )
382 {
383 is_ok = true;
384 last_reading = this->RefImageHeader.time;
385
386 /*std::cout << "Expected image w: " << ImageHeader.image.width << std::endl;
387 std::cout << "Expected image h: " << ImageHeader.image.height << std::endl;
388 std::cout << "Expected image c: " << ImageHeader.image.channels << std::endl;
389 std::cout << "Expected image data: " << ImageHeader.image.data_size << std::endl;
390 std::cout << "Expected image size: " << image_mem << std::endl;*/
391 }
392 /*else
393 {
394 LOG_ERROR("Error in the image data size!");
395 }*/
396
397 if(is_ok)
398 {
399 // Image data
400 memcpy( (unsigned char*)(this->CurrentReferenceFrame.data), (unsigned char*)((TimestampedStructImage*)this->ref_mem + 1), this->RefImageHeader.image.data_size);
401 memcpy( (unsigned short*)(this->CurrentDisparityMap16.data), (unsigned short*)((TimestampedStructImage*)this->dispin_mem + 1), this->DispInImageHeader.image.data_size);
402
403 if(this->showdebug)
404 {
405 cv::namedWindow( "ObstacleDetectionComponent - Current Reference Frame", CV_WINDOW_NORMAL );
406 cv::imshow("ObstacleDetectionComponent - Current Reference Frame",this->CurrentReferenceFrame);
407 cv::waitKey(1);
408 }
409
410 //============================================= Obstacle Detection ================================================
411
412 cv::Mat v_disp_map, u_disp_map;
413
414 cv::Mat roi_disp = (this->use_roi) ? this->CurrentDisparityMap16(cv::Rect(this->destiny_roi_x, this->destiny_roi_y, this->destiny_roi_width, this->destiny_roi_height)) :
415 this->CurrentDisparityMap16;
416
417 // U/V disparity maps calculation
418 this->CalcUVDisparityMapNorm(roi_disp, v_disp_map, u_disp_map, Disp_map, this->min_disp_norm, this->max_disp_norm);
419
420 /*
421 // Real disparity information
422 Disp_map16 = this->CurrentDisparityMap16/16;
423
424 // Display it as a CV_8UC1 image
425 Disp_map16.convertTo( Disp_map, CV_8UC1);//, double(255)/double(this->max_disp - this->min_disp));
426
427 if(this->showdebug)
428 {
429 cv::namedWindow( "ObstacleDetectionComponent - Current Disparity Map", CV_WINDOW_AUTOSIZE );
430 cv::imshow("ObstacleDetectionComponent - Current Disparity Map", Disp_map);
431 cv::waitKey(1);
432 }
433
434 // U/V disparity maps calculation
435 std::pair<cv::Mat,cv::Mat> par_uv = this->CalcUVDisparityMap(Disp_map);
436 cv::Mat v_disp_map = par_uv.second;
437
438 cv::Mat u_disp_map = par_uv.first;
439 */
440
441 if(this->showdebug)
442 {
443 cv::namedWindow( "ObstacleDetectionComponent - Current V Disparity Map", CV_WINDOW_AUTOSIZE );
444 cv::imshow("ObstacleDetectionComponent - Current V Disparity Map", v_disp_map);
445
446 cv::namedWindow( "ObstacleDetectionComponent - Current U Disparity Map", CV_WINDOW_AUTOSIZE );
447 cv::imshow("ObstacleDetectionComponent - Current U Disparity Map", u_disp_map);
448
449 cv::namedWindow( "ObstacleDetectionComponent - Current Disparity Map Normalized", CV_WINDOW_AUTOSIZE );
450 cv::imshow("ObstacleDetectionComponent - Current Disparity Map Normalized", Disp_map);
451 cv::waitKey(1);
452 }
453
454 // Image to detect near obstacles
455 cv::Mat v_disp_map2 = v_disp_map.clone();
456
457 // Find the driveable surface
458 //cv::Mat color_v_disp_map1 = this->FindSurface(v_disp_map, v_disp_map2);
459 cv::Mat color_v_disp_map1 = this->FindSurface2(v_disp_map, v_disp_map2);
460
461 // Find near obstacles
462 //cv::Mat color_v_disp_map2 = this->FindNearObstacles(v_disp_map2, this->min_disp, this->max_disp);
463 std::pair<cv::Mat, cv::Mat> color_uv_disp_map = this->FindNearObstaclesUV(v_disp_map2, u_disp_map, this->min_disp, this->max_disp);
464
465 if(this->showdebug)
466 {
467 cv::namedWindow("ObstacleDetectionComponent - Mapa de Disparidade V + Free Space",CV_WINDOW_AUTOSIZE);
468 cv::imshow("ObstacleDetectionComponent - Mapa de Disparidade V + Free Space", color_v_disp_map1);
469
470 cv::namedWindow("ObstacleDetectionComponent - Mapa de Disparidade V + Obstacles",CV_WINDOW_AUTOSIZE);
471 cv::imshow("ObstacleDetectionComponent - Mapa de Disparidade V + Obstacles", color_uv_disp_map.first);
472
473 cv::namedWindow("ObstacleDetectionComponent - Mapa de Disparidade U + Obstacles",CV_WINDOW_AUTOSIZE);
474 cv::imshow("ObstacleDetectionComponent - Mapa de Disparidade U + Obstacles", color_uv_disp_map.second);
475 cv::waitKey(1);
476 }
477
478 // Remap the v-disparity point in the original image and create binary masks
479 //std::pair<cv::Mat, cv::Mat> masks = this->MaskSurface2(Disp_map, color_v_disp_map1, color_uv_disp_map.first, this->min_disp, this->max_disp, 1);
480 std::pair<cv::Mat, cv::Mat> masks = this->MaskSurface3(Disp_map, color_v_disp_map1, color_uv_disp_map.first, color_uv_disp_map.second, this->min_disp, this->max_disp, 1);
481
482 /*if(this->showdebug)
483 {
484 cv::namedWindow("Mapa de Disparidade V + Mask1",CV_WINDOW_AUTOSIZE);
485 cv::imshow("Mapa de Disparidade V + Mask1", masks.first*255);
486
487 cv::namedWindow("Mapa de Disparidade V + Mask2",CV_WINDOW_AUTOSIZE);
488 cv::imshow("Mapa de Disparidade V + Mask2", masks.second*255);
489 }*/
490
491 //---------------------- Remove commun information -------------------------
492
493 masks.second = masks.second - masks.second.mul( masks.first);
494
495 //---------------------------------------------------------------------------
496
497 //------------------ Write the result in the shared memory ------------------
498
499 if(this->use_roi)
500 {
501 masks.first.copyTo(this->CurrentSurfaceMask(cv::Rect(this->destiny_roi_x, this->destiny_roi_y, this->destiny_roi_width, this->destiny_roi_height)));
502 masks.second.copyTo(this->CurrentObstaclesMask(cv::Rect(this->destiny_roi_x, this->destiny_roi_y, this->destiny_roi_width, this->destiny_roi_height)));
503 }
504 else
505 {
506 this->CurrentSurfaceMask = masks.first;
507 this->CurrentObstaclesMask = masks.second;
508 }
509
510 //----------------------------- Mask 1 --------------------------------------
511 // Complete timestamp header of the mask image 1
512 this->Mask1ImageHeader.time = this->DispInImageHeader.time;
513 this->Mask1ImageHeader.timerange = this->DispInImageHeader.timerange;
514
515 // Copy images header and data to memory
516 memcpy(this->mask1_mem, &this->Mask1ImageHeader, sizeof(TimestampedStructImage));
517 memcpy((void*)((TimestampedStructImage*)this->mask1_mem + 1), (void*)this->CurrentSurfaceMask.data, this->Mask1ImageHeader.image.data_size);
518 this->shmem_mask1->write(this->mask1_mem, this->mask1_mem_size);
519
520 //----------------------------- Mask 2 --------------------------------------
521 // Complete timestamp header of the mask image 2
522 this->Mask2ImageHeader.time = this->DispInImageHeader.time;
523 this->Mask2ImageHeader.timerange = this->DispInImageHeader.timerange;
524
525 // Copy images header and data to memory
526 memcpy(this->mask2_mem, &this->Mask2ImageHeader, sizeof(TimestampedStructImage));
527 memcpy((void*)((TimestampedStructImage*)this->mask2_mem + 1), (void*)this->CurrentObstaclesMask.data, this->Mask2ImageHeader.image.data_size);
528 this->shmem_mask2->write(this->mask2_mem, this->mask2_mem_size);
529
530 //------------------------- Disparity map out -------------------------------
531 // Complete timestamp header of the disp image out
532 this->DispOutImageHeader.time = this->DispInImageHeader.time;
533 this->DispOutImageHeader.timerange = this->DispInImageHeader.timerange;
534
535 // Copy images header and data to memory
536 memcpy(this->dispout_mem, &this->DispOutImageHeader, sizeof(TimestampedStructImage));
537 memcpy((void*)((TimestampedStructImage*)this->dispout_mem + 1), (void*)this->CurrentDisparityMap16.data, this->DispOutImageHeader.image.data_size);
538 this->shmem_dispout->write(this->dispout_mem, this->dispout_mem_size);
539 //---------------------------------------------------------------------------
540
541 // ----------------- Apply the mask in the reference image ------------------
542 if(this->showdebug)
543 {
544 std::vector<cv::Mat> channels(3);
545 cv::Mat reference;
546 if(this->cam_channels == 1)
547 {
548 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();
549 cv::cvtColor(img_gray, reference, CV_GRAY2BGR);
550 }
551 else
552 {
553 reference = (this->CurrentReferenceFrame(cv::Rect(this->destiny_roi_x, this->destiny_roi_y, this->destiny_roi_width, this->destiny_roi_height))).clone();
554 }
555
556 cv::split(reference, channels);
557
558 masks.second = 1 - masks.second;
559 channels[0] = masks.second.mul(channels[0]); // Activate the red color as obstacles
560 channels[1] = masks.second.mul(channels[1]); // Activate the red color as obstacles
561
562 masks.second = masks.second - masks.first;
563 channels[0] = (1 - masks.second).mul(channels[0]); // Activate the yellow color for unclassified area
564
565 cv::merge(channels, reference);
566
567 cv::namedWindow("ObstacleDetectionComponent - Final Classification",CV_WINDOW_AUTOSIZE);
568 cv::imshow("ObstacleDetectionComponent - Final Classification", reference);
569 }
570 //---------------------------------------------------------------------------
571
572 //==================================================================================================================
573
574 //std::cout << componentName.toStdString() << " cicle time: " << (road_time() - init_time)/1000000.0 << std::endl;
575 }
576 else
577 {
578 msleep(/*MS_DELAY*/10);
579 }
580
581 if(this->showdebug)
582 cv::waitKey(1); // Give the system permission
583
584 //std::cout << componentName.toStdString() << " cicle time: " << (road_time() - init_time)/1000000.0 << std::endl;
585 }
586
587 this->is_running = false;
588
589 // Destroy the window frame
590 if(this->showdebug)
591 cvDestroyAllWindows();
592}
593
594// Function to calculate the U/V disparity map
595std::pair<cv::Mat, cv::Mat> ObstacleDetectionComponent::CalcUVDisparityMap(cv::Mat disp_map)
596{
597 int l, c, pixel; // local variables for row, line and pixel
598 unsigned char intensity; // pixel intensity
599
600 unsigned char* ptr1; // row pointer for 8 bits image
601 //unsigned short* ptr2; // row pointer for 16 bits image
602
603 // U disparity map iamge
604 cv::Mat u_disp = cv::Mat::zeros(cv::Size(disp_map.cols, 256), CV_8UC1);
605
606 // V disparity map image
607 cv::Mat v_disp = cv::Mat::zeros(cv::Size(256, disp_map.rows), CV_8UC1);
608
609 // run accross the image and add 1 to the respective U/V disparity column
610 for (l = 0; l < disp_map.rows; ++l)
611 {
612 ptr1 = disp_map.ptr<unsigned char>(l);
613
614 for (c = 0; c < disp_map.cols; ++c)
615 {
616 intensity = (unsigned char)ptr1[c];
617
618 if( (intensity > this->min_disp)&&(intensity < this->max_disp))
619 {
620 pixel = intensity*u_disp.cols + c;
621 u_disp.data[pixel] = (unsigned char)(u_disp.data[pixel] + 1);
622
623 pixel = l*v_disp.cols + intensity;
624 v_disp.data[pixel] = (unsigned char)(v_disp.data[pixel] + 1);
625 }
626 }
627 }
628
629 return std::make_pair(u_disp, v_disp);
630}
631
632/* CalcUVDisparityMapNorm
633 Description:
634 Function to calculate the U/V disparity map from a disp map normalized
635 Parameters:
636 disp_map16 = original disparity map 16
637 disp_map_norm = resulted disparity map normalized
638 min_d_norm = Minimum disparity value to equalize the disp map 16
639 max_d_norm = Maximum disparity value to equalize the disp map 16
640*/
641void ObstacleDetectionComponent::CalcUVDisparityMapNorm(cv::Mat disp_map16, cv::Mat &v_disp_map, cv::Mat &u_disp_map, cv::Mat &disp_map_norm, int min_d_norm, int max_d_norm)
642{
643 int l, c, pixel; // local variables for row, line and pixel
644 unsigned char intensity; // pixel intensity
645 int intensity_norm; // pixel intensity
646
647 unsigned char* ptr1; // row pointer for 8 bits image
648 unsigned short* ptr2; // row pointer for 16 bits image
649
650 // Disparity map image normalized
651 disp_map_norm = cv::Mat::zeros(cv::Size(disp_map16.cols, disp_map16.rows), CV_8UC1);
652
653 // U disparity map image
654 u_disp_map = cv::Mat::zeros(cv::Size(disp_map16.cols, 256), CV_8UC1);
655
656 // V disparity map image
657 v_disp_map = cv::Mat::zeros(cv::Size(256, disp_map16.rows), CV_8UC1);
658
659 // percorre a imagem original e soma 1 na coluna do mapa de disparidade V com a mesma
660 // intensidade do pixel
661 for (l = 0; l < disp_map16.rows; ++l)
662 {
663 ptr1 = disp_map_norm.ptr<unsigned char>(l);
664 ptr2 = disp_map16.ptr<unsigned short>(l);
665
666 for (c = 0; c < disp_map16.cols; ++c)
667 {
668 intensity = (unsigned char)(ptr2[c]/16);
669 intensity_norm = (int)((float)((ptr2[c]/16.0f - (float)min_d_norm)*(255.0f)/((float)max_d_norm - (float)min_d_norm)) + 0.5f);
670
671 if( (intensity > this->min_disp)&&(intensity < this->max_disp)&&(intensity_norm > 0)&&(intensity_norm < 255))
672 {
673 pixel = intensity_norm*u_disp_map.cols + c;
674 u_disp_map.data[pixel] = (unsigned char)(u_disp_map.data[pixel] + 1);
675
676 pixel = l*v_disp_map.cols + intensity_norm;
677 v_disp_map.data[pixel] = (unsigned char)(v_disp_map.data[pixel] + 1);
678
679 ptr1[c] = (unsigned char)intensity_norm;
680 }
681 }
682 }
683
684 return;
685}
686
687// Function to find the free space surface from a V-disparity map
688cv::Mat ObstacleDetectionComponent::FindSurface(cv::Mat &v_disp_map, cv::Mat &v_disp_map2)
689{
690 // Parameters of canny and hough transform
691 int tshold1 = 154;
692 int tshold2 = 48;
693 int n_points = 48; //59
694 int minLineLenght = 35; //40
695 int maxLineGap = 12;
696
697
698 // Binary image
699 cv::Mat Img_bin = v_disp_map.clone();//cvCloneImage(v_disp_map);
700
701 // Color V disparity map with red lines
702 cv::Mat color_img = cv::Mat( cv::Size(v_disp_map.cols, v_disp_map.rows), CV_8UC3 );
703
704 // Convert to color image
705 cv::cvtColor(v_disp_map, color_img, CV_GRAY2BGR);
706
707 cv::equalizeHist( Img_bin, Img_bin);
708
709 if(this->showdebug)
710 {
711 // Janela de exibicao
712 cv::namedWindow("ObstacleDetectionComponent - Equalized Image",CV_WINDOW_AUTOSIZE);
713 cv::imshow("ObstacleDetectionComponent - Equalized Image", Img_bin);
714 }
715
716 cv::Canny(Img_bin, Img_bin, tshold1, tshold2, 3);
717
718 // Closing
719 //cv::dilate(Img_bin, Img_bin, cv::Mat(), cv::Point(-1,-1), 2 );
720 //cv::erode(Img_bin, Img_bin, cv::Mat(), cv::Point(-1,-1), 1 );
721
722 if(this->showdebug)
723 {
724 // Janela de exibicao
725 cv::namedWindow("ObstacleDetectionComponent - Binary Image",CV_WINDOW_AUTOSIZE);
726 cv::imshow("ObstacleDetectionComponent - Binary Image", Img_bin);
727 }
728
729 std::vector<cv::Vec4i> lines; //vector for storing the lines found by HoughLine
730
731 // Probabilistic Hough Transform
732 cv::HoughLinesP( Img_bin, lines, 1, CV_PI/180, n_points, minLineLenght, maxLineGap );
733
734 //=============================== Use the lines filter to remove invalid segments ============================
735
736 std::vector<cv::Point> nova_lista = this->LinesFiltering(lines);
737
738 if(!nova_lista.empty())
739 {
740 cv::Point pt_ant = *(nova_lista.begin());
741
742 // Filter the mean angle
743 for(std::vector<cv::Point>::iterator it = nova_lista.begin(); it != nova_lista.end(); ++it)
744 {
745 cv::line( color_img, pt_ant, *it, CV_RGB(255,0,0), 8, 8 );
746
747 pt_ant = *it;
748 }
749 }
750
751 //============================================================================================================
752
753 //======================== Remove invalid line segments by slope angle only ==================================
754 //if (lines.size() != 0)
755 //{
756 // cv::Point pt1, pt2;
757 // double theta;
758
759 // for(int i = 0; i < (int)lines.size();++i)
760 // {
761 // pt1.x = lines[i][0];//(CvPoint*)cvGetSeqElem(lines,i);
762 // pt1.y = lines[i][1];
763 // pt2.x = lines[i][2];
764 // pt2.y = lines[i][3];
765
766 // CheckPoints(pt1, pt2); //Verifica a ordem dos pontos
767 //
768 // theta = Inclination(pt1, pt2); //calcula a inclinacao da reta encontrada
769
770 // // Valor atual do angulo em graus
771 // theta = ((theta*360.0)/(2.0*CV_PI));
772
773 // // Verifica se a reta possui inclinacao para ser possivel plano
774 // if(theta> (90.0 + ANG_VARIATION))
775 // {
776
777 // //Desenha as retas em vermelho
778 // cv::line( color_img, pt1, pt2, CV_RGB(255,0,0), 4, 8 );
779 // }
780 // }
781 //}
782 //==========================================================================================================
783
784 std::vector<cv::Mat> channels(3);
785
786 // Get the V-disparity without detected red lines
787 cv::split(color_img, channels);
788 v_disp_map2 = channels[0];
789
790 // Janela de exibicao
791 //cv::namedWindow("Mapa de Disparidade V + Hough",CV_WINDOW_AUTOSIZE);
792 //cv::imshow("Mapa de Disparidade V + Hough", color_img);
793 //cv::imshow("Mapa de Disparidade V + Hough", v_disp_map2);
794
795 return color_img;
796}
797
798/* FindSurface2
799 Description:
800 Function to find the free space surface from a V-disparity map, based on the frontal plane.
801 Return the V-dysparity map with the red line representing the free surface.
802 Parameters:
803 v_disp_map = Original V-disparity map
804 v_disp_map2 = Orignal V-disparity map less the surface detected
805*/
806cv::Mat ObstacleDetectionComponent::FindSurface2(cv::Mat &v_disp_map, cv::Mat &v_disp_map2)
807{
808 // Parameters
809 int tshold = 40; // Min threshold for the first max value
810 int tshold2 = 35; // Min threshold for the step variation
811 int maxLineLenght = 25;
812 int min_disp_value = 20;
813
814 // Binary image
815 cv::Mat Img_bin = v_disp_map.clone();
816 cv::Mat Img_mask = v_disp_map.clone();
817
818 // Color V disparity map with red lines
819 cv::Mat color_img = cv::Mat( cv::Size(v_disp_map.cols, v_disp_map.rows), CV_8UC3 );
820
821 // Convert to color image
822 cv::cvtColor(v_disp_map, color_img, CV_GRAY2BGR);
823
824
825 //================= Segment the most probable road surface region ===================================
826 /*cv::threshold(Img_mask, Img_mask, tshold, 1, CV_THRESH_BINARY);
827 cv::dilate(Img_mask, Img_mask, cv::Mat(), cv::Point(-1,-1), 2 );
828 cv::erode(Img_mask, Img_mask, cv::Mat(), cv::Point(-1,-1), 1 );
829
830 if(this->showdebug)
831 {
832 cv::imshow( "ObstacleDetectionComponent - Img_mask", Img_mask*255 );
833 }
834
835 Img_bin = Img_bin.mul(Img_mask);*/
836 //===================================================================================================
837
838 // Generate grad_x and grad_y
839 cv::Mat grad, grad_H, grad_S, grad_x, grad_y;
840 cv::Mat abs_grad_x, abs_grad_y;
841
842 // Gradient X
843 //Scharr( src_gray, grad_x, ddepth, 1, 0, scale, delta, BORDER_DEFAULT );
844 //cv::Sobel( Img_bin, grad_x, CV_16S, 1, 0, 3, 1, 0, cv::BORDER_DEFAULT );
845 //cv::convertScaleAbs( grad_x, abs_grad_x );
846
847 // Gradient Y
848 //Scharr( src_gray, grad_y, ddepth, 0, 1, scale, delta, BORDER_DEFAULT );
849 cv::Sobel( Img_bin, grad_y, CV_16S, 0, 1, 3, 1, 0, cv::BORDER_DEFAULT );
850 cv::convertScaleAbs( grad_y, abs_grad_y );
851
852 // Total Gradient (approximate)
853 //cv::addWeighted( abs_grad_x, 0.2, abs_grad_y, 0.8, 0, grad );
854
855 //abs_grad_y = abs_grad_y*255.0;
856 abs_grad_y.convertTo(Img_mask, CV_8U);
857
858 cv::threshold(Img_mask, Img_mask, tshold2, 1, CV_THRESH_BINARY);
859 cv::dilate(Img_mask, Img_mask, cv::Mat(), cv::Point(-1,-1), 2 );
860 cv::erode(Img_mask, Img_mask, cv::Mat(), cv::Point(-1,-1), 1 );
861
862 Img_bin = Img_bin.mul(Img_mask);
863
864 //cv::equalizeHist( abs_grad_y, Img_bin);
865
866 if(this->showdebug)
867 {
868 //cv::imshow( "ObstacleDetectionComponent - Sobel X", abs_grad_x );
869 cv::imshow( "ObstacleDetectionComponent - Sobel Y", abs_grad_y );
870 cv::imshow( "ObstacleDetectionComponent - Img_mask", Img_mask*255 );
871 //cv::imshow( "ObstacleDetectionComponent - Sobel", grad );
872 //cv::imshow( "ObstacleDetectionComponent - Equalized Image", Img_bin);
873 }
874
875 //============================== Mark the most significative points as free space ==============================
876
877 int row_step_count, last_row_step_count; // Keep the sum of the last valid pixels in the step
878 int previous_col = 0;
879
880 int left_limit, right_limit; // Auxiliary variables
881
882 for(int row = Img_bin.rows - 1; row > 0; --row)
883 {
884 row_step_count = 0;
885 last_row_step_count = 0;
886 left_limit = -1;
887 right_limit = -1;
888
889 unsigned char* ptr1 = Img_bin.ptr<unsigned char>(row);
890
891 if(previous_col == 0)
892 {
893 for(int col = 0; col < Img_bin.cols; ++col)
894 {
895 // Find max
896 if(ptr1[previous_col] < ptr1[col])
897 {
898 previous_col = col;
899
900 tshold2 = (int)(0.95*ptr1[previous_col]); // adjust the tshold
901 }
902 }
903
904 if(ptr1[previous_col] < tshold)
905 {
906 previous_col = 0; // position
907 }
908 }
909
910 if(previous_col != 0)
911 {
912 int left = (previous_col - maxLineLenght >= 0) ? previous_col-maxLineLenght : 0;
913 int right = (previous_col + maxLineLenght < Img_bin.cols) ? previous_col + maxLineLenght : Img_bin.cols;
914
915 int limits_found = 0;
916 int l_col = previous_col;
917 int r_col = previous_col;
918
919 int left_count = 0;
920 int right_count = 0;
921
922 while(!limits_found)
923 {
924 //------------------------------- To the left ----------------------------------
925 if(left_limit == -1)
926 {
927 if(l_col > left)
928 --l_col;
929 else
930 left_limit = l_col + left_count;
931 }
932
933 // Find max
934 if(ptr1[previous_col] < ptr1[l_col])
935 {
936 previous_col = l_col;
937 tshold2 = (int)(0.95*ptr1[previous_col]); // adjust the tshold
938 }
939
940 // Check if the pixel intensity is lower than tshold
941 if(left_count < 5)
942 {
943 if(ptr1[l_col] < tshold2)
944 {
945 ++left_count;
946 }
947 else
948 {
949 left_count = 0;
950 }
951 }
952 else
953 {
954 if(left_limit == -1)
955 left_limit = l_col + left_count;
956 }
957 //------------------------------------------------------------------------------
958
959 //------------------------------- To the right ----------------------------------
960 if(right_limit == -1)
961 {
962 if(r_col < right)
963 ++r_col;
964 else
965 right_limit = r_col - right_count;
966 }
967
968 // Find max
969 if(ptr1[previous_col] < ptr1[r_col])
970 {
971 previous_col = r_col;
972 tshold2 = (int)(0.95*ptr1[previous_col]); // adjust the tshold
973 }
974
975 // Check if the pixel intensity is lower than tshold
976 if(right_count < 5)
977 {
978 if(ptr1[r_col] < tshold2)
979 {
980 ++right_count;
981 }
982 else
983 {
984 right_count = 0;
985 }
986 }
987 else
988 {
989 if(right_limit == -1)
990 right_limit = r_col - right_count;
991 }
992 //------------------------------------------------------------------------------
993
994 if((left_limit != -1)&&(right_limit != -1))
995 {
996 limits_found = 1;
997 }
998 }
999
1000 if((left_limit != -1)&&(right_limit != -1)&&(ptr1[previous_col] > tshold2))
1001 {
1002 cv::line( color_img, cv::Point(left_limit, row), cv::Point(right_limit, row), CV_RGB(255,0,0), 1, 8 );
1003 }
1004 }
1005 }
1006
1007 //==============================================================================================================
1008
1009 std::vector<cv::Mat> channels(3);
1010
1011 // Get the V-disparity without detected red lines
1012 cv::split(color_img, channels);
1013 v_disp_map2 = channels[0];
1014
1015 // Janela de exibicao
1016 //cv::namedWindow("Mapa de Disparidade V + Hough",CV_WINDOW_AUTOSIZE);
1017 //cv::imshow("Mapa de Disparidade V + Hough", color_img);
1018 //cv::imshow("Mapa de Disparidade V + Hough", v_disp_map2);
1019
1020 return color_img;
1021}
1022
1023// Function to find the free space surface from a V-disparity map with mean average
1024cv::Mat ObstacleDetectionComponent::FindAverageSurface(cv::Mat &v_disp_map, cv::Mat &v_disp_map2)
1025{
1026 // Parameters of threshold and hough transform
1027 int tshold1 = 3;
1028 int n_points = 48; //59
1029 int minLineLenght = 35; //40
1030 int maxLineGap = 12;
1031
1032 // Imagem atual em 32F
1033 cv::Mat v_disp_map_current32F = cv::Mat(cv::Size(v_disp_map.cols, v_disp_map.rows), CV_32F, 1);
1034
1035 // Converte tipo de imagem
1036 v_disp_map.convertTo(v_disp_map_current32F, CV_32F);//cvConvertScale(v_disp_map, v_disp_map_current32F);
1037
1038 //Mean disparity map for noise attenuation
1039 static cv::Mat v_disp_map_mean = v_disp_map_current32F.clone();
1040
1041 // Imagem binária da diferenca
1042 cv::Mat ImgBinaria = v_disp_map.clone();//cvCloneImage(v_disp_map);
1043
1044 // Color V disparity map with red lines
1045 cv::Mat color_img = cv::Mat( cv::Size(v_disp_map.cols, v_disp_map.rows), CV_8UC3 );
1046
1047 // Convert to color image
1048 cv::cvtColor(v_disp_map, color_img, CV_GRAY2BGR);
1049
1050 // Running Average
1051 cv::accumulateWeighted(v_disp_map_current32F, v_disp_map_mean, 0.20);//cvRunningAvg(v_disp_map_current32F, v_disp_map_mean, 0.20);
1052
1053 // Convert scale
1054 v_disp_map_mean.convertTo( ImgBinaria, CV_8U); //cvConvertScale(v_disp_map_mean, ImgBinaria, 1.0, 0.0);
1055
1056 // Janela de exibicao
1057 //cv::namedWindow("Imagem Media Movel",CV_WINDOW_AUTOSIZE);
1058 //cv::imshow("Imagem Media Movel", ImgBinaria);
1059
1060 cv::threshold(ImgBinaria, ImgBinaria, tshold1, 255,CV_THRESH_BINARY);
1061
1062 // Janela de exibicao
1063 //cv::namedWindow("Mapa de Disparidade V + binarizacao",CV_WINDOW_AUTOSIZE);
1064 //cv::imshow("Mapa de Disparidade V + binarizacao", ImgBinaria);
1065
1066 // create 3x3 matrix
1067 //static cv::Mat kernel_3x3 = (cv::Mat_<unsigned char>(3,3) << 1, 0, 0, 1, 1, 0, 1, 1, 1);
1068
1069 // Fechamento
1070 //cv::erode(ImgBinaria,ImgBinaria,NULL, cv::Point(-1,-1), 1);
1071 //cv::erode(ImgBinaria,ImgBinaria,kernel_3x3, cv::Point(1,1), 1);
1072
1073 // Janela de exibicao
1074 //cv::namedWindow("Mapa de Disparidade V + binarizacao + erode",CV_WINDOW_AUTOSIZE);
1075 //cv::imshow("Mapa de Disparidade V + binarizacao + erode", ImgBinaria);
1076
1077 // Fechamento
1078 //cv::dilate(ImgBinaria,ImgBinaria,NULL, cv::Point(-1,-1), 1);
1079 //cv::erode(ImgBinaria,ImgBinaria,NULL, cv::Point(-1,-1), 2);
1080
1081 std::vector<cv::Vec4i> lines; //vector for storing the lines found by HoughLine
1082
1083 // Probabilistic Hough Transform
1084 cv::HoughLinesP( ImgBinaria, lines, 1, CV_PI/180, n_points, minLineLenght, maxLineGap );
1085
1086 //=============================== Use the lines filter to remove invalid segments ============================
1087
1088 //std::vector<cv::Point> nova_lista = this->LinesFiltering(lines);
1089 //
1090 //if(!nova_lista.empty())
1091 //{
1092 // cv::Point pt_ant = *(nova_lista.begin());
1093
1094 // // Filter the mean angle
1095 // for(std::vector<cv::Point>::iterator it = nova_lista.begin(); it != nova_lista.end(); ++it)
1096 // {
1097 // cv::line( color_img, pt_ant, *it, CV_RGB(255,0,0), 3, 8 );
1098
1099 // pt_ant = *it;
1100 // }
1101 //}
1102
1103 //============================================================================================================
1104
1105 //======================== Remove invalid line segments by slope angle only ==================================
1106 if (lines.size() != 0)
1107 {
1108 cv::Point pt1, pt2;
1109 double theta;
1110
1111 for(int i = 0; i < (int)lines.size();++i)
1112 {
1113 pt1.x = lines[i][0];//(CvPoint*)cvGetSeqElem(lines,i);
1114 pt1.y = lines[i][1];
1115 pt2.x = lines[i][2];
1116 pt2.y = lines[i][3];
1117
1118 CheckPoints(pt1, pt2); //Verifica a ordem dos pontos
1119
1120 theta = Inclination(pt1, pt2); //calcula a inclinacao da reta encontrada
1121
1122 // Valor atual do angulo em graus
1123 theta = ((theta*360.0)/(2.0*CV_PI));
1124
1125 // Verifica se a reta possui inclinacao para ser possivel plano
1126 if(theta> (90.0 + ANG_VARIATION))
1127 {
1128
1129 //Desenha as retas em vermelho
1130 cv::line( color_img, pt1, pt2, CV_RGB(255,0,0), 6, 8 );
1131 }
1132 }
1133 }
1134 //==========================================================================================================
1135
1136 std::vector<cv::Mat> channels(3);
1137
1138 // Get the V-disparity without detected red lines
1139 cv::split(color_img, channels);
1140 v_disp_map2 = channels[0];
1141
1142 // Janela de exibicao
1143 //cv::namedWindow("Mapa de Disparidade V + Hough",CV_WINDOW_AUTOSIZE);
1144 //cv::imshow("Mapa de Disparidade V + Hough", color_img);
1145 //cv::imshow("Mapa de Disparidade V + Hough", v_disp_map2);
1146
1147 return color_img;
1148}
1149
1150// Function to find the near obstacles from a v-Disparity map
1151cv::Mat ObstacleDetectionComponent::FindNearObstacles(cv::Mat v_disp_map, int min_d, int max_d)
1152{
1153 // Parameters of threshold and hough transform
1154 int tshold1 = 3;
1155 int n_points = 15;
1156 int minLineLenght = 10;
1157 int maxLineGap = 12;
1158
1159 // Image to be processed
1160 cv::Mat v_disp_map_aux = v_disp_map.clone();
1161
1162 // Image with the obstacles highlighted in red
1163 cv::Mat color_img = cv::Mat( cv::Size(v_disp_map.cols, v_disp_map.rows), CV_8UC3);
1164
1165 // Convert color space
1166 cv::cvtColor(v_disp_map_aux, color_img, CV_GRAY2BGR);
1167
1168 // Image binarization
1169 cv::threshold(v_disp_map_aux, v_disp_map_aux, tshold1, 255, CV_THRESH_BINARY);
1170
1171 // Janela de exibicao
1172 //cv::namedWindow("Mapa de Disparidade V + Threshold 2",CV_WINDOW_AUTOSIZE);
1173 //cv::imshow("Mapa de Disparidade V + Threshold 2", v_disp_map_aux);
1174
1175 // Define valid ROI
1176 cv::Mat v_disp_map_aux_ROI = v_disp_map_aux(cv::Rect(min_d, 0, max_d-min_d+1, v_disp_map_aux.rows));
1177 cv::Mat color_img_ROI = color_img( cv::Rect(min_d, 0, max_d-min_d+1, v_disp_map_aux.rows));
1178
1179 std::vector<cv::Vec4i> lines; //vector for storing the lines found by HoughLine
1180
1181 // Probabilistic hough transform
1182 cv::HoughLinesP( v_disp_map_aux_ROI, lines, 1, CV_PI/180, n_points, minLineLenght, maxLineGap );
1183
1184 if (lines.size() != 0)
1185 {
1186 cv::Point pt1, pt2;
1187 double theta;
1188
1189 for(int i = 0; i < (int)lines.size(); ++i)
1190 {
1191 pt1.x = lines[i][0];//(CvPoint*)cvGetSeqElem(lines,i);
1192 pt1.y = lines[i][1];
1193 pt2.x = lines[i][2];
1194 pt2.y = lines[i][3];
1195
1196 this->CheckPoints(pt1, pt2); //Verify the points order
1197
1198 theta = this->Inclination(pt1, pt2); // line slope
1199
1200 // In degrees
1201 theta = ((theta*360.0)/(2.0*CV_PI));
1202
1203 // Verifica se a reta possui inclinacao para ser possivel plano
1204 if((theta < (90.0 + ANG_VARIATION2))&& (theta > (90.0 - ANG_VARIATION2)))
1205 {
1206 //Desenha as retas em vermelho
1207 cv::line( color_img_ROI, pt1, pt2, CV_RGB(255,0,0), 5, 8 );
1208 }
1209 }
1210 }
1211
1212 // Janela de exibicao
1213 //cv::namedWindow("Mapa de Disparidade V + Hough1",CV_WINDOW_AUTOSIZE);
1214 //cv::imshow("Mapa de Disparidade V + Hough1", v_disp_map_aux);
1215
1216 // Janela de exibicao
1217 //cv::namedWindow("Mapa de Disparidade V + Hough2",CV_WINDOW_AUTOSIZE);
1218 //cv::imshow("Mapa de Disparidade V + Hough2", color_img);
1219
1220 return color_img;
1221}
1222
1223// Function to find the near obstacles from the v/u-Disparity maps
1224std::pair<cv::Mat, cv::Mat> ObstacleDetectionComponent::FindNearObstaclesUV(cv::Mat v_disp_map, cv::Mat u_disp_map, int min_d, int max_d)
1225{
1226 // Parameters of threshold and hough transform
1227 int tshold1 = 3;
1228 int tshold2 = 15;
1229 int n_points = 15;
1230 int minLineLenght = 10;
1231 int maxLineGap = 12;
1232
1233 std::vector<cv::Mat> channels(3);
1234
1235 //=================================== V-disparity map process =============================================================
1236 //// Image to be processed
1237 //cv::Mat v_disp_map_aux = v_disp_map.clone();
1238 //
1239 //// Image with the obstacles highlighted in red
1240 //cv::Mat v_color_img = cv::Mat( cv::Size(v_disp_map.cols, v_disp_map.rows), CV_8UC3);
1241
1242 //// Convert color space
1243 //cv::cvtColor(v_disp_map_aux, v_color_img, CV_GRAY2BGR);
1244
1245 //cv::equalizeHist( v_disp_map_aux, v_disp_map_aux);
1246
1247 //// Image binarization
1248 //cv::threshold(v_disp_map_aux, v_disp_map_aux, tshold1, 255, CV_THRESH_BINARY);
1249
1250 //// Janela de exibicao
1251 //if(this->showdebug)
1252 //{
1253 // cv::namedWindow("Mapa de Disparidade V + Threshold 2",CV_WINDOW_AUTOSIZE);
1254 // cv::imshow("Mapa de Disparidade V + Threshold 2", v_disp_map_aux);
1255 //}
1256
1257 //// Define valid ROI
1258 //cv::Mat v_disp_map_aux_ROI = v_disp_map_aux(cv::Rect(min_d, 0, max_d-min_d+1, v_disp_map_aux.rows));
1259 //cv::Mat v_color_img_ROI = v_color_img( cv::Rect(min_d, 0, max_d-min_d+1, v_disp_map_aux.rows));
1260
1261 //std::vector<cv::Vec4i> lines; //vector for storing the lines found by HoughLine
1262
1263 //// Probabilistic hough transform
1264 //cv::HoughLinesP( v_disp_map_aux_ROI, lines, 1, CV_PI/180, n_points, minLineLenght, maxLineGap );
1265 //
1266 //if (lines.size() != 0)
1267 //{
1268 // cv::Point pt1, pt2;
1269 // double theta;
1270
1271 // for(int i = 0; i < (int)lines.size(); ++i)
1272 // {
1273 // pt1.x = lines[i][0];//(CvPoint*)cvGetSeqElem(lines,i);
1274 // pt1.y = lines[i][1];
1275 // pt2.x = lines[i][2];
1276 // pt2.y = lines[i][3];
1277
1278 // this->CheckPoints(pt1, pt2); //Verify the points order
1279 //
1280 // theta = this->Inclination(pt1, pt2); // line slope
1281
1282 // // In degrees
1283 // theta = ((theta*360.0)/(2.0*CV_PI));
1284
1285 // // Verifica se a reta possui inclinacao para ser possivel plano
1286 // //if((theta < (90.0 + ANG_VARIATION2))&& (theta > (90.0 - ANG_VARIATION2)))
1287 // //{
1288 // //Desenha as retas em vermelho
1289 // cv::line( v_color_img_ROI, pt1, pt2, CV_RGB(255,0,0), 5, 8 );
1290 // //}
1291 // }
1292 //}
1293 //
1294 //// Janela de exibicao
1295 ////cv::namedWindow("Mapa de Disparidade V + Hough1",CV_WINDOW_AUTOSIZE);
1296 ////cv::imshow("Mapa de Disparidade V + Hough1", v_disp_map_aux);
1297
1298 //// Janela de exibicao
1299 ////cv::namedWindow("Mapa de Disparidade V + Hough2",CV_WINDOW_AUTOSIZE);
1300 ////cv::imshow("Mapa de Disparidade V + Hough2", color_img);
1301
1302 //================================================================================================================================
1303
1304 //============================================== V-disparity map process =========================================================
1305 // Image to be processed
1306 cv::Mat v_disp_map_aux = v_disp_map.clone();
1307
1308 // Image with the obstacles highlighted in red
1309 cv::Mat v_color_img = cv::Mat( v_disp_map.rows, v_disp_map.cols, CV_8UC3, cv::Scalar(0,0,0));
1310
1311 // Convert color space
1312 //cv::cvtColor(v_disp_map_aux, v_color_img, CV_GRAY2BGR);
1313
1314 //cv::equalizeHist( v_disp_map_aux, v_disp_map_aux);
1315
1316 // Image binarization
1317 cv::threshold(v_disp_map_aux, v_disp_map_aux, tshold1, 255, CV_THRESH_BINARY);
1318
1319 // Janela de exibicao
1320 if(this->showdebug)
1321 {
1322 cv::namedWindow("Mapa de Disparidade V + Threshold 2",CV_WINDOW_AUTOSIZE);
1323 cv::imshow("Mapa de Disparidade V + Threshold 2", v_disp_map_aux);
1324 }
1325
1326 // Define valid ROI
1327 cv::Mat v_disp_map_aux_ROI = v_disp_map_aux(cv::Rect(min_d, 0, max_d-min_d+1, v_disp_map_aux.rows));
1328 cv::Mat v_color_img_ROI = v_color_img( cv::Rect(min_d, 0, max_d-min_d+1, v_disp_map_aux.rows));
1329
1330 cv::split( v_color_img, channels);
1331 channels[2] = v_disp_map_aux;
1332 cv::merge( channels, v_color_img);
1333
1334 //================================================================================================================================
1335
1336 //=============================================== U-disparity map process ========================================================
1337 // Image to be processed
1338 cv::Mat u_disp_map_aux = u_disp_map.clone();
1339
1340 // Image with the obstacles highlighted in red
1341 cv::Mat u_color_img = cv::Mat( cv::Size(u_disp_map.cols, u_disp_map.rows), CV_8UC3, cv::Scalar(0, 0, 0));
1342
1343 // Convert color space
1344 //cv::cvtColor(u_disp_map_aux, u_color_img, CV_GRAY2BGR);
1345
1346 // Image binarization
1347 cv::threshold(u_disp_map_aux, u_disp_map_aux, tshold2, 255, CV_THRESH_BINARY);
1348
1349 // Closing
1350 cv::dilate(u_disp_map_aux, u_disp_map_aux, cv::Mat(), cv::Point(-1,-1), 1 );
1351 cv::erode(u_disp_map_aux, u_disp_map_aux, cv::Mat(), cv::Point(-1,-1), 1 );
1352
1353
1354 if(this->showdebug)
1355 {
1356 // Janela de exibicao
1357 cv::namedWindow("ObstacleDetectionComponent - Image bin u-disp",CV_WINDOW_AUTOSIZE);
1358 cv::imshow("ObstacleDetectionComponent - Image bin u-disp", u_disp_map_aux);
1359 }
1360
1361 cv::split( u_color_img, channels);
1362 channels[2] = u_disp_map_aux;
1363 cv::merge( channels, u_color_img);
1364
1365 // Define valid ROI
1366 //cv::Mat u_disp_map_aux_ROI = v_disp_map_aux(cv::Rect(min_d, 0, max_d-min_d+1, v_disp_map_aux.rows));
1367 //cv::Mat u_color_img_ROI = v_color_img( cv::Rect(min_d, 0, max_d-min_d+1, v_disp_map_aux.rows));
1368 //================================================================================================================================
1369
1370 return std::make_pair(v_color_img, u_color_img);
1371}
1372
1373/* LinesFiltering
1374 Description:
1375 Filter the detected lines related to the distance and angle between them.
1376 Parameters:
1377 lines = line list (point 1 and 2)
1378*/
1379std::vector<cv::Point> ObstacleDetectionComponent::LinesFiltering(std::vector<cv::Vec4i> lines)
1380{
1381 std::vector<cv::Point> lista_pontos;
1382
1383 std::vector<cv::Point> lista_retorno;
1384
1385 std::vector<double> angles;
1386
1387 cv::Point pt1, pt2;
1388 double theta;
1389 double angulo_medio = 0.0;
1390
1391 if (lines.size() != 0)
1392 {
1393 //std::cout << "Found "<< lines.size() << " lines!\n";
1394
1395 // Filtro de angulo maximo
1396 for(int i = 0; i < (int)lines.size(); ++i)
1397 {
1398 pt1.x = lines[i][0];//(CvPoint*)cvGetSeqElem(lines,i);
1399 pt1.y = lines[i][1];
1400 pt2.x = lines[i][2];
1401 pt2.y = lines[i][3];
1402
1403 CheckPoints(pt1, pt2); //Verifica a ordem dos pontos
1404
1405 theta = Inclination(pt1, pt2); //calcula a inclinacao da reta encontrada
1406
1407 // Valor atual do angulo em graus
1408 theta = ((theta*360.0)/(2.0*CV_PI));
1409
1410 //std::cout << "Angle "<< theta << "!\n";
1411
1412 // Verifica se a reta possui inclinacao para ser possivel plano
1413 if(theta> (90.0 + ANG_VARIATION))
1414 {
1415 lista_pontos.push_back(pt1);
1416 lista_pontos.push_back(pt2);
1417
1418 angles.push_back(theta);
1419 }
1420 }
1421
1422 if(!lista_pontos.empty())
1423 {
1424 std::sort(lista_pontos.begin(), lista_pontos.end(), ComparePoints1);
1425 angulo_medio = this->CalcMedian(angles);
1426 }
1427
1428 CvPoint last_item;
1429
1430 // Percorre os pontos ordenados e procura a casca convexa mais a esquerda
1431 for(std::vector<cv::Point>::iterator it = lista_pontos.begin(); it != lista_pontos.end(); ++it)
1432 {
1433 if(lista_retorno.empty())
1434 {
1435 lista_retorno.push_back(*it);
1436
1437 last_item = *it;
1438 }
1439 else
1440 {
1441 if( (it + 1) != lista_pontos.end())
1442 {
1443 // Se tiverem o mesmo y, substitui o ponto
1444 if(last_item.y == (*it).y)
1445 {
1446 // Troca o ultimo elemento
1447 lista_retorno.pop_back();
1448
1449 lista_retorno.push_back(*it);
1450
1451 last_item = *it;
1452 }
1453 else
1454 {
1455 theta = Inclination(*it, last_item); //calcula a inclinacao da reta encontrada
1456
1457 // Valor atual do angulo em graus
1458 theta = ((theta*360.0)/(2.0*CV_PI));
1459
1460 lista_retorno.push_back(*it);
1461
1462 // Percorre os pontos ordenados e procura o maior angulo entre a casca convexa mais a esquerda
1463 for(std::vector<cv::Point>::iterator it2 = it + 1; it2 != lista_pontos.end(); ++it2)
1464 {
1465 // Verifica se o angulo atual e menor que o anterior
1466 if(theta < ((Inclination(*it2, last_item)*360.0)/(2.0*CV_PI)))
1467 {
1468 // Troca o ultimo elemento
1469 lista_retorno.pop_back();
1470
1471 lista_retorno.push_back(*it2);
1472
1473 theta = Inclination(*it2, last_item); //calcula a inclinacao da reta encontrada
1474
1475 // Valor atual do angulo em graus
1476 theta = ((theta*360.0)/(2.0*CV_PI));
1477
1478 it = it2;
1479 }
1480 } // for
1481
1482 last_item = *it;
1483 } // else
1484 } // if( (it + 1) != lista_pontos.end())
1485 } // else
1486 } // for
1487
1488 if(!lista_retorno.empty())
1489 {
1490 last_item = *(lista_retorno.begin());
1491
1492 // Filtro de angulo medio
1493 for(std::vector<cv::Point>::iterator it = lista_retorno.begin() + 1; it != lista_retorno.end(); ++it)
1494 {
1495 theta = Inclination(*it, last_item); //calcula a inclinacao da reta encontrada
1496
1497 // Valor atual do angulo em graus
1498 theta = ((theta*360.0)/(2.0*CV_PI));
1499
1500 // Verifica se a reta possui inclinacao fora da media
1501 if(theta < (0.95*angulo_medio))//((theta > (1.05*angulo_medio))&&(theta < (0.95*angulo_medio)))
1502 {
1503 lista_retorno.resize(it - lista_retorno.begin());
1504
1505 break;
1506 }
1507
1508 last_item = *it;
1509 }
1510 }
1511
1512 }
1513
1514 return lista_retorno;
1515}
1516
1517//Function to check the points order, making the second one with the highest y ever
1518void ObstacleDetectionComponent::CheckPoints(cv::Point &pt1, cv::Point &pt2)
1519{
1520 int aux_x, aux_y;
1521
1522 if(pt1.y > pt2.y)
1523 {
1524 aux_x = pt1.x;
1525 aux_y = pt1.y;
1526
1527 pt1.x = pt2.x;
1528 pt1.y = pt2.y;
1529
1530 pt2.x = aux_x;
1531 pt2.y = aux_y;
1532 }
1533
1534 return;
1535}
1536
1537// Function to calculate the line slope of pt1 to pt2
1538double ObstacleDetectionComponent::Inclination(cv::Point pt1, cv::Point pt2)
1539{
1540 double theta; //angle
1541
1542 theta = fabs((atan2((pt1.y-pt2.y+0.0),(pt1.x-pt2.x+0.0)))); // slope
1543
1544 return theta;
1545}
1546
1547/* CalcMedian
1548 Description:
1549 Calcule the median value of a vector.
1550 Parametros:
1551 vector = vector with data to calculate the median
1552*/
1553template<class A>
1554A ObstacleDetectionComponent::CalcMedian(std::vector<A> vetor) const
1555{
1556 A mediana;
1557
1558 std::sort(vetor.begin(), vetor.end());
1559
1560 mediana = vetor[(int)((double)(vetor.size())/2.0 + 0.5) - 1];
1561
1562 return mediana;
1563}
1564
1565// Function to calculate the free space (v_disp_1) and obstacles (v_disp_2) masks from
1566// a red highlighted V-Disparity map
1567std::pair<cv::Mat, cv::Mat> ObstacleDetectionComponent::MaskSurface2(cv::Mat disp_map, cv::Mat v_disp_1, cv::Mat v_disp_2, int min_d, int max_d, int value)
1568{
1569 // Free space mask
1570 cv::Mat mask1;
1571
1572 // Obstacle Mask
1573 cv::Mat mask2;
1574
1575 // Fill the destiny images with background value
1576 if(value == 1)
1577 {
1578 mask1 = cv::Mat::zeros( cv::Size(disp_map.cols, disp_map.rows), CV_8UC1 );
1579 mask2 = cv::Mat::zeros( cv::Size(disp_map.cols, disp_map.rows), CV_8UC1 );
1580 }
1581 else
1582 {
1583 mask1 = cv::Mat::ones( cv::Size(disp_map.cols, disp_map.rows), CV_8UC1 );
1584 mask2 = cv::Mat::ones( cv::Size(disp_map.cols, disp_map.rows), CV_8UC1 );
1585 }
1586
1587 int l, c, pixel, v_disp_pixel; // local variables for row, column and pixel
1588 unsigned char intensity; // pixel intensity
1589 unsigned char intensity_B, intensity_G, intensity_R; //pixel intensity
1590
1591 /*uint8_t* pixelPtr_mask1 = (uint8_t*)mask1.data;
1592 uint8_t* pixelPtr_mask2 = (uint8_t*)mask2.data;
1593 uint8_t* pixelPtr_disp_map = (uint8_t*)disp_map.data;
1594 uint8_t* pixelPtr_v_disp1 = (uint8_t*)v_disp_1.data;
1595 uint8_t* pixelPtr_v_disp2 = (uint8_t*)v_disp_2.data;*/
1596
1597 // run the images associating the red pixels in the v-disparities in the mask1 e mask2
1598 for (l = 0; l < disp_map.rows; ++l)
1599 {
1600 for (c = 0; c < disp_map.cols; ++c)
1601 {
1602 pixel = l*disp_map.cols + c;
1603
1604 intensity = (unsigned char)disp_map.data[pixel];
1605
1606 //--------- Marca os planos trafegaveis --------------------
1607 // Pixel Azul
1608 v_disp_pixel = l*v_disp_1.cols*3 + intensity*3;
1609 intensity_B = (unsigned char)v_disp_1.data[v_disp_pixel];
1610
1611 // Pixel Verde
1612 v_disp_pixel = l*v_disp_1.cols*3 + intensity*3 + 1;
1613 intensity_G = (unsigned char)v_disp_1.data[v_disp_pixel];
1614
1615 // Pixel Vermelho
1616 v_disp_pixel = l*v_disp_1.cols*3 + intensity*3 + 2;
1617 intensity_R = (unsigned char)v_disp_1.data[v_disp_pixel];
1618
1619 if((intensity_B == 0)&&(intensity_G == 0)&&(intensity_R == 255))
1620 {
1621 mask1.data[pixel] = value;
1622 }
1623 //----------------------------------------------------------
1624
1625 //--------- Marca os obstaculos ----------------------------
1626 if( (intensity >= min_d)&&(intensity <= max_d))
1627 {
1628 // Pixel Azul
1629 v_disp_pixel = l*v_disp_2.cols*3 + intensity*3;
1630 intensity_B = (unsigned char)v_disp_2.data[v_disp_pixel];
1631
1632 // Pixel Verde
1633 v_disp_pixel = l*v_disp_2.cols*3 + intensity*3 + 1;
1634 intensity_G = (unsigned char)v_disp_2.data[v_disp_pixel];
1635
1636 // Pixel Vermelho
1637 v_disp_pixel = l*v_disp_2.cols*3 + intensity*3 + 2;
1638 intensity_R = (unsigned char)v_disp_2.data[v_disp_pixel];
1639
1640 if((intensity_B == 0)&&(intensity_G == 0)&&(intensity_R == 255))
1641 {
1642 mask2.data[pixel] = value;
1643 }
1644 }
1645 //----------------------------------------------------------
1646 }
1647 }
1648
1649 /*cv::Mat element = cv::getStructuringElement( cv::MORPH_RECT,
1650 cv::Size( 3, 3 ),
1651 cv::Point( 1, 1 ) );*/
1652
1653 // Espande as regioes brancas da imagem
1654 if(value == 1)
1655 {
1656 //cv::dilate(mask1, mask1, element, cv::Point(1,1), 2);
1657 //cv::dilate(mask2, mask2, element, cv::Point(1,1), 2);
1658
1659 cv::dilate(mask1, mask1, cv::Mat(), cv::Point(-1,-1), 2 );
1660 cv::dilate(mask2, mask2, cv::Mat(), cv::Point(-1,-1), 2 );
1661 }
1662 else
1663 {
1664 //cv::erode(mask1, mask1, element, cv::Point(1,1), 2);
1665 //cv::erode(mask2, mask2, element, cv::Point(1,1), 2);
1666
1667 cv::erode(mask1,mask1,NULL, cv::Point(-1,-1), 2);
1668 cv::erode(mask2,mask2,NULL, cv::Point(-1,-1), 2);
1669 }
1670
1671 //cvNamedWindow("Imagem da mascara1",CV_WINDOW_AUTOSIZE);
1672 //cvShowImage("Imagem da mascara1", mask1);
1673
1674 //cvNamedWindow("Imagem da mascara2",CV_WINDOW_AUTOSIZE);
1675 //cvShowImage("Imagem da mascara2", mask2);
1676
1677 return std::make_pair(mask1, mask2);
1678}
1679
1680// Function to calculate the free space (v_disp_1) and obstacles (v_disp_2/u_disp) masks from
1681// a red highlighted V-Disparity map
1682std::pair<cv::Mat, cv::Mat> ObstacleDetectionComponent::MaskSurface3(cv::Mat disp_map, cv::Mat v_disp_1, cv::Mat v_disp_2, cv::Mat u_disp, int min_d, int max_d, int value)
1683{
1684 // Free space mask
1685 cv::Mat mask1;
1686
1687 // Obstacle Mask
1688 cv::Mat mask2;
1689
1690 // Fill the destiny images with background value
1691 if(value == 1)
1692 {
1693 mask1 = cv::Mat::zeros( cv::Size(disp_map.cols, disp_map.rows), CV_8UC1 );
1694 mask2 = cv::Mat::zeros( cv::Size(disp_map.cols, disp_map.rows), CV_8UC1 );
1695 }
1696 else
1697 {
1698 mask1 = cv::Mat::ones( cv::Size(disp_map.cols, disp_map.rows), CV_8UC1 );
1699 mask2 = cv::Mat::ones( cv::Size(disp_map.cols, disp_map.rows), CV_8UC1 );
1700 }
1701
1702 int l, c, pixel, v_disp_pixel, u_disp_pixel; // local variables for row, column and pixel
1703 unsigned char intensity; // pixel intensity
1704 unsigned char intensity_B_v, intensity_G_v, intensity_R_v; //pixel intensity
1705 unsigned char intensity_B_u, intensity_G_u, intensity_R_u; //pixel intensity
1706
1707 /*uint8_t* pixelPtr_mask1 = (uint8_t*)mask1.data;
1708 uint8_t* pixelPtr_mask2 = (uint8_t*)mask2.data;
1709 uint8_t* pixelPtr_disp_map = (uint8_t*)disp_map.data;
1710 uint8_t* pixelPtr_v_disp1 = (uint8_t*)v_disp_1.data;
1711 uint8_t* pixelPtr_v_disp2 = (uint8_t*)v_disp_2.data;*/
1712
1713 // run the images associating the red pixels in the v-disparities in the mask1 e mask2
1714 for (l = 0; l < disp_map.rows; ++l)
1715 {
1716 for (c = 0; c < disp_map.cols; ++c)
1717 {
1718 pixel = l*disp_map.cols + c;
1719
1720 intensity = (unsigned char)disp_map.data[pixel];
1721
1722 //--------- Marca os planos trafegaveis --------------------
1723 // Pixel Azul
1724 v_disp_pixel = l*v_disp_1.cols*3 + intensity*3;
1725 intensity_B_v = (unsigned char)v_disp_1.data[v_disp_pixel];
1726
1727 // Pixel Verde
1728 v_disp_pixel = l*v_disp_1.cols*3 + intensity*3 + 1;
1729 intensity_G_v = (unsigned char)v_disp_1.data[v_disp_pixel];
1730
1731 // Pixel Vermelho
1732 v_disp_pixel = l*v_disp_1.cols*3 + intensity*3 + 2;
1733 intensity_R_v = (unsigned char)v_disp_1.data[v_disp_pixel];
1734
1735 if((intensity_B_v == 0)&&(intensity_G_v == 0)&&(intensity_R_v == 255))
1736 {
1737 mask1.data[pixel] = value;
1738 }
1739 //----------------------------------------------------------
1740
1741 //--------- Marca os obstaculos ----------------------------
1742 if( (intensity >= min_d)&&(intensity <= max_d))
1743 {
1744 // Pixel Azul
1745 v_disp_pixel = l*v_disp_2.cols*3 + intensity*3;
1746 u_disp_pixel = intensity*u_disp.cols*3 + c*3;
1747 intensity_B_v = (unsigned char)v_disp_2.data[v_disp_pixel];
1748 intensity_B_u = (unsigned char)u_disp.data[u_disp_pixel];
1749
1750 // Pixel Verde
1751 v_disp_pixel = l*v_disp_2.cols*3 + intensity*3 + 1;
1752 u_disp_pixel = intensity*u_disp.cols*3 + c*3 + 1;
1753 intensity_G_v = (unsigned char)v_disp_2.data[v_disp_pixel];
1754 intensity_G_u = (unsigned char)u_disp.data[u_disp_pixel];
1755
1756 // Pixel Vermelho
1757 v_disp_pixel = l*v_disp_2.cols*3 + intensity*3 + 2;
1758 u_disp_pixel = intensity*u_disp.cols*3 + c*3 + 2;
1759 intensity_R_v = (unsigned char)v_disp_2.data[v_disp_pixel];
1760 intensity_R_u = (unsigned char)u_disp.data[u_disp_pixel];
1761
1762 if((intensity_B_v == 0)&&(intensity_G_v == 0)&&(intensity_R_v == 255)&&(intensity_B_u == 0)&&(intensity_G_u == 0)&&(intensity_R_u == 255))
1763 {
1764 mask2.data[pixel] = value;
1765 }
1766 }
1767 //----------------------------------------------------------
1768 }
1769 }
1770
1771 /*cv::Mat element = cv::getStructuringElement( cv::MORPH_RECT,
1772 cv::Size( 3, 3 ),
1773 cv::Point( 1, 1 ) );*/
1774
1775 // Espande as regioes brancas da imagem
1776 if(value == 1)
1777 {
1778 //cv::dilate(mask1, mask1, element, cv::Point(1,1), 2);
1779 //cv::dilate(mask2, mask2, element, cv::Point(1,1), 2);
1780
1781 cv::dilate(mask1, mask1, cv::Mat(), cv::Point(-1,-1), 2 );
1782 cv::dilate(mask2, mask2, cv::Mat(), cv::Point(-1,-1), 2 );
1783 }
1784 else
1785 {
1786 //cv::erode(mask1, mask1, element, cv::Point(1,1), 2);
1787 //cv::erode(mask2, mask2, element, cv::Point(1,1), 2);
1788
1789 cv::erode(mask1,mask1,NULL, cv::Point(-1,-1), 2);
1790 cv::erode(mask2,mask2,NULL, cv::Point(-1,-1), 2);
1791 }
1792
1793 //cvNamedWindow("Imagem da mascara1",CV_WINDOW_AUTOSIZE);
1794 //cvShowImage("Imagem da mascara1", mask1);
1795
1796 //cvNamedWindow("Imagem da mascara2",CV_WINDOW_AUTOSIZE);
1797 //cvShowImage("Imagem da mascara2", mask2);
1798
1799 return std::make_pair(mask1, mask2);
1800}
Note: See TracBrowser for help on using the repository browser.