Point Cloud Library (PCL)  1.14.1-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 <pcl/point_cloud.h> // for PointCloud
41 #include <pcl/point_types.h>
42 
43 #include <algorithm>
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:
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
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
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
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:
431  ~BlockBasedStereoMatching() override = default;
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 weights
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 
468  ~AdaptiveCostSOStereoMatching() override = default;
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
Adaptive Cost 2-pass Scanline Optimization Stereo Matching class.
void setGammaC(int gamma_c)
setter for the color bandwidth used for cost aggregation based on adaptive weights
void setRadius(int radius)
setter for the radius (half length) of the column used for cost aggregation
void setGammaS(int gamma_s)
setter for the spatial bandwidth used for cost aggregation based on adaptive weights
void setSmoothStrong(int smoothness_strong)
"strong" smoothness penalty used within 2-pass Scanline Optimization
~AdaptiveCostSOStereoMatching() override=default
void setSmoothWeak(int smoothness_weak)
"weak" smoothness penalty used within 2-pass Scanline Optimization
Block based (or fixed window) Stereo Matching class.
void setRadius(int radius)
setter for the radius of the squared window
~BlockBasedStereoMatching() override=default
Stereo Matching abstract class for Grayscale images.
void compute(unsigned char *ref_img, unsigned char *trg_img, int width, int height) override
stereo processing, it computes a disparity map stored internally by the class
void preProcessing(unsigned char *img, unsigned char *pp_img) override
~GrayStereoMatching() override
void compute_impl(unsigned char *ref_img, unsigned char *trg_img) override=0
void imgFlip(unsigned char *&img) override
void compute(pcl::PointCloud< pcl::RGB > &ref, pcl::PointCloud< pcl::RGB > &trg) override
stereo processing, it computes a disparity map stored internally by the class
shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:413
Stereo Matching abstract class.
void setPeakFilter(int peak_filter)
setter for the value of the peak filter
short int computeStereoSubpixel(int dbest, float s1, float s2, float s3)
virtual void compute_impl(unsigned char *ref_img, unsigned char *trg_img)=0
void setMaxDisparity(int max_disp)
setter for number of disparity candidates (disparity range)
virtual ~StereoMatching()
unsigned char * pp_ref_img_
Local aligned copies used for pre processing.
int peak_filter_
Threshold for the peak filter, .
void setXOffset(int x_off)
setter for horizontal offset, i.e.
virtual void imgFlip(unsigned char *&img)=0
bool is_lr_check_
toggle for the activation of the left-right consistency check stage
void setLeftRightCheck(bool is_lr_check)
setter for the left-right consistency check stage, that eliminates inconsistent/wrong disparity value...
short int * disp_map_trg_
Disparity map used for left-right check.
unsigned char * ref_img_
Local aligned copies of the cloud data.
int lr_check_th_
Threshold for the left-right consistency check, typically either 0 or 1.
int width_
number of pixels per column of the input stereo pair .
virtual void compute(unsigned char *ref_img, unsigned char *trg_img, int width, int height)=0
stereo processing, it computes a disparity map stored internally by the class
unsigned char * pp_trg_img_
void setLeftRightCheckThreshold(int lr_check_th)
setter for the left-right consistency check threshold
int ratio_filter_
Threshold for the ratio filter, .
void getVisualMap(pcl::PointCloud< pcl::RGB >::Ptr vMap)
computation of a pcl::RGB cloud with scaled disparity values it can be used to display a rescaled ver...
int max_disp_
Disparity range used for stereo processing.
virtual void preProcessing(unsigned char *img, unsigned char *pp_img)=0
virtual void compute(pcl::PointCloud< pcl::RGB > &ref, pcl::PointCloud< pcl::RGB > &trg)=0
stereo processing, it computes a disparity map stored internally by the class
void setRatioFilter(int ratio_filter)
setter for the value of the ratio filter
void medianFilter(int radius)
median filter applied on the previously computed disparity map
int height_
number of pixels per row of the input stereo pair .
int x_off_
Horizontal displacemente (x offset) used for stereo processing.
void setPreProcessing(bool is_pre_proc)
setter for the pre processing step
short int computeStereoSubpixel(int dbest, int s1, int s2, int s3)
virtual bool getPointCloud(float u_c, float v_c, float focal, float baseline, pcl::PointCloud< pcl::PointXYZ >::Ptr cloud)
computation of the 3D point cloud from the previously computed disparity map without color informatio...
short int * disp_map_
The internal disparity map.
virtual bool getPointCloud(float u_c, float v_c, float focal, float baseline, pcl::PointCloud< pcl::PointXYZRGB >::Ptr cloud, pcl::PointCloud< pcl::RGB >::Ptr texture)
computation of the 3D point cloud from the previously computed disparity map including color informat...
bool is_pre_proc_
toggle for the activation of the pre-processing stage
unsigned char * trg_img_
Defines all the PCL implemented PointT point type structures.
short int doStereoRatioFilter(const T *const acc, short int dbest, T sad_min, int ratio_filter, int maxdisp, int precision=100)
short int doStereoPeakFilter(const T *const acc, short int dbest, int peak_filter, int maxdisp)
#define PCL_EXPORTS
Definition: pcl_macros.h:323