Point Cloud Library (PCL)  1.9.1
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends Modules Pages
stereo_matching.h
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2012-, Open Perception, Inc.
6  *
7  * All rights reserved.
8  *
9  * Redistribution and use in source and binary forms, with or without
10  * modification, are permitted provided that the following conditions
11  * are met:
12  *
13  * * Redistributions of source code must retain the above copyright
14  * notice, this list of conditions and the following disclaimer.
15  * * Redistributions in binary form must reproduce the above
16  * copyright notice, this list of conditions and the following
17  * disclaimer in the documentation and/or other materials provided
18  * with the distribution.
19  * * Neither the name of the copyright holder(s) nor the names of its
20  * contributors may be used to endorse or promote products derived
21  * from this software without specific prior written permission.
22  *
23  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34  * POSSIBILITY OF SUCH DAMAGE.
35  *
36  */
37 #ifndef PCL_STEREO_H_
38 #define PCL_STEREO_H_
39 
40 #include <pcl/conversions.h>
41 #include <pcl/point_types.h>
42 
43 namespace pcl
44 {
45  /** \brief Stereo Matching abstract class
46  *
47  * The class performs stereo matching on a rectified stereo pair
48  * Includes the following functionalities:
49  * * preprocessing of the image pair, to improve robustness against photometric distortions
50  * (wrt. to a spatially constant additive photometric factor)
51  * * postprocessing: filtering of wrong disparities via Peak Filter (eliminating ambiguities due to low-textured regions)
52  * and Ratio Filter (eliminating generic matching ambiguities, similar to that present in OpenCV Block Matching Stereo)
53  * * postprocessing: Left-Right consistency check (eliminates wrong disparities at the cost of twice the stereo matching
54  * computation)
55  * * postprocessing: subpixel refinement of computed disparities, to reduce the depth quantization effect
56  * * postprocessing: smoothing of the disparity map via median filter
57  * * after stereo matching a PCL point cloud can be computed, given the stereo intrinsic (focal, principal point
58  * coordinates) and extrinsic (baseline) calibration parameters
59  *
60  * \author Federico Tombari (federico.tombari@unibo.it)
61  * \ingroup stereo
62  */
63  class PCL_EXPORTS StereoMatching
64  {
65  public:
66  StereoMatching(void);
67 
68  virtual ~StereoMatching(void);
69 
70  /** \brief setter for number of disparity candidates (disparity range)
71  *
72  * \param[in] max_disp number of disparity candidates (disparity range); has to be > 0
73  */
74  void
75  setMaxDisparity (int max_disp)
76  {
77  max_disp_ = max_disp;
78  };
79 
80  /** \brief setter for horizontal offset, i.e. number of pixels to shift the disparity range over the target image
81  *
82  * \param[in] x_off horizontal offset value; has to be >= 0
83  */
84  void
85  setXOffset (int x_off)
86  {
87  x_off_ = x_off;
88  };
89 
90  /** \brief setter for the value of the ratio filter
91  *
92  * \param[in] ratio_filter value of the ratio filter; it is a number in the range [0, 100]
93  * (0: no filtering action; 100: all disparities are filtered)
94  */
95  void
96  setRatioFilter (int ratio_filter)
97  {
98  ratio_filter_ = ratio_filter;
99  };
100 
101  /** \brief setter for the value of the peak filter
102  *
103  * \param[in] peak_filter value of the peak filter; it is a number in the range [0, inf]
104  * (0: no filtering action)
105  */
106  void
107  setPeakFilter (int peak_filter)
108  {
109  peak_filter_ = peak_filter;
110  };
111 
112  /** \brief setter for the pre processing step
113  *
114  * \param[in] is_pre_proc setting the boolean to true activates the pre-processing step for both stereo images
115  */
116  void
117  setPreProcessing (bool is_pre_proc)
118  {
119  is_pre_proc_ = is_pre_proc;
120  };
121 
122  /** \brief setter for the left-right consistency check stage, that eliminates inconsistent/wrong disparity
123  * values from the disparity map at approx. twice the processing cost of the selected stereo algorithm
124  *
125  * \param[in] is_lr_check setting the boolean to true activates the left-right consistency check
126  */
127  void
128  setLeftRightCheck (bool is_lr_check)
129  {
130  is_lr_check_ = is_lr_check;
131  };
132 
133  /** \brief setter for the left-right consistency check threshold
134  *
135  * \param[in] lr_check_th sets the value of the left-right consistency check threshold
136  * only has some influence if the left-right check is active
137  * typically has either the value 0 ("strong" consistency check, more points being filtered) or 1 ("weak"
138  * consistency check, less points being filtered)
139  */
140  void
141  setLeftRightCheckThreshold (int lr_check_th)
142  {
143  lr_check_th_ = lr_check_th;
144  };
145 
146  /** \brief stereo processing, it computes a disparity map stored internally by the class
147  *
148  * \param[in] ref_img reference array of image pixels (left image)
149  * \param[in] trg_img target array of image pixels (right image)
150  * \param[in] width number of elements per row for both input arrays
151  * \param[in] height number of elements per column for both input arrays
152  */
153  virtual void
154  compute (unsigned char* ref_img, unsigned char* trg_img, int width, int height) = 0;
155 
156  /** \brief stereo processing, it computes a disparity map stored internally by the class
157  *
158  * \param[in] ref point cloud of pcl::RGB type containing the pixels of the reference image (left image)
159  * \param[in] trg point cloud of pcl::RGB type containing the pixels of the target image (right image)
160  */
161  virtual void
162  compute (pcl::PointCloud<pcl::RGB> &ref, pcl::PointCloud<pcl::RGB> &trg) = 0;
163 
164  /** \brief median filter applied on the previously computed disparity map
165  * Note: the "compute" method must have been previously called at least once in order for this function
166  * to have any effect
167  * \param[in] radius radius of the squared window used to compute the median filter; the window side is
168  * equal to 2*radius + 1
169  */
170  void
171  medianFilter (int radius);
172 
173  /** \brief computation of the 3D point cloud from the previously computed disparity map without color information
174  * Note: the "compute" method must have been previously called at least once in order for this function
175  * to have any effect
176  * \param[in] u_c horizontal coordinate of the principal point (calibration parameter)
177  * \param[in] v_c vertical coordinate of the principal point (calibration parameter)
178  * \param[in] focal focal length in pixels (calibration parameter)
179  * \param[in] baseline distance between the two cameras (calibration parameter); the measure unit used to
180  * specify this parameter will be the same as the 3D points in the output point cloud
181  * \param[out] cloud output 3D point cloud; it is organized and non-dense, with NaNs where 3D points are invalid
182  */
183  virtual bool
184  getPointCloud (float u_c, float v_c, float focal, float baseline, pcl::PointCloud<pcl::PointXYZ>::Ptr cloud);
185 
186  /** \brief computation of the 3D point cloud from the previously computed disparity map including color information
187  * Note: the "compute" method must have been previously called at least once in order for this function
188  * to have any effect
189  * \param[in] u_c horizontal coordinate of the principal point (calibration parameter)
190  * \param[in] v_c vertical coordinate of the principal point (calibration parameter)
191  * \param[in] focal focal length in pixels (calibration parameter)
192  * \param[in] baseline distance between the two cameras (calibration parameter); the measure unit used to
193  * specify this parameter will be the same as the 3D points in the output point cloud
194  * \param[out] cloud output 3D point cloud; it is organized and non-dense, with NaNs where 3D points are invalid
195  * \param[in] texture 3D cloud (same size of the output cloud) used to associate to each 3D point of the
196  * output cloud a color triplet
197  */
198  virtual bool
199  getPointCloud (float u_c, float v_c, float focal, float baseline, pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud, pcl::PointCloud<pcl::RGB>::Ptr texture);
200 
201  /** \brief computation of a pcl::RGB cloud with scaled disparity values
202  * it can be used to display a rescaled version of the disparity map by means of the pcl::ImageViewer
203  * invalid disparity values are shown in green
204  * Note: the "compute" method must have been previously called at least once in order for this function
205  * to have any effect
206  * \param[out] vMap output cloud
207  */
208  void
209  getVisualMap (pcl::PointCloud<pcl::RGB>::Ptr vMap);
210 
211  protected:
212  /** \brief The internal disparity map. */
213  short int *disp_map_;
214 
215  /** \brief Local aligned copies of the cloud data. */
216  unsigned char* ref_img_;
217  unsigned char* trg_img_;
218 
219  /** \brief Disparity map used for left-right check. */
220  short int *disp_map_trg_;
221 
222  /** \brief Local aligned copies used for pre processing. */
223  unsigned char* pp_ref_img_;
224  unsigned char* pp_trg_img_;
225 
226  /** \brief number of pixels per column of the input stereo pair . */
227  int width_;
228 
229  /** \brief number of pixels per row of the input stereo pair . */
230  int height_;
231 
232  /** \brief Disparity range used for stereo processing. */
234 
235  /** \brief Horizontal displacemente (x offset) used for stereo processing */
236  int x_off_;
237 
238  /** \brief Threshold for the ratio filter, \f$\in [0 100]\f$ */
240 
241  /** \brief Threshold for the peak filter, \f$\in [0 \infty]\f$ */
243 
244  /** \brief toggle for the activation of the pre-processing stage */
246 
247  /** \brief toggle for the activation of the left-right consistency check stage */
249 
250  /** \brief Threshold for the left-right consistency check, typically either 0 or 1 */
252 
253  virtual void
254  preProcessing (unsigned char *img, unsigned char *pp_img) = 0;
255 
256  virtual void
257  imgFlip (unsigned char * & img) = 0;
258 
259  virtual void
260  compute_impl (unsigned char* ref_img, unsigned char* trg_img) = 0;
261 
262  void
263  leftRightCheck ();
264 
265  inline short int
266  computeStereoSubpixel (int dbest, int s1, int s2, int s3)
267  {
268  int den = (s1+s3-2*s2);
269  if (den != 0)
270  return (static_cast<short int> (16*dbest + (((s1 - s3)*8) / den)));
271  else
272  return (static_cast<short int> (dbest*16));
273  }
274 
275  inline short int
276  computeStereoSubpixel (int dbest, float s1, float s2, float s3)
277  {
278  float den = (s1+s3-2*s2);
279  if (den != 0)
280  return (static_cast<short int> (16*dbest + floor(.5 + (((s1 - s3)*8) / den))));
281  else
282  return (static_cast<short int> (dbest*16));
283  }
284 
285  inline short int
286  doStereoRatioFilter (int *acc, short int dbest, int sad_min, int ratio_filter, int maxdisp, int precision = 100)
287  {
288  int sad_second_min = std::numeric_limits<int>::max ();
289 
290  for (int d = 0; d < dbest - 1; d++)
291  if (acc[d] < sad_second_min)
292  sad_second_min = acc[d];
293 
294  for (int d = dbest + 2; d < maxdisp; d++)
295  if (acc[d] < sad_second_min)
296  sad_second_min = acc[d];
297 
298  if (sad_min * precision > (precision - ratio_filter) * sad_second_min)
299  return (-2);
300  else
301  return (dbest);
302  }
303 
304  inline short int
305  doStereoRatioFilter (float *acc, short int dbest, float sad_min, int ratio_filter, int maxdisp, int precision = 100)
306  {
307  float sad_second_min = std::numeric_limits<float>::max ();
308 
309  for (int d = 0; d < dbest - 1; d++)
310  if (acc[d] < sad_second_min)
311  sad_second_min = acc[d];
312 
313  for (int d = dbest + 2; d < maxdisp; d++)
314  if (acc[d] < sad_second_min)
315  sad_second_min = acc[d];
316 
317  if (sad_min * static_cast<float> (precision) > static_cast<float> (precision - ratio_filter) * sad_second_min)
318  return (-2);
319  else
320  return (dbest);
321  }
322 
323  inline short int
324  doStereoPeakFilter (int *acc, short int dbest, int peak_filter, int maxdisp)
325  {
326  int da = (dbest>1) ? ( acc[dbest-2] - acc[dbest] ) : (acc[dbest+2] - acc[dbest]);
327  int db = (dbest<maxdisp-2) ? (acc[dbest+2] - acc[dbest]) : (acc[dbest-2] - acc[dbest]);
328 
329  if (da + db < peak_filter)
330  return (-4);
331  else
332  return (dbest);
333  }
334 
335  inline short int
336  doStereoPeakFilter (float *acc, short int dbest, int peak_filter, int maxdisp)
337  {
338  float da = (dbest>1) ? ( acc[dbest-2] - acc[dbest] ) : (acc[dbest+2] - acc[dbest]);
339  float db = (dbest<maxdisp-2) ? (acc[dbest+2] - acc[dbest]) : (acc[dbest-2] - acc[dbest]);
340 
341  if (da + db < peak_filter)
342  return (-4);
343  else
344  return (dbest);
345  }
346 
347  };
348 
349  /** \brief Stereo Matching abstract class for Grayscale images
350  *
351  * The class implements some functionalities of pcl::StereoMatching specific for grayscale stereo processing,
352  * such as image pre-processing and left
353  *
354  * \author Federico Tombari (federico.tombari@unibo.it)
355  * \ingroup stereo
356  */
357  class PCL_EXPORTS GrayStereoMatching : public StereoMatching
358  {
359  public:
360  GrayStereoMatching (void);
361  virtual ~GrayStereoMatching (void);
362 
363  /** \brief stereo processing, it computes a disparity map stored internally by the class
364  *
365  * \param[in] ref_img reference array of image pixels (left image), has to be grayscale single channel
366  * \param[in] trg_img target array of image pixels (right image), has to be grayscale single channel
367  * \param[in] width number of elements per row for both input arrays
368  * \param[in] height number of elements per column for both input arrays
369  */
370  virtual void
371  compute (unsigned char* ref_img, unsigned char* trg_img, int width, int height);
372 
373  /** \brief stereo processing, it computes a disparity map stored internally by the class
374  *
375  * \param[in] ref point cloud of pcl::RGB type containing the pixels of the reference image (left image)
376  * the pcl::RGB triplets are automatically converted to grayscale upon call of the method
377  * \param[in] trg point cloud of pcl::RGB type containing the pixels of the target image (right image)
378  * the pcl::RGB triplets are automatically converted to grayscale upon call of the method
379  */
380  virtual void
382  protected:
383  virtual void
384  compute_impl (unsigned char* ref_img, unsigned char* trg_img) = 0;
385 
386  virtual void
387  preProcessing (unsigned char *img, unsigned char *pp_img);
388 
389  virtual void
390  imgFlip (unsigned char * & img);
391  };
392 
393  /** \brief Block based (or fixed window) Stereo Matching class
394  *
395  * This class implements the baseline Block-based - aka Fixed Window - stereo matching algorithm.
396  * The algorithm includes a running box filter so that the computational complexity is independent of
397  * the size of the window ( O(1) wrt. to the size of window)
398  * The algorithm is based on the Sum of Absolute Differences (SAD) matching function
399  * Only works with grayscale (single channel) rectified images
400  *
401  * \author Federico Tombari (federico.tombari@unibo.it)
402  * \ingroup stereo
403  */
404 
405  class PCL_EXPORTS BlockBasedStereoMatching : public GrayStereoMatching
406  {
407  public:
410  {
411  };
412 
413  /** \brief setter for the radius of the squared window
414  * \param[in] radius radius of the squared window used to compute the block-based stereo algorithm
415  * the window side is equal to 2*radius + 1
416  */
417  void
418  setRadius (int radius)
419  {
420  radius_ = radius;
421  };
422  private:
423  virtual void
424  compute_impl (unsigned char* ref_img, unsigned char* trg_img);
425 
426  int radius_;
427  };
428 
429  /** \brief Adaptive Cost 2-pass Scanline Optimization Stereo Matching class
430  *
431  * This class implements an adaptive-cost stereo matching algorithm based on 2-pass Scanline Optimization.
432  * The algorithm is inspired by the paper:
433  * [1] L. Wang et al., "High Quality Real-time Stereo using Adaptive Cost Aggregation and Dynamic Programming", 3DPVT 2006
434  * Cost aggregation is performed using adaptive weigths computed on a single column as proposed in [1].
435  * Instead of using Dynamic Programming as in [1], the optimization is performed via 2-pass Scanline Optimization.
436  * The algorithm is based on the Sum of Absolute Differences (SAD) matching function
437  * Only works with grayscale (single channel) rectified images
438  *
439  * \author Federico Tombari (federico.tombari@unibo.it)
440  * \ingroup stereo
441  */
443  {
444  public:
446 
448  {
449  };
450 
451  /** \brief setter for the radius (half length) of the column used for cost aggregation
452  * \param[in] radius radius (half length) of the column used for cost aggregation; the total column length
453  * is equal to 2*radius + 1
454  */
455  void
456  setRadius (int radius)
457  {
458  radius_ = radius;
459  };
460 
461  /** \brief setter for the spatial bandwidth used for cost aggregation based on adaptive weights
462  * \param[in] gamma_s spatial bandwidth used for cost aggregation based on adaptive weights
463  */
464  void
465  setGammaS (int gamma_s)
466  {
467  gamma_s_ = gamma_s;
468  };
469 
470  /** \brief setter for the color bandwidth used for cost aggregation based on adaptive weights
471  * \param[in] gamma_c color bandwidth used for cost aggregation based on adaptive weights
472  */
473  void
474  setGammaC (int gamma_c)
475  {
476  gamma_c_ = gamma_c;
477  };
478 
479  /** \brief "weak" smoothness penalty used within 2-pass Scanline Optimization
480  * \param[in] smoothness_weak "weak" smoothness penalty cost
481  */
482  void
483  setSmoothWeak (int smoothness_weak)
484  {
485  smoothness_weak_ = smoothness_weak;
486  };
487 
488  /** \brief "strong" smoothness penalty used within 2-pass Scanline Optimization
489  * \param[in] smoothness_strong "strong" smoothness penalty cost
490  */
491  void
492  setSmoothStrong (int smoothness_strong)
493  {
494  smoothness_strong_ = smoothness_strong;
495  };
496 
497  private:
498  virtual void
499  compute_impl (unsigned char* ref_img, unsigned char* trg_img);
500 
501  int radius_;
502 
503  //parameters for adaptive weight cost aggregation
504  double gamma_c_;
505  double gamma_s_;
506 
507  //Parameters for 2-pass SO optimization
508  int smoothness_strong_;
509  int smoothness_weak_;
510  };
511 }
512 
513 #endif
void setMaxDisparity(int max_disp)
setter for number of disparity candidates (disparity range)
boost::shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:428
short int computeStereoSubpixel(int dbest, int s1, int s2, int s3)
void setSmoothWeak(int smoothness_weak)
"weak" smoothness penalty used within 2-pass Scanline Optimization
void setGammaC(int gamma_c)
setter for the color bandwidth used for cost aggregation based on adaptive weights ...
short int * disp_map_
The internal disparity map.
void setLeftRightCheck(bool is_lr_check)
setter for the left-right consistency check stage, that eliminates inconsistent/wrong disparity value...
Block based (or fixed window) Stereo Matching class.
short int doStereoRatioFilter(float *acc, short int dbest, float sad_min, int ratio_filter, int maxdisp, int precision=100)
void setLeftRightCheckThreshold(int lr_check_th)
setter for the left-right consistency check threshold
Stereo Matching abstract class for Grayscale images.
This file defines compatibility wrappers for low level I/O functions.
Definition: convolution.h:45
int ratio_filter_
Threshold for the ratio filter, .
short int * disp_map_trg_
Disparity map used for left-right check.
int lr_check_th_
Threshold for the left-right consistency check, typically either 0 or 1.
void setPreProcessing(bool is_pre_proc)
setter for the pre processing step
void setGammaS(int gamma_s)
setter for the spatial bandwidth used for cost aggregation based on adaptive weights ...
void setRatioFilter(int ratio_filter)
setter for the value of the ratio filter
void setSmoothStrong(int smoothness_strong)
"strong" smoothness penalty used within 2-pass Scanline Optimization
void setXOffset(int x_off)
setter for horizontal offset, i.e.
Stereo Matching abstract class.
short int doStereoRatioFilter(int *acc, short int dbest, int sad_min, int ratio_filter, int maxdisp, int precision=100)
bool is_pre_proc_
toggle for the activation of the pre-processing stage
void setPeakFilter(int peak_filter)
setter for the value of the peak filter
bool is_lr_check_
toggle for the activation of the left-right consistency check stage
Adaptive Cost 2-pass Scanline Optimization Stereo Matching class.
int x_off_
Horizontal displacemente (x offset) used for stereo processing.
short int doStereoPeakFilter(float *acc, short int dbest, int peak_filter, int maxdisp)
int max_disp_
Disparity range used for stereo processing.
PointCloud represents the base class in PCL for storing collections of 3D points. ...
void setRadius(int radius)
setter for the radius (half length) of the column used for cost aggregation
unsigned char * pp_trg_img_
short int doStereoPeakFilter(int *acc, short int dbest, int peak_filter, int maxdisp)
unsigned char * trg_img_
void setRadius(int radius)
setter for the radius of the squared window
int height_
number of pixels per row of the input stereo pair .
unsigned char * pp_ref_img_
Local aligned copies used for pre processing.
short int computeStereoSubpixel(int dbest, float s1, float s2, float s3)
int peak_filter_
Threshold for the peak filter, .
unsigned char * ref_img_
Local aligned copies of the cloud data.
int width_
number of pixels per column of the input stereo pair .