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

Last change on this file since 82 was 75, checked in by phudelai, 9 years ago

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