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

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

Vision component updated for StereoVision for PointGrey Flea3
File size: 36.2 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 emit minimumDisparityChanged(this->min_disp);
148 emit numberOfDisparityChanged(this->num_disp);
149 emit SADWindowSizeChanged(this->sbm_SADWindowSize);
150 emit textureTresholdChanged(this->sbm_textureThreshold);
151 emit uniquenessRatioChanged(this->sbm_uniquenessRatio);
152}
153
154
155////////////////////////////////////////////////////////////////////////////////
156/// Called by the ComponentManager to stop the component
157////////////////////////////////////////////////////////////////////////////////
158void DisparityMap::stopActivity()
159{
160 LOG_TRACE(Q_FUNC_INFO);
161
162 if(THREAD_ALIVE)
163 {
164 // Stop thread
165 THREAD_ALIVE = false;
166
167 while(is_running)
168 {
169 msleep(/*MS_DELAY*/10);
170 }
171
172 if(this->shmem_left)
173 delete shmem_left;
174
175 this->shmem_left = NULL;
176
177 if(this->shmem_right)
178 delete shmem_right;
179
180 this->shmem_right = NULL;
181
182 if(this->shmem_ref)
183 delete shmem_ref;
184
185 this->shmem_ref = NULL;
186
187 if(this->shmem_disp)
188 delete shmem_disp;
189
190 this->shmem_disp = NULL;
191
192 free(this->left_mem);
193 free(this->right_mem);
194 free(this->ref_mem);
195 free(this->disp_mem);
196 }
197
198 LOG_INFO("stopped component '" << name() << "'");
199}
200
201
202////////////////////////////////////////////////////////////////////////////////
203/// Called by the ComponentManager to pass the XML parameters to the
204/// component
205////////////////////////////////////////////////////////////////////////////////
206ComponentBase::COMPONENT_CONFIGURATION DisparityMap::configureComponent(XmlComponentConfig config)
207{
208 LOG_TRACE(Q_FUNC_INFO);
209
210 // Initialize with default values
211 InitDefault();
212
213 if (config.getProperty("recording") != QString::null)
214 recording = config.getProperty("recording").toInt();
215
216 if (config.getProperty("cam_width") != QString::null)
217 this->cam_width = config.getProperty("cam_width").toInt();
218
219 if (config.getProperty("cam_height") != QString::null)
220 this->cam_height = config.getProperty("cam_height").toInt();
221
222 if (config.getProperty("cam_channels") != QString::null)
223 this->cam_channels = config.getProperty("cam_channels").toInt();
224
225 if (config.getProperty("destiny_width") != QString::null)
226 this->destiny_width = config.getProperty("destiny_width").toInt();
227 else
228 this->destiny_width = this->cam_width;
229
230 if (config.getProperty("destiny_height") != QString::null)
231 this->destiny_height = config.getProperty("destiny_height").toInt();
232 else
233 this->destiny_height = this->cam_height;
234
235 if (config.getProperty("destiny_roi_x") != QString::null)
236 this->destiny_roi_x = config.getProperty("destiny_roi_x").toInt();
237
238 if (config.getProperty("destiny_roi_y") != QString::null)
239 this->destiny_roi_y = config.getProperty("destiny_roi_y").toInt();
240
241 if (config.getProperty("destiny_roi_width") != QString::null)
242 this->destiny_roi_width = config.getProperty("destiny_roi_width").toInt();
243
244 if (config.getProperty("destiny_roi_height") != QString::null)
245 this->destiny_roi_height = config.getProperty("destiny_roi_height").toInt();
246
247 if( ((this->destiny_roi_height != this->destiny_height)||(this->destiny_roi_width != this->destiny_width))&&
248 ((this->destiny_roi_height <= this->destiny_height)&&(this->destiny_roi_width <= this->destiny_width)) )
249 this->use_roi = true;
250
251 if (config.getProperty("showdebug") != QString::null)
252 this->showdebug = (bool)config.getProperty("showdebug").toInt();
253
254 if (config.getProperty("dispmap_type") != QString::null)
255 this->dispmap_type = (Type)config.getProperty("dispmap_type").toInt();
256
257 if (config.getProperty("min_disp") != QString::null)
258 this->min_disp = config.getProperty("min_disp").toInt();
259
260 if (config.getProperty("num_disp") != QString::null)
261 this->num_disp = config.getProperty("num_disp").toInt();
262
263 //---------------------------------------------- SBM Configuration --------------------------------------------------------------
264 if (config.getProperty("sbm_preFilterCap") != QString::null)
265 this->sbm_preFilterCap = config.getProperty("sbm_preFilterCap").toInt();
266
267 if (config.getProperty("sbm_preFilterSize") != QString::null)
268 this->sbm_preFilterSize = config.getProperty("sbm_preFilterSize").toInt();
269
270 if (config.getProperty("sbm_SADWindowSize") != QString::null)
271 this->sbm_SADWindowSize = config.getProperty("sbm_SADWindowSize").toInt();
272
273 if (config.getProperty("sbm_textureThreshold") != QString::null)
274 this->sbm_textureThreshold = config.getProperty("sbm_textureThreshold").toInt();
275
276 if (config.getProperty("sbm_uniquenessRatio") != QString::null)
277 this->sbm_uniquenessRatio = config.getProperty("sbm_uniquenessRatio").toInt();
278
279 if (config.getProperty("sbm_speckleWindowSize") != QString::null)
280 this->sbm_speckleWindowSize = config.getProperty("sbm_speckleWindowSize").toInt();
281
282 if (config.getProperty("sbm_speckleRange") != QString::null)
283 this->sbm_speckleRange = config.getProperty("sbm_speckleRange").toInt();
284
285 //---------------------------------------------- SGBM Configuration -------------------------------------------------------------
286 if (config.getProperty("sgbm_preFilterCap") != QString::null)
287 this->sgbm_preFilterCap = config.getProperty("sgbm_preFilterCap").toInt();
288
289 if (config.getProperty("sgbm_SADWindowSize") != QString::null)
290 this->sgbm_SADWindowSize = config.getProperty("sgbm_SADWindowSize").toInt();
291
292 if (config.getProperty("sgbm_P1") != QString::null)
293 this->sgbm_P1 = config.getProperty("sgbm_P1").toInt();
294
295 if (config.getProperty("sgbm_P2") != QString::null)
296 this->sgbm_P2 = config.getProperty("sgbm_P2").toInt();
297
298 if (config.getProperty("sgbm_uniquenessRatio") != QString::null)
299 this->sgbm_uniquenessRatio = config.getProperty("sgbm_uniquenessRatio").toInt();
300
301 if (config.getProperty("sgbm_speckleWindowSize") != QString::null)
302 this->sgbm_speckleWindowSize = config.getProperty("sgbm_speckleWindowSize").toInt();
303
304 if (config.getProperty("sgbm_speckleRange") != QString::null)
305 this->sgbm_speckleRange = config.getProperty("sgbm_speckleRange").toInt();
306
307 if (config.getProperty("sgbm_disp12MaxDiff") != QString::null)
308 this->sgbm_disp12MaxDiff = config.getProperty("sgbm_disp12MaxDiff").toInt();
309
310 if (config.getProperty("img_source") != QString::null)
311 this->img_source = config.getProperty("img_source").toStdString();
312 else
313 this->img_source = DisparityMapMemoryName_imagesrc;
314
315 if (config.getProperty("img_source_left") != QString::null)
316 this->img_source_left = config.getProperty("img_source_left").toStdString();
317
318 if (config.getProperty("img_source_right") != QString::null)
319 this->img_source_right = config.getProperty("img_source_right").toStdString();
320
321 // Size of the image data sizeof(char)*width*height*channels
322 this->mMaxImageInputSize = sizeof(char)*this->cam_width*this->cam_height*this->cam_channels;
323
324 LOG_INFO("configured component '" << name() << "'");
325 return ComponentBase::CONFIGURED_OK;
326}
327
328
329////////////////////////////////////////////////////////////////////////////////
330/// Initialize default values
331////////////////////////////////////////////////////////////////////////////////
332void DisparityMap::InitDefault()
333{
334 // Default
335 recording = 0;
336
337 this->cam_width = this->destiny_width = this->destiny_roi_width = 1280; // Image width
338 this->cam_height = this->destiny_height = this->destiny_roi_height = 960; // Image height
339
340 this->destiny_roi_x = this->destiny_roi_y = 0; // Destiny image roi x and y
341
342 this->use_roi = false;
343
344 this->cam_channels = 4; // Image channels
345 this->showdebug = false; // Show frame acquired
346 this->dispmap_type = SGBM;
347 this->min_disp = 0;
348 this->num_disp = 256;
349
350 //---------------------------------------------- SBM Configuration --------------------------------------------------------------
351 this->sbm_preFilterCap = 31; // Truncation value for the prefiltered image pixels.
352 // The algorithm first computes x-derivative at each pixel and clips its
353 // value by [-preFilterCap, preFilterCap] interval. The result values are
354 // passed to the Birchfield-Tomasi pixel cost function.
355 this->sbm_preFilterSize = 41;
356 this->sbm_SADWindowSize = 9;
357 this->sbm_textureThreshold = 20; // Validation threashold for texture variation
358 this->sbm_uniquenessRatio = 10; // Margin in percentage by which the best (minimum) computed cost function value should “win”
359 // the second best value to consider the found match correct. Normally, a value within the 5-15
360 // range is good enough.
361 this->sbm_speckleWindowSize = 100; // Maximum size of smooth disparity regions to consider their noise speckles and invalidate. Set
362 // it to 0 to disable speckle filtering. Otherwise, set it somewhere in the 50-200 range.
363 this->sbm_speckleRange = 32; // Maximum disparity variation within each connected component. If you do speckle filtering, set
364 // the configeter to a positive value, it will be implicitly multiplied by 16. Normally, 1 or 2 is
365 // good enough.
366
367 //---------------------------------------------- SGBM Configuration -------------------------------------------------------------
368 this->sgbm_preFilterCap = 63; // Truncation value for the prefiltered image pixels.
369 // The algorithm first computes x-derivative at each pixel and clips its
370 // value by [-preFilterCap, preFilterCap] interval. The result values are
371 // passed to the Birchfield-Tomasi pixel cost function.
372 this->sgbm_SADWindowSize = 3;
373 this->sgbm_P1 = 4*this->sgbm_SADWindowSize*this->sgbm_SADWindowSize; // The first parameter controlling the disparity smoothness.
374 this->sgbm_P2 = 32*this->sgbm_SADWindowSize*this->sgbm_SADWindowSize; // The second parameter controlling the disparity smoothness. The larger the values are,
375 // the smoother the disparity is. P1 is the penalty on the disparity change by plus or
376 // minus 1 between neighbor pixels. P2 is the penalty on the disparity change by more than
377 // 1 between neighbor pixels. The algorithm requires P2 > P1 .
378 this->sgbm_uniquenessRatio = 10; // Margin in percentage by which the best (minimum) computed cost function value should “win”
379 // the second best value to consider the found match correct. Normally, a value within the 5-15
380 // range is good enough.
381 this->sgbm_speckleWindowSize = 100; // Maximum size of smooth disparity regions to consider their noise speckles and invalidate. Set
382 // it to 0 to disable speckle filtering. Otherwise, set it somewhere in the 50-200 range.
383 this->sgbm_speckleRange = 32; // Maximum disparity variation within each connected component. If you do speckle filtering, set
384 // the parameter to a positive value, it will be implicitly multiplied by 16. Normally, 1 or 2 is
385 // good enough.
386 this->sgbm_disp12MaxDiff = 1; // Maximum allowed difference (in integer pixel units) in the left-right disparity check. Set it to
387 // a non-positive value to disable the check.
388}
389
390
391////////////////////////////////////////////////////////////////////////////////
392/// Thread loop
393////////////////////////////////////////////////////////////////////////////////
394void DisparityMap::run()
395{
396 LOG_TRACE(Q_FUNC_INFO);
397
398 this->is_running = true;
399
400 // "channels" is a vector of 4 Mat arrays: BGRU
401 cv::Mat Img_BGRU;
402 cv::Mat Img_BGRU_resized;
403 std::vector<cv::Mat> channels(4);
404
405 Img_BGRU = cv::Mat(cv::Size(this->cam_width , this->cam_height), CV_MAKETYPE(CV_8U, this->cam_channels));
406 Img_BGRU_resized = cv::Mat(cv::Size(this->destiny_width , this->destiny_height), CV_MAKETYPE(CV_8U, this->cam_channels));
407
408 this->CurrentLeftFrame = cv::Mat(cv::Size(this->destiny_width , this->destiny_height), CV_MAKETYPE(CV_8U, this->destiny_channels));
409 this->CurrentRightFrame = cv::Mat(cv::Size(this->destiny_width , this->destiny_height), CV_MAKETYPE(CV_8U, this->destiny_channels));
410
411
412 // Create the image in which we will save the disparities
413 if(this->CurrentDisparityMap.cols != this->destiny_width)
414 this->CurrentDisparityMap = cv::Mat( this->destiny_height, this->destiny_width, CV_16S, cv::Scalar(0));
415
416 // Keeps the last image timestamp;
417 road_time_t last_reading = 0;
418
419 // Initialize the output images header
420 this->RefImage.image.width = this->destiny_width;
421 this->RefImage.image.height = this->destiny_height;
422 this->RefImage.image.channels = 3;
423 this->RefImage.image.width_step = (size_t)(this->RefImage.image.height * this->RefImage.image.channels);
424 this->RefImage.image.data_size = this->mMaxImageOutputSize1;
425
426 this->DispImage.image.width = this->destiny_width;
427 this->DispImage.image.height = this->destiny_height;
428 this->DispImage.image.channels = 1;
429 this->DispImage.image.width_step = (size_t)(sizeof(unsigned short)*this->DispImage.image.height * this->DispImage.image.channels);
430 this->DispImage.image.data_size = this->mMaxImageOutputSize2;
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_left->read(this->left_mem, this->left_mem_size);
442 this->shmem_right->read(this->right_mem, this->right_mem_size);
443
444 // Header
445 memcpy( &this->LeftImage, this->left_mem, sizeof(TimestampedStructImage));
446 memcpy( &this->RightImage, this->right_mem, sizeof(TimestampedStructImage));
447
448 // Check image header
449 bool is_ok = false;
450 if( (this->LeftImage.image.data_size == this->mMaxImageInputSize) && (this->LeftImage.time != last_reading) &&
451 (this->RightImage.image.data_size == this->mMaxImageInputSize) && (this->LeftImage.time == this->RightImage.time) )
452 {
453 is_ok = true;
454 last_reading = this->LeftImage.time;
455
456 //cout << "Left - Right = " << (int64_t)this->LeftImage.time - (int64_t)this->RightImage.time << endl;
457 }
458
459 if(is_ok)
460 {
461 if (this->cam_channels == 3)
462 {
463 if(this->cam_width != this->destiny_width)
464 {
465 memcpy( (unsigned char*)(Img_BGRU.data), (unsigned char*)((TimestampedStructImage*)this->left_mem + 1), this->LeftImage.image.data_size);
466
467 cv::resize(Img_BGRU, this->CurrentLeftFrame, cv::Size(this->destiny_width, this->destiny_height), 0.0, 0.0, CV_INTER_CUBIC);
468
469 memcpy( (unsigned char*)(Img_BGRU.data), (unsigned char*)((TimestampedStructImage*)this->right_mem + 1), this->RightImage.image.data_size);
470
471 cv::resize(Img_BGRU, this->CurrentRightFrame, cv::Size(this->destiny_width, this->destiny_height), 0.0, 0.0, CV_INTER_CUBIC);
472 }
473 else
474 {
475 // Image data
476 memcpy( (unsigned char*)(this->CurrentLeftFrame.data), (unsigned char*)((TimestampedStructImage*)this->left_mem + 1), this->LeftImage.image.data_size);
477 memcpy( (unsigned char*)(this->CurrentRightFrame.data), (unsigned char*)((TimestampedStructImage*)this->right_mem + 1), this->RightImage.image.data_size);
478 }
479
480 }
481 else if(this->cam_channels == 4)//this->cam_channels == 4, use bumblebee image
482 {
483 // If is to resize the image...
484 if(this->cam_width != this->destiny_width)
485 {
486 memcpy( (unsigned char*)(Img_BGRU.data), (unsigned char*)((TimestampedStructImage*)this->left_mem + 1), this->LeftImage.image.data_size);
487 cv::resize(Img_BGRU, Img_BGRU_resized, cv::Size(this->destiny_width, this->destiny_height), 0.0, 0.0, CV_INTER_CUBIC);
488 cv::cvtColor(Img_BGRU_resized, this->CurrentLeftFrame, CV_BGRA2BGR);
489
490 memcpy( (unsigned char*)(Img_BGRU.data), (unsigned char*)((TimestampedStructImage*)this->right_mem + 1), this->RightImage.image.data_size);
491 cv::resize(Img_BGRU, Img_BGRU_resized, cv::Size(this->destiny_width, this->destiny_height), 0.0, 0.0, CV_INTER_CUBIC);
492 cv::cvtColor(Img_BGRU_resized, this->CurrentRightFrame, CV_BGRA2BGR);
493 }
494 else
495 {
496 // Image data
497 memcpy( (unsigned char*)(Img_BGRU.data), (unsigned char*)((TimestampedStructImage*)this->left_mem + 1), this->LeftImage.image.data_size);
498 cv::cvtColor(Img_BGRU, this->CurrentLeftFrame, CV_BGRA2BGR);
499
500 // Image data
501 memcpy( (unsigned char*)(Img_BGRU.data), (unsigned char*)((TimestampedStructImage*)this->right_mem + 1), this->RightImage.image.data_size);
502 cv::cvtColor(Img_BGRU, this->CurrentRightFrame, CV_BGRA2BGR);
503 }
504 }
505 else if (this->cam_channels == 1)
506 {
507 // If is to resize the image...
508 if(this->cam_width != this->destiny_width)
509 {
510 // this is a gray image!!!
511 memcpy( (unsigned char*)(Img_BGRU.data), (unsigned char*)((TimestampedStructImage*)this->left_mem + 1), this->LeftImage.image.data_size);
512 cv::resize(Img_BGRU, this->CurrentLeftFrame, cv::Size(this->destiny_width, this->destiny_height), 0.0, 0.0, CV_INTER_CUBIC);
513
514 memcpy( (unsigned char*)(Img_BGRU.data), (unsigned char*)((TimestampedStructImage*)this->right_mem + 1), this->RightImage.image.data_size);
515 cv::resize(Img_BGRU, this->CurrentRightFrame, cv::Size(this->destiny_width, this->destiny_height), 0.0, 0.0, CV_INTER_CUBIC);
516 }
517 else
518 {
519 memcpy( (unsigned char*)(this->CurrentLeftFrame.data), (unsigned char*)((TimestampedStructImage*)this->left_mem + 1), this->LeftImage.image.data_size);
520
521 memcpy( (unsigned char*)(this->CurrentRightFrame.data), (unsigned char*)((TimestampedStructImage*)this->right_mem + 1), this->RightImage.image.data_size);
522 }
523 }
524
525 if(this->showdebug)
526 {
527 cv::namedWindow( "DisparityMapComponent - Current Left Frame", CV_WINDOW_NORMAL );
528 cv::imshow("DisparityMapComponent - Current Left Frame",this->CurrentLeftFrame);
529 cv::namedWindow( "DisparityMapComponent - Current Right Frame", CV_WINDOW_NORMAL );
530 cv::imshow("DisparityMapComponent - Current Right Frame",this->CurrentRightFrame);
531 cv::waitKey(1);
532 }
533
534 //======================================= Disparity Map Calculation ================================================
535
536 this->CalcDisparityMap(this->CurrentLeftFrame, this->CurrentRightFrame, this->CurrentDisparityMap, this->dispmap_type);
537
538 // Complete timestamp header of the left image
539 this->RefImage.time = this->LeftImage.time;
540 this->RefImage.timerange = this->LeftImage.timerange;
541
542 // Copy images header and data to memory
543 memcpy(this->ref_mem, &RefImage, sizeof(TimestampedStructImage));
544 memcpy((void*)((TimestampedStructImage*)this->ref_mem + 1), (void*)this->CurrentLeftFrame.data, this->RefImage.image.data_size);
545 this->shmem_ref->write(this->ref_mem, this->ref_mem_size);
546
547 // Complete timestamp header of the disp image
548 this->DispImage.time = this->LeftImage.time;
549 this->DispImage.timerange = this->LeftImage.timerange;
550
551 // Copy images header and data to memory
552 memcpy(this->disp_mem, &DispImage, sizeof(TimestampedStructImage));
553 memcpy((void*)((TimestampedStructImage*)this->disp_mem + 1), (void*)this->CurrentDisparityMap.data, this->DispImage.image.data_size);
554 this->shmem_disp->write(this->disp_mem, this->disp_mem_size);
555
556 //==================================================================================================================
557
558 if(this->showdebug)
559 {
560 cv::Mat imgDisparity8U = cv::Mat( this->CurrentDisparityMap.rows, this->CurrentDisparityMap.cols, CV_8UC1 );
561
562 this->CurrentDisparityMap = this->CurrentDisparityMap/16;
563
564 // Display it as a CV_8UC1 image
565 this->CurrentDisparityMap.convertTo( imgDisparity8U, CV_8UC1);
566
567 cv::namedWindow( "DisparityMapComponent - Disparity Map", CV_WINDOW_NORMAL );
568 imshow( "DisparityMapComponent - Disparity Map", imgDisparity8U );
569 }
570 }
571 else
572 msleep(/*MS_DELAY*/10);
573
574 if(this->showdebug)
575 cv::waitKey(1); // Give the system permission
576 }
577
578 this->is_running = false;
579
580 // Destroy the window frame
581 if(this->showdebug)
582 cvDestroyAllWindows();
583}
584
585
586////////////////////////////////////////////////////////////////////////////////
587void DisparityMap::setMinimumDisparity(int minimum_disp)
588{
589 this->min_disp = minimum_disp;
590
591 emit minimumDisparityChanged(this->min_disp);
592}
593
594
595////////////////////////////////////////////////////////////////////////////////
596void DisparityMap::setNumberOfDisparity(int numberOfDisparity)
597{
598 // Must be a multiple of 16
599 this->num_disp = numberOfDisparity & ~15;
600 if (this->num_disp < 16)
601 this->num_disp = 16;
602
603 emit numberOfDisparityChanged(this->num_disp);
604}
605
606
607////////////////////////////////////////////////////////////////////////////////
608void DisparityMap::setSADWindowSize(int SADWindowSize)
609{
610 // Must be an odd number
611 this->sbm_SADWindowSize = SADWindowSize | 1;
612
613 emit SADWindowSizeChanged(this->sbm_SADWindowSize);
614}
615
616
617////////////////////////////////////////////////////////////////////////////////
618void DisparityMap::setTextureTreshold(int textureTreshold)
619{
620 this->sbm_textureThreshold = textureTreshold;
621
622 emit textureTresholdChanged(this->sbm_textureThreshold);
623}
624
625
626////////////////////////////////////////////////////////////////////////////////
627void DisparityMap::setUniquenessRatio(int uniquenessRatio)
628{
629 this->sbm_uniquenessRatio = uniquenessRatio;
630
631 emit uniquenessRatioChanged(this->sbm_uniquenessRatio);
632}
633
634
635////////////////////////////////////////////////////////////////////////////////
636/// CalcDisparityMap
637/// Description: Calculate the Disparity Map of a stereo pair
638/// Parameters: Left_img = left image
639/// Right_img = right image
640/// Disp_map = disparity map
641/// Type = Method: 0 = SAD (Sum of Absolute Differences algorithm)
642/// 1 = SGBM (Semi-Global Block Matching algorithm)
643/// 2 = SGBM+HH (full-scale two-pass dynamic programming algorithm)
644/// 3 = SAD from PtGrey
645////////////////////////////////////////////////////////////////////////////////
646void DisparityMap::CalcDisparityMap(cv::Mat Left_img, cv::Mat Right_img, cv::Mat &Disp_map, Type type)
647{
648 //Left_img = cv::imread("C:/Bumblebee_Images/BumblebeeXB3_2013-09-17/rectified-left-00102.ppm");
649 //Right_img = cv::imread("C:/Bumblebee_Images/BumblebeeXB3_2013-09-17/rectified-right-00102.ppm");
650
651 cv::Mat Left_img_gray, Right_img_gray;
652
653 int SADWindowSize_ratio = type == 0 ? this->sbm_SADWindowSize/2 : this->sgbm_SADWindowSize/2;
654
655 cv::Mat Left_img_exp, Right_img_exp, Disp_map_exp;
656
657 // Expand the image size to better process the disparity range
658 if(this->use_roi)
659 {
660 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));
661 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));
662 Disp_map_exp = cv::Mat(this->destiny_roi_height + 2*SADWindowSize_ratio, this->destiny_roi_width + 2*this->num_disp, CV_16S);
663
664 (Left_img(cv::Rect(this->destiny_roi_x, this->destiny_roi_y, this->destiny_roi_width, this->destiny_roi_height))).copyTo(
665 Left_img_exp(cv::Rect(this->num_disp-1, SADWindowSize_ratio, this->destiny_roi_width, this->destiny_roi_height)));
666
667 (Right_img(cv::Rect(this->destiny_roi_x, this->destiny_roi_y, this->destiny_roi_width, this->destiny_roi_height))).copyTo(
668 Right_img_exp(cv::Rect(this->num_disp-1, SADWindowSize_ratio, this->destiny_roi_width, this->destiny_roi_height)));
669 }
670 else
671 {
672 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));
673 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));
674 Disp_map_exp = cv::Mat(Disp_map.rows + 2*SADWindowSize_ratio, Disp_map.cols + 2*this->num_disp, CV_16S);
675
676 Left_img.copyTo(Left_img_exp(cv::Rect(this->num_disp-1, SADWindowSize_ratio, Left_img.cols, Left_img.rows)));
677 Right_img.copyTo(Right_img_exp(cv::Rect(this->num_disp-1, SADWindowSize_ratio, Right_img.cols, Right_img.rows)));
678 }
679
680 // Disparity Map by the SAD algorithm
681 if(type == SAD)
682 {
683 cv::StereoBM sbm;
684
685 // Gray scale images
686 if(this->destiny_channels != 1)
687 {
688 Left_img_gray = cv::Mat(cv::Size(Left_img_exp.cols, Left_img_exp.rows), CV_8UC1);
689 cv::cvtColor(Left_img_exp, Left_img_gray, CV_BGR2GRAY);
690 Right_img_gray = cv::Mat(cv::Size(Right_img_exp.cols, Right_img_exp.rows), CV_8UC1);
691 cv::cvtColor(Right_img_exp, Right_img_gray, CV_BGR2GRAY);
692 }
693 else
694 {
695 Left_img_gray = Left_img_exp;
696 Right_img_gray = Right_img_exp;
697 }
698
699 /*cv::namedWindow("Disparity trackbar", CV_WINDOW_NORMAL);
700 cv::createTrackbar("sbm_preFilterCap", "Disparity trackbar", &sbm_preFilterCap, 63);
701 cv::createTrackbar("sbm_preFilterSize", "Disparity trackbar", &sbm_preFilterSize, 250);
702 cv::createTrackbar("sbm_SADWindowSize", "Disparity trackbar", &sbm_SADWindowSize, 250);
703 cv::createTrackbar("min_disp", "Disparity trackbar", &min_disp, 250);
704 cv::createTrackbar("num_disp", "Disparity trackbar", &num_disp, 250);
705 cv::createTrackbar("sbm_textureThreshold", "Disparity trackbar", &sbm_textureThreshold, 250);
706 cv::createTrackbar("sbm_uniquenessRatio", "Disparity trackbar", &sbm_uniquenessRatio, 250);
707 cv::createTrackbar("sbm_speckleWindowSize", "Disparity trackbar", &sbm_speckleWindowSize, 250);
708 cv::createTrackbar("sbm_speckleRange", "Disparity trackbar", &sbm_speckleRange, 250);
709 this->sbm_SADWindowSize = (this->sbm_SADWindowSize % 2 > 0) ? this->sbm_SADWindowSize : this->sbm_SADWindowSize + 1;
710 this->num_disp = (this->num_disp % 16 == 0) ? this->num_disp : this->num_disp - (this->num_disp % 16);*/
711
712 // Configuration
713 //sbm.state->roi1 = roi1; // Valid pixels ROI of the left image
714 //sbm.state->roi2 = roi2; // Valid pixels ROI of the right image
715 sbm.state->preFilterCap = this->sbm_preFilterCap; // Truncation value for the prefiltered image pixels.
716 // The algorithm first computes x-derivative at each pixel and clips its
717 // value by [-preFilterCap, preFilterCap] interval. The result values are
718 // passed to the Birchfield-Tomasi pixel cost function.
719 sbm.state->preFilterSize = this->sbm_preFilterSize >= 5 ? this->sbm_preFilterSize : 5;
720 sbm.state->SADWindowSize = this->sbm_SADWindowSize >= 5 ? this->sbm_SADWindowSize : 11; // Matched block size. It must be an odd number >=1.
721 // Normally, it should be somewhere in the 3..11 range
722 sbm.state->minDisparity = this->min_disp; // Minimum possible disparity value. Normally, it is zero but sometimes
723 // rectification algorithms can shift images, so this parameter needs to be adjusted accordingly.
724 sbm.state->numberOfDisparities = this->num_disp > 0 ? this->num_disp : Left_img.cols/8; // Maximum disparity minus minimum disparity.
725 // The value is always greater than zero.
726 // In the current implementation, this parameter must be
727 // divisible by 16.
728 sbm.state->textureThreshold = this->sbm_textureThreshold; // Validation threashold for texture variation
729 sbm.state->uniquenessRatio = this->sbm_uniquenessRatio; // Margin in percentage by which the best (minimum) computed cost function value should “win”
730 // the second best value to consider the found match correct. Normally, a value within the 5-15
731 // range is good enough.
732 sbm.state->speckleWindowSize = this->sbm_speckleWindowSize; // Maximum size of smooth disparity regions to consider their noise speckles and invalidate. Set
733 // it to 0 to disable speckle filtering. Otherwise, set it somewhere in the 50-200 range.
734 sbm.state->speckleRange = this->sbm_speckleRange; // Maximum disparity variation within each connected component. If you do speckle filtering, set
735 // the parameter to a positive value, it will be implicitly multiplied by 16. Normally, 1 or 2 is
736 // good enough.
737
738 // Calculate the disparity image (16S = scaled by 16, must be divided by 16; 32F = original value)
739 sbm(Left_img_gray, Right_img_gray, Disp_map_exp, CV_16S);
740 }
741 else if(type == SGBM || type == SGBM_HH)
742 {
743 cv::StereoSGBM sgbm;
744
745 sgbm.preFilterCap = this->sgbm_preFilterCap; // Truncation value for the prefiltered image pixels.
746 // The algorithm first computes x-derivative at each pixel and clips its
747 // value by [-preFilterCap, preFilterCap] interval. The result values are
748 // passed to the Birchfield-Tomasi pixel cost function.
749 sgbm.SADWindowSize = this->sgbm_SADWindowSize > 0 ? this->sgbm_SADWindowSize : 7; // Matched block size. It must be an odd number >=1.
750 // Normally, it should be somewhere in the 3..11 range
751 sgbm.P1 = this->sgbm_P1; // The first parameter controlling the disparity smoothness.
752 sgbm.P2 = this->sgbm_P2; // The second parameter controlling the disparity smoothness. The larger the values are,
753 // the smoother the disparity is. P1 is the penalty on the disparity change by plus or
754 // minus 1 between neighbor pixels. P2 is the penalty on the disparity change by more than
755 // 1 between neighbor pixels. The algorithm requires P2 > P1 .
756 sgbm.minDisparity = this->min_disp; // Minimum possible disparity value. Normally, it is zero but sometimes
757 // rectification algorithms can shift images, so this parameter needs to be adjusted accordingly.
758 sgbm.numberOfDisparities = this->num_disp > 0 ? this->num_disp : Left_img.cols/8; // Maximum disparity minus minimum disparity.
759 // The value is always greater than zero.
760 // In the current implementation, this parameter must be
761 // divisible by 16.
762 sgbm.uniquenessRatio = this->sgbm_uniquenessRatio; // Margin in percentage by which the best (minimum) computed cost function value should “win”
763 // the second best value to consider the found match correct. Normally, a value within the 5-15
764 // range is good enough.
765 sgbm.speckleWindowSize = this->sgbm_speckleWindowSize; // Maximum size of smooth disparity regions to consider their noise speckles and invalidate. Set
766 // it to 0 to disable speckle filtering. Otherwise, set it somewhere in the 50-200 range.
767 sgbm.speckleRange = this->sgbm_speckleRange; // Maximum disparity variation within each connected component. If you do speckle filtering, set
768 // the parameter to a positive value, it will be implicitly multiplied by 16. Normally, 1 or 2 is
769 // good enough.
770 sgbm.disp12MaxDiff = this->sgbm_disp12MaxDiff; // Maximum allowed difference (in integer pixel units) in the left-right disparity check. Set it to
771 // a non-positive value to disable the check.
772 sgbm.fullDP = type == SGBM; // Set it to true to run the full-scale two-pass dynamic programming algorithm.
773 // It will consume O(W*H*numDisparities) bytes, which is large for 640x480 stereo
774 // and huge for HD-size pictures.
775
776 sgbm(Left_img_exp, Right_img_exp, Disp_map_exp);
777 }
778
779 if(this->use_roi)
780 {
781 (Disp_map_exp(cv::Rect(this->num_disp-1, SADWindowSize_ratio, this->destiny_roi_width, this->destiny_roi_height))).copyTo(
782 Disp_map(cv::Rect(this->destiny_roi_x, this->destiny_roi_y, this->destiny_roi_width, this->destiny_roi_height)));
783 }
784 else
785 {
786 (Disp_map_exp(cv::Rect(this->num_disp-1, SADWindowSize_ratio, Disp_map.cols, Disp_map.rows))).copyTo(Disp_map);
787 }
788}
Note: See TracBrowser for help on using the repository browser.