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

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

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