Point Cloud Library (PCL)  1.8.1
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 bandwith used for cost aggregation based on adaptive weights
462  * \param[in] gamma_s spatial bandwith 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 bandwith used for cost aggregation based on adaptive weights
471  * \param[in] gamma_c color bandwith 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)
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 bandwith 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.
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 bandwith 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.
boost::shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:428
short int doStereoRatioFilter(int *acc, short int dbest, int sad_min, int ratio_filter, int maxdisp, int precision=100)
Defines all the PCL implemented PointT point type structures.
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.
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 .