Point Cloud Library (PCL)  1.10.0-dev
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 
38 #pragma once
39 
40 #include <algorithm>
41 
42 #include <pcl/conversions.h>
43 #include <pcl/point_types.h>
44 
45 namespace pcl {
46 template <class T>
47 short int
48 doStereoRatioFilter(const T* const acc,
49  short int dbest,
50  T sad_min,
51  int ratio_filter,
52  int maxdisp,
53  int precision = 100)
54 {
55  const auto sad_min_1st_part_it = std::min_element(acc, acc + dbest - 1);
56  const auto sad_min_2nd_part_it = std::min_element(acc + dbest + 2, acc + maxdisp);
57 
58  const auto sad_second_min = std::min(*sad_min_1st_part_it, *sad_min_2nd_part_it);
59 
60  if ((sad_min * precision) > ((precision - ratio_filter) * sad_second_min)) {
61  return -2;
62  }
63  return dbest;
64 }
65 
66 template <class T>
67 inline short int
68 doStereoPeakFilter(const T* const acc, short int dbest, int peak_filter, int maxdisp)
69 {
70  // da and db = acc[index] - acc[dbest],
71  // where index = (dbest + 2) or (dbest - 2)
72  // => index = dbest + 2 - (0 or 4) = dbest - 2 + (0 or 4)
73  // => index = dbest + 2 - (0 << 2 or 1 << 2) = dbest - 2 + (0 << 2 or 1 << 2)
74  // => index = dbest + 2 - (condition << 2) = dbest - 2 + (condition << 2)
75  const auto da_condition = (dbest > 1);
76  const auto db_condition = (dbest < maxdisp - 2);
77  const auto da_index = dbest + 2 - (da_condition << 2);
78  const auto db_index = dbest - 2 + (db_condition << 2);
79 
80  const auto da = acc[da_index] - acc[dbest];
81  const auto db = acc[db_index] - acc[dbest];
82  if ((da + db) < peak_filter) {
83  return -4;
84  }
85  return dbest;
86 }
87 
88 /** \brief Stereo Matching abstract class
89  *
90  * The class performs stereo matching on a rectified stereo pair.
91  *
92  * Includes the following functionalities:
93  * * preprocessing of the image pair, to improve robustness against photometric
94  * distortions (wrt. to a spatially constant additive photometric factor)
95  * * postprocessing: filtering of wrong disparities via Peak Filter (eliminating
96  * ambiguities due to low-textured regions) and Ratio Filter (eliminating generic
97  * matching ambiguities, similar to that present in OpenCV Block Matching Stereo)
98  * * postprocessing: Left-Right consistency check (eliminates wrong disparities at the
99  * cost of twice the stereo matching computation)
100  * * postprocessing: subpixel refinement of computed disparities, to reduce the depth
101  * quantization effect
102  * * postprocessing: smoothing of the disparity map via median filter
103  * * after stereo matching a PCL point cloud can be computed, given the stereo
104  * intrinsic (focal, principal point coordinates) and extrinsic (baseline)
105  * calibration parameters
106  *
107  * \author Federico Tombari (federico.tombari@unibo.it)
108  * \ingroup stereo
109  */
111 public:
112  StereoMatching();
113 
114  virtual ~StereoMatching();
115 
116  /** \brief setter for number of disparity candidates (disparity range)
117  *
118  * \param[in] max_disp number of disparity candidates (disparity range); has to be > 0
119  */
120  void
121  setMaxDisparity(int max_disp)
122  {
123  max_disp_ = max_disp;
124  };
125 
126  /** \brief setter for horizontal offset, i.e. number of pixels to shift the disparity
127  * range over the target image
128  *
129  * \param[in] x_off horizontal offset value; has to be >= 0
130  */
131  void
132  setXOffset(int x_off)
133  {
134  x_off_ = x_off;
135  };
136 
137  /** \brief setter for the value of the ratio filter
138  *
139  * \param[in] ratio_filter value of the ratio filter; it is a number in the range
140  * [0, 100] (0: no filtering action; 100: all disparities are
141  * filtered)
142  */
143  void
144  setRatioFilter(int ratio_filter)
145  {
146  ratio_filter_ = ratio_filter;
147  };
148 
149  /** \brief setter for the value of the peak filter
150  *
151  * \param[in] peak_filter value of the peak filter; it is a number in the range
152  * [0, inf] (0: no filtering action)
153  */
154  void
155  setPeakFilter(int peak_filter)
156  {
157  peak_filter_ = peak_filter;
158  };
159 
160  /** \brief setter for the pre processing step
161  *
162  * \param[in] is_pre_proc setting the boolean to true activates the pre-processing
163  * step for both stereo images
164  */
165  void
166  setPreProcessing(bool is_pre_proc)
167  {
168  is_pre_proc_ = is_pre_proc;
169  };
170 
171  /** \brief setter for the left-right consistency check stage, that eliminates
172  * inconsistent/wrong disparity values from the disparity map at approx. twice the
173  * processing cost of the selected stereo algorithm
174  *
175  * \param[in] is_lr_check setting the boolean to true activates the left-right
176  * consistency check
177  */
178  void
179  setLeftRightCheck(bool is_lr_check)
180  {
181  is_lr_check_ = is_lr_check;
182  };
183 
184  /** \brief setter for the left-right consistency check threshold
185  *
186  * \param[in] lr_check_th sets the value of the left-right consistency check threshold
187  * only has some influence if the left-right check is active typically has
188  * either the value 0 ("strong" consistency check, more points being
189  * filtered) or 1 ("weak" consistency check, less points being filtered)
190  */
191  void
192  setLeftRightCheckThreshold(int lr_check_th)
193  {
194  lr_check_th_ = lr_check_th;
195  };
196 
197  /** \brief stereo processing, it computes a disparity map stored internally by the
198  * class
199  *
200  * \param[in] ref_img reference array of image pixels (left image)
201  * \param[in] trg_img target array of image pixels (right image)
202  * \param[in] width number of elements per row for both input arrays
203  * \param[in] height number of elements per column for both input arrays
204  */
205  virtual void
206  compute(unsigned char* ref_img, unsigned char* trg_img, int width, int height) = 0;
207 
208  /** \brief stereo processing, it computes a disparity map stored internally by the
209  * class
210  *
211  * \param[in] ref point cloud of pcl::RGB type containing the pixels of the reference
212  * image (left image)
213  * \param[in] trg point cloud of pcl::RGB type containing the pixels of the target
214  * image (right image)
215  */
216  virtual void
218 
219  /** \brief median filter applied on the previously computed disparity map
220  *
221  * \note The "compute" method must have been previously called at least once in order
222  * for this function to have any effect
223  *
224  * \param[in] radius radius of the squared window used to compute the median filter;
225  * the window side is equal to 2*radius + 1
226  */
227  void
228  medianFilter(int radius);
229 
230  /** \brief computation of the 3D point cloud from the previously computed disparity
231  * map without color information
232  *
233  * \note The "compute" method must have been previously called at least once in order
234  * for this function to have any effect
235  *
236  * \param[in] u_c horizontal coordinate of the principal point (calibration parameter)
237  * \param[in] v_c vertical coordinate of the principal point (calibration parameter)
238  * \param[in] focal focal length in pixels (calibration parameter)
239  * \param[in] baseline distance between the two cameras (calibration parameter); the
240  * measure unit used to specify this parameter will be the same as the 3D
241  * points in the output point cloud
242  * \param[out] cloud output 3D point cloud; it is organized and non-dense, with NaNs
243  * where 3D points are invalid
244  */
245  virtual bool
246  getPointCloud(float u_c,
247  float v_c,
248  float focal,
249  float baseline,
251 
252  /** \brief computation of the 3D point cloud from the previously computed disparity
253  * map including color information
254  *
255  * \note The "compute" method must have been previously called at least once in order
256  * for this function to have any effect
257  *
258  * \param[in] u_c horizontal coordinate of the principal point (calibration parameter)
259  * \param[in] v_c vertical coordinate of the principal point (calibration parameter)
260  * \param[in] focal focal length in pixels (calibration parameter)
261  * \param[in] baseline distance between the two cameras (calibration parameter); the
262  * measure unit used to specify this parameter will be the same as the 3D
263  * points in the output point cloud \param[out] cloud output 3D point
264  * cloud; it is organized and non-dense, with NaNs where 3D points are
265  * invalid
266  * \param[in] texture 3D cloud (same size of the output cloud) used to associate to
267  * each 3D point of the output cloud a color triplet
268  */
269  virtual bool
270  getPointCloud(float u_c,
271  float v_c,
272  float focal,
273  float baseline,
276 
277  /** \brief computation of a pcl::RGB cloud with scaled disparity values it can be used
278  * to display a rescaled version of the disparity map by means of the pcl::ImageViewer
279  * invalid disparity values are shown in green
280  *
281  * \note The "compute" method must have been previously called at least once in order
282  * for this function to have any effect
283  *
284  * \param[out] vMap output cloud
285  */
286  void
287  getVisualMap(pcl::PointCloud<pcl::RGB>::Ptr vMap);
288 
289 protected:
290  /** \brief The internal disparity map. */
291  short int* disp_map_;
292 
293  /** \brief Local aligned copies of the cloud data. */
294  unsigned char* ref_img_;
295  unsigned char* trg_img_;
296 
297  /** \brief Disparity map used for left-right check. */
298  short int* disp_map_trg_;
299 
300  /** \brief Local aligned copies used for pre processing. */
301  unsigned char* pp_ref_img_;
302  unsigned char* pp_trg_img_;
303 
304  /** \brief number of pixels per column of the input stereo pair . */
305  int width_;
306 
307  /** \brief number of pixels per row of the input stereo pair . */
308  int height_;
309 
310  /** \brief Disparity range used for stereo processing. */
312 
313  /** \brief Horizontal displacemente (x offset) used for stereo processing */
314  int x_off_;
315 
316  /** \brief Threshold for the ratio filter, \f$\in [0 100]\f$ */
318 
319  /** \brief Threshold for the peak filter, \f$\in [0 \infty]\f$ */
321 
322  /** \brief toggle for the activation of the pre-processing stage */
324 
325  /** \brief toggle for the activation of the left-right consistency check stage */
327 
328  /** \brief Threshold for the left-right consistency check, typically either 0 or 1 */
330 
331  virtual void
332  preProcessing(unsigned char* img, unsigned char* pp_img) = 0;
333 
334  virtual void
335  imgFlip(unsigned char*& img) = 0;
336 
337  virtual void
338  compute_impl(unsigned char* ref_img, unsigned char* trg_img) = 0;
339 
340  void
341  leftRightCheck();
342 
343  inline short int
344  computeStereoSubpixel(int dbest, int s1, int s2, int s3)
345  {
346  int den = (s1 + s3 - 2 * s2);
347  if (den != 0)
348  return (static_cast<short int>(16 * dbest + (((s1 - s3) * 8) / den)));
349  return (static_cast<short int>(dbest * 16));
350  }
351 
352  inline short int
353  computeStereoSubpixel(int dbest, float s1, float s2, float s3)
354  {
355  float den = (s1 + s3 - 2 * s2);
356  if (den != 0)
357  return (static_cast<short int>(16 * dbest +
358  std::floor(.5 + (((s1 - s3) * 8) / den))));
359  return (static_cast<short int>(dbest * 16));
360  }
361 };
362 
363 /** \brief Stereo Matching abstract class for Grayscale images
364  *
365  * The class implements some functionalities of pcl::StereoMatching specific for
366  * grayscale stereo processing, such as image pre-processing and left
367  *
368  * \author Federico Tombari (federico.tombari@unibo.it)
369  * \ingroup stereo
370  */
372 public:
375 
376  /** \brief stereo processing, it computes a disparity map stored internally by the
377  * class
378  *
379  * \param[in] ref_img reference array of image pixels (left image), has to be
380  * grayscale single channel
381  * \param[in] trg_img target array of image pixels (right image), has to be grayscale
382  * single channel
383  * \param[in] width number of elements per row for both input arrays
384  * \param[in] height number of elements per column for both input arrays
385  */
386  void
387  compute(unsigned char* ref_img,
388  unsigned char* trg_img,
389  int width,
390  int height) override;
391 
392  /** \brief stereo processing, it computes a disparity map stored internally by the
393  * class
394  *
395  * \param[in] ref point cloud of pcl::RGB type containing the pixels of the reference
396  * image (left image) the pcl::RGB triplets are automatically converted to
397  * grayscale upon call of the method
398  * \param[in] trg point cloud of pcl::RGB type containing the pixels of the target
399  * image (right image) the pcl::RGB triplets are automatically converted to
400  * grayscale upon call of the method
401  */
402  void
403  compute(pcl::PointCloud<pcl::RGB>& ref, pcl::PointCloud<pcl::RGB>& trg) override;
404 
405 protected:
406  void
407  compute_impl(unsigned char* ref_img, unsigned char* trg_img) override = 0;
408 
409  void
410  preProcessing(unsigned char* img, unsigned char* pp_img) override;
411 
412  void
413  imgFlip(unsigned char*& img) override;
414 };
415 
416 /** \brief Block based (or fixed window) Stereo Matching class
417  *
418  * This class implements the baseline Block-based - aka Fixed Window - stereo matching
419  * algorithm. The algorithm includes a running box filter so that the computational
420  * complexity is independent of the size of the window ( O(1) wrt. to the size of
421  * window) The algorithm is based on the Sum of Absolute Differences (SAD) matching
422  * function Only works with grayscale (single channel) rectified images
423  *
424  * \author Federico Tombari (federico.tombari@unibo.it)
425  * \ingroup stereo
426  */
427 
429 public:
432 
433  /** \brief setter for the radius of the squared window
434  * \param[in] radius radius of the squared window used to compute the block-based
435  * stereo algorithm the window side is equal to 2*radius + 1
436  */
437  void
438  setRadius(int radius)
439  {
440  radius_ = radius;
441  };
442 
443 private:
444  void
445  compute_impl(unsigned char* ref_img, unsigned char* trg_img) override;
446 
447  int radius_;
448 };
449 
450 /** \brief Adaptive Cost 2-pass Scanline Optimization Stereo Matching class
451  *
452  * This class implements an adaptive-cost stereo matching algorithm based on 2-pass
453  * Scanline Optimization. The algorithm is inspired by the paper: [1] L. Wang et al.,
454  * "High Quality Real-time Stereo using Adaptive Cost Aggregation and Dynamic
455  * Programming", 3DPVT 2006 Cost aggregation is performed using adaptive weigths
456  * computed on a single column as proposed in [1]. Instead of using Dynamic Programming
457  * as in [1], the optimization is performed via 2-pass Scanline Optimization. The
458  * algorithm is based on the Sum of Absolute Differences (SAD) matching function Only
459  * works with grayscale (single channel) rectified images
460  *
461  * \author Federico Tombari (federico.tombari@unibo.it)
462  * \ingroup stereo
463  */
465 public:
467 
469 
470  /** \brief setter for the radius (half length) of the column used for cost aggregation
471  * \param[in] radius radius (half length) of the column used for cost aggregation; the
472  * total column length is equal to 2*radius + 1
473  */
474  void
475  setRadius(int radius)
476  {
477  radius_ = radius;
478  };
479 
480  /** \brief setter for the spatial bandwidth used for cost aggregation based on
481  * adaptive weights
482  * \param[in] gamma_s spatial bandwidth used for cost aggregation based on adaptive
483  * weights
484  */
485  void
486  setGammaS(int gamma_s)
487  {
488  gamma_s_ = gamma_s;
489  };
490 
491  /** \brief setter for the color bandwidth used for cost aggregation based on adaptive
492  * weights
493  * \param[in] gamma_c color bandwidth used for cost aggregation based on
494  * adaptive weights
495  */
496  void
497  setGammaC(int gamma_c)
498  {
499  gamma_c_ = gamma_c;
500  };
501 
502  /** \brief "weak" smoothness penalty used within 2-pass Scanline Optimization
503  * \param[in] smoothness_weak "weak" smoothness penalty cost
504  */
505  void
506  setSmoothWeak(int smoothness_weak)
507  {
508  smoothness_weak_ = smoothness_weak;
509  };
510 
511  /** \brief "strong" smoothness penalty used within 2-pass Scanline Optimization
512  * \param[in] smoothness_strong "strong" smoothness penalty cost
513  */
514  void
515  setSmoothStrong(int smoothness_strong)
516  {
517  smoothness_strong_ = smoothness_strong;
518  };
519 
520 private:
521  void
522  compute_impl(unsigned char* ref_img, unsigned char* trg_img) override;
523 
524  int radius_;
525 
526  // parameters for adaptive weight cost aggregation
527  double gamma_c_;
528  double gamma_s_;
529 
530  // Parameters for 2-pass SO optimization
531  int smoothness_strong_;
532  int smoothness_weak_;
533 };
534 
535 } // namespace pcl
void setMaxDisparity(int max_disp)
setter for number of disparity candidates (disparity range)
shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:415
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.
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
short int doStereoRatioFilter(const T *const acc, short int dbest, T sad_min, int ratio_filter, int maxdisp, int precision=100)
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.
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(const T *const 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_
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)
#define PCL_EXPORTS
Definition: pcl_macros.h:253
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 .