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

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

Flea3Component: Shared memory changed for stereovision
StereoVisionDisparity: Added for the PFE of Pierre
File size: 33.7 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 + "-left").c_str(), this->left_mem_size);
132 this->shmem_right = new ShMem((this->img_source_right + "-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 /* /////!!!!!\\\\ A DECOMMENTER PLUS TARD (avec le delta de temps entre l'image droite et gauche)*/
447 {
448 is_ok = true;
449 last_reading = this->LeftImage.time;
450
451 //cout << "Left - Right = " << this->LeftImage.time - this->RightImage.time << endl;
452 }
453
454 if(is_ok)
455 {
456 if (this->cam_channels == 3)
457 {
458 if(this->cam_width != this->destiny_width)
459 {
460 memcpy( (unsigned char*)(Img_BGRU.data), (unsigned char*)((TimestampedStructImage*)this->left_mem + 1), this->LeftImage.image.data_size);
461
462 cv::resize(Img_BGRU, this->CurrentLeftFrame, cv::Size(this->destiny_width, this->destiny_height), 0.0, 0.0, CV_INTER_CUBIC);
463
464 memcpy( (unsigned char*)(Img_BGRU.data), (unsigned char*)((TimestampedStructImage*)this->right_mem + 1), this->RightImage.image.data_size);
465
466 cv::resize(Img_BGRU, this->CurrentRightFrame, cv::Size(this->destiny_width, this->destiny_height), 0.0, 0.0, CV_INTER_CUBIC);
467 }
468 else
469 {
470 // Image data
471 memcpy( (unsigned char*)(this->CurrentLeftFrame.data), (unsigned char*)((TimestampedStructImage*)this->left_mem + 1), this->LeftImage.image.data_size);
472 memcpy( (unsigned char*)(this->CurrentRightFrame.data), (unsigned char*)((TimestampedStructImage*)this->right_mem + 1), this->RightImage.image.data_size);
473 }
474
475 }
476 else if(this->cam_channels == 4)//this->cam_channels == 4, use bumblebee image
477 {
478 // If is to resize the image...
479 if(this->cam_width != this->destiny_width)
480 {
481 memcpy( (unsigned char*)(Img_BGRU.data), (unsigned char*)((TimestampedStructImage*)this->left_mem + 1), this->LeftImage.image.data_size);
482 cv::resize(Img_BGRU, Img_BGRU_resized, cv::Size(this->destiny_width, this->destiny_height), 0.0, 0.0, CV_INTER_CUBIC);
483 cv::cvtColor(Img_BGRU_resized, this->CurrentLeftFrame, CV_BGRA2BGR);
484
485 memcpy( (unsigned char*)(Img_BGRU.data), (unsigned char*)((TimestampedStructImage*)this->right_mem + 1), this->RightImage.image.data_size);
486 cv::resize(Img_BGRU, Img_BGRU_resized, cv::Size(this->destiny_width, this->destiny_height), 0.0, 0.0, CV_INTER_CUBIC);
487 cv::cvtColor(Img_BGRU_resized, this->CurrentRightFrame, CV_BGRA2BGR);
488 }
489 else
490 {
491 // Image data
492 memcpy( (unsigned char*)(Img_BGRU.data), (unsigned char*)((TimestampedStructImage*)this->left_mem + 1), this->LeftImage.image.data_size);
493 cv::cvtColor(Img_BGRU, this->CurrentLeftFrame, CV_BGRA2BGR);
494
495 // Image data
496 memcpy( (unsigned char*)(Img_BGRU.data), (unsigned char*)((TimestampedStructImage*)this->right_mem + 1), this->RightImage.image.data_size);
497 cv::cvtColor(Img_BGRU, this->CurrentRightFrame, CV_BGRA2BGR);
498 }
499 }
500 else if (this->cam_channels == 1)
501 {
502 // If is to resize the image...
503 if(this->cam_width != this->destiny_width)
504 {
505 // this is a gray image!!!
506 memcpy( (unsigned char*)(Img_BGRU.data), (unsigned char*)((TimestampedStructImage*)this->left_mem + 1), this->LeftImage.image.data_size);
507 cv::resize(Img_BGRU, this->CurrentLeftFrame, cv::Size(this->destiny_width, this->destiny_height), 0.0, 0.0, CV_INTER_CUBIC);
508
509 memcpy( (unsigned char*)(Img_BGRU.data), (unsigned char*)((TimestampedStructImage*)this->right_mem + 1), this->RightImage.image.data_size);
510 cv::resize(Img_BGRU, this->CurrentRightFrame, cv::Size(this->destiny_width, this->destiny_height), 0.0, 0.0, CV_INTER_CUBIC);
511 }
512 else
513 {
514 memcpy( (unsigned char*)(this->CurrentLeftFrame.data), (unsigned char*)((TimestampedStructImage*)this->left_mem + 1), this->LeftImage.image.data_size);
515
516 memcpy( (unsigned char*)(this->CurrentRightFrame.data), (unsigned char*)((TimestampedStructImage*)this->right_mem + 1), this->RightImage.image.data_size);
517 }
518 }
519
520 if(this->showdebug)
521 {
522 cv::namedWindow( "DisparityMapComponent - Current Left Frame", CV_WINDOW_NORMAL );
523 cv::imshow("DisparityMapComponent - Current Left Frame",this->CurrentLeftFrame);
524 cv::namedWindow( "DisparityMapComponent - Current Right Frame", CV_WINDOW_NORMAL );
525 cv::imshow("DisparityMapComponent - Current Right Frame",this->CurrentRightFrame);
526 cv::waitKey(1);
527 }
528
529 //======================================= Disparity Map Calculation ================================================
530
531 this->CalcDisparityMap(this->CurrentLeftFrame, this->CurrentRightFrame, this->CurrentDisparityMap, this->dispmap_type);
532
533 // Complete timestamp header of the left image
534 this->RefImage.time = this->LeftImage.time;
535 this->RefImage.timerange = this->LeftImage.timerange;
536
537 // Copy images header and data to memory
538 memcpy(this->ref_mem, &RefImage, sizeof(TimestampedStructImage));
539 memcpy((void*)((TimestampedStructImage*)this->ref_mem + 1), (void*)this->CurrentLeftFrame.data, this->RefImage.image.data_size);
540 this->shmem_ref->write(this->ref_mem, this->ref_mem_size);
541
542 // Complete timestamp header of the disp image
543 this->DispImage.time = this->LeftImage.time;
544 this->DispImage.timerange = this->LeftImage.timerange;
545
546 // Copy images header and data to memory
547 memcpy(this->disp_mem, &DispImage, sizeof(TimestampedStructImage));
548 memcpy((void*)((TimestampedStructImage*)this->disp_mem + 1), (void*)this->CurrentDisparityMap.data, this->DispImage.image.data_size);
549 this->shmem_disp->write(this->disp_mem, this->disp_mem_size);
550
551 //==================================================================================================================
552
553 if(this->showdebug)
554 {
555 cv::Mat imgDisparity8U = cv::Mat( this->CurrentDisparityMap.rows, this->CurrentDisparityMap.cols, CV_8UC1 );
556
557 this->CurrentDisparityMap = this->CurrentDisparityMap/16;
558
559 // Display it as a CV_8UC1 image
560 this->CurrentDisparityMap.convertTo( imgDisparity8U, CV_8UC1);
561
562 cv::namedWindow( "DisparityMapComponent - Disparity Map", CV_WINDOW_NORMAL );
563 imshow( "DisparityMapComponent - Disparity Map", imgDisparity8U );
564 }
565 }
566 else
567 msleep(/*MS_DELAY*/10);
568
569 if(this->showdebug)
570 cv::waitKey(1); // Give the system permission
571 }
572
573 this->is_running = false;
574
575 // Destroy the window frame
576 if(this->showdebug)
577 cvDestroyAllWindows();
578}
579
580////////////////////////////////////////////////////////////////////////////////
581/// CalcDisparityMap
582/// Description: Calculate the Disparity Map of a stereo pair
583/// Parameters: Left_img = left image
584/// Right_img = right image
585/// Disp_map = disparity map
586/// Type = Method: 0 = SAD (Sum of Absolute Differences algorithm)
587/// 1 = SGBM (Semi-Global Block Matching algorithm)
588/// 2 = SGBM+HH (full-scale two-pass dynamic programming algorithm)
589/// 3 = SAD from PtGrey
590////////////////////////////////////////////////////////////////////////////////
591void DisparityMap::CalcDisparityMap(cv::Mat Left_img, cv::Mat Right_img, cv::Mat &Disp_map, Type type)
592{
593 //Left_img = cv::imread("C:/Bumblebee_Images/BumblebeeXB3_2013-09-17/rectified-left-00102.ppm");
594 //Right_img = cv::imread("C:/Bumblebee_Images/BumblebeeXB3_2013-09-17/rectified-right-00102.ppm");
595
596 cv::Mat Left_img_gray, Right_img_gray;
597
598 int SADWindowSize_ratio = type == 0 ? this->sbm_SADWindowSize/2 : this->sgbm_SADWindowSize/2;
599
600 cv::Mat Left_img_exp, Right_img_exp, Disp_map_exp;
601
602 // Expand the image size to better process the disparity range
603 if(this->use_roi)
604 {
605 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));
606 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));
607 Disp_map_exp = cv::Mat(this->destiny_roi_height + 2*SADWindowSize_ratio, this->destiny_roi_width + 2*this->num_disp, CV_16S);
608
609 (Left_img(cv::Rect(this->destiny_roi_x, this->destiny_roi_y, this->destiny_roi_width, this->destiny_roi_height))).copyTo(
610 Left_img_exp(cv::Rect(this->num_disp-1, SADWindowSize_ratio, this->destiny_roi_width, this->destiny_roi_height)));
611
612 (Right_img(cv::Rect(this->destiny_roi_x, this->destiny_roi_y, this->destiny_roi_width, this->destiny_roi_height))).copyTo(
613 Right_img_exp(cv::Rect(this->num_disp-1, SADWindowSize_ratio, this->destiny_roi_width, this->destiny_roi_height)));
614 }
615 else
616 {
617 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));
618 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));
619 Disp_map_exp = cv::Mat(Disp_map.rows + 2*SADWindowSize_ratio, Disp_map.cols + 2*this->num_disp, CV_16S);
620
621 Left_img.copyTo(Left_img_exp(cv::Rect(this->num_disp-1, SADWindowSize_ratio, Left_img.cols, Left_img.rows)));
622 Right_img.copyTo(Right_img_exp(cv::Rect(this->num_disp-1, SADWindowSize_ratio, Right_img.cols, Right_img.rows)));
623 }
624
625 // Disparity Map by the SAD algorithm
626 if(type == SAD)
627 {
628 cv::StereoBM sbm;
629
630 // Gray scale images
631 if(this->destiny_channels != 1)
632 {
633 Left_img_gray = cv::Mat(cv::Size(Left_img_exp.cols, Left_img_exp.rows), CV_8UC1);
634 cv::cvtColor(Left_img_exp, Left_img_gray, CV_BGR2GRAY);
635 Right_img_gray = cv::Mat(cv::Size(Right_img_exp.cols, Right_img_exp.rows), CV_8UC1);
636 cv::cvtColor(Right_img_exp, Right_img_gray, CV_BGR2GRAY);
637 }
638 else
639 {
640 Left_img_gray = Left_img_exp;
641 Right_img_gray = Right_img_exp;
642 }
643
644 // Configuration
645 //sbm.state->roi1 = roi1; // Valid pixels ROI of the left image
646 //sbm.state->roi2 = roi2; // Valid pixels ROI of the right image
647 sbm.state->preFilterCap = this->sbm_preFilterCap; // Truncation value for the prefiltered image pixels.
648 // The algorithm first computes x-derivative at each pixel and clips its
649 // value by [-preFilterCap, preFilterCap] interval. The result values are
650 // passed to the Birchfield-Tomasi pixel cost function.
651 sbm.state->preFilterSize = this->sbm_preFilterSize >= 5 ? this->sbm_preFilterSize : 5;
652 sbm.state->SADWindowSize = this->sbm_SADWindowSize >= 5 ? this->sbm_SADWindowSize : 11; // Matched block size. It must be an odd number >=1.
653 // Normally, it should be somewhere in the 3..11 range
654 sbm.state->minDisparity = this->min_disp; // Minimum possible disparity value. Normally, it is zero but sometimes
655 // rectification algorithms can shift images, so this parameter needs to be adjusted accordingly.
656 sbm.state->numberOfDisparities = this->num_disp > 0 ? this->num_disp : Left_img.cols/8; // Maximum disparity minus minimum disparity.
657 // The value is always greater than zero.
658 // In the current implementation, this parameter must be
659 // divisible by 16.
660 sbm.state->textureThreshold = this->sbm_textureThreshold; // Validation threashold for texture variation
661 sbm.state->uniquenessRatio = this->sbm_uniquenessRatio; // Margin in percentage by which the best (minimum) computed cost function value should “win”
662 // the second best value to consider the found match correct. Normally, a value within the 5-15
663 // range is good enough.
664 sbm.state->speckleWindowSize = this->sbm_speckleWindowSize; // Maximum size of smooth disparity regions to consider their noise speckles and invalidate. Set
665 // it to 0 to disable speckle filtering. Otherwise, set it somewhere in the 50-200 range.
666 sbm.state->speckleRange = this->sbm_speckleRange; // Maximum disparity variation within each connected component. If you do speckle filtering, set
667 // the parameter to a positive value, it will be implicitly multiplied by 16. Normally, 1 or 2 is
668 // good enough.
669
670 // Calculate the disparity image (16S = scaled by 16, must be divided by 16; 32F = original value)
671 sbm(Left_img_gray, Right_img_gray, Disp_map_exp, CV_16S);
672 }
673 else if(type == SGBM || type == SGBM_HH)
674 {
675 cv::StereoSGBM sgbm;
676
677 sgbm.preFilterCap = this->sgbm_preFilterCap; // Truncation value for the prefiltered image pixels.
678 // The algorithm first computes x-derivative at each pixel and clips its
679 // value by [-preFilterCap, preFilterCap] interval. The result values are
680 // passed to the Birchfield-Tomasi pixel cost function.
681 sgbm.SADWindowSize = this->sgbm_SADWindowSize > 0 ? this->sgbm_SADWindowSize : 7; // Matched block size. It must be an odd number >=1.
682 // Normally, it should be somewhere in the 3..11 range
683 sgbm.P1 = this->sgbm_P1; // The first parameter controlling the disparity smoothness.
684 sgbm.P2 = this->sgbm_P2; // The second parameter controlling the disparity smoothness. The larger the values are,
685 // the smoother the disparity is. P1 is the penalty on the disparity change by plus or
686 // minus 1 between neighbor pixels. P2 is the penalty on the disparity change by more than
687 // 1 between neighbor pixels. The algorithm requires P2 > P1 .
688 sgbm.minDisparity = this->min_disp; // Minimum possible disparity value. Normally, it is zero but sometimes
689 // rectification algorithms can shift images, so this parameter needs to be adjusted accordingly.
690 sgbm.numberOfDisparities = this->num_disp > 0 ? this->num_disp : Left_img.cols/8; // Maximum disparity minus minimum disparity.
691 // The value is always greater than zero.
692 // In the current implementation, this parameter must be
693 // divisible by 16.
694 sgbm.uniquenessRatio = this->sgbm_uniquenessRatio; // Margin in percentage by which the best (minimum) computed cost function value should “win”
695 // the second best value to consider the found match correct. Normally, a value within the 5-15
696 // range is good enough.
697 sgbm.speckleWindowSize = this->sgbm_speckleWindowSize; // Maximum size of smooth disparity regions to consider their noise speckles and invalidate. Set
698 // it to 0 to disable speckle filtering. Otherwise, set it somewhere in the 50-200 range.
699 sgbm.speckleRange = this->sgbm_speckleRange; // Maximum disparity variation within each connected component. If you do speckle filtering, set
700 // the parameter to a positive value, it will be implicitly multiplied by 16. Normally, 1 or 2 is
701 // good enough.
702 sgbm.disp12MaxDiff = this->sgbm_disp12MaxDiff; // Maximum allowed difference (in integer pixel units) in the left-right disparity check. Set it to
703 // a non-positive value to disable the check.
704 sgbm.fullDP = type == SGBM; // Set it to true to run the full-scale two-pass dynamic programming algorithm.
705 // It will consume O(W*H*numDisparities) bytes, which is large for 640x480 stereo
706 // and huge for HD-size pictures.
707
708 sgbm(Left_img_exp, Right_img_exp, Disp_map_exp);
709 }
710
711 if(this->use_roi)
712 {
713 (Disp_map_exp(cv::Rect(this->num_disp-1, SADWindowSize_ratio, this->destiny_roi_width, this->destiny_roi_height))).copyTo(
714 Disp_map(cv::Rect(this->destiny_roi_x, this->destiny_roi_y, this->destiny_roi_width, this->destiny_roi_height)));
715 }
716 else
717 {
718 (Disp_map_exp(cv::Rect(this->num_disp-1, SADWindowSize_ratio, Disp_map.cols, Disp_map.rows))).copyTo(Disp_map);
719 }
720}
Note: See TracBrowser for help on using the repository browser.