DepthFromStereo.cpp

Go to the documentation of this file.

/*
 * This file is part of ArmarX.
 *
 * Copyright (C) 2011-2016, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
 *
 * ArmarX is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License version 2 as
 * published by the Free Software Foundation.
 *
 * ArmarX is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program. If not, see <http://www.gnu.org/licenses/>.
 *
 * @package
 * @author
 * @date
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
#include "DepthFromStereo.h"
 
// OpenCV
#include <opencv2/opencv.hpp>
 
 
// IVT
#include <Math/Math3d.h>
#include <Image/ByteImage.h>
#include <Image/ImageProcessor.h>
#include <Image/IplImageAdaptor.h>
#include <Image/StereoMatcher.h>
#include <Calibration/StereoCalibration.h>
#include <Calibration/Calibration.h>
#include <Calibration/Rectification.h>
#include <Threading/Threading.h>
 
#include <opencv2/calib3d.hpp>
 
#include <cmath>
#include <limits>
 
 
namespace visionx::depthfromstereo
{
 
    static void doStereoSGBM(int minDisparity, int numDisparities, int nSADWindowSize, int P1, int P2, int disp12MaxDiff,
                             cv::Mat leftInput, cv::Mat rightInput, cv::Mat output)
    {
        int preFilterCap = 0;
        int uniquenessRatio = 0;
        int speckleWindowSize = 0;
        int speckleRange = 0;
#if CV_MAJOR_VERSION == 2
        bool fullDP = true;
        cv::StereoSGBM stereoSGBM(minDisparity, numDisparities, nSADWindowSize, P1, P2, disp12MaxDiff, preFilterCap, uniquenessRatio, speckleWindowSize, speckleRange, fullDP);
        stereoSGBM(leftInput, rightInput, output);
#else
        cv::Ptr<cv::StereoSGBM> stereoSGBM = cv::StereoSGBM::create(minDisparity, numDisparities, nSADWindowSize, P1, P2, disp12MaxDiff,  preFilterCap, uniquenessRatio, speckleWindowSize, speckleRange, cv::StereoSGBM::MODE_SGBM);
        stereoSGBM->compute(leftInput, rightInput, output);
#endif
    }
 
    void GetPointsFromDisparity(const CByteImage* pImageLeftColor, const CByteImage* pImageRightColor, const CByteImage* pImageLeftGrey, const CByteImage* pImageRightGrey,
                                const int nDisparityPointDistance, pcl::PointCloud<pcl::PointXYZRGBA>& aPointsFromDisparity, CByteImage* pDisparityImage,
                                const int imageWidth, const int imageHeight, CStereoCalibration* pStereoCalibration, bool imagesAreUndistorted, const bool smoothDisparities)
    {
        // rectify images
        CByteImage*  pRectifiedImageLeftGrey, *pRectifiedImageRightGrey, *pRectifiedImageLeftGreyHalfsize, *pRectifiedImageRightGreyHalfsize, *pRectifiedImageLeftGreyQuartersize, *pRectifiedImageRightGreyQuartersize;
        IplImage* pRectifiedIplImageLeft, *pRectifiedIplImageRight, *pRectifiedIplImageLeftHalfsize, *pRectifiedIplImageRightHalfsize, *pRectifiedIplImageLeftQuartersize, *pRectifiedIplImageRightQuartersize;
        CByteImage* pRectifiedImageLeftColorGaussFiltered;
        cv::Mat mDispImg, mDispImgHalfsize, mDispImgQuartersize;
        bool bGreyImagesReady = false;
        bool bGreyImagesHalfsizeReady = false;
        bool bGreyImagesQuartersizeReady = false;
        const int nPenaltyDispDiffOneFactor = 8;
        const int nPenaltyDispDiffBiggerOneFactor = 32;
 
        CStereoMatcher* pStereoMatcher = new CStereoMatcher();
        CStereoCalibration* pStereoCalibrationCopy = new CStereoCalibration(*pStereoCalibration);
        pStereoMatcher->InitCameraParameters(pStereoCalibrationCopy, false);
 
        const int nDispMin = pStereoMatcher->GetDisparityEstimate(8000);
        const int nDispMax = pStereoMatcher->GetDisparityEstimate(400);
 
        const int imageOverlapCutoffLeft = 0; // 110
        const int imageOverlapCutoffRight = 0; // 5
        const int imageOverlapCutoffTop = 0; // 5
        const int imageOverlapCutoffBottom = 0; // 5
 
        const bool fillHoles = false;
 
 
        #pragma omp parallel sections
        {
            #pragma omp section
            {
                const CByteImage* ppOriginalImages[2];
                ppOriginalImages[0] = pImageLeftGrey;
                ppOriginalImages[1] = pImageRightGrey;
                pRectifiedImageLeftGrey = new CByteImage(imageWidth, imageHeight, CByteImage::eGrayScale);
                pRectifiedImageRightGrey = new CByteImage(imageWidth, imageHeight, CByteImage::eGrayScale);
                CByteImage* ppRectifiedImages[2];
                ppRectifiedImages[0] = pRectifiedImageLeftGrey;
                ppRectifiedImages[1] = pRectifiedImageRightGrey;
 
                CRectification* pRectification = new CRectification(true, !imagesAreUndistorted);
                pRectification->Init(pStereoCalibration);
                pRectification->Rectify(ppOriginalImages, ppRectifiedImages);
 
                pRectifiedIplImageLeft = IplImageAdaptor::Adapt(pRectifiedImageLeftGrey);
                pRectifiedIplImageRight = IplImageAdaptor::Adapt(pRectifiedImageRightGrey);
                bGreyImagesReady = true;
 
                // half size
                pRectifiedImageLeftGreyHalfsize = new CByteImage(imageWidth / 2, imageHeight / 2, CByteImage::eGrayScale);
                pRectifiedImageRightGreyHalfsize = new CByteImage(imageWidth / 2, imageHeight / 2, CByteImage::eGrayScale);
                ::ImageProcessor::Resize(pRectifiedImageLeftGrey, pRectifiedImageLeftGreyHalfsize);
                ::ImageProcessor::Resize(pRectifiedImageRightGrey, pRectifiedImageRightGreyHalfsize);
 
                pRectifiedIplImageLeftHalfsize = IplImageAdaptor::Adapt(pRectifiedImageLeftGreyHalfsize);
                pRectifiedIplImageRightHalfsize = IplImageAdaptor::Adapt(pRectifiedImageRightGreyHalfsize);
                bGreyImagesHalfsizeReady = true;
 
                // quarter size
                pRectifiedImageLeftGreyQuartersize = new CByteImage(imageWidth / 4, imageHeight / 4, CByteImage::eGrayScale);
                pRectifiedImageRightGreyQuartersize = new CByteImage(imageWidth / 4, imageHeight / 4, CByteImage::eGrayScale);
                ::ImageProcessor::Resize(pRectifiedImageLeftGrey, pRectifiedImageLeftGreyQuartersize);
                ::ImageProcessor::Resize(pRectifiedImageRightGrey, pRectifiedImageRightGreyQuartersize);
 
                pRectifiedIplImageLeftQuartersize = IplImageAdaptor::Adapt(pRectifiedImageLeftGreyQuartersize);
                pRectifiedIplImageRightQuartersize = IplImageAdaptor::Adapt(pRectifiedImageRightGreyQuartersize);
                bGreyImagesQuartersizeReady = true;
 
                delete pRectification;
            }
 
            #pragma omp section
            {
                const CByteImage* ppOriginalImages[2];
                ppOriginalImages[0] = pImageLeftColor;
                ppOriginalImages[1] = pImageRightColor;
                CByteImage* pRectifiedImageLeftColor = new CByteImage(imageWidth, imageHeight, CByteImage::eRGB24);
                CByteImage* pRectifiedImageRightColor = new CByteImage(imageWidth, imageHeight, CByteImage::eRGB24);
                CByteImage* ppRectifiedImages[2];
                ppRectifiedImages[0] = pRectifiedImageLeftColor;
                ppRectifiedImages[1] = pRectifiedImageRightColor;
 
                CRectification* pRectification = new CRectification(true, !imagesAreUndistorted);
                pRectification->Init(pStereoCalibration);
                pRectification->Rectify(ppOriginalImages, ppRectifiedImages);
 
                pRectifiedImageLeftColorGaussFiltered = new CByteImage(imageWidth, imageHeight, CByteImage::eRGB24);
                ::ImageProcessor::GaussianSmooth3x3(pRectifiedImageLeftColor, pRectifiedImageLeftColorGaussFiltered);
 
                delete pRectifiedImageLeftColor;
                delete pRectifiedImageRightColor;
                delete pRectification;
            }
 
 
            // get disparity using semi-global block matching
 
            #pragma omp section
            {
                while (!bGreyImagesReady)
                {
                    Threading::YieldThread();
                }
 
                // StereoSGBM::StereoSGBM(int minDisparity, int numDisparities, int SADWindowSize, int P1=0, int P2=0, int disp12MaxDiff=0,
                //                          int preFilterCap=0, int uniquenessRatio=0, int speckleWindowSize=0, int speckleRange=0, bool fullDP=false)
                //
                //    minDisparity – Minimum possible disparity value. Normally, it is zero but sometimes rectification algorithms can shift images, so this parameter needs to be adjusted accordingly.
                //    numDisparities – Maximum disparity minus minimum disparity. The value is always greater than zero. In the current implementation, this parameter must be divisible by 16.
                //    SADWindowSize – Matched block size. It must be an odd number >=1 . Normally, it should be somewhere in the 3..11 range.
                //    P1 – The first parameter controlling the disparity smoothness. See below.
                //    P2 – The second parameter controlling the disparity smoothness. The larger the values are, the smoother the disparity is. P1 is the penalty on the disparity change by plus or minus 1 between neighbor pixels. P2 is the penalty on the disparity change by more than 1 between neighbor pixels. The algorithm requires P2 > P1 . See stereo_match.cpp sample where some reasonably good P1 and P2 values are shown (like 8*number_of_image_channels*SADWindowSize*SADWindowSize and 32*number_of_image_channels*SADWindowSize*SADWindowSize , respectively).
                //    disp12MaxDiff – Maximum allowed difference (in integer pixel units) in the left-right disparity check. Set it to a non-positive value to disable the check.
                //    preFilterCap – Truncation value for the prefiltered image pixels. The algorithm first computes x-derivative at each pixel and clips its value by [-preFilterCap, preFilterCap] interval. The result values are passed to the Birchfield-Tomasi pixel cost function.
                //    uniquenessRatio – Margin in percentage by which the best (minimum) computed cost function value should “win” the second best value to consider the found match correct. Normally, a value within the 5-15 range is good enough.
                //    speckleWindowSize – Maximum size of smooth disparity regions to consider their noise speckles and invalidate. Set it to 0 to disable speckle filtering. Otherwise, set it somewhere in the 50-200 range.
                //    speckleRange – Maximum disparity variation within each connected component. If you do speckle filtering, set the parameter to a positive value, multiple of 16. Normally, 16 or 32 is good enough.
                //    fullDP – Set it to true to run the full-scale two-pass dynamic programming algorithm. It will consume O(W*H*numDisparities) bytes, which is large for 640x480 stereo and huge for HD-size pictures. By default, it is set to false.
                const cv::Mat mLeftImg = cv::cvarrToMat(pRectifiedIplImageLeft);
                const cv::Mat mRightImg = cv::cvarrToMat(pRectifiedIplImageRight);
                const int nSADWindowSize = 7; // 11
                const int nPenaltyDispDiffOne = nPenaltyDispDiffOneFactor * 3 * nSADWindowSize * nSADWindowSize; // 8
                const int nPenaltyDispDiffBiggerOne = nPenaltyDispDiffBiggerOneFactor * 3 * nSADWindowSize * nSADWindowSize; // 32
 
                doStereoSGBM(1, 8 * 16, nSADWindowSize, nPenaltyDispDiffOne, nPenaltyDispDiffBiggerOne, 4, mLeftImg, mRightImg, mDispImg);
            }
 
 
            #pragma omp section
            {
                while (!bGreyImagesHalfsizeReady)
                {
                    Threading::YieldThread();
                }
 
                const cv::Mat mLeftImg = cv::cvarrToMat(pRectifiedIplImageLeftHalfsize);
                const cv::Mat mRightImg = cv::cvarrToMat(pRectifiedIplImageRightHalfsize);
                const int nSADWindowSize = 7;
                const int nPenaltyDispDiffOne = nPenaltyDispDiffOneFactor * 3 * nSADWindowSize * nSADWindowSize; // 8
                const int nPenaltyDispDiffBiggerOne = nPenaltyDispDiffBiggerOneFactor * 3 * nSADWindowSize * nSADWindowSize; // 32
                doStereoSGBM(1, 4 * 16, nSADWindowSize, nPenaltyDispDiffOne, nPenaltyDispDiffBiggerOne, 4, mLeftImg, mRightImg, mDispImgHalfsize);
            }
 
 
            #pragma omp section
            {
                while (!bGreyImagesQuartersizeReady)
                {
                    Threading::YieldThread();
                }
 
                const cv::Mat mLeftImg = cv::cvarrToMat(pRectifiedIplImageLeftQuartersize);
                const cv::Mat mRightImg  = cv::cvarrToMat(pRectifiedIplImageRightQuartersize);
                const int nSADWindowSize = 7;
                const int nPenaltyDispDiffOne = nPenaltyDispDiffOneFactor * 3 * nSADWindowSize * nSADWindowSize; // 8
                const int nPenaltyDispDiffBiggerOne = nPenaltyDispDiffBiggerOneFactor * 3 * nSADWindowSize * nSADWindowSize; // 32
 
                doStereoSGBM(1, 4 * 16, nSADWindowSize, nPenaltyDispDiffOne, nPenaltyDispDiffBiggerOne, 4, mLeftImg, mRightImg, mDispImgQuartersize);
            }
        }
 
 
        // combine disparities
        float* pDisparities = new float[imageWidth * imageHeight];
        #pragma omp parallel for schedule(static, 80)
        for (int i = 0; i < imageHeight; i++)
        {
            for (int j = 0; j < imageWidth; j++)
            {
                int nDispFullsize = mDispImg.at<short>(i, j) / 16;
                int nDispHalfsize = 2 * mDispImgHalfsize.at<short>(i / 2, j / 2) / 16;
                int nDispQuartersize = 4 * mDispImgQuartersize.at<short>(i / 4, j / 4) / 16;
 
                int nDisp = 0;
                float fDisp = 0;
                int nNumValidDisparities = 0;
 
                if ((nDispFullsize > nDispMin) && (nDispFullsize < nDispMax))
                {
                    nDisp += nDispFullsize;
                    nNumValidDisparities++;
                }
 
                if ((nDispHalfsize > nDispMin) && (nDispHalfsize < nDispMax))
                {
                    nDisp += nDispHalfsize;
                    nNumValidDisparities++;
                }
 
                if ((nDispQuartersize > nDispMin) && (nDispQuartersize < nDispMax))
                {
                    nDisp += nDispQuartersize;
                    nNumValidDisparities++;
                }
 
                if (nNumValidDisparities > 0)
                {
                    fDisp = (float)nDisp / (float)nNumValidDisparities;
                }
 
                pDisparities[i * imageWidth + j] = fDisp;
            }
        }
 
 
        // fill holes
        if (fillHoles)
        {
            std::vector<int> pDisparitiesCopy(imageWidth * imageHeight);
 
            for (int i = 0; i < imageHeight * imageWidth; i++)
            {
                pDisparitiesCopy[i] = pDisparities[i];
            }
 
            const int nMargin = 10;
            const int nHoleFillingAveragingRadius = 5;
 
            for (int i = nMargin; i < imageHeight - nMargin; i++)
            {
                for (int j = nMargin; j < imageWidth - nMargin; j++)
                {
                    if (pDisparitiesCopy[i * imageWidth + j] == 0)
                    {
                        int nSum = 0;
                        int nNumValidDisparities = 0;
 
                        for (int k = -nHoleFillingAveragingRadius; k <= nHoleFillingAveragingRadius; k++)
                        {
                            for (int l = -nHoleFillingAveragingRadius; l <= nHoleFillingAveragingRadius; l++)
                            {
                                if (pDisparitiesCopy[(i + k)*imageWidth + (j + l)] > 0)
                                {
                                    nSum += pDisparitiesCopy[(i + k) * imageWidth + (j + l)];
                                    nNumValidDisparities++;
                                }
                            }
                        }
 
                        if (nNumValidDisparities > 0)
                        {
                            pDisparities[i * imageWidth + j] = nSum / nNumValidDisparities;
                        }
                    }
                }
            }
        }
 
 
 
        // get smoothed disparity
        float* pSmoothedDisparity = pDisparities;
 
        if (smoothDisparities)
        {
            pSmoothedDisparity = new float[imageWidth * imageHeight];
            float* pSmoothedDisparity1 = new float[imageWidth * imageHeight];
            float* pSmoothedDisparity2 = new float[imageWidth * imageHeight];
            float* pSmoothedDisparity3 = new float[imageWidth * imageHeight];
            float* pSmoothedDisparity4 = new float[imageWidth * imageHeight];
 
            #pragma omp parallel sections
            {
                #pragma omp section
                {
                    CalculateSmoothedDisparityImage(pDisparities, pSmoothedDisparity1, imageWidth, imageHeight, 1);
                }
 
                #pragma omp section
                {
                    CalculateSmoothedDisparityImage(pDisparities, pSmoothedDisparity2, imageWidth, imageHeight, 2);
                }
 
                #pragma omp section
                {
                    //CalculateSmoothedDisparityImage(pDisparities, pSmoothedDisparity3, imageWidth, imageHeight, 3);
                }
 
                #pragma omp section
                {
                    //CalculateSmoothedDisparityImage(pDisparities, pSmoothedDisparity4, imageWidth, imageHeight, 4);
                }
            }
 
            for (int i = 0; i < imageWidth * imageHeight; i++)
            {
                pSmoothedDisparity[i] = 0.8f * pDisparities[i] + 0.15f * pSmoothedDisparity1[i] + 0.05f * pSmoothedDisparity2[i];// + 0.0f * pSmoothedDisparity3[i] + 0.0f * pSmoothedDisparity4[i];
            }
 
            delete[] pSmoothedDisparity1;
            delete[] pSmoothedDisparity2;
            delete[] pSmoothedDisparity3;
            delete[] pSmoothedDisparity4;
        }
 
 
        // cut of the edges to avoid noise points
        for (int i = 0; i < imageHeight; i++)
        {
            for (int j = 0; j < imageWidth; j++)
            {
                if (i < imageOverlapCutoffTop || i >= imageHeight - imageOverlapCutoffBottom || j < imageOverlapCutoffLeft || j > imageWidth - imageOverlapCutoffRight)
                {
                    pSmoothedDisparity[i * imageWidth + j] = 0;
                }
            }
        }
 
 
 
        // visualize
        if (pDisparityImage)
        {
            CByteImage* pRectifiedDisparityImage = new CByteImage(imageWidth, imageHeight, CByteImage::eGrayScale);
 
            for (int i = 0; i < imageHeight; i++)
            {
                for (int j = 0; j < imageWidth; j++)
                {
                    int nDisp = pSmoothedDisparity[i * imageWidth + j];
                    pRectifiedDisparityImage->pixels[i * imageWidth + j] = (nDisp > 255) ? 255 : nDisp;
                }
            }
 
            // get unrectified disparity image
            ::ImageProcessor::Zero(pDisparityImage);
            Vec2d vRectPos, vUnRectPos;
            Mat3d mRectificationHomography = pStereoCalibration->rectificationHomographyLeft;
 
            for (int i = 0; i < imageHeight; i++)
            {
                for (int j = 0; j < imageWidth; j++)
                {
                    vRectPos.x = j;
                    vRectPos.y = i;
                    Math2d::ApplyHomography(mRectificationHomography, vRectPos, vUnRectPos);
 
                    if (0 <= vUnRectPos.y && vUnRectPos.y < imageHeight && 0 <= vUnRectPos.x && vUnRectPos.x < imageWidth)
                    {
                        pDisparityImage->pixels[(int)vUnRectPos.y * imageWidth + (int)vUnRectPos.x] = pRectifiedDisparityImage->pixels[i * imageWidth + j];
                    }
                }
            }
 
            FillHolesGray(pDisparityImage, pRectifiedDisparityImage, imageWidth, imageHeight, 1);
            FillHolesGray(pRectifiedDisparityImage, pDisparityImage, imageWidth, imageHeight, 1);
            delete pRectifiedDisparityImage;
        }
 
 
 
        Vec2d vImgPointLeft, vImgPointRight;
        Vec3d vPoint3D;
        const float fDispMin = nDispMin;
        const float fDispMax = nDispMax;
        aPointsFromDisparity.width = ceil((float)imageWidth / (float)nDisparityPointDistance);
        aPointsFromDisparity.height = ceil((float)imageHeight / (float)nDisparityPointDistance);
        aPointsFromDisparity.resize(aPointsFromDisparity.width * aPointsFromDisparity.height);
 
        int index = 0;
 
        for (int i = 0; i < imageHeight; i += nDisparityPointDistance)
        {
            for (int j = 0; j < imageWidth; j += nDisparityPointDistance, index++)
            {
 
                float fDisp = pSmoothedDisparity[i * imageWidth + j];
 
                if ((fDisp > fDispMin) && (fDisp < fDispMax))
                {
                    vImgPointLeft.x = j;
                    vImgPointLeft.y = i;
                    vImgPointRight.x = j - fDisp;
                    vImgPointRight.y = i;
 
                    pStereoCalibration->Calculate3DPoint(vImgPointLeft, vImgPointRight, vPoint3D, true, false);
 
                    // create point descriptor
                    aPointsFromDisparity.points[index].x = vPoint3D.x;
                    aPointsFromDisparity.points[index].y = vPoint3D.y;
                    aPointsFromDisparity.points[index].z = vPoint3D.z;
                    aPointsFromDisparity.points[index].r = pRectifiedImageLeftColorGaussFiltered->pixels[3 * (i * imageWidth + j)];
                    aPointsFromDisparity.points[index].g = pRectifiedImageLeftColorGaussFiltered->pixels[3 * (i * imageWidth + j) + 1];
                    aPointsFromDisparity.points[index].b = pRectifiedImageLeftColorGaussFiltered->pixels[3 * (i * imageWidth + j) + 2];
                    aPointsFromDisparity.points[index].a = 1.0f;
                }
                //                    else if (false)
                //                    {
                //                        aPointsFromDisparity.points[index].x = std::numeric_limits<float>::quiet_NaN();
                //                        aPointsFromDisparity.points[index].y = std::numeric_limits<float>::quiet_NaN();
                //                        aPointsFromDisparity.points[index].z = std::numeric_limits<float>::quiet_NaN();
                //                        aPointsFromDisparity.points[index].r = std::numeric_limits<float>::quiet_NaN();
                //                        aPointsFromDisparity.points[index].g = std::numeric_limits<float>::quiet_NaN();
                //                        aPointsFromDisparity.points[index].b = std::numeric_limits<float>::quiet_NaN();
                //                        aPointsFromDisparity.points[index].a = std::numeric_limits<float>::quiet_NaN();
                //                    }
                else
                {
                    aPointsFromDisparity.points[index].x = 0;
                    aPointsFromDisparity.points[index].y = 0;
                    aPointsFromDisparity.points[index].z = 0;
                    aPointsFromDisparity.points[index].r = 0;
                    aPointsFromDisparity.points[index].g = 0;
                    aPointsFromDisparity.points[index].b = 0;
                    aPointsFromDisparity.points[index].a = 0;
                }
            }
        }
 
        delete pRectifiedImageLeftGrey;
        delete pRectifiedImageRightGrey;
        delete pRectifiedImageLeftGreyHalfsize;
        delete pRectifiedImageRightGreyHalfsize;
        delete pRectifiedImageLeftGreyQuartersize;
        delete pRectifiedImageRightGreyQuartersize;
        delete pRectifiedImageLeftColorGaussFiltered;
        delete pStereoMatcher;
        delete pStereoCalibrationCopy;
        cvReleaseImageHeader(&pRectifiedIplImageLeft);
        cvReleaseImageHeader(&pRectifiedIplImageRight);
 
        if (pSmoothedDisparity != pDisparities)
        {
            delete[] pSmoothedDisparity;
        }
 
        delete[] pDisparities;
    }
 
 
 
 
    void FillHolesGray(const CByteImage* pInputImage, CByteImage* pOutputImage, const int imageWidth, const int imageHeight, const int nRadius)
    {
 
        for (int i = 0; i < imageWidth * imageHeight; i++)
        {
            pOutputImage->pixels[i] = pInputImage->pixels[i];
        }
 
        for (int i = nRadius; i < imageHeight - nRadius; i++)
        {
            for (int j = nRadius; j < imageWidth - nRadius; j++)
            {
                int nIndex = i * imageWidth + j;
 
                if (pInputImage->pixels[nIndex] == 0)
                {
                    int nSum = 0;
                    int nNumPixels = 0;
 
                    for (int l = -nRadius; l <= nRadius; l++)
                    {
                        for (int k = -nRadius; k <= nRadius; k++)
                        {
                            int nTempIndex = nIndex + l * imageWidth + k;
 
                            if (pInputImage->pixels[nTempIndex] != 0)
                            {
                                nSum += pInputImage->pixels[nTempIndex];
                                nNumPixels++;
                            }
                        }
                    }
 
                    if (nNumPixels > 0)
                    {
                        pOutputImage->pixels[nIndex] = nSum / nNumPixels;
                    }
                }
            }
        }
    }
 
 
 
 
    void CalculateSmoothedDisparityImage(float* pInputDisparity, float* pSmoothedDisparity, const int imageWidth, const int imageHeight, const int nRadius)
    {
 
        for (int i = 0; i < imageWidth * imageHeight; i++)
        {
            pSmoothedDisparity[i] = pInputDisparity[i];
        }
 
        for (int i = nRadius; i < imageHeight - nRadius; i++)
        {
            for (int j = nRadius; j < imageWidth - nRadius; j++)
            {
                int nIndex = i * imageWidth + j;
 
                if (pInputDisparity[nIndex] != 0)
                {
                    float fSum = 0;
                    int nNumPixels = 0;
 
                    for (int l = -nRadius; l <= nRadius; l++)
                    {
                        for (int k = -nRadius; k <= nRadius; k++)
                        {
                            int nTempIndex = nIndex + l * imageWidth + k;
 
                            if (pInputDisparity[nTempIndex] != 0)
                            {
                                fSum += pInputDisparity[nTempIndex];
                                nNumPixels++;
                            }
                        }
                    }
 
                    pSmoothedDisparity[nIndex] = fSum / (float)nNumPixels;
                }
            }
        }
    }
 
}