Point Cloud Library (PCL) 1.12.0
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
45namespace pcl {
46template <class T>
47short int
48doStereoRatioFilter(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
66template <class T>
67inline short int
68doStereoPeakFilter(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 */
111public:
113
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
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
289protected:
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 . */
306
307 /** \brief number of pixels per row of the input stereo pair . */
309
310 /** \brief Disparity range used for stereo processing. */
312
313 /** \brief Horizontal displacemente (x offset) used for stereo processing */
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 */
372public:
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
405protected:
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
429public:
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
443private:
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 */
465public:
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
520private:
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
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
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
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