source: pacpussensors/trunk/StereoVisionDisparity/CameraObstacleGridComponent.cpp@ 73

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

Version finale de l'application provel
File size: 40.9 KB
RevLine 
[70]1/*********************************************************************
2// created: 2013/06/19 - 18:40
3// filename: CameraObstacleGridComponent.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: Camera obstacle's grid calculation
11//
12//
13*********************************************************************/
14
15//#include "GeneralDefinitions.h"
16
17#include "CameraObstacleGridComponent.h"
18
19#include <iostream>
20#include <string>
21#include "opencv2/calib3d/calib3d.hpp"
22#include "opencv2/core/core.hpp"
23
24#include <qapplication.h>
25#include <string>
26
27// Includes, qt.
28#include <QMetaType>
29
30#include "Pacpus/kernel/ComponentFactory.h"
31#include "Pacpus/kernel/DbiteException.h"
32#include "Pacpus/kernel/DbiteFileTypes.h"
33#include "Pacpus/kernel/Log.h"
34#include "GeneralDefinitions.h"
35
36using namespace std;
37using namespace pacpus;
38
39DECLARE_STATIC_LOGGER("pacpus.base.CameraObstacleGridComponent");
40
41// Construct the factory
42static ComponentFactory<CameraObstacleGridComponent> sFactory("CameraObstacleGridComponent");
43
44const int kMaxFilepathLength = 512; // TODO: should be same for all images
45#define PI_VALUE 3.1415926535897932
46
47//------------------------------- From stereo data----------------------------------
48static const string CameraObstacleGridMemoryName_mask1 = "ObstacleDetection-mask1";
49static const string CameraObstacleGridMemoryName_mask2 = "ObstacleDetection-mask2";
50static const string CameraObstacleGridMemoryName_disp16 = "ObstacleDetection-disp";
51
52static const string CameraObstacleGridMemoryName_obstgrid = "CameraObstacleGrid-obst";
53//----------------------------------------------------------------------------------
54
55//-------------------------------- From mono data ----------------------------------
56static const string CameraObstacleGridMemoryName_roadseg = "LineDetection-mask";
57static const string CameraObstacleGridMemoryName_roadgrid = "CameraObstacleGrid-road";
58//----------------------------------------------------------------------------------
59
60//////////////////////////////////////////////////////////////////////////
61/// Constructor.
62CameraObstacleGridComponent::CameraObstacleGridComponent(QString name)
63 : ComponentBase(name)
64{
65 LOG_TRACE(Q_FUNC_INFO);
66
67 recording = 0;
68
69 this->cam_width = 1280; // Image width
70 this->cam_height = 960; // Image height
71 this->showdebug = false; // Show frame acquired
72
73 // Size of the image data sizeof(char)*width*height*channels
74 this->mMaxImageInputSize1 = sizeof(char)*this->cam_width*this->cam_height*3;
75
76 // Input data
77 this->shmem_mask1 = 0; // Shared memory control access to the image data (free space mask)
78 this->shmem_mask2 = 0; // Shared memory control access to the image data (obstacles mask)
79 this->shmem_disp16 = 0; // Shared memory control access to the image data (disparity map 16)
80
81 // Output data
82 this->shmem_obst = 0; // Shared memory control access to the image data
83
84 // Output data via UDP
85 //this->udp_con = 0;
86}
87
88//////////////////////////////////////////////////////////////////////////
89/// Destructor.
90CameraObstacleGridComponent::~CameraObstacleGridComponent()
91{
92 LOG_TRACE(Q_FUNC_INFO);
93
94 if(this->shmem_mask1)
95 delete shmem_mask1;
96
97 this->shmem_mask1 = NULL;
98
99 if(this->shmem_mask2)
100 delete shmem_mask2;
101
102 this->shmem_mask2 = NULL;
103
104 if(this->shmem_disp16)
105 delete shmem_disp16;
106
107 this->shmem_disp16 = NULL;
108
109 if(this->shmem_obst)
110 delete shmem_obst;
111
112 this->shmem_obst = NULL;
113
114/* if(this->udp_con)
115 delete this->udp_con;
116
117 this->udp_con = NULL;*/
118}
119
120//////////////////////////////////////////////////////////////////////////
121/// Called by the ComponentManager to start the component
122void CameraObstacleGridComponent::startActivity()
123{
124 LOG_TRACE(Q_FUNC_INFO);
125
126 if(this->run_stereo)
127 {
128 this->mMaxImageInputSize1 = sizeof(unsigned char)*this->cam_width*this->cam_height;
129 this->mMaxImageInputSize2 = sizeof(unsigned short)*this->cam_width*this->cam_height;
130
131 if(this->data_type == 0)
132 {
133 this->mMaxOutputSize1 = sizeof(TimestampedCameraOccData);
134 }
135 else
136 {
137 this->mMaxOutputSize1 = sizeof(TimestampedSensorOccData);
138 }
139
140 this->mask1_mem_size = sizeof(TimestampedStructImage) + this->mMaxImageInputSize1;
141 this->mask2_mem_size = sizeof(TimestampedStructImage) + this->mMaxImageInputSize1;
142 this->disp_mem_size = sizeof(TimestampedStructImage) + this->mMaxImageInputSize2;
143
144 // Allocate memory position for the maximum expected data
145 this->mask1_mem = malloc(this->mask1_mem_size);
146 this->mask2_mem = malloc(this->mask2_mem_size);
147 this->disp_mem = malloc(this->disp_mem_size);
148
149 this->shmem_mask1 = new ShMem(CameraObstacleGridMemoryName_mask1.c_str(), this->mask1_mem_size);
150
151 this->shmem_mask2 = new ShMem(CameraObstacleGridMemoryName_mask2.c_str(), this->mask2_mem_size);
152
153 this->shmem_disp16 = new ShMem(CameraObstacleGridMemoryName_disp16.c_str(), this->disp_mem_size);
154
155 if(this->use_udpconnection)
156 {
157 //this->udp_con = new UDPConnection();
158 //this->udp_con->CreateConnection(this->destiny_IP, this->communication_Port, this->mMaxOutputSize1, false);
159 this->shmem_obst = NULL;
160 }
161 else
162 {
163 //this->udp_con = NULL;
164 this->shmem_obst = new ShMem(CameraObstacleGridMemoryName_obstgrid.c_str(), this->mMaxOutputSize1);
165 }
166 }
167 else
168 {
169 this->mMaxImageInputSize1 = sizeof(unsigned char)*this->cam_width*this->cam_height;
170
171 this->mMaxOutputSize1 = sizeof(TimestampedSensorOccData);
172
173 this->mask1_mem_size = sizeof(TimestampedStructImage) + this->mMaxImageInputSize1;
174
175 // Allocate memory position for the maximum expected data
176 this->mask1_mem = malloc(this->mask1_mem_size);
177
178 this->shmem_mask1 = new ShMem(CameraObstacleGridMemoryName_roadseg.c_str(), this->mask1_mem_size);
179
180 this->shmem_mask2 = NULL;
181
182 this->shmem_disp16 = NULL;
183
184 if(this->use_udpconnection)
185 {
186 //this->udp_con = new UDPConnection();
187 //this->udp_con->CreateConnection(this->destiny_IP, this->communication_Port, this->mMaxOutputSize1, false);
188 this->shmem_obst = NULL;
189 }
190 else
191 {
192 //this->udp_con = NULL;
193 this->shmem_obst = new ShMem(CameraObstacleGridMemoryName_roadgrid.c_str(), this->mMaxOutputSize1);
194 }
195 }
196
197 // Run thread
198 THREAD_ALIVE = true;
199 start();
200}
201
202//////////////////////////////////////////////////////////////////////////
203/// Called by the ComponentManager to stop the component
204void CameraObstacleGridComponent::stopActivity()
205{
206 LOG_TRACE(Q_FUNC_INFO);
207
208 if(THREAD_ALIVE)
209 {
210 // Stop thread
211 THREAD_ALIVE = false;
212
213 while(is_running)
214 {
215 msleep(/*MS_DELAY*/10);
216 }
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_disp16)
229 delete shmem_disp16;
230
231 this->shmem_disp16 = NULL;
232
233 if(this->shmem_obst)
234 delete shmem_obst;
235
236 this->shmem_obst = NULL;
237
238 /*if(this->udp_con)
239 delete this->udp_con;
240
241 this->udp_con = NULL;*/
242 }
243
244 LOG_INFO("stopped component '" << name() << "'");
245}
246
247//////////////////////////////////////////////////////////////////////////
248/// Called by the ComponentManager to pass the XML parameters to the
249/// component
250ComponentBase::COMPONENT_CONFIGURATION CameraObstacleGridComponent::configureComponent(XmlComponentConfig config)
251{
252 LOG_TRACE(Q_FUNC_INFO);
253
254 // Initialize with default values
255 InitDefault();
256
257 if (config.getProperty("run_stereo") != QString::null)
258 this->run_stereo = config.getProperty("run_stereo").toInt();
259
260 if (config.getProperty("recording") != QString::null)
261 recording = config.getProperty("recording").toInt();
262
263 if (config.getProperty("cam_width") != QString::null)
264 this->cam_width = config.getProperty("cam_width").toInt();
265
266 if (config.getProperty("cam_height") != QString::null)
267 this->cam_height = config.getProperty("cam_height").toInt();
268
269 if (config.getProperty("cam_fov") != QString::null)
270 this->cam_fov = config.getProperty("cam_fov").toDouble();
271
272 if (config.getProperty("cam_fx") != QString::null)
273 this->cam_fx = config.getProperty("cam_fx").toDouble();
274
275 if (config.getProperty("cam_fy") != QString::null)
276 this->cam_fy = config.getProperty("cam_fy").toDouble();
277
278 if (config.getProperty("cam_cx") != QString::null)
279 this->cam_cx = config.getProperty("cam_cx").toDouble();
280
281 if (config.getProperty("cam_cy") != QString::null)
282 this->cam_cy = config.getProperty("cam_cy").toDouble();
283
284 if (config.getProperty("cam_baseline") != QString::null)
285 this->cam_baseline = config.getProperty("cam_baseline").toDouble();
286
287 if (config.getProperty("cam_tilt_angle") != QString::null)
288 this->cam_tilt_angle = config.getProperty("cam_tilt_angle").toDouble()*PI_VALUE/180.0;
289
290 if (config.getProperty("CorrAccuracy") != QString::null)
291 this->CorrAccuracy = config.getProperty("CorrAccuracy").toDouble();
292
293 if (config.getProperty("cam_valid_col0") != QString::null)
294 this->cam_valid_col0 = config.getProperty("cam_valid_col0").toInt();
295 else
296 this->cam_valid_col0 = 0;
297
298 if (config.getProperty("cam_valid_row0") != QString::null)
299 this->cam_valid_row0 = config.getProperty("cam_valid_row0").toInt();
300 else
301 this->cam_valid_row0 = 0;
302
303 if (config.getProperty("cam_valid_cols") != QString::null)
304 this->cam_valid_cols = config.getProperty("cam_valid_cols").toInt();
305 else
306 this->cam_valid_cols = this->cam_width;
307
308 if (config.getProperty("cam_valid_rows") != QString::null)
309 this->cam_valid_rows = config.getProperty("cam_valid_rows").toInt();
310 else
311 this->cam_valid_rows = this->cam_height;
312
313 if (config.getProperty("data_type") != QString::null)
314 this->data_type = config.getProperty("data_type").toInt();
315
316 if (config.getProperty("D_MAX_CAM_GRID") != QString::null)
317 this->D_MAX_CAM_GRID = config.getProperty("D_MAX_CAM_GRID").toDouble();
318
319 if (config.getProperty("sub_div") != QString::null)
320 this->sub_div = config.getProperty("sub_div").toDouble();
321
322 if (config.getProperty("free_area_guess") != QString::null)
323 this->free_area_guess = config.getProperty("free_area_guess").toDouble();
324
325 if (config.getProperty("showdebug") != QString::null)
326 this->showdebug = (bool)config.getProperty("showdebug").toInt();
327
328 if (config.getProperty("use_udpconnection") != QString::null)
329 this->use_udpconnection = (bool)config.getProperty("use_udpconnection").toInt();
330
331 if ((this->use_udpconnection)&&(this->data_type == 1))
332 {
333 LOG_WARN("This data type is not valid for UDP connexion! Data type changed to default.");
334 this->data_type = 0;
335 }
336
337 if (config.getProperty("MAX_DIST_TO_BRAKE") != QString::null)
338 this->maxDistToBrake = config.getProperty("MAX_DIST_TO_BRAKE").toDouble();
339
340 if(this->use_udpconnection)
341 {
342 //this->destiny_IP = (config.getProperty("destiny_IP") != QString::null) ? QHostAddress(config.getProperty("destiny_IP")) : QHostAddress::LocalHost;
343 //this->communication_Port = (config.getProperty("communication_Port") != QString::null) ? (quint16)config.getProperty("communication_Port").toInt() : (quint16)1200;
344 }
345
346
347 LOG_INFO("configured component '" << name() << "'");
348 return ComponentBase::CONFIGURED_OK;
349}
350
351/**
352* Initialize default values
353*/
354void CameraObstacleGridComponent::InitDefault()
355{
356 // Default
357 recording = 0;
358
359 this->maxDistToBrake = 3.0;
360
361 this->run_stereo = 1; // Select the work mode (stereo camera data or mono camera data)
362
363 // Default values for bumblebee X3 - 6mm - narrow - 400x300
364 this->cam_width = 400; // Image width
365 this->cam_height = 300; // Image height
366 this->cam_fov = 43.0;
367 this->cam_valid_col0 = 0; // Initial valid column
368 this->cam_valid_row0 = 0; // Initial valid row
369 this->cam_valid_cols = 400; // Valid columns
370 this->cam_valid_rows = 300; // Valid rows
371 this->cam_fx = 1.26209; // Focus in x (pixels)
372 this->cam_fy = 1.68279; // Focus in y (pixels)
373 this->cam_cx = 0.50875; // Focus in x (pixels)
374 this->cam_cy = 0.510218; // Focus in y (pixels)
375 this->cam_baseline = 0.12; // Baseline (meters)
376 this->cam_tilt_angle = 0.0; // Tilt angle (rad)
377 this->cam_h = 1.615; // Camera height
378 this->CorrAccuracy = 0.2; // Estimated camera correlation accuracy
379 this->showdebug = false; // Show frame acquired
380
381 this->use_udpconnection = false;
382
383 //this->destiny_IP = QHostAddress::LocalHost;
384 //this->communication_Port = (quint16)1200;
385
386 this->D_MAX_CAM_GRID = D_MAX_CAM_GRID_DEFAULT;
387 this->data_type = 0;
388 this->sub_div = 0.20;
389
390 this->free_area_guess = 4.0; // Frontal projected area expected to be a road surface (frontal road surface uncouvered by camera FOV)
391}
392
393// Thread loop for Stereo data
394void CameraObstacleGridComponent::run()
395{
396 LOG_TRACE(Q_FUNC_INFO);
397
398 this->is_running = true;
399
400 if(this->run_stereo)
401 {
402 if(this->CurrentMask1Frame.cols != this->cam_width)
403 {
404 this->CurrentMask1Frame = cv::Mat(cv::Size(this->cam_width , this->cam_height), CV_MAKETYPE(CV_8U, 1));
405 }
406
407 if(this->CurrentMask2Frame.cols != this->cam_width)
408 {
409 this->CurrentMask2Frame = cv::Mat(cv::Size(this->cam_width , this->cam_height), CV_MAKETYPE(CV_8U, 1));
410 }
411
412 // Create the image in which we will save the disparities
413 if(this->CurrentDisparityMap16.cols != this->cam_width)
414 {
415 this->CurrentDisparityMap16 = cv::Mat( this->cam_height, this->cam_width, CV_16S );
416 }
417
418 // Keeps the last image timestamp;
419 road_time_t last_reading = 0;
420
421 // Time measurement
422 road_time_t init_time = 0;
423
424 while (THREAD_ALIVE)
425 {
426 //init_time = road_time();
427
428 //LOG_INFO("Grab new image");
429 // header + image
430 this->shmem_mask1->read(this->mask1_mem, this->mask1_mem_size);
431 this->shmem_mask2->read(this->mask2_mem, this->mask2_mem_size);
432 this->shmem_disp16->read(this->disp_mem, this->disp_mem_size);
433
434 // Header
435 memcpy( &this->Mask1ImageHeader, this->mask1_mem, sizeof(TimestampedStructImage));
436 memcpy( &this->Mask2ImageHeader, this->mask2_mem, sizeof(TimestampedStructImage));
437 memcpy( &this->DispImageHeader, this->disp_mem, sizeof(TimestampedStructImage));
438
439 // Check image header
440 bool is_ok = false;
441 if( (this->Mask1ImageHeader.image.data_size == this->mMaxImageInputSize1) && (this->Mask1ImageHeader.time != last_reading) &&
442 (this->Mask2ImageHeader.image.data_size == this->mMaxImageInputSize1) && (this->Mask2ImageHeader.time == this->Mask1ImageHeader.time) &&
443 (this->DispImageHeader.image.data_size == this->mMaxImageInputSize2) && (this->DispImageHeader.time == this->Mask1ImageHeader.time) )
444 {
445 is_ok = true;
446 last_reading = this->Mask1ImageHeader.time;
447
448 /*std::cout << "Expected image w: " << ImageHeader.image.width << std::endl;
449 std::cout << "Expected image h: " << ImageHeader.image.height << std::endl;
450 std::cout << "Expected image c: " << ImageHeader.image.channels << std::endl;
451 std::cout << "Expected image data: " << ImageHeader.image.data_size << std::endl;
452 std::cout << "Expected image size: " << image_mem << std::endl;*/
453 }
454 /*else
455 {
456 LOG_ERROR("Error in the image data size!");
457 }*/
458
459 //LOG_INFO("Grab new image");
460 if(is_ok)
461 {
462 // Image data
463 memcpy( (unsigned char*)(this->CurrentMask1Frame.data), (unsigned char*)((TimestampedStructImage*)this->mask1_mem + 1), this->Mask1ImageHeader.image.data_size);
464 memcpy( (unsigned char*)(this->CurrentMask2Frame.data), (unsigned char*)((TimestampedStructImage*)this->mask2_mem + 1), this->Mask2ImageHeader.image.data_size);
465 memcpy( (unsigned short*)(this->CurrentDisparityMap16.data), (unsigned short*)((TimestampedStructImage*)this->disp_mem + 1), this->DispImageHeader.image.data_size);
466
467 //======================================= Obstacle Grid Calculation ================================================
468 if(this->data_type == 0)
469 {
470 // Camera to laser obstacles grid
471 this->CreateCamera2LaserGrid(this->CurrentMask2Frame, this->CurrentDisparityMap16, this->Camdata2Laser.data.radius, this->Camdata2Laser.data.angle);
472
473 this->Camdata2Laser.time = this->DispImageHeader.time;
474 this->Camdata2Laser.timerange = this->DispImageHeader.timerange;
475 this->Camdata2Laser.data.num_readings = this->cam_valid_cols;
476
477 if(this->use_udpconnection)
478 {
479 //this->udp_con->write(&this->Camdata2Laser, this->mMaxOutputSize1);
480 }
481 else
482 {
483 this->shmem_obst->write(&this->Camdata2Laser, this->mMaxOutputSize1);
484 }
485
486 if(this->showdebug)
487 {
488 cv::namedWindow( "CameraObstacleGridComponent - Final Result", CV_WINDOW_AUTOSIZE );
489 cv::imshow("CameraObstacleGridComponent - Final Result", this->DrawGrid(this->Camdata2Laser.data.radius, this->Camdata2Laser.data.angle));
490 cv::waitKey(1);
491 }
492 }
493 else
494 {
495 // Camera occupancy grid
496 this->CreateCameraGrid(this->CurrentMask1Frame, this->CurrentMask2Frame, this->CurrentDisparityMap16, this->CamOccGrid.data);
497
498 this->CamOccGrid.time = this->DispImageHeader.time;
499 this->CamOccGrid.timerange = this->DispImageHeader.timerange;
500
501 this->shmem_obst->write(&this->CamOccGrid, this->mMaxOutputSize1);
502
503 if(this->showdebug)
504 {
505 cv::namedWindow( "CameraObstacleGridComponent - Final Result", CV_WINDOW_AUTOSIZE );
506 cv::imshow("CameraObstacleGridComponent - Final Result", this->DrawGrid(this->CamOccGrid.data) );
507 cv::waitKey(1);
508 }
509 }
510
511 //==================================================================================================================
512
513 //std::cout << componentName.toStdString() << " cicle time: " << (road_time() - init_time)/1000000.0 << std::endl;
514 }
515 else
516 {
517 msleep(/*MS_DELAY*/10);
518 }
519
520 if(this->showdebug)
521 cv::waitKey(1); // Give the system permission
522 }
523 }
524 else
525 {
526 this->run2();
527 }
528
529 this->is_running = false;
530
531 // Destroy the window frame
532 if(this->showdebug)
533 cvDestroyAllWindows();
534}
535
536// Thread loop for mono camera data
537void CameraObstacleGridComponent::run2()
538{
539 if(this->CurrentMask1Frame.cols != this->cam_width)
540 {
541 this->CurrentMask1Frame = cv::Mat(cv::Size(this->cam_width , this->cam_height), CV_MAKETYPE(CV_8U, 1));
542 }
543
544 this->CreateProjectionMatrix(this->D_MAX_CAM_GRID);
545
546 // Keeps the last image timestamp;
547 road_time_t last_reading = 0;
548
549 // Time measurement
550 road_time_t init_time = 0;
551
552 while (THREAD_ALIVE)
553 {
554 //init_time = road_time();
555
556 //LOG_INFO("Grab new image");
557 // header + image
558 this->shmem_mask1->read(this->mask1_mem, this->mask1_mem_size);
559
560 // Header
561 memcpy( &this->Mask1ImageHeader, this->mask1_mem, sizeof(TimestampedStructImage));
562
563 // Check image header
564 bool is_ok = false;
565 if( (this->Mask1ImageHeader.image.data_size == this->mMaxImageInputSize1) && (this->Mask1ImageHeader.time != last_reading) )
566 {
567 is_ok = true;
568 last_reading = this->Mask1ImageHeader.time;
569
570 /*std::cout << "Expected image w: " << ImageHeader.image.width << std::endl;
571 std::cout << "Expected image h: " << ImageHeader.image.height << std::endl;
572 std::cout << "Expected image c: " << ImageHeader.image.channels << std::endl;
573 std::cout << "Expected image data: " << ImageHeader.image.data_size << std::endl;
574 std::cout << "Expected image size: " << image_mem << std::endl;*/
575 }
576 /*else
577 {
578 LOG_ERROR("Error in the image data size!");
579 }*/
580
581 //LOG_INFO("Grab new image");
582 if(is_ok)
583 {
584 // Image data
585 memcpy( (unsigned char*)(this->CurrentMask1Frame.data), (unsigned char*)((TimestampedStructImage*)this->mask1_mem + 1), this->Mask1ImageHeader.image.data_size);
586
587 //======================================= Obstacle Grid Calculation ================================================
588
589 // Camera occupancy grid
590 this->CreateMonoCameraGrid(this->CurrentMask1Frame, this->CamOccGrid.data, this->D_MAX_CAM_GRID, 1);
591
592 this->CamOccGrid.time = this->Mask1ImageHeader.time;
593 this->CamOccGrid.timerange = this->Mask1ImageHeader.timerange;
594
595 this->shmem_obst->write(&this->CamOccGrid, this->mMaxOutputSize1);
596
597 if(this->showdebug)
598 {
599 cv::namedWindow( "CameraObstacleGridComponent2 - Final Result", CV_WINDOW_AUTOSIZE );
600 cv::imshow("CameraObstacleGridComponent2 - Final Result", this->DrawGrid(this->CamOccGrid.data) );
601 cv::waitKey(1);
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);
611 }
612
613 if(this->showdebug)
614 cv::waitKey(1); // Give the system permission
615 }
616}
617
618// Function to create the obstacle's grid the mask obstacles image
619void CameraObstacleGridComponent::CreateCamera2LaserGrid(cv::Mat mask_obstacles, cv::Mat disp_map16, double* radius_data, double* angle_data)
620{
621 // Delta angle (rad)
622 const double angle_dec = (this->cam_fov/(double)this->cam_width)*CV_PI/180.0;
623
624 // left angle (rad) (positive to the left and negative to the right)
625 const double angle_esq = (this->cam_fov/2.0)*CV_PI/180.0 - (double)this->cam_valid_col0*angle_dec;
626
627 // Current Angle
628 double angle = angle_esq;
629
630 // d = f*b/Z
631 double disp_limit_max = this->cam_width*this->cam_fx*this->cam_baseline/ROBOT_FRONT_DIST;
632 double disp_limit_min = this->cam_width*this->cam_fx*this->cam_baseline/D_MAX_CAM_GRID;
633 double max_cam_distance = this->cam_width*this->cam_fx*this->cam_baseline/1.0;
634
635 // Coordenadas no espaco
636 double x = 0.0;
637 double y = 0.0;
638 double z = 0.0;
639
640 // Coordenadas na imagem
641 int linha, coluna;
642 double max_col_disp;
643
644 double dist_min = 100.0;
645
646 //------------------------------------------------------------------------------
647
648 // Auxiliary variables
649 unsigned short* row;
650
651 // Run across the image checking the high disparity value for each column
652 for (int i = this->cam_valid_col0; i <= (this->cam_valid_cols + this->cam_valid_col0 - 1); ++i)
653 {
654 // Max distance = disparity 1.0
655 double distance = max_cam_distance;
656
657 max_col_disp = 0.0;
658
659 for (int j = this->cam_valid_row0; j <= (this->cam_valid_rows + this->cam_valid_row0 - 1); ++j)
660 {
661 // Calcula a posicao do pixel na mascara e no mapa de disparidade
662 int pixel1 = (j)*mask_obstacles.cols + (i);
663
664 row = (unsigned short*)disp_map16.data + j*this->cam_width;
665
666 //se for branco verifica se a disparidade esta dentro de uma margem 2 m e 20 m
667 if ((mask_obstacles.data[pixel1] != 0)&&
668 (row[i]/16.0 > disp_limit_min)&&(row[i]/16.0 < disp_limit_max)&&(row[i]/16.0 > max_col_disp))
669 {
670 max_col_disp = row[i]/16.0;
671
672 linha = j;
673 coluna = i;
674 }
675 }
676
677 if (max_col_disp != 0)
678 {
679 // It is a valid point
680 if(this->PointTriangulate( linha , coluna, x, y, z, max_col_disp ) )
681 {
682 z = z*std::cos(this->cam_tilt_angle);
683
684 double dist_aux = sqrt( x*x + z*z );
685
686 if (dist_aux < distance)
687 distance = dist_aux;
688
689 if (distance < dist_min)
690 dist_min = distance;
691 }
692 }
693
694 //Insert value in the grid
695 radius_data[i - this->cam_valid_col0] = distance;
696 angle_data[i - this->cam_valid_col0] = angle;
697
698 // Next angle
699 angle = angle - angle_dec;
700 }
701
702 if (dist_min < maxDistToBrake)
703 emit smallestDistance();
704
705 //-----------------------------------------------------------------------------
706
707 return;
708}
709
710// Function to create the obstacle's grid the mask obstacles image
711void CameraObstacleGridComponent::CreateCameraGrid(cv::Mat mask_free, cv::Mat mask_obstacles, cv::Mat disp_map16, sensor_occ_data &occ_data)
712{
713 double std_deviation; // calculated with the camera distance = (f*B*CorrAccuracy/(d*d))/3
714
715 // d = f*b/Z
716 double disp_limit_max = this->cam_width*this->cam_fx*this->cam_baseline/ROBOT_FRONT_DIST;
717 double disp_limit_min = this->cam_width*this->cam_fx*this->cam_baseline/D_MAX_CAM_GRID;
718 double max_cam_distance = this->cam_width*this->cam_fx*this->cam_baseline/1.0;
719
720 // 3D coordinates of the image point
721 double x = 0.0;
722 double y = 0.0;
723 double z = 0.0;
724
725 // 2D image coordinates
726 int row, col;
727
728 // Current grid position
729 int grid_x, grid_y, grid_x_aux, grid_y_aux;
730
731 //------------------------------------------------------------------------------
732 occ_data.cols = D_MAX_CAM_GRID/this->sub_div;
733 occ_data.rows = occ_data.cols;
734
735 occ_data.sensor_x0 = 0.0f;
736 occ_data.sensor_y0 = (float)(occ_data.rows/2.0);
737
738 occ_data.ratio = this->sub_div;
739
740 // Auxiliary variables
741 unsigned short* disp16_row;
742
743 float l_0 = 0.5f;
744
745 for (int i = 0; i < (occ_data.cols*occ_data.rows); ++i)
746 {
747 occ_data.occ_data[i] = l_0;
748 }
749
750 //--------------------- Draw uncouvered road surface ---------------------------
751 // Triangle points
752 cv::Point2f triangle_vertices[3];
753 triangle_vertices[0] = cv::Point2f( occ_data.sensor_x0, occ_data.sensor_y0 ); // sensor origin
754
755 double img_u = 0.0 - this->cam_cx;
756 double img_x = img_u*this->free_area_guess/this->cam_fx;
757 triangle_vertices[1] = cv::Point( this->free_area_guess/occ_data.ratio, occ_data.sensor_y0 + (float)(img_x)/occ_data.ratio );
758
759 img_u = 1.0 - this->cam_cx;
760 img_x = img_u*this->free_area_guess/this->cam_fx;
761 triangle_vertices[2] = cv::Point( this->free_area_guess/occ_data.ratio, occ_data.sensor_y0 + (float)(img_x)/occ_data.ratio );
762
763 // Triangle top edge line equation y = m*x + b
764 double m_1 = (triangle_vertices[1].y - triangle_vertices[0].y)/(triangle_vertices[1].x - triangle_vertices[0].x);
765 double b_1 = triangle_vertices[0].y - m_1*triangle_vertices[0].x;
766
767 double m_2 = (triangle_vertices[2].y - triangle_vertices[0].y)/(triangle_vertices[2].x - triangle_vertices[0].x);
768 double b_2 = triangle_vertices[0].y - m_1*triangle_vertices[0].x;
769
770 // Fill the blind region in front of the robot
771 for (col = (int)(triangle_vertices[0].x + 0.5f); col <= (int)(triangle_vertices[1].x + 0.5f); ++col)
772 {
773 for (row = (int)(m_1*col + b_1 + 0.5f); row <= (int)(m_2*col + b_2 + 0.5f); ++row)
774 {
775 int cel_index = row*occ_data.cols + col;
776
777 // The region in front of the vehicle, uncouvered by the camera is just a guess
778 occ_data.occ_data[cel_index] = 0.4f; //0.0001f;
779 }
780 }
781 //------------------------------------------------------------------------------
782
783 // Run across the image...
784 for (col = this->cam_valid_col0; col <= (this->cam_valid_cols + this->cam_valid_col0 - 1); ++col)
785 {
786 for (row = this->cam_valid_row0; row <= (this->cam_valid_rows + this->cam_valid_row0 - 1); ++row)
787 {
788 // Pixel position in the obstacles mask and disparidade map
789 int pixel = (row)*mask_obstacles.cols + (col);
790
791 disp16_row = (unsigned short*)disp_map16.data + row*this->cam_width;
792
793 double pixel_disp = (disp16_row[col]/16.0);
794
795 // If the mask_free or mask_obstacles is different of zero and the disparity is in the desired range
796 if (((mask_free.data[pixel] != 0)||(mask_obstacles.data[pixel] != 0))&&(pixel_disp > disp_limit_min)&&(pixel_disp < disp_limit_max))
797 {
798 // It is a valid point
799 if(this->PointTriangulate( row , col, x, y, z, pixel_disp ) )
800 {
801 z = z*std::cos(this->cam_tilt_angle);
802
803 // Distance from the grid cell to the robot origin
804 double D_xz = std::sqrt( x*x + z*z );
805
806 grid_x = (int)(z/this->sub_div + occ_data.sensor_x0 + 0.5);
807 grid_y = (int)(x/this->sub_div + occ_data.sensor_y0 + 0.5);
808
809 double D_cell = std::sqrt( (double)((grid_x - occ_data.sensor_x0)*(grid_x - occ_data.sensor_x0) + (grid_y - occ_data.sensor_y0)*(grid_y - occ_data.sensor_y0)) )*this->sub_div;
810
811 if((grid_x >= 0) && (grid_x < occ_data.cols) && (grid_y >= 0) && (grid_y < occ_data.rows))
812 {
813 // Estimated standard deviation for the current disparity value
814 std_deviation = (this->cam_fx*this->cam_width*this->cam_baseline*this->CorrAccuracy/(pixel_disp*pixel_disp))/3.0;
815
816 // If the grid cell have influence in their neighbours
817 if(std_deviation*3.0 > this->CorrAccuracy)
818 {
819 int n_cels = (int)(std_deviation*3.0/this->sub_div + 0.5);
820
821 for(int i = -n_cels/2; i <= n_cels/2; ++i)
822 {
823 for(int j = -n_cels/2; j <= n_cels/2; ++j)
824 {
825 grid_x_aux = grid_x + i;
826 grid_y_aux = grid_y + j;
827
828 if((grid_x_aux >= 0) && (grid_x_aux < occ_data.cols) && (grid_y_aux >= 0) && (grid_y_aux < occ_data.rows))
829 {
830 D_cell = std::sqrt( (double)((grid_x_aux - occ_data.sensor_x0)*(grid_x_aux - occ_data.sensor_x0) + (grid_y_aux - occ_data.sensor_y0)*(grid_y_aux - occ_data.sensor_y0)) )*this->sub_div;
831
832 double prob = 0.5;
833 int cel_index = (grid_y_aux)*occ_data.cols + (grid_x_aux);
834
835 // Gaussian model for the probability
836 if(mask_obstacles.data[pixel] != 0)
837 {
838 prob = std::exp( -0.5*(((D_xz - D_cell)*(D_xz - D_cell))/(std_deviation*std_deviation)) )/(std::sqrt(2.0*PI_VALUE)*std_deviation);
839
840 prob = (prob > 0.5f) ? prob : 0.5f;
841
842 occ_data.occ_data[cel_index] = occ_data.occ_data[cel_index] + prob - l_0;
843 }
844 else // (mask_free.data[pixel] != 0)
845 {
846 prob = 1.0 - std::exp( -0.5*(((D_xz - D_cell)*(D_xz - D_cell))/(std_deviation*std_deviation)) )/(std::sqrt(2.0*PI_VALUE)*std_deviation);
847
848 prob = (prob < 0.5f) ? prob : 0.5f;
849
850 if(occ_data.occ_data[cel_index] <= l_0)
851 occ_data.occ_data[cel_index] = occ_data.occ_data[cel_index] + prob - l_0;
852 }//*/
853
854
855
856 if (occ_data.occ_data[cel_index] > 0.9999f)
857 occ_data.occ_data[cel_index] = 0.9999f;
858
859 if (occ_data.occ_data[cel_index] < 0.0001f)
860 occ_data.occ_data[cel_index] = 0.0001f;//*/
861 } //if
862 } // for
863 } // for
864 } //if
865 else // If the point have all the probability
866 {
867 int cel_index = (grid_y)*occ_data.cols + (grid_x);
868
869 if(mask_obstacles.data[pixel] != 0)
870 {
871 occ_data.occ_data[cel_index] = 0.9999f;
872 }
873 else // (mask_free.data[pixel] != 0)
874 {
875 if(occ_data.occ_data[cel_index] <= l_0)
876 occ_data.occ_data[cel_index] = 0.0001f;
877 }//*/
878 }
879 } // if grid
880 } // if point triangulate
881 }
882 } // for row
883 } // for col
884
885 //-----------------------------------------------------------------------------
886
887 return;
888}
889
890/* CreateMonoCameraGrid
891 Description:
892 Function to create the road surface grid by the mono camera segmented image.
893 Parameters:
894 mask_free = road surface mask
895 occ_data = occupancy grid
896 max_dist = max distance to be considerated
897 img_step = step between the image rows and cols
898 */
899void CameraObstacleGridComponent::CreateMonoCameraGrid(cv::Mat mask_free, sensor_occ_data &occ_data, double max_dist, int img_step)
900{
901 double std_deviation; // calculated with the camera distance = (f*B*CorrAccuracy/(d*d))/3
902
903 // 3D coordinates of the image point
904 double x = 0.0;
905 double y = 0.0;
906 double z = 0.0;
907
908 // 2D image coordinates
909 int row, col;
910
911 // Current grid position
912 int grid_x, grid_y, grid_x_aux, grid_y_aux;
913
914 double* x_ptr;
915 double* x_ptr1;
916 double* y_ptr;
917
918 //------------------------------------------------------------------------------
919 occ_data.cols = D_MAX_CAM_GRID/this->sub_div;
920 occ_data.rows = 2*occ_data.cols;
921
922 occ_data.sensor_x0 = 0.0f;
923 occ_data.sensor_y0 = (float)(occ_data.rows/2.0);
924
925 occ_data.ratio = this->sub_div;
926
927 float l_0 = 0.5f;
928
929 for (int i = 0; i < (occ_data.cols*occ_data.rows); ++i)
930 {
931 occ_data.occ_data[i] = l_0;
932 }
933
934 // Run across the image...
935 for (row = this->cam_valid_row0; row <= (this->cam_valid_rows + this->cam_valid_row0 - 1); row = row + img_step)
936 {
937 x_ptr = (double*)this->CurrentXMatrix.data + row*this->cam_width;
938 y_ptr = (double*)this->CurrentYMatrix.data + row*this->cam_width;
939
940 // Estimated standard deviation for the current distance
941 if((row + img_step) < (this->cam_valid_rows + this->cam_valid_row0 - 1))
942 {
943 x_ptr1 = (double*)this->CurrentXMatrix.data + (row + img_step)*this->cam_width;
944 std_deviation = std::fabs(x_ptr[this->cam_valid_col0] - x_ptr1[this->cam_valid_col0]);
945 }
946
947 for (col = this->cam_valid_col0; col <= (this->cam_valid_cols + this->cam_valid_col0 - 1); col = col + img_step)
948 {
949 // Pixel position in the road surface mask
950 int pixel = (row)*mask_free.cols + (col);
951
952 // If the mask_free is different of zero and the distance is in the max_dist range
953 if ((mask_free.data[pixel] != 0)&&(x_ptr[col] < max_dist))
954 {
955 // Distance from the grid cell to the robot origin
956 double D_xy = std::sqrt( x_ptr[col]*x_ptr[col] + y_ptr[col]*y_ptr[col] );
957
958 grid_x = (int)(x_ptr[col]/this->sub_div + occ_data.sensor_x0 + 0.5);
959 grid_y = (int)(y_ptr[col]/this->sub_div + occ_data.sensor_y0 + 0.5);
960
961 double D_cell = std::sqrt( (double)((grid_x - occ_data.sensor_x0)*(grid_x - occ_data.sensor_x0) + (grid_y - occ_data.sensor_y0)*(grid_y - occ_data.sensor_y0)) )*this->sub_div;
962
963 if((grid_x >= 0) && (grid_x < occ_data.cols) && (grid_y >= 0) && (grid_y < occ_data.rows))
964 {
965 // If the grid cell have influence in their neighbours
966 if(std_deviation*3.0 > this->CorrAccuracy)
967 {
968 int n_cels = (int)(std_deviation*3.0/this->sub_div + 0.5);
969
970 for(int i = -n_cels/2; i <= n_cels/2; ++i)
971 {
972 for(int j = -n_cels/2; j <= n_cels/2; ++j)
973 {
974 grid_x_aux = grid_x + i;
975 grid_y_aux = grid_y + j;
976
977 if((grid_x_aux >= 0) && (grid_x_aux < occ_data.cols) && (grid_y_aux >= 0) && (grid_y_aux < occ_data.rows))
978 {
979 D_cell = std::sqrt( (double)((grid_x_aux - occ_data.sensor_x0)*(grid_x_aux - occ_data.sensor_x0) + (grid_y_aux - occ_data.sensor_y0)*(grid_y_aux - occ_data.sensor_y0)) )*this->sub_div;
980
981 double prob = 0.5;
982 int cel_index = (grid_y_aux)*occ_data.cols + (grid_x_aux);
983
984 prob = 1.0 - std::exp( -0.5*(((D_xy - D_cell)*(D_xy - D_cell))/(std_deviation*std_deviation)) )/(std::sqrt(2.0*PI_VALUE)*std_deviation);
985
986 prob = (prob < 0.5f) ? prob : 0.5f;
987
988 if(occ_data.occ_data[cel_index] <= l_0)
989 occ_data.occ_data[cel_index] = occ_data.occ_data[cel_index] + prob - l_0;
990
991 if (occ_data.occ_data[cel_index] < 0.0001f)
992 occ_data.occ_data[cel_index] = 0.0001f;//*/
993 } //if
994 } // for
995 } // for
996 } //if
997 else // If the point have all the probability
998 {
999 int cel_index = (grid_y)*occ_data.cols + (grid_x);
1000
1001 if(occ_data.occ_data[cel_index] <= l_0)
1002 occ_data.occ_data[cel_index] = 0.0001f;
1003
1004 }
1005 } // if grid
1006 }
1007 } // for row
1008 } // for col
1009
1010 //-----------------------------------------------------------------------------
1011
1012 return;
1013}
1014
1015/* CreateProjectionMatrix
1016 Description:
1017 Function to create the matrixes with the image projection in the
1018 robot plane.
1019 Parameters:
1020 max_dist = max distance to be considerated
1021*/
1022void CameraObstacleGridComponent::CreateProjectionMatrix(double max_dist)
1023{
1024 this->CurrentXMatrix = cv::Mat::zeros(cv::Size(this->cam_width , this->cam_height), CV_64FC1);
1025 this->CurrentYMatrix = cv::Mat::zeros(cv::Size(this->cam_width , this->cam_height), CV_64FC1);
1026
1027 // Run across the the matrixes to complete the X, Y, Z projections
1028 for (int row = (this->cam_valid_rows + this->cam_valid_row0 - 1); row >= this->cam_valid_row0; --row)
1029 {
1030 double* x_ptr = this->CurrentXMatrix.ptr<double>(row);
1031 double* y_ptr = this->CurrentYMatrix.ptr<double>(row);
1032
1033 for (int col = this->cam_valid_col0; col <= (this->cam_valid_cols + this->cam_valid_col0 - 1); ++col)
1034 {
1035 this->Calc3DPointProjection(row, col, x_ptr[col], y_ptr[col]);
1036 }
1037
1038 if(x_ptr[this->cam_valid_col0] >= max_dist)
1039 break;
1040 }
1041}
1042
1043/* PointTriangulate
1044 Description:
1045 Calculate the point triangulation in the world
1046 Parameters:
1047 row,col = row and column in the image
1048 x,y,z = world coordinates
1049 disparity = disparity value
1050*/
1051bool CameraObstacleGridComponent::PointTriangulate(int row, int col, double &x, double &y, double &z, double disparity)
1052{
1053 bool valid_point = false;
1054
1055 if(disparity > 0.0 && disparity < 255.0)
1056 {
1057 // Z = f*b/d
1058 z = this->cam_width*this->cam_fx*this->cam_baseline/disparity;
1059
1060 //double u = (double(col) - double(this->cam_width)*this->cam_cx);
1061 //double v = (double(row) - double(this->cam_height)*this->cam_cy);
1062
1063 double u = col/(this->cam_width - 1.0) - this->cam_cx;
1064 double v = row/(this->cam_height - 1.0) - this->cam_cy;
1065
1066 // X = u*Z/f
1067 //x = u*z/(this->cam_width*this->cam_fx);
1068 x = u*z/this->cam_fx;
1069
1070 // Y = v*Z/f
1071 //y = v*z/(this->cam_height*this->cam_fy);
1072 y = v*z/this->cam_fy;
1073
1074 valid_point = true;
1075 }
1076
1077 return valid_point;
1078}
1079
1080/* Calc3DPointProjection
1081 Description:
1082 Calculate the 3D point projection in a world plane
1083 Parameters:
1084 row, col = row and column in the image
1085 x_r, y_r = world coordinates (in the robot frame)
1086*/
1087void CameraObstacleGridComponent::Calc3DPointProjection(int row, int col, double &x_r, double &y_r)
1088{
1089 double X = (col/(this->cam_width - 1.0) - this->cam_cx)/this->cam_fx;
1090 double Y = (row/(this->cam_height - 1.0) - this->cam_cy)/this->cam_fy;
1091
1092 // x_c = X*t_y/(sin(rho) + Ycos(rho))
1093 double x_c = X*this->cam_h/(std::sin(this->cam_tilt_angle) + Y*std::cos(this->cam_tilt_angle));
1094
1095 // y_c = Y*t_y/(sin(rho) + Ycos(rho))
1096 double y_c = Y*this->cam_h/(std::sin(this->cam_tilt_angle) + Y*std::cos(this->cam_tilt_angle));
1097
1098 // z_c = t_y/(sin(rho) + Ycos(rho))
1099 double z_c = this->cam_h/(std::sin(this->cam_tilt_angle) + Y*std::cos(this->cam_tilt_angle));
1100
1101 if((z_c*z_c - this->cam_h*this->cam_h) >= 0)
1102 x_r = std::sqrt(z_c*z_c - this->cam_h*this->cam_h);
1103 else
1104 x_r = 10000.0;
1105
1106 y_r = x_c;
1107
1108 return;
1109}
1110
1111// Draw the camera obstacle grid
1112cv::Mat CameraObstacleGridComponent::DrawGrid(double* radius_data, double* angle_data)
1113{
1114 double CAM_GRID_DIST = CAM_GRID_ROWS/this->D_MAX_CAM_GRID; // Pixel/meter ratio
1115
1116 // Grid image
1117 cv::Mat GridImage = cv::Mat(cv::Size(CAM_GRID_COLS, CAM_GRID_ROWS), CV_8UC3, cv::Scalar(255,255,255));
1118
1119 // Obstacle coordinates
1120 int coord_x, coord_y;
1121 int coord_x_ant = -1;
1122 int coord_y_ant = -1;
1123
1124 // Condition to know if the previous coordinate was an obstacle
1125 bool Obstaculo_ant = false;
1126
1127 const int orig_x = (int)((CAM_GRID_COLS - 1)/2);
1128 const int orig_y = CAM_GRID_ROWS - 1;
1129
1130 int it_raio, it_angulo;
1131
1132 for( it_raio = 0, it_angulo = 0; it_raio < this->cam_valid_cols; ++it_raio,++it_angulo)
1133 {
1134
1135 if(radius_data[it_raio] != D_MAX_CAM_GRID)
1136 {
1137 coord_x = (int)((double)orig_x - CAM_GRID_DIST*((radius_data[it_raio])*sin(angle_data[it_angulo])) + 0.5);
1138 coord_y = (int)((double)orig_y - CAM_GRID_DIST*((radius_data[it_raio])*cos(angle_data[it_angulo])) + 0.5);
1139
1140 // Black circle
1141 cv::circle(GridImage, cv::Point(coord_x, coord_y), 1, CV_RGB(0, 0, 0), 1);
1142
1143 //// If the previous point was a obstacle, trace a line
1144 //if( Obstaculo_ant == true)
1145 //{
1146 // cv::line( GridImage, cv::Point(coord_x_ant, coord_y_ant), cv::Point(coord_x, coord_y), CV_RGB(0,0,0), 1, 8 );
1147 //}
1148
1149 coord_x_ant = coord_x;
1150 coord_y_ant = coord_y;
1151
1152 //Obstaculo_ant = true;
1153 }
1154 /*else
1155 {
1156 Obstaculo_ant = false;
1157 }*/
1158 }
1159
1160 // Draw the robot limit in blue
1161 cv::rectangle( GridImage, cvPoint( (orig_x - (int)(CAM_GRID_DIST*ROBOT_FRONT_WIDTH/2.0 + 0.5)), (orig_y - (int)(CAM_GRID_DIST*ROBOT_FRONT_DIST + 0.5))),
1162 cvPoint((orig_x + (int)(CAM_GRID_DIST*ROBOT_FRONT_WIDTH/2.0 + 0.5)), orig_y), CV_RGB(0,0,255), 1, 8);
1163
1164 // Desenha camera em amarelo
1165 cv::rectangle( GridImage, cv::Point( (orig_x - 4), (orig_y - 1)), cv::Point((orig_x + 4), orig_y), CV_RGB(200,200,0), 2, 8);
1166
1167 //---------------- Campo de visao --------------------------------------------------------
1168
1169 // Incremento do angulo
1170 const double angle_inc = this->cam_fov/(double)this->cam_width;
1171
1172 // Angulo mais a esquerda
1173 const double angle_esq = -1*(this->cam_fov/2.0) + (double)this->cam_valid_col0*angle_inc;
1174
1175 // Calcula variacao em x da reta que representa o campo de visao
1176 int recuo_x = abs((int)(((double)CAM_GRID_ROWS/cos(angle_esq*2.0*CV_PI/360.0))*sin(angle_esq*2.0*CV_PI/360.0) + 0.5));
1177
1178 // Desenha as retas do campo de visao em vermelho
1179 cv::line( GridImage, cv::Point(orig_x, orig_y), cv::Point((orig_x - recuo_x), 0), CV_RGB(255,0,0), 1, 8 );
1180 cv::line( GridImage, cv::Point(orig_x, orig_y), cv::Point((orig_x + recuo_x), 0), CV_RGB(255,0,0), 1, 8 );
1181
1182 //-----------------------------------------------------------------------------------------
1183
1184 // Desenha circulos de distancia em verde
1185 cv::circle(GridImage, cv::Point(orig_x, orig_y), (int)(5.0*CAM_GRID_DIST), CV_RGB(0, 255, 0), 1);
1186 cv::circle(GridImage, cv::Point(orig_x, orig_y), (int)(10.0*CAM_GRID_DIST), CV_RGB(0, 255, 0), 1);
1187 cv::circle(GridImage, cv::Point(orig_x, orig_y), (int)(15.0*CAM_GRID_DIST), CV_RGB(0, 255, 0), 1);
1188 cv::circle(GridImage, cv::Point(orig_x, orig_y), (int)(20.0*CAM_GRID_DIST), CV_RGB(0, 255, 0), 1);
1189
1190 return GridImage;
1191}
1192
1193// Draw the camera occupancy grid
1194cv::Mat CameraObstacleGridComponent::DrawGrid(sensor_occ_data &occ_data)
1195{
1196 // Grid image
1197 return cv::Mat(occ_data.rows, occ_data.cols, CV_32FC1, occ_data.occ_data);
1198
1199
1200}
Note: See TracBrowser for help on using the repository browser.