source: pacpussensors/trunk/StereoVisionDisparity/DisparityMap.cpp@ 54

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

StereoVisionDisparity updated
File size: 33.6 KB
Line 
1/*******************************************************************************
2// created: 2013/06/19 - 18:40
3// filename: DisparityMap.cpp
4//
5// author: Danilo Alves de Lima and Pierre Hudelaine
6// Copyright Heudiasyc UMR UTC/CNRS 6599
7//
8// version: $Id: $
9//
10// purpose: Disparity map calculation from stereo image data
11//
12*******************************************************************************/
13
14#include "DisparityMap.h"
15
16#include <iostream>
17#include <string>
18#include "opencv2/calib3d/calib3d.hpp"
19#include "opencv2/core/core.hpp"
20
21#include "Pacpus/kernel/ComponentFactory.h"
22#include "Pacpus/kernel/DbiteException.h"
23#include "Pacpus/kernel/DbiteFileTypes.h"
24#include "Pacpus/kernel/Log.h"
25
26using namespace std;
27using namespace pacpus;
28
29DECLARE_STATIC_LOGGER("pacpus.base.DisparityMap");
30
31// Construct the factory
32static ComponentFactory<DisparityMap> sFactory("DisparityMap");
33
34const int kMaxFilepathLength = 512; // TODO: should be same for all images
35static const string DisparityMapMemoryName_imagesrc = "image";
36static const string DisparityMapMemoryName_ref = "DisparityMap-ref";
37static const string DisparityMapMemoryName_disp = "DisparityMap-disp";
38
39
40////////////////////////////////////////////////////////////////////////////////
41/// Constructor
42////////////////////////////////////////////////////////////////////////////////
43DisparityMap::DisparityMap(QString name)
44 : ComponentBase(name)
45{
46 LOG_TRACE(Q_FUNC_INFO);
47
48 recording = 0;
49 THREAD_ALIVE = 0;
50
51 this->cam_width = this->destiny_width = 1280; // Image width
52 this->cam_height = this->destiny_height = 960; // Image height
53 this->cam_channels = 4; // Image channels
54 this->showdebug = false; // Show frame acquired
55
56 this->min_disp = 0;
57 this->num_disp = 256;
58
59 // Size of the image data sizeof(char)*width*height*channels
60 this->mMaxImageInputSize = sizeof(char)*this->cam_width*this->cam_height*this->cam_channels;
61
62 // Input data
63 this->shmem_left = 0; // Shared memory control access to the image data
64 this->shmem_right = 0; // Shared memory control access to the image data
65
66 // Output data
67 this->shmem_ref = 0; // Shared memory control access to the image data
68 this->shmem_disp = 0; // Shared memory control access to the image data
69
70 this->CurrentLeftFrame = cv::Mat(cv::Size(this->destiny_width , this->destiny_height), CV_MAKETYPE(CV_8U, 3));
71 this->CurrentRightFrame = cv::Mat(cv::Size(this->destiny_width , this->destiny_height), CV_MAKETYPE(CV_8U, 3));
72}
73
74
75////////////////////////////////////////////////////////////////////////////////
76/// Destructor
77////////////////////////////////////////////////////////////////////////////////
78DisparityMap::~DisparityMap()
79{
80 LOG_TRACE(Q_FUNC_INFO);
81
82 if(this->shmem_left)
83 delete shmem_left;
84
85 this->shmem_left = NULL;
86
87 if(this->shmem_right)
88 delete shmem_right;
89
90 this->shmem_right = NULL;
91
92 if(this->shmem_ref)
93 delete shmem_ref;
94
95 this->shmem_ref = NULL;
96
97 if(this->shmem_disp)
98 delete shmem_disp;
99
100 this->shmem_disp = NULL;
101}
102
103
104////////////////////////////////////////////////////////////////////////////////
105/// Called by the ComponentManager to start the component
106////////////////////////////////////////////////////////////////////////////////
107void DisparityMap::startActivity()
108{
109 LOG_TRACE(Q_FUNC_INFO);
110
111 this->destiny_channels = (this->cam_channels != 1) ? 3 : 1;
112
113 this->mMaxImageInputSize = sizeof(unsigned char) * this->cam_width * this->cam_height * this->cam_channels;
114 this->mMaxImageOutputSize1 = sizeof(unsigned char) * this->destiny_width * this->destiny_height * this->destiny_channels;
115 this->mMaxImageOutputSize2 = sizeof(unsigned short) * this->destiny_width * this->destiny_height;
116
117 this->left_mem_size = sizeof(TimestampedStructImage) + this->mMaxImageInputSize;
118 this->right_mem_size = sizeof(TimestampedStructImage) + this->mMaxImageInputSize;
119 this->ref_mem_size = sizeof(TimestampedStructImage) + this->mMaxImageOutputSize1;
120 this->disp_mem_size = sizeof(TimestampedStructImage) + this->mMaxImageOutputSize2;
121
122 // Allocate memory position for the maximum expected data
123 this->left_mem = malloc(this->left_mem_size);
124 this->right_mem = malloc(this->right_mem_size);
125 this->ref_mem = malloc(this->ref_mem_size);
126 this->disp_mem = malloc(this->disp_mem_size);
127
128 // Size of the image data sizeof(char)*width*height*channels
129 if (this->img_source_left.size() != 0 && this->img_source_right.size() != 0)
130 {
131 this->shmem_left = new ShMem((this->img_source_left).c_str(), this->left_mem_size);
132 this->shmem_right = new ShMem((this->img_source_right).c_str(), this->right_mem_size);
133 }
134 else
135 {
136 this->shmem_left = new ShMem((this->img_source + "-left").c_str(), this->left_mem_size);
137 this->shmem_right = new ShMem((this->img_source + "-right").c_str(), this->right_mem_size);
138 }
139
140 this->shmem_ref = new ShMem(DisparityMapMemoryName_ref.c_str(), this->ref_mem_size);
141 this->shmem_disp = new ShMem(DisparityMapMemoryName_disp.c_str(), this->disp_mem_size);
142
143 // Run thread
144 THREAD_ALIVE = true;
145 start();
146}
147
148
149////////////////////////////////////////////////////////////////////////////////
150/// Called by the ComponentManager to stop the component
151////////////////////////////////////////////////////////////////////////////////
152void DisparityMap::stopActivity()
153{
154 LOG_TRACE(Q_FUNC_INFO);
155
156 if(THREAD_ALIVE)
157 {
158 // Stop thread
159 THREAD_ALIVE = false;
160
161 while(is_running)
162 {
163 msleep(/*MS_DELAY*/10);
164 }
165
166 if(this->shmem_left)
167 delete shmem_left;
168
169 this->shmem_left = NULL;
170
171 if(this->shmem_right)
172 delete shmem_right;
173
174 this->shmem_right = NULL;
175
176 if(this->shmem_ref)
177 delete shmem_ref;
178
179 this->shmem_ref = NULL;
180
181 if(this->shmem_disp)
182 delete shmem_disp;
183
184 this->shmem_disp = NULL;
185
186 free(this->left_mem);
187 free(this->right_mem);
188 free(this->ref_mem);
189 free(this->disp_mem);
190 }
191
192 LOG_INFO("stopped component '" << name() << "'");
193}
194
195
196////////////////////////////////////////////////////////////////////////////////
197/// Called by the ComponentManager to pass the XML parameters to the
198/// component
199////////////////////////////////////////////////////////////////////////////////
200ComponentBase::COMPONENT_CONFIGURATION DisparityMap::configureComponent(XmlComponentConfig config)
201{
202 LOG_TRACE(Q_FUNC_INFO);
203
204 // Initialize with default values
205 InitDefault();
206
207 if (config.getProperty("recording") != QString::null)
208 recording = config.getProperty("recording").toInt();
209
210 if (config.getProperty("cam_width") != QString::null)
211 this->cam_width = config.getProperty("cam_width").toInt();
212
213 if (config.getProperty("cam_height") != QString::null)
214 this->cam_height = config.getProperty("cam_height").toInt();
215
216 if (config.getProperty("cam_channels") != QString::null)
217 this->cam_channels = config.getProperty("cam_channels").toInt();
218
219 if (config.getProperty("destiny_width") != QString::null)
220 this->destiny_width = config.getProperty("destiny_width").toInt();
221 else
222 this->destiny_width = this->cam_width;
223
224 if (config.getProperty("destiny_height") != QString::null)
225 this->destiny_height = config.getProperty("destiny_height").toInt();
226 else
227 this->destiny_height = this->cam_height;
228
229 if (config.getProperty("destiny_roi_x") != QString::null)
230 this->destiny_roi_x = config.getProperty("destiny_roi_x").toInt();
231
232 if (config.getProperty("destiny_roi_y") != QString::null)
233 this->destiny_roi_y = config.getProperty("destiny_roi_y").toInt();
234
235 if (config.getProperty("destiny_roi_width") != QString::null)
236 this->destiny_roi_width = config.getProperty("destiny_roi_width").toInt();
237
238 if (config.getProperty("destiny_roi_height") != QString::null)
239 this->destiny_roi_height = config.getProperty("destiny_roi_height").toInt();
240
241 if( ((this->destiny_roi_height != this->destiny_height)||(this->destiny_roi_width != this->destiny_width))&&
242 ((this->destiny_roi_height <= this->destiny_height)&&(this->destiny_roi_width <= this->destiny_width)) )
243 this->use_roi = true;
244
245 if (config.getProperty("showdebug") != QString::null)
246 this->showdebug = (bool)config.getProperty("showdebug").toInt();
247
248 if (config.getProperty("dispmap_type") != QString::null)
249 this->dispmap_type = (Type)config.getProperty("dispmap_type").toInt();
250
251 if (config.getProperty("min_disp") != QString::null)
252 this->min_disp = config.getProperty("min_disp").toInt();
253
254 if (config.getProperty("num_disp") != QString::null)
255 this->num_disp = config.getProperty("num_disp").toInt();
256
257 //---------------------------------------------- SBM Configuration --------------------------------------------------------------
258 if (config.getProperty("sbm_preFilterCap") != QString::null)
259 this->sbm_preFilterCap = config.getProperty("sbm_preFilterCap").toInt();
260
261 if (config.getProperty("sbm_preFilterSize") != QString::null)
262 this->sbm_preFilterSize = config.getProperty("sbm_preFilterSize").toInt();
263
264 if (config.getProperty("sbm_SADWindowSize") != QString::null)
265 this->sbm_SADWindowSize = config.getProperty("sbm_SADWindowSize").toInt();
266
267 if (config.getProperty("sbm_textureThreshold") != QString::null)
268 this->sbm_textureThreshold = config.getProperty("sbm_textureThreshold").toInt();
269
270 if (config.getProperty("sbm_uniquenessRatio") != QString::null)
271 this->sbm_uniquenessRatio = config.getProperty("sbm_uniquenessRatio").toInt();
272
273 if (config.getProperty("sbm_speckleWindowSize") != QString::null)
274 this->sbm_speckleWindowSize = config.getProperty("sbm_speckleWindowSize").toInt();
275
276 if (config.getProperty("sbm_speckleRange") != QString::null)
277 this->sbm_speckleRange = config.getProperty("sbm_speckleRange").toInt();
278
279 //---------------------------------------------- SGBM Configuration -------------------------------------------------------------
280 if (config.getProperty("sgbm_preFilterCap") != QString::null)
281 this->sgbm_preFilterCap = config.getProperty("sgbm_preFilterCap").toInt();
282
283 if (config.getProperty("sgbm_SADWindowSize") != QString::null)
284 this->sgbm_SADWindowSize = config.getProperty("sgbm_SADWindowSize").toInt();
285
286 if (config.getProperty("sgbm_P1") != QString::null)
287 this->sgbm_P1 = config.getProperty("sgbm_P1").toInt();
288
289 if (config.getProperty("sgbm_P2") != QString::null)
290 this->sgbm_P2 = config.getProperty("sgbm_P2").toInt();
291
292 if (config.getProperty("sgbm_uniquenessRatio") != QString::null)
293 this->sgbm_uniquenessRatio = config.getProperty("sgbm_uniquenessRatio").toInt();
294
295 if (config.getProperty("sgbm_speckleWindowSize") != QString::null)
296 this->sgbm_speckleWindowSize = config.getProperty("sgbm_speckleWindowSize").toInt();
297
298 if (config.getProperty("sgbm_speckleRange") != QString::null)
299 this->sgbm_speckleRange = config.getProperty("sgbm_speckleRange").toInt();
300
301 if (config.getProperty("sgbm_disp12MaxDiff") != QString::null)
302 this->sgbm_disp12MaxDiff = config.getProperty("sgbm_disp12MaxDiff").toInt();
303
304 if (config.getProperty("img_source") != QString::null)
305 this->img_source = config.getProperty("img_source").toStdString();
306 else
307 this->img_source = DisparityMapMemoryName_imagesrc;
308
309 if (config.getProperty("img_source_left") != QString::null)
310 this->img_source_left = config.getProperty("img_source_left").toStdString();
311
312 if (config.getProperty("img_source_right") != QString::null)
313 this->img_source_right = config.getProperty("img_source_right").toStdString();
314
315 // Size of the image data sizeof(char)*width*height*channels
316 this->mMaxImageInputSize = sizeof(char)*this->cam_width*this->cam_height*this->cam_channels;
317
318 LOG_INFO("configured component '" << name() << "'");
319 return ComponentBase::CONFIGURED_OK;
320}
321
322
323////////////////////////////////////////////////////////////////////////////////
324/// Initialize default values
325////////////////////////////////////////////////////////////////////////////////
326void DisparityMap::InitDefault()
327{
328 // Default
329 recording = 0;
330
331 this->cam_width = this->destiny_width = this->destiny_roi_width = 1280; // Image width
332 this->cam_height = this->destiny_height = this->destiny_roi_height = 960; // Image height
333
334 this->destiny_roi_x = this->destiny_roi_y = 0; // Destiny image roi x and y
335
336 this->use_roi = false;
337
338 this->cam_channels = 4; // Image channels
339 this->showdebug = false; // Show frame acquired
340 this->dispmap_type = SGBM;
341 this->min_disp = 0;
342 this->num_disp = 256;
343
344 //---------------------------------------------- SBM Configuration --------------------------------------------------------------
345 this->sbm_preFilterCap = 31; // Truncation value for the prefiltered image pixels.
346 // The algorithm first computes x-derivative at each pixel and clips its
347 // value by [-preFilterCap, preFilterCap] interval. The result values are
348 // passed to the Birchfield-Tomasi pixel cost function.
349 this->sbm_preFilterSize = 41;
350 this->sbm_SADWindowSize = 9;
351 this->sbm_textureThreshold = 20; // Validation threashold for texture variation
352 this->sbm_uniquenessRatio = 10; // Margin in percentage by which the best (minimum) computed cost function value should “win”
353 // the second best value to consider the found match correct. Normally, a value within the 5-15
354 // range is good enough.
355 this->sbm_speckleWindowSize = 100; // Maximum size of smooth disparity regions to consider their noise speckles and invalidate. Set
356 // it to 0 to disable speckle filtering. Otherwise, set it somewhere in the 50-200 range.
357 this->sbm_speckleRange = 32; // Maximum disparity variation within each connected component. If you do speckle filtering, set
358 // the configeter to a positive value, it will be implicitly multiplied by 16. Normally, 1 or 2 is
359 // good enough.
360
361 //---------------------------------------------- SGBM Configuration -------------------------------------------------------------
362 this->sgbm_preFilterCap = 63; // Truncation value for the prefiltered image pixels.
363 // The algorithm first computes x-derivative at each pixel and clips its
364 // value by [-preFilterCap, preFilterCap] interval. The result values are
365 // passed to the Birchfield-Tomasi pixel cost function.
366 this->sgbm_SADWindowSize = 3;
367 this->sgbm_P1 = 4*this->sgbm_SADWindowSize*this->sgbm_SADWindowSize; // The first parameter controlling the disparity smoothness.
368 this->sgbm_P2 = 32*this->sgbm_SADWindowSize*this->sgbm_SADWindowSize; // The second parameter controlling the disparity smoothness. The larger the values are,
369 // the smoother the disparity is. P1 is the penalty on the disparity change by plus or
370 // minus 1 between neighbor pixels. P2 is the penalty on the disparity change by more than
371 // 1 between neighbor pixels. The algorithm requires P2 > P1 .
372 this->sgbm_uniquenessRatio = 10; // Margin in percentage by which the best (minimum) computed cost function value should “win”
373 // the second best value to consider the found match correct. Normally, a value within the 5-15
374 // range is good enough.
375 this->sgbm_speckleWindowSize = 100; // Maximum size of smooth disparity regions to consider their noise speckles and invalidate. Set
376 // it to 0 to disable speckle filtering. Otherwise, set it somewhere in the 50-200 range.
377 this->sgbm_speckleRange = 32; // Maximum disparity variation within each connected component. If you do speckle filtering, set
378 // the parameter to a positive value, it will be implicitly multiplied by 16. Normally, 1 or 2 is
379 // good enough.
380 this->sgbm_disp12MaxDiff = 1; // Maximum allowed difference (in integer pixel units) in the left-right disparity check. Set it to
381 // a non-positive value to disable the check.
382}
383
384
385////////////////////////////////////////////////////////////////////////////////
386/// Thread loop
387////////////////////////////////////////////////////////////////////////////////
388void DisparityMap::run()
389{
390 LOG_TRACE(Q_FUNC_INFO);
391
392 this->is_running = true;
393
394 // "channels" is a vector of 4 Mat arrays: BGRU
395 cv::Mat Img_BGRU;
396 cv::Mat Img_BGRU_resized;
397 std::vector<cv::Mat> channels(4);
398
399 Img_BGRU = cv::Mat(cv::Size(this->cam_width , this->cam_height), CV_MAKETYPE(CV_8U, this->cam_channels));
400 Img_BGRU_resized = cv::Mat(cv::Size(this->destiny_width , this->destiny_height), CV_MAKETYPE(CV_8U, this->cam_channels));
401
402 this->CurrentLeftFrame = cv::Mat(cv::Size(this->destiny_width , this->destiny_height), CV_MAKETYPE(CV_8U, this->destiny_channels));
403 this->CurrentRightFrame = cv::Mat(cv::Size(this->destiny_width , this->destiny_height), CV_MAKETYPE(CV_8U, this->destiny_channels));
404
405
406 // Create the image in which we will save the disparities
407 if(this->CurrentDisparityMap.cols != this->destiny_width)
408 this->CurrentDisparityMap = cv::Mat( this->destiny_height, this->destiny_width, CV_16S, cv::Scalar(0));
409
410 // Keeps the last image timestamp;
411 road_time_t last_reading = 0;
412
413 // Initialize the output images header
414 this->RefImage.image.width = this->destiny_width;
415 this->RefImage.image.height = this->destiny_height;
416 this->RefImage.image.channels = 3;
417 this->RefImage.image.width_step = (size_t)(this->RefImage.image.height * this->RefImage.image.channels);
418 this->RefImage.image.data_size = this->mMaxImageOutputSize1;
419
420 this->DispImage.image.width = this->destiny_width;
421 this->DispImage.image.height = this->destiny_height;
422 this->DispImage.image.channels = 1;
423 this->DispImage.image.width_step = (size_t)(sizeof(unsigned short)*this->DispImage.image.height * this->DispImage.image.channels);
424 this->DispImage.image.data_size = this->mMaxImageOutputSize2;
425
426 // Time measurement
427 road_time_t init_time = 0;
428
429 while (THREAD_ALIVE)
430 {
431 init_time = road_time();
432
433 //LOG_INFO("Grab new image");
434 // header + image
435 this->shmem_left->read(this->left_mem, this->left_mem_size);
436 this->shmem_right->read(this->right_mem, this->right_mem_size);
437
438 // Header
439 memcpy( &this->LeftImage, this->left_mem, sizeof(TimestampedStructImage));
440 memcpy( &this->RightImage, this->right_mem, sizeof(TimestampedStructImage));
441
442 // Check image header
443 bool is_ok = false;
444 if( (this->LeftImage.image.data_size == this->mMaxImageInputSize) && (this->LeftImage.time != last_reading) &&
445 (this->RightImage.image.data_size == this->mMaxImageInputSize) && (this->LeftImage.time == this->RightImage.time) )
446 {
447 is_ok = true;
448 last_reading = this->LeftImage.time;
449
450 //cout << "Left - Right = " << (int64_t)this->LeftImage.time - (int64_t)this->RightImage.time << endl;
451 }
452
453 if(is_ok)
454 {
455 if (this->cam_channels == 3)
456 {
457 if(this->cam_width != this->destiny_width)
458 {
459 memcpy( (unsigned char*)(Img_BGRU.data), (unsigned char*)((TimestampedStructImage*)this->left_mem + 1), this->LeftImage.image.data_size);
460
461 cv::resize(Img_BGRU, this->CurrentLeftFrame, cv::Size(this->destiny_width, this->destiny_height), 0.0, 0.0, CV_INTER_CUBIC);
462
463 memcpy( (unsigned char*)(Img_BGRU.data), (unsigned char*)((TimestampedStructImage*)this->right_mem + 1), this->RightImage.image.data_size);
464
465 cv::resize(Img_BGRU, this->CurrentRightFrame, cv::Size(this->destiny_width, this->destiny_height), 0.0, 0.0, CV_INTER_CUBIC);
466 }
467 else
468 {
469 // Image data
470 memcpy( (unsigned char*)(this->CurrentLeftFrame.data), (unsigned char*)((TimestampedStructImage*)this->left_mem + 1), this->LeftImage.image.data_size);
471 memcpy( (unsigned char*)(this->CurrentRightFrame.data), (unsigned char*)((TimestampedStructImage*)this->right_mem + 1), this->RightImage.image.data_size);
472 }
473
474 }
475 else if(this->cam_channels == 4)//this->cam_channels == 4, use bumblebee image
476 {
477 // If is to resize the image...
478 if(this->cam_width != this->destiny_width)
479 {
480 memcpy( (unsigned char*)(Img_BGRU.data), (unsigned char*)((TimestampedStructImage*)this->left_mem + 1), this->LeftImage.image.data_size);
481 cv::resize(Img_BGRU, Img_BGRU_resized, cv::Size(this->destiny_width, this->destiny_height), 0.0, 0.0, CV_INTER_CUBIC);
482 cv::cvtColor(Img_BGRU_resized, this->CurrentLeftFrame, CV_BGRA2BGR);
483
484 memcpy( (unsigned char*)(Img_BGRU.data), (unsigned char*)((TimestampedStructImage*)this->right_mem + 1), this->RightImage.image.data_size);
485 cv::resize(Img_BGRU, Img_BGRU_resized, cv::Size(this->destiny_width, this->destiny_height), 0.0, 0.0, CV_INTER_CUBIC);
486 cv::cvtColor(Img_BGRU_resized, this->CurrentRightFrame, CV_BGRA2BGR);
487 }
488 else
489 {
490 // Image data
491 memcpy( (unsigned char*)(Img_BGRU.data), (unsigned char*)((TimestampedStructImage*)this->left_mem + 1), this->LeftImage.image.data_size);
492 cv::cvtColor(Img_BGRU, this->CurrentLeftFrame, CV_BGRA2BGR);
493
494 // Image data
495 memcpy( (unsigned char*)(Img_BGRU.data), (unsigned char*)((TimestampedStructImage*)this->right_mem + 1), this->RightImage.image.data_size);
496 cv::cvtColor(Img_BGRU, this->CurrentRightFrame, CV_BGRA2BGR);
497 }
498 }
499 else if (this->cam_channels == 1)
500 {
501 // If is to resize the image...
502 if(this->cam_width != this->destiny_width)
503 {
504 // this is a gray image!!!
505 memcpy( (unsigned char*)(Img_BGRU.data), (unsigned char*)((TimestampedStructImage*)this->left_mem + 1), this->LeftImage.image.data_size);
506 cv::resize(Img_BGRU, this->CurrentLeftFrame, cv::Size(this->destiny_width, this->destiny_height), 0.0, 0.0, CV_INTER_CUBIC);
507
508 memcpy( (unsigned char*)(Img_BGRU.data), (unsigned char*)((TimestampedStructImage*)this->right_mem + 1), this->RightImage.image.data_size);
509 cv::resize(Img_BGRU, this->CurrentRightFrame, cv::Size(this->destiny_width, this->destiny_height), 0.0, 0.0, CV_INTER_CUBIC);
510 }
511 else
512 {
513 memcpy( (unsigned char*)(this->CurrentLeftFrame.data), (unsigned char*)((TimestampedStructImage*)this->left_mem + 1), this->LeftImage.image.data_size);
514
515 memcpy( (unsigned char*)(this->CurrentRightFrame.data), (unsigned char*)((TimestampedStructImage*)this->right_mem + 1), this->RightImage.image.data_size);
516 }
517 }
518
519 if(this->showdebug)
520 {
521 cv::namedWindow( "DisparityMapComponent - Current Left Frame", CV_WINDOW_NORMAL );
522 cv::imshow("DisparityMapComponent - Current Left Frame",this->CurrentLeftFrame);
523 cv::namedWindow( "DisparityMapComponent - Current Right Frame", CV_WINDOW_NORMAL );
524 cv::imshow("DisparityMapComponent - Current Right Frame",this->CurrentRightFrame);
525 cv::waitKey(1);
526 }
527
528 //======================================= Disparity Map Calculation ================================================
529
530 this->CalcDisparityMap(this->CurrentLeftFrame, this->CurrentRightFrame, this->CurrentDisparityMap, this->dispmap_type);
531
532 // Complete timestamp header of the left image
533 this->RefImage.time = this->LeftImage.time;
534 this->RefImage.timerange = this->LeftImage.timerange;
535
536 // Copy images header and data to memory
537 memcpy(this->ref_mem, &RefImage, sizeof(TimestampedStructImage));
538 memcpy((void*)((TimestampedStructImage*)this->ref_mem + 1), (void*)this->CurrentLeftFrame.data, this->RefImage.image.data_size);
539 this->shmem_ref->write(this->ref_mem, this->ref_mem_size);
540
541 // Complete timestamp header of the disp image
542 this->DispImage.time = this->LeftImage.time;
543 this->DispImage.timerange = this->LeftImage.timerange;
544
545 // Copy images header and data to memory
546 memcpy(this->disp_mem, &DispImage, sizeof(TimestampedStructImage));
547 memcpy((void*)((TimestampedStructImage*)this->disp_mem + 1), (void*)this->CurrentDisparityMap.data, this->DispImage.image.data_size);
548 this->shmem_disp->write(this->disp_mem, this->disp_mem_size);
549
550 //==================================================================================================================
551
552 if(this->showdebug)
553 {
554 cv::Mat imgDisparity8U = cv::Mat( this->CurrentDisparityMap.rows, this->CurrentDisparityMap.cols, CV_8UC1 );
555
556 this->CurrentDisparityMap = this->CurrentDisparityMap/16;
557
558 // Display it as a CV_8UC1 image
559 this->CurrentDisparityMap.convertTo( imgDisparity8U, CV_8UC1);
560
561 cv::namedWindow( "DisparityMapComponent - Disparity Map", CV_WINDOW_NORMAL );
562 imshow( "DisparityMapComponent - Disparity Map", imgDisparity8U );
563 }
564 }
565 else
566 msleep(/*MS_DELAY*/10);
567
568 if(this->showdebug)
569 cv::waitKey(1); // Give the system permission
570 }
571
572 this->is_running = false;
573
574 // Destroy the window frame
575 if(this->showdebug)
576 cvDestroyAllWindows();
577}
578
579////////////////////////////////////////////////////////////////////////////////
580/// CalcDisparityMap
581/// Description: Calculate the Disparity Map of a stereo pair
582/// Parameters: Left_img = left image
583/// Right_img = right image
584/// Disp_map = disparity map
585/// Type = Method: 0 = SAD (Sum of Absolute Differences algorithm)
586/// 1 = SGBM (Semi-Global Block Matching algorithm)
587/// 2 = SGBM+HH (full-scale two-pass dynamic programming algorithm)
588/// 3 = SAD from PtGrey
589////////////////////////////////////////////////////////////////////////////////
590void DisparityMap::CalcDisparityMap(cv::Mat Left_img, cv::Mat Right_img, cv::Mat &Disp_map, Type type)
591{
592 //Left_img = cv::imread("C:/Bumblebee_Images/BumblebeeXB3_2013-09-17/rectified-left-00102.ppm");
593 //Right_img = cv::imread("C:/Bumblebee_Images/BumblebeeXB3_2013-09-17/rectified-right-00102.ppm");
594
595 cv::Mat Left_img_gray, Right_img_gray;
596
597 int SADWindowSize_ratio = type == 0 ? this->sbm_SADWindowSize/2 : this->sgbm_SADWindowSize/2;
598
599 cv::Mat Left_img_exp, Right_img_exp, Disp_map_exp;
600
601 // Expand the image size to better process the disparity range
602 if(this->use_roi)
603 {
604 Left_img_exp = cv::Mat(this->destiny_roi_height + 2*SADWindowSize_ratio, this->destiny_roi_width + 2*this->num_disp, CV_MAKETYPE(CV_8U, this->destiny_channels), cv::Scalar(0));
605 Right_img_exp = cv::Mat(this->destiny_roi_height + 2*SADWindowSize_ratio, this->destiny_roi_width + 2*this->num_disp, CV_MAKETYPE(CV_8U, this->destiny_channels), cv::Scalar(0));
606 Disp_map_exp = cv::Mat(this->destiny_roi_height + 2*SADWindowSize_ratio, this->destiny_roi_width + 2*this->num_disp, CV_16S);
607
608 (Left_img(cv::Rect(this->destiny_roi_x, this->destiny_roi_y, this->destiny_roi_width, this->destiny_roi_height))).copyTo(
609 Left_img_exp(cv::Rect(this->num_disp-1, SADWindowSize_ratio, this->destiny_roi_width, this->destiny_roi_height)));
610
611 (Right_img(cv::Rect(this->destiny_roi_x, this->destiny_roi_y, this->destiny_roi_width, this->destiny_roi_height))).copyTo(
612 Right_img_exp(cv::Rect(this->num_disp-1, SADWindowSize_ratio, this->destiny_roi_width, this->destiny_roi_height)));
613 }
614 else
615 {
616 Left_img_exp = cv::Mat(Left_img.rows + 2*SADWindowSize_ratio, Left_img.cols + 2*this->num_disp, CV_MAKETYPE(CV_8U, this->destiny_channels), cv::Scalar(0));
617 Right_img_exp = cv::Mat(Right_img.rows + 2*SADWindowSize_ratio, Right_img.cols + 2*this->num_disp, CV_MAKETYPE(CV_8U, this->destiny_channels), cv::Scalar(0));
618 Disp_map_exp = cv::Mat(Disp_map.rows + 2*SADWindowSize_ratio, Disp_map.cols + 2*this->num_disp, CV_16S);
619
620 Left_img.copyTo(Left_img_exp(cv::Rect(this->num_disp-1, SADWindowSize_ratio, Left_img.cols, Left_img.rows)));
621 Right_img.copyTo(Right_img_exp(cv::Rect(this->num_disp-1, SADWindowSize_ratio, Right_img.cols, Right_img.rows)));
622 }
623
624 // Disparity Map by the SAD algorithm
625 if(type == SAD)
626 {
627 cv::StereoBM sbm;
628
629 // Gray scale images
630 if(this->destiny_channels != 1)
631 {
632 Left_img_gray = cv::Mat(cv::Size(Left_img_exp.cols, Left_img_exp.rows), CV_8UC1);
633 cv::cvtColor(Left_img_exp, Left_img_gray, CV_BGR2GRAY);
634 Right_img_gray = cv::Mat(cv::Size(Right_img_exp.cols, Right_img_exp.rows), CV_8UC1);
635 cv::cvtColor(Right_img_exp, Right_img_gray, CV_BGR2GRAY);
636 }
637 else
638 {
639 Left_img_gray = Left_img_exp;
640 Right_img_gray = Right_img_exp;
641 }
642
643 // Configuration
644 //sbm.state->roi1 = roi1; // Valid pixels ROI of the left image
645 //sbm.state->roi2 = roi2; // Valid pixels ROI of the right image
646 sbm.state->preFilterCap = this->sbm_preFilterCap; // Truncation value for the prefiltered image pixels.
647 // The algorithm first computes x-derivative at each pixel and clips its
648 // value by [-preFilterCap, preFilterCap] interval. The result values are
649 // passed to the Birchfield-Tomasi pixel cost function.
650 sbm.state->preFilterSize = this->sbm_preFilterSize >= 5 ? this->sbm_preFilterSize : 5;
651 sbm.state->SADWindowSize = this->sbm_SADWindowSize >= 5 ? this->sbm_SADWindowSize : 11; // Matched block size. It must be an odd number >=1.
652 // Normally, it should be somewhere in the 3..11 range
653 sbm.state->minDisparity = this->min_disp; // Minimum possible disparity value. Normally, it is zero but sometimes
654 // rectification algorithms can shift images, so this parameter needs to be adjusted accordingly.
655 sbm.state->numberOfDisparities = this->num_disp > 0 ? this->num_disp : Left_img.cols/8; // Maximum disparity minus minimum disparity.
656 // The value is always greater than zero.
657 // In the current implementation, this parameter must be
658 // divisible by 16.
659 sbm.state->textureThreshold = this->sbm_textureThreshold; // Validation threashold for texture variation
660 sbm.state->uniquenessRatio = this->sbm_uniquenessRatio; // Margin in percentage by which the best (minimum) computed cost function value should “win”
661 // the second best value to consider the found match correct. Normally, a value within the 5-15
662 // range is good enough.
663 sbm.state->speckleWindowSize = this->sbm_speckleWindowSize; // Maximum size of smooth disparity regions to consider their noise speckles and invalidate. Set
664 // it to 0 to disable speckle filtering. Otherwise, set it somewhere in the 50-200 range.
665 sbm.state->speckleRange = this->sbm_speckleRange; // Maximum disparity variation within each connected component. If you do speckle filtering, set
666 // the parameter to a positive value, it will be implicitly multiplied by 16. Normally, 1 or 2 is
667 // good enough.
668
669 // Calculate the disparity image (16S = scaled by 16, must be divided by 16; 32F = original value)
670 sbm(Left_img_gray, Right_img_gray, Disp_map_exp, CV_16S);
671 }
672 else if(type == SGBM || type == SGBM_HH)
673 {
674 cv::StereoSGBM sgbm;
675
676 sgbm.preFilterCap = this->sgbm_preFilterCap; // Truncation value for the prefiltered image pixels.
677 // The algorithm first computes x-derivative at each pixel and clips its
678 // value by [-preFilterCap, preFilterCap] interval. The result values are
679 // passed to the Birchfield-Tomasi pixel cost function.
680 sgbm.SADWindowSize = this->sgbm_SADWindowSize > 0 ? this->sgbm_SADWindowSize : 7; // Matched block size. It must be an odd number >=1.
681 // Normally, it should be somewhere in the 3..11 range
682 sgbm.P1 = this->sgbm_P1; // The first parameter controlling the disparity smoothness.
683 sgbm.P2 = this->sgbm_P2; // The second parameter controlling the disparity smoothness. The larger the values are,
684 // the smoother the disparity is. P1 is the penalty on the disparity change by plus or
685 // minus 1 between neighbor pixels. P2 is the penalty on the disparity change by more than
686 // 1 between neighbor pixels. The algorithm requires P2 > P1 .
687 sgbm.minDisparity = this->min_disp; // Minimum possible disparity value. Normally, it is zero but sometimes
688 // rectification algorithms can shift images, so this parameter needs to be adjusted accordingly.
689 sgbm.numberOfDisparities = this->num_disp > 0 ? this->num_disp : Left_img.cols/8; // Maximum disparity minus minimum disparity.
690 // The value is always greater than zero.
691 // In the current implementation, this parameter must be
692 // divisible by 16.
693 sgbm.uniquenessRatio = this->sgbm_uniquenessRatio; // Margin in percentage by which the best (minimum) computed cost function value should “win”
694 // the second best value to consider the found match correct. Normally, a value within the 5-15
695 // range is good enough.
696 sgbm.speckleWindowSize = this->sgbm_speckleWindowSize; // Maximum size of smooth disparity regions to consider their noise speckles and invalidate. Set
697 // it to 0 to disable speckle filtering. Otherwise, set it somewhere in the 50-200 range.
698 sgbm.speckleRange = this->sgbm_speckleRange; // Maximum disparity variation within each connected component. If you do speckle filtering, set
699 // the parameter to a positive value, it will be implicitly multiplied by 16. Normally, 1 or 2 is
700 // good enough.
701 sgbm.disp12MaxDiff = this->sgbm_disp12MaxDiff; // Maximum allowed difference (in integer pixel units) in the left-right disparity check. Set it to
702 // a non-positive value to disable the check.
703 sgbm.fullDP = type == SGBM; // Set it to true to run the full-scale two-pass dynamic programming algorithm.
704 // It will consume O(W*H*numDisparities) bytes, which is large for 640x480 stereo
705 // and huge for HD-size pictures.
706
707 sgbm(Left_img_exp, Right_img_exp, Disp_map_exp);
708 }
709
710 if(this->use_roi)
711 {
712 (Disp_map_exp(cv::Rect(this->num_disp-1, SADWindowSize_ratio, this->destiny_roi_width, this->destiny_roi_height))).copyTo(
713 Disp_map(cv::Rect(this->destiny_roi_x, this->destiny_roi_y, this->destiny_roi_width, this->destiny_roi_height)));
714 }
715 else
716 {
717 (Disp_map_exp(cv::Rect(this->num_disp-1, SADWindowSize_ratio, Disp_map.cols, Disp_map.rows))).copyTo(Disp_map);
718 }
719}
Note: See TracBrowser for help on using the repository browser.