修改OpenCV SGBM代码实现per-pixel searching range constrain

新增StereoSGBM实现

需要确认我们正在修改的头文件是处于OpenCV的源码路径，而不是OpenCV的安装路径。OpenCV在安装后， 会复制头文件到安装路径，不推荐直接修改安装路径下的文件。

增加per-pixel searching range的描述

在calib3d.hpp定义一个新类， 这个类用于描述disparity的检索index。OpenCV的SGBM的实现要求disparity的数量必须是16的整倍数。所以有一些流程是用来确保这一要求的。

class CV_EXPORTS_W PPSR
{
public:
    PPSR(int h, int w, int gdmin, int gdmax)
            : height(h), width(w), gDMin(gdmin), gDMax(gdmax), gD( gdmax - gdmin + 1 )
    {
        ppsrMin = new int[height * width];
        ppsrMax = new int[height * width];
    }

    ~PPSR()
    {
        if ( NULL != ppsrMax )
        {
            delete [] ppsrMax; ppsrMax = NULL;
        }

        if ( NULL != ppsrMin )
        {
            delete [] ppsrMin; ppsrMin = NULL;
        }
    }

    void copy_pixel_disp_range(const int* fromStart,
                               const int* fromEnd,
                               const int base = 16)
    {
        const int n = height * width;

        int s, e, r; // Start, end, reminder.

        for ( int i = 0; i < n; ++i )
        {
            s = fromStart[i] - gDMin;
            r = s % base;

            ppsrMin[i] = std::max(s - r, 0);

            e = fromEnd[i] - gDMin;

            e = std::min( ( e / base + 1 ) * base - 1, gD - 1 );

            ppsrMax[i] = e <= 0 ? base : e;
        }
    }

    void initialize_pixel_disp_range(void)
    {
        const int n    = height * width;
        const int maxD = gD - 1;

        for ( int i = 0; i < n; ++i )
        {
            ppsrMin[i] = 0;
            ppsrMax[i] = maxD;
        }
    }

    int start(int row, int col) const { return ppsrMin[ row*width + col ]; }
    int start_max(int row, int col, int m) const { return std::max(m, ppsrMin[ row*width + col ]); }
    int end(int row, int col) const { return ppsrMax[ row*width + col ]; }
    int end_min(int row, int col, int m) const { return std::min( m, ppsrMax[ row*width + col ] ); }
    int end_1(int row, int col) const { return ppsrMax[ row*width + col ] + 1; }
    int end_1_min(int row, int col, int m) const { return std::min( m, ppsrMax[ row*width + col ] + 1 ); }
    int num(int row, int col) const { return ppsrMax[ row*width + col ] - ppsrMin[ row*width + col ] + 1; }

public:
    // Per-pixel searching range capability.
    const int height;
    const int width;
    const int gDMin; // Global minimum disparity value.
    const int gDMax; // Global maximum disparity value.
    const int gD;    // Global number of disparities.
    int* ppsrMin;
    int* ppsrMax;
};

修改calib3d.hpp内StereoSGBM类的定义，增加public 成员变量

PPSR* ppsr;

增加MODE标签

修改calib3d.hpp内， StereoSGBM类中枚举类型的定义。

enum
{
	MODE_SGBM = 0,
	MODE_HH   = 1,
	MODE_SGBM_3WAY = 2,
	MODE_HH4  = 3,
	MODE_PPSR = 4 // Newly added mode.
};

增加新函数

复制一份computeDisparitySGBM()函数到一个头文件ppsr.hpp中，函数名称修改为computeDisparitySGBM_PPSR。
在stereosgbm.cpp内include该文件。 修改computeDisparitySGBM_PPSR()的参数表，使之包含per-pixel的searching range （即ppsr参数）.

static void computeDisparitySGBM_PPSR( const Mat& img1, const Mat& img2,
                                 Mat& disp1, const StereoSGBMParams& params,
                                 Mat& buffer, const PPSR* ppsr = NULL )

修改StereoSGBMImpl类中compute()函数的定义， 增加对computeDisparitySGBM_PPSR()的调用。

void compute( InputArray leftarr, InputArray rightarr, OutputArray disparr ) CV_OVERRIDE
    {
        CV_INSTRUMENT_REGION();

        Mat left = leftarr.getMat(), right = rightarr.getMat();
        CV_Assert( left.size() == right.size() && left.type() == right.type() &&
                   left.depth() == CV_8U );

        disparr.create( left.size(), CV_16S );
        Mat disp = disparr.getMat();

        if(params.mode==MODE_SGBM_3WAY)
            computeDisparity3WaySGBM( left, right, disp, params, buffers, num_stripes );
        else if(params.mode==MODE_HH4)
            computeDisparitySGBM_HH4( left, right, disp, params, buffer );
        else if ( params.mode == MODE_PPSR ) // Newly added branch.
            computeDisparitySGBM_PPSR( left, right, disp, params, buffer, ppsr );
        else
            computeDisparitySGBM( left, right, disp, params, buffer );

        medianBlur(disp, disp, 3);

        if( params.speckleWindowSize > 0 )
            filterSpeckles(disp, (params.minDisparity - 1)*StereoMatcher::DISP_SCALE, params.speckleWindowSize,
                           StereoMatcher::DISP_SCALE*params.speckleRange, buffer);
    }

编译OpenCV以确保当前代码结构正确。

computeDisparitySGBM_PPSR() 函数实现

仅修改computeDisparitySGBM_PPSR()函数中关于SGM部分的实现，并且 不修改 cost aggregation的部分，仅修改搜索最优匹配的部分。将类似于

for ( d = 0; d < D; d++ )

替换为

for( d = ppsr->start(y,x); d < ppsr->end_1(y,x); d++ )

将类似于

if( d < D )

替换为

if( d < ppsr->end_1(y,x) )

将

if( 0 < d && d < D-1 )

替换为

if( ppsr->start(y,x) < d && d < ppsr->end(y,x) )

对于受宏CV_SIMD128控制的代码部分，将

for( d = 0; d < D; d+= 8 )

替换为

for( d = ppsr->start(y,x); d < ppsr->end_1(y,x); d+= 8 )

对于使用SIMD优化的代码部分，在对d进行循环前，还需进行类似于下述的修改

const short d8Shift = static_cast<short>( ppsr->start(y,x) );
v_int16x8 _d8Shift = v_setall_s16(d8Shift);
_bestDisp += _d8Shift;
_d8 += _d8Shift;

使_bsetDisp保存的索引值有正确的偏置（ppsr->start(y,x)）

注意 : 在stereosgbm.cpp源码中，if( pass == npasses ) 后的部分，对与图像列的循环是从x=width1-1开始的，此时x并不是图像的原始列编号，所以per-pixel searching range的起始和终止值应为ppsr->start(y,x + minX1)和ppsr->end_1(y,x + minX1).

简略测试

首先需要测试当每个pixel的searching range都相同，并且与全局searching range一致时，输出的结果应当和没有进行per-pixel searching range设定时是一致的。设计了一个简单的函数调用我们刚刚修改过的computeDisparitySGBM_PPSR() 进行stereo reconstruction。

void stereo_reconstruction(const cv::Mat& img0, const cv::Mat& img1,
        const int minD, const int maxD, cv::Mat& disp)
{
    // Check if the minD and maxD are appropriate.
    if ( minD >= maxD )
    {
        std::stringstream ss;
        ss << "minD (" << minD << ") or maxD (" << maxD << ") is not right.";
        throw std::runtime_error(ss.str());
    }

    if ( 0 != ( maxD - minD + 1 ) % 16 )
    {
        std::stringstream ss;
        ss << "The number of disparities (" << maxD - minD + 1 << ") is not integer times of 16. "
           << "Pleas check the values of minD (" << minD << ") and maxD (" << maxD << ").";
        throw std::runtime_error(ss.str());
    }

    // Create PPSR object.
    cv::PPSR ppsr(img0.rows, img0.cols, minD, maxD);
    ppsr.initialize_pixel_disp_range();

    // Create OpenCV SGBM object.
    cv::Ptr<cv::StereoSGBM> matcher = cv::StereoSGBM::create(
            minD, maxD - minD + 1, /* min and num of disparities */
            3, /* block size */
            1800, 7200, /* P1, P2 */
            1, /* disp12MaxDiff */
            31, 10, /* preFilterCap and uniqueness ratio */
            1000, 2, /* speckle window size and range */
            cv::StereoSGBM::MODE_PPSR
            );

	// Set per-pxiel searching range.
    matcher->ppsr = &ppsr;

    // Compute the disparity.
    matcher->compute(img0, img1, disp);
}

注意在编译前需要将改好的OpenCV install一次，以确保编译和链接阶段能够找到正确的文件。

减少编译内容

在对OpenCV源码（主要是calib3d模块）进行更改后，每次进行进行编译都会连带编译所有依赖于calib3d模块的其他模块。这是比较耗时的，可以通过修改cmake配置来进行尽可能小规模的编译。修改时推荐使用cmake-gui。在ubuntu上安装cmake-gui可使用apt命令sudo apt install cmake-qt-gui。

首先进入编译目录，通过终端打开cmake-gui时使用OpenCV源码所在目录作为参数。启动后在Seach栏内搜索“BUILD_LIST”。配置BUILD_LIST的值为core,imgproc,highgui,calib3d,cudev。然后点击configure和generate按钮即可完成配置。

完成配置后通过make指令重新编译OpenCV时可以发现，编译内容明显减少了。

若在ubuntu18上 ，cmake-gui配置结束后，OpenCV编译时报出找不到Eigen的错误，那么可能需要通过cmake的命令行重新配置编译，在原来命令的基础上增加-DBUILD_LIST=core,imgproc,highgui,calib3d,cudev。

后记

测试通过后，接下来是如何获得可信的per-pixel searching range，这在后面的工作中将会实现。