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

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

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