stereo_matching.h
/*
* Software License Agreement (BSD License)
*
* Point Cloud Library (PCL) - www.pointclouds.org
* Copyright (c) 2012-, Open Perception, Inc.
*
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the copyright holder(s) nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
*/
#ifndef PCL_STEREO_H_
#define PCL_STEREO_H_
#include <pcl/conversions.h>
#include <pcl/point_types.h>
namespace pcl
{
/** \brief Stereo Matching abstract class
*
* The class performs stereo matching on a rectified stereo pair
* Includes the following functionalities:
* * preprocessing of the image pair, to improve robustness against photometric distortions
* (wrt. to a spatially constant additive photometric factor)
* * postprocessing: filtering of wrong disparities via Peak Filter (eliminating ambiguities due to low-textured regions)
* and Ratio Filter (eliminating generic matching ambiguities, similar to that present in OpenCV Block Matching Stereo)
* * postprocessing: Left-Right consistency check (eliminates wrong disparities at the cost of twice the stereo matching
* computation)
* * postprocessing: subpixel refinement of computed disparities, to reduce the depth quantization effect
* * postprocessing: smoothing of the disparity map via median filter
* * after stereo matching a PCL point cloud can be computed, given the stereo intrinsic (focal, principal point
* coordinates) and extrinsic (baseline) calibration parameters
*
* \author Federico Tombari (federico.tombari@unibo.it)
* \ingroup stereo
*/
class PCL_EXPORTS StereoMatching
{
public:
StereoMatching(void);
virtual ~StereoMatching(void);
/** \brief setter for number of disparity candidates (disparity range)
*
* \param[in] max_disp number of disparity candidates (disparity range); has to be > 0
*/
void
setMaxDisparity (int max_disp)
{
max_disp_ = max_disp;
};
/** \brief setter for horizontal offset, i.e. number of pixels to shift the disparity range over the target image
*
* \param[in] x_off horizontal offset value; has to be >= 0
*/
void
setXOffset (int x_off)
{
x_off_ = x_off;
};
/** \brief setter for the value of the ratio filter
*
* \param[in] ratio_filter value of the ratio filter; it is a number in the range [0, 100]
* (0: no filtering action; 100: all disparities are filtered)
*/
void
setRatioFilter (int ratio_filter)
{
ratio_filter_ = ratio_filter;
};
/** \brief setter for the value of the peak filter
*
* \param[in] peak_filter value of the peak filter; it is a number in the range [0, inf]
* (0: no filtering action)
*/
void
setPeakFilter (int peak_filter)
{
peak_filter_ = peak_filter;
};
/** \brief setter for the pre processing step
*
* \param[in] is_pre_proc setting the boolean to true activates the pre-processing step for both stereo images
*/
void
setPreProcessing (bool is_pre_proc)
{
is_pre_proc_ = is_pre_proc;
};
/** \brief setter for the left-right consistency check stage, that eliminates inconsistent/wrong disparity
* values from the disparity map at approx. twice the processing cost of the selected stereo algorithm
*
* \param[in] is_lr_check setting the boolean to true activates the left-right consistency check
*/
void
setLeftRightCheck (bool is_lr_check)
{
is_lr_check_ = is_lr_check;
};
/** \brief setter for the left-right consistency check threshold
*
* \param[in] lr_check_th sets the value of the left-right consistency check threshold
* only has some influence if the left-right check is active
* typically has either the value 0 ("strong" consistency check, more points being filtered) or 1 ("weak"
* consistency check, less points being filtered)
*/
void
setLeftRightCheckThreshold (int lr_check_th)
{
lr_check_th_ = lr_check_th;
};
/** \brief stereo processing, it computes a disparity map stored internally by the class
*
* \param[in] ref_img reference array of image pixels (left image)
* \param[in] trg_img target array of image pixels (right image)
* \param[in] width number of elements per row for both input arrays
* \param[in] height number of elements per column for both input arrays
*/
virtual void
compute (unsigned char* ref_img, unsigned char* trg_img, int width, int height) = 0;
/** \brief stereo processing, it computes a disparity map stored internally by the class
*
* \param[in] ref point cloud of pcl::RGB type containing the pixels of the reference image (left image)
* \param[in] trg point cloud of pcl::RGB type containing the pixels of the target image (right image)
*/
virtual void
compute (pcl::PointCloud<pcl::RGB> &ref, pcl::PointCloud<pcl::RGB> &trg) = 0;
/** \brief median filter applied on the previously computed disparity map
* Note: the "compute" method must have been previously called at least once in order for this function
* to have any effect
* \param[in] radius radius of the squared window used to compute the median filter; the window side is
* equal to 2*radius + 1
*/
void
medianFilter (int radius);
/** \brief computation of the 3D point cloud from the previously computed disparity map without color information
* Note: the "compute" method must have been previously called at least once in order for this function
* to have any effect
* \param[in] u_c horizontal coordinate of the principal point (calibration parameter)
* \param[in] v_c vertical coordinate of the principal point (calibration parameter)
* \param[in] focal focal length in pixels (calibration parameter)
* \param[in] baseline distance between the two cameras (calibration parameter); the measure unit used to
* specify this parameter will be the same as the 3D points in the output point cloud
* \param[out] cloud output 3D point cloud; it is organized and non-dense, with NaNs where 3D points are invalid
*/
virtual bool
getPointCloud (float u_c, float v_c, float focal, float baseline, pcl::PointCloud<pcl::PointXYZ>::Ptr cloud);
/** \brief computation of the 3D point cloud from the previously computed disparity map including color information
* Note: the "compute" method must have been previously called at least once in order for this function
* to have any effect
* \param[in] u_c horizontal coordinate of the principal point (calibration parameter)
* \param[in] v_c vertical coordinate of the principal point (calibration parameter)
* \param[in] focal focal length in pixels (calibration parameter)
* \param[in] baseline distance between the two cameras (calibration parameter); the measure unit used to
* specify this parameter will be the same as the 3D points in the output point cloud
* \param[out] cloud output 3D point cloud; it is organized and non-dense, with NaNs where 3D points are invalid
* \param[in] texture 3D cloud (same size of the output cloud) used to associate to each 3D point of the
* output cloud a color triplet
*/
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);
/** \brief computation of a pcl::RGB cloud with scaled disparity values
* it can be used to display a rescaled version of the disparity map by means of the pcl::ImageViewer
* invalid disparity values are shown in green
* Note: the "compute" method must have been previously called at least once in order for this function
* to have any effect
* \param[out] vMap output cloud
*/
void
getVisualMap (pcl::PointCloud<pcl::RGB>::Ptr vMap);
protected:
/** \brief The internal disparity map. */
short int *disp_map_;
/** \brief Local aligned copies of the cloud data. */
unsigned char* ref_img_;
unsigned char* trg_img_;
/** \brief Disparity map used for left-right check. */
short int *disp_map_trg_;
/** \brief Local aligned copies used for pre processing. */
unsigned char* pp_ref_img_;
unsigned char* pp_trg_img_;
/** \brief number of pixels per column of the input stereo pair . */
int width_;
/** \brief number of pixels per row of the input stereo pair . */
int height_;
/** \brief Disparity range used for stereo processing. */
int max_disp_;
/** \brief Horizontal displacemente (x offset) used for stereo processing */
int x_off_;
/** \brief Threshold for the ratio filter, \f$\in [0 100]\f$ */
int ratio_filter_;
/** \brief Threshold for the peak filter, \f$\in [0 \infty]\f$ */
int peak_filter_;
/** \brief toggle for the activation of the pre-processing stage */
bool is_pre_proc_;
/** \brief toggle for the activation of the left-right consistency check stage */
bool is_lr_check_;
/** \brief Threshold for the left-right consistency check, typically either 0 or 1 */
int lr_check_th_;
virtual void
preProcessing (unsigned char *img, unsigned char *pp_img) = 0;
virtual void
imgFlip (unsigned char * & img) = 0;
virtual void
compute_impl (unsigned char* ref_img, unsigned char* trg_img) = 0;
void
leftRightCheck ();
inline short int
computeStereoSubpixel (int dbest, int s1, int s2, int s3)
{
int den = (s1+s3-2*s2);
if (den != 0)
return (static_cast<short int> (16*dbest + (((s1 - s3)*8) / den)));
else
return (static_cast<short int> (dbest*16));
}
inline short int
computeStereoSubpixel (int dbest, float s1, float s2, float s3)
{
float den = (s1+s3-2*s2);
if (den != 0)
return (static_cast<short int> (16*dbest + floor(.5 + (((s1 - s3)*8) / den))));
else
return (static_cast<short int> (dbest*16));
}
inline short int
doStereoRatioFilter (int *acc, short int dbest, int sad_min, int ratio_filter, int maxdisp, int precision = 100)
{
int sad_second_min = std::numeric_limits<int>::max ();
for (int d = 0; d < dbest - 1; d++)
if (acc[d] < sad_second_min)
sad_second_min = acc[d];
for (int d = dbest + 2; d < maxdisp; d++)
if (acc[d] < sad_second_min)
sad_second_min = acc[d];
if (sad_min * precision > (precision - ratio_filter) * sad_second_min)
return (-2);
else
return (dbest);
}
inline short int
doStereoRatioFilter (float *acc, short int dbest, float sad_min, int ratio_filter, int maxdisp, int precision = 100)
{
float sad_second_min = std::numeric_limits<float>::max ();
for (int d = 0; d < dbest - 1; d++)
if (acc[d] < sad_second_min)
sad_second_min = acc[d];
for (int d = dbest + 2; d < maxdisp; d++)
if (acc[d] < sad_second_min)
sad_second_min = acc[d];
if (sad_min * static_cast<float> (precision) > static_cast<float> (precision - ratio_filter) * sad_second_min)
return (-2);
else
return (dbest);
}
inline short int
doStereoPeakFilter (int *acc, short int dbest, int peak_filter, int maxdisp)
{
int da = (dbest>1) ? ( acc[dbest-2] - acc[dbest] ) : (acc[dbest+2] - acc[dbest]);
int db = (dbest<maxdisp-2) ? (acc[dbest+2] - acc[dbest]) : (acc[dbest-2] - acc[dbest]);
if (da + db < peak_filter)
return (-4);
else
return (dbest);
}
inline short int
doStereoPeakFilter (float *acc, short int dbest, int peak_filter, int maxdisp)
{
float da = (dbest>1) ? ( acc[dbest-2] - acc[dbest] ) : (acc[dbest+2] - acc[dbest]);
float db = (dbest<maxdisp-2) ? (acc[dbest+2] - acc[dbest]) : (acc[dbest-2] - acc[dbest]);
if (da + db < peak_filter)
return (-4);
else
return (dbest);
}
};
/** \brief Stereo Matching abstract class for Grayscale images
*
* The class implements some functionalities of pcl::StereoMatching specific for grayscale stereo processing,
* such as image pre-processing and left
*
* \author Federico Tombari (federico.tombari@unibo.it)
* \ingroup stereo
*/
class PCL_EXPORTS GrayStereoMatching : public StereoMatching
{
public:
GrayStereoMatching (void);
virtual ~GrayStereoMatching (void);
/** \brief stereo processing, it computes a disparity map stored internally by the class
*
* \param[in] ref_img reference array of image pixels (left image), has to be grayscale single channel
* \param[in] trg_img target array of image pixels (right image), has to be grayscale single channel
* \param[in] width number of elements per row for both input arrays
* \param[in] height number of elements per column for both input arrays
*/
virtual void
compute (unsigned char* ref_img, unsigned char* trg_img, int width, int height);
/** \brief stereo processing, it computes a disparity map stored internally by the class
*
* \param[in] ref point cloud of pcl::RGB type containing the pixels of the reference image (left image)
* the pcl::RGB triplets are automatically converted to grayscale upon call of the method
* \param[in] trg point cloud of pcl::RGB type containing the pixels of the target image (right image)
* the pcl::RGB triplets are automatically converted to grayscale upon call of the method
*/
virtual void
compute (pcl::PointCloud<pcl::RGB> &ref, pcl::PointCloud<pcl::RGB> &trg);
protected:
virtual void
compute_impl (unsigned char* ref_img, unsigned char* trg_img) = 0;
virtual void
preProcessing (unsigned char *img, unsigned char *pp_img);
virtual void
imgFlip (unsigned char * & img);
};
/** \brief Block based (or fixed window) Stereo Matching class
*
* This class implements the baseline Block-based - aka Fixed Window - stereo matching algorithm.
* The algorithm includes a running box filter so that the computational complexity is independent of
* the size of the window ( O(1) wrt. to the size of window)
* The algorithm is based on the Sum of Absolute Differences (SAD) matching function
* Only works with grayscale (single channel) rectified images
*
* \author Federico Tombari (federico.tombari@unibo.it)
* \ingroup stereo
*/
class PCL_EXPORTS BlockBasedStereoMatching : public GrayStereoMatching
{
public:
BlockBasedStereoMatching (void);
virtual ~BlockBasedStereoMatching (void)
{
};
/** \brief setter for the radius of the squared window
* \param[in] radius radius of the squared window used to compute the block-based stereo algorithm
* the window side is equal to 2*radius + 1
*/
void
setRadius (int radius)
{
radius_ = radius;
};
private:
virtual void
compute_impl (unsigned char* ref_img, unsigned char* trg_img);
int radius_;
};
/** \brief Adaptive Cost 2-pass Scanline Optimization Stereo Matching class
*
* This class implements an adaptive-cost stereo matching algorithm based on 2-pass Scanline Optimization.
* The algorithm is inspired by the paper:
* [1] L. Wang et al., "High Quality Real-time Stereo using Adaptive Cost Aggregation and Dynamic Programming", 3DPVT 2006
* Cost aggregation is performed using adaptive weigths computed on a single column as proposed in [1].
* Instead of using Dynamic Programming as in [1], the optimization is performed via 2-pass Scanline Optimization.
* The algorithm is based on the Sum of Absolute Differences (SAD) matching function
* Only works with grayscale (single channel) rectified images
*
* \author Federico Tombari (federico.tombari@unibo.it)
* \ingroup stereo
*/
class PCL_EXPORTS AdaptiveCostSOStereoMatching : public GrayStereoMatching
{
public:
AdaptiveCostSOStereoMatching (void);
virtual ~AdaptiveCostSOStereoMatching (void)
{
};
/** \brief setter for the radius (half length) of the column used for cost aggregation
* \param[in] radius radius (half length) of the column used for cost aggregation; the total column length
* is equal to 2*radius + 1
*/
void
setRadius (int radius)
{
radius_ = radius;
};
/** \brief setter for the spatial bandwith used for cost aggregation based on adaptive weights
* \param[in] gamma_s spatial bandwith used for cost aggregation based on adaptive weights
*/
void
setGammaS (int gamma_s)
{
gamma_s_ = gamma_s;
};
/** \brief setter for the color bandwith used for cost aggregation based on adaptive weights
* \param[in] gamma_c color bandwith used for cost aggregation based on adaptive weights
*/
void
setGammaC (int gamma_c)
{
gamma_c_ = gamma_c;
};
/** \brief "weak" smoothness penalty used within 2-pass Scanline Optimization
* \param[in] smoothness_weak "weak" smoothness penalty cost
*/
void
setSmoothWeak (int smoothness_weak)
{
smoothness_weak_ = smoothness_weak;
};
/** \brief "strong" smoothness penalty used within 2-pass Scanline Optimization
* \param[in] smoothness_strong "strong" smoothness penalty cost
*/
void
setSmoothStrong (int smoothness_strong)
{
smoothness_strong_ = smoothness_strong;
};
private:
virtual void
compute_impl (unsigned char* ref_img, unsigned char* trg_img);
int radius_;
//parameters for adaptive weight cost aggregation
double gamma_c_;
double gamma_s_;
//Parameters for 2-pass SO optimization
int smoothness_strong_;
int smoothness_weak_;
};
}
#endif
pcl::StereoMatching::setMaxDisparity
void setMaxDisparity(int max_disp)
setter for number of disparity candidates (disparity range)
Definition: stereo_matching.h:75
pcl::StereoMatching::computeStereoSubpixel
short int computeStereoSubpixel(int dbest, int s1, int s2, int s3)
Definition: stereo_matching.h:266
pcl::AdaptiveCostSOStereoMatching::setSmoothWeak
void setSmoothWeak(int smoothness_weak)
"weak" smoothness penalty used within 2-pass Scanline Optimization
Definition: stereo_matching.h:483
pcl::AdaptiveCostSOStereoMatching::setGammaC
void setGammaC(int gamma_c)
setter for the color bandwith used for cost aggregation based on adaptive weights ...
Definition: stereo_matching.h:474
pcl::StereoMatching::disp_map_
short int * disp_map_
The internal disparity map.
Definition: stereo_matching.h:213
pcl::StereoMatching::setLeftRightCheck
void setLeftRightCheck(bool is_lr_check)
setter for the left-right consistency check stage, that eliminates inconsistent/wrong disparity value...
Definition: stereo_matching.h:128
pcl::BlockBasedStereoMatching
Block based (or fixed window) Stereo Matching class.
Definition: stereo_matching.h:405
pcl::StereoMatching::doStereoRatioFilter
short int doStereoRatioFilter(float *acc, short int dbest, float sad_min, int ratio_filter, int maxdisp, int precision=100)
Definition: stereo_matching.h:305
pcl::StereoMatching::setLeftRightCheckThreshold
void setLeftRightCheckThreshold(int lr_check_th)
setter for the left-right consistency check threshold
Definition: stereo_matching.h:141
pcl::GrayStereoMatching
Stereo Matching abstract class for Grayscale images.
Definition: stereo_matching.h:357
pcl
Definition: convolution.h:45
pcl::StereoMatching::ratio_filter_
int ratio_filter_
Threshold for the ratio filter, .
Definition: stereo_matching.h:239
pcl::StereoMatching::disp_map_trg_
short int * disp_map_trg_
Disparity map used for left-right check.
Definition: stereo_matching.h:220
pcl::StereoMatching::lr_check_th_
int lr_check_th_
Threshold for the left-right consistency check, typically either 0 or 1.
Definition: stereo_matching.h:251
pcl::StereoMatching::setPreProcessing
void setPreProcessing(bool is_pre_proc)
setter for the pre processing step
Definition: stereo_matching.h:117
pcl::AdaptiveCostSOStereoMatching::setGammaS
void setGammaS(int gamma_s)
setter for the spatial bandwith used for cost aggregation based on adaptive weights ...
Definition: stereo_matching.h:465
pcl::StereoMatching::setRatioFilter
void setRatioFilter(int ratio_filter)
setter for the value of the ratio filter
Definition: stereo_matching.h:96
pcl::AdaptiveCostSOStereoMatching::setSmoothStrong
void setSmoothStrong(int smoothness_strong)
"strong" smoothness penalty used within 2-pass Scanline Optimization
Definition: stereo_matching.h:492
pcl::StereoMatching::setXOffset
void setXOffset(int x_off)
setter for horizontal offset, i.e.
Definition: stereo_matching.h:85
pcl::StereoMatching
Stereo Matching abstract class.
Definition: stereo_matching.h:63
pcl::PointCloud::Ptr
boost::shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:428
pcl::StereoMatching::doStereoRatioFilter
short int doStereoRatioFilter(int *acc, short int dbest, int sad_min, int ratio_filter, int maxdisp, int precision=100)
Definition: stereo_matching.h:286
pcl::BlockBasedStereoMatching::~BlockBasedStereoMatching
virtual ~BlockBasedStereoMatching(void)
Definition: stereo_matching.h:409
pcl::AdaptiveCostSOStereoMatching::~AdaptiveCostSOStereoMatching
virtual ~AdaptiveCostSOStereoMatching(void)
Definition: stereo_matching.h:447
point_types.h
Defines all the PCL implemented PointT point type structures.
pcl::StereoMatching::is_pre_proc_
bool is_pre_proc_
toggle for the activation of the pre-processing stage
Definition: stereo_matching.h:245
pcl::StereoMatching::setPeakFilter
void setPeakFilter(int peak_filter)
setter for the value of the peak filter
Definition: stereo_matching.h:107
pcl::StereoMatching::is_lr_check_
bool is_lr_check_
toggle for the activation of the left-right consistency check stage
Definition: stereo_matching.h:248
pcl::AdaptiveCostSOStereoMatching
Adaptive Cost 2-pass Scanline Optimization Stereo Matching class.
Definition: stereo_matching.h:442
pcl::StereoMatching::x_off_
int x_off_
Horizontal displacemente (x offset) used for stereo processing.
Definition: stereo_matching.h:236
pcl::PointCloud< pcl::RGB >
pcl::StereoMatching::doStereoPeakFilter
short int doStereoPeakFilter(float *acc, short int dbest, int peak_filter, int maxdisp)
Definition: stereo_matching.h:336
pcl::StereoMatching::max_disp_
int max_disp_
Disparity range used for stereo processing.
Definition: stereo_matching.h:233
pcl::AdaptiveCostSOStereoMatching::setRadius
void setRadius(int radius)
setter for the radius (half length) of the column used for cost aggregation
Definition: stereo_matching.h:456
pcl::StereoMatching::pp_trg_img_
unsigned char * pp_trg_img_
Definition: stereo_matching.h:224
pcl::StereoMatching::doStereoPeakFilter
short int doStereoPeakFilter(int *acc, short int dbest, int peak_filter, int maxdisp)
Definition: stereo_matching.h:324
pcl::StereoMatching::trg_img_
unsigned char * trg_img_
Definition: stereo_matching.h:217
pcl::BlockBasedStereoMatching::setRadius
void setRadius(int radius)
setter for the radius of the squared window
Definition: stereo_matching.h:418
pcl::StereoMatching::height_
int height_
number of pixels per row of the input stereo pair .
Definition: stereo_matching.h:230
pcl::StereoMatching::pp_ref_img_
unsigned char * pp_ref_img_
Local aligned copies used for pre processing.
Definition: stereo_matching.h:223
pcl::StereoMatching::computeStereoSubpixel
short int computeStereoSubpixel(int dbest, float s1, float s2, float s3)
Definition: stereo_matching.h:276
pcl::StereoMatching::peak_filter_
int peak_filter_
Threshold for the peak filter, .
Definition: stereo_matching.h:242
pcl::StereoMatching::ref_img_
unsigned char * ref_img_
Local aligned copies of the cloud data.
Definition: stereo_matching.h:216
pcl::StereoMatching::width_
int width_
number of pixels per column of the input stereo pair .
Definition: stereo_matching.h:227