FeatureCalculation.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 "FeatureCalculation.h"
 
#include "MSERCalculation.h"
#include "OLPTools.h"
 
 
// IVT
#include <opencv2/calib3d.hpp>
 
#include <Calibration/Calibration.h>
#include <Calibration/Rectification.h>
#include <Calibration/StereoCalibration.h>
#include <Features/SIFTFeatures/SIFTFeatureCalculator.h>
#include <Features/SIFTFeatures/SIFTFeatureEntry.h>
#include <Image/ByteImage.h>
#include <Image/ImageProcessor.h>
#include <Image/IplImageAdaptor.h>
#include <Image/StereoMatcher.h>
#include <Math/Math3d.h>
#include <Threading/Threading.h>
 
 
// OpenCV
#include <opencv2/opencv.hpp>
 
// OpenMP
#include <omp.h>
 
// system
#include <sys/time.h>
 
#include <ArmarXCore/core/logging/Logging.h>
 
CFeatureCalculation::CFeatureCalculation()
{
    // create SIFTFeatureCalculator
    m_pSIFTFeatureCalculator = new CSIFTFeatureCalculator();
    // create HarrisSIFTFeatureCalculator
    //m_pHarrisSIFTFeatureCalculator = new CHarrisSIFTFeatureCalculator(0.0002f, 3, 1000); // 0.01f, 3, 500
    //CDynamicArray* pResultList = new CDynamicArray(100);
 
    m_pInterestPoints = new Vec2d[m_nMaxNumInterestPoints];
}
 
CFeatureCalculation::~CFeatureCalculation()
{
    //delete m_pSIFTFeatureCalculator;
    delete[] m_pInterestPoints;
}
 
void
CFeatureCalculation::GetAllFeaturePoints(const CByteImage* pImageLeftColor,
                                         const CByteImage* pImageRightColor,
                                         const CByteImage* pImageLeftGrey,
                                         const CByteImage* pImageRightGrey,
                                         const int nDisparityPointDistance,
                                         CStereoCalibration* pStereoCalibration,
                                         CSIFTFeatureArray& aAllSIFTPoints,
                                         std::vector<CMSERDescriptor3D*>& aAllMSERs,
                                         std::vector<CHypothesisPoint*>& aPointsFromDepthImage,
                                         CByteImage* pDisparityImage,
                                         std::vector<Vec3d>* pAll3DPoints)
{
    timeval tStart, tEnd;
    gettimeofday(&tStart, 0);
    omp_set_nested(true);
 
#pragma omp parallel sections
    {
 
        //**************************************************************************************************************
        // calculate Harris interest points and find stereo correspondences
        //**************************************************************************************************************
 
 
#pragma omp section
        {
            const int nNumFeatures =
                ImageProcessor::CalculateHarrisInterestPoints(pImageLeftGrey,
                                                              m_pInterestPoints,
                                                              m_nMaxNumInterestPoints,
                                                              OLP_HARRIS_POINT_QUALITY,
                                                              OLP_HARRIS_POINT_DISTANCE); // 0.001
            //ARMARX_VERBOSE_S <<  << " --- Number of Features: %d ---\n", nNumFeatures);
 
            // search corresponding features in right image
            // create StereoMatcher
            CStereoMatcher* stereoMatcher = new CStereoMatcher();
            stereoMatcher->InitCameraParameters(pStereoCalibration, false);
            const int nDispMin = stereoMatcher->GetDisparityEstimate(4 * OLP_MAX_OBJECT_DISTANCE);
            const int nDispMax =
                stereoMatcher->GetDisparityEstimate(0.2f * OLP_MAX_OBJECT_DISTANCE);
 
#pragma omp parallel
            {
                CDynamicArray afSIFTDescriptors(4);
 
#pragma omp for schedule(static, 32)
 
                for (int i = 0; i < nNumFeatures; i++)
                {
                    if (m_pInterestPoints[i].x > OLP_MIN_X_VALUE_SIFT_POINTS)
                    {
                        Vec2d vCorrespondingPointRight;
                        Vec3d vPoint3D;
 
                        // img l, img r, px, py, size of correlation window, min disparity, max disparity,
                        // corresponding point 2d, 3d point, correlation threshold, images are undistorted
                        int nMatchingResult = stereoMatcher->Match(pImageLeftGrey,
                                                                   pImageRightGrey,
                                                                   (int)m_pInterestPoints[i].x,
                                                                   (int)m_pInterestPoints[i].y,
                                                                   10,
                                                                   nDispMin,
                                                                   nDispMax,
                                                                   vCorrespondingPointRight,
                                                                   vPoint3D,
                                                                   0.7f,
                                                                   true);
 
                        if (nMatchingResult >= 0)
                        {
                            if (vPoint3D.z < OLP_MAX_OBJECT_DISTANCE)
                            {
                                m_pSIFTFeatureCalculator->CreateSIFTDescriptors(
                                    pImageLeftGrey,
                                    &afSIFTDescriptors,
                                    m_pInterestPoints[i].x,
                                    m_pInterestPoints[i].y,
                                    1.0f,
                                    false,
                                    false);
 
                                if (afSIFTDescriptors.GetSize() > 0)
                                {
#pragma omp critical
                                    {
                                        aAllSIFTPoints.AddElement(
                                            (CSIFTFeatureEntry*)((CSIFTFeatureEntry*)
                                                                     afSIFTDescriptors[0])
                                                ->Clone());
                                        aAllSIFTPoints[aAllSIFTPoints.GetSize() - 1]->point3d =
                                            vPoint3D;
                                    }
                                    CSIFTFeatureEntry* pFeature =
                                        (CSIFTFeatureEntry*)afSIFTDescriptors[0];
                                    afSIFTDescriptors[0]->bDelete = false;
                                    afSIFTDescriptors.Clear();
                                    delete pFeature;
                                }
                            }
                        }
                    }
                }
            }
 
            delete stereoMatcher;
        }
 
 
        //**************************************************************************************************************
        // find MSER regions
        //**************************************************************************************************************
 
 
#pragma omp section
        {
#ifdef OLP_USE_MSERS
            CMSERCalculation::FindMSERs3D(pImageLeftColor,
                                          pImageRightColor,
                                          pStereoCalibration,
                                          OLP_TOLERANCE_MODIFICATOR,
                                          aAllMSERs);
#endif
 
            //gettimeofday(&tEnd, 0);
            //tTimeDiff = (1000*tEnd.tv_sec+tEnd.tv_usec/1000) - (1000*tStart.tv_sec+tStart.tv_usec/1000);
            //ARMARX_VERBOSE_S <<  << "Time for stereo correspondences: %d ms\n", tTimeDiff);
 
 
            //ARMARX_VERBOSE_S <<  << "z distances:\n");
            //for (int i=0; i<vFeaturePoints3d.GetSize(); i++)
            //  ARMARX_VERBOSE_S <<  << "%d  ", (int)vFeaturePoints3d[i].z);
        }
 
 
        //**************************************************************************************************************
        // get 3-d points from a disparity map
        //**************************************************************************************************************
 
#pragma omp section
        {
#ifdef OLP_USE_DEPTH_MAP
 
            //timeval tStart, tEnd;
            //gettimeofday(&tStart, 0);
 
            GetPointsFromDisparity(pImageLeftColor,
                                   pImageRightColor,
                                   pImageLeftGrey,
                                   pImageRightGrey,
                                   pStereoCalibration,
                                   nDisparityPointDistance,
                                   aPointsFromDepthImage,
                                   pDisparityImage,
                                   pAll3DPoints);
 
            //ARMARX_VERBOSE_S <<  << "\nGot %d points from the disparity map\n\n", (int)aPointsFromDisparity.size());
 
            //gettimeofday(&tEnd, 0);
            //long tTimeDiff = (1000*tEnd.tv_sec+tEnd.tv_usec/1000) - (1000*tStart.tv_sec+tStart.tv_usec/1000);
            //ARMARX_VERBOSE_S <<  << "Time for disparity: %ld ms\n", tTimeDiff);
 
#endif
        }
    }
 
    gettimeofday(&tEnd, 0);
    long tTimeDiff =
        (1000 * tEnd.tv_sec + tEnd.tv_usec / 1000) - (1000 * tStart.tv_sec + tStart.tv_usec / 1000);
    ARMARX_VERBOSE_S << "Time for GetAllFeaturePoints(): " << tTimeDiff << " ms";
}
 
 
#ifdef OLP_USE_DEPTH_MAP
 
void
CFeatureCalculation::GetAllFeaturePoints(const CByteImage* pImageLeftColor,
                                         const CByteImage* pImageLeftGrey,
                                         const pcl::PointCloud<pcl::PointXYZRGBA>::Ptr pointcloud,
                                         const int nDisparityPointDistance,
                                         CSIFTFeatureArray& aAllSIFTPoints,
                                         std::vector<CMSERDescriptor3D*>& aAllMSERs,
                                         std::vector<CHypothesisPoint*>& aPointsFromDepthImage,
                                         const CCalibration* calibration)
{
    timeval tStart, tEnd;
    gettimeofday(&tStart, 0);
    omp_set_nested(true);
 
#pragma omp parallel sections
    {
 
        //**************************************************************************************************************
        // calculate Harris interest points and find stereo correspondences
        //**************************************************************************************************************
 
 
#pragma omp section
        {
            const int nNumFeatures =
                ImageProcessor::CalculateHarrisInterestPoints(pImageLeftGrey,
                                                              m_pInterestPoints,
                                                              m_nMaxNumInterestPoints,
                                                              OLP_HARRIS_POINT_QUALITY,
                                                              OLP_HARRIS_POINT_DISTANCE); // 0.001
            //ARMARX_VERBOSE_S <<  << " --- Number of Features: %d ---\n", nNumFeatures);
 
#pragma omp parallel
            {
                CDynamicArray afSIFTDescriptors(4);
 
#pragma omp for schedule(static, 32)
                for (int i = 0; i < nNumFeatures; i++)
                {
                    if (m_pInterestPoints[i].x > OLP_MIN_X_VALUE_SIFT_POINTS)
                    {
                        Vec3d point3D = GetCorresponding3DPoint(m_pInterestPoints[i], pointcloud);
 
                        if (point3D.z > 0)
                        {
                            if (point3D.z < OLP_MAX_OBJECT_DISTANCE)
                            {
                                m_pSIFTFeatureCalculator->CreateSIFTDescriptors(
                                    pImageLeftGrey,
                                    &afSIFTDescriptors,
                                    m_pInterestPoints[i].x,
                                    m_pInterestPoints[i].y,
                                    1.0f,
                                    false,
                                    false);
 
                                if (afSIFTDescriptors.GetSize() > 0)
                                {
#pragma omp critical
                                    {
                                        aAllSIFTPoints.AddElement(
                                            (CSIFTFeatureEntry*)((CSIFTFeatureEntry*)
                                                                     afSIFTDescriptors[0])
                                                ->Clone());
                                        aAllSIFTPoints[aAllSIFTPoints.GetSize() - 1]->point3d.x =
                                            point3D.x;
                                        aAllSIFTPoints[aAllSIFTPoints.GetSize() - 1]->point3d.y =
                                            point3D.y;
                                        aAllSIFTPoints[aAllSIFTPoints.GetSize() - 1]->point3d.z =
                                            point3D.z;
                                    }
                                    CSIFTFeatureEntry* pFeature =
                                        (CSIFTFeatureEntry*)afSIFTDescriptors[0];
                                    afSIFTDescriptors[0]->bDelete = false;
                                    afSIFTDescriptors.Clear();
                                    delete pFeature;
                                }
                            }
                        }
                    }
                }
            }
        }
 
 
        //**************************************************************************************************************
        // find MSER regions
        //**************************************************************************************************************
 
 
#pragma omp section
        {
#ifdef OLP_USE_MSERS
 
            CByteImage* pHSVImageLeft = new CByteImage(pImageLeftColor);
            ImageProcessor::CalculateHSVImage(pImageLeftColor, pHSVImageLeft);
 
            std::vector<CMSERDescriptor*> aMSERDescriptors2D;
            CMSERCalculation::FindMSERs2D(pImageLeftColor, pHSVImageLeft, aMSERDescriptors2D);
 
            for (size_t i = 0; i < aMSERDescriptors2D.size(); i++)
            {
                CMSERDescriptor* descriptor2D = aMSERDescriptors2D.at(i);
                CMSERDescriptor3D* descriptor3D = new CMSERDescriptor3D();
                descriptor3D->pRegionLeft = descriptor2D;
 
                const Vec2d mean2D = descriptor2D->vMean;
                descriptor3D->vPosition = GetCorresponding3DPoint(mean2D, pointcloud);
 
                Vec2d vSigmaPoint2D, vTemp;
 
                Math2d::MulVecScalar(
                    descriptor2D->vEigenvector1, -sqrtf(descriptor2D->fEigenvalue1), vTemp);
                Math2d::AddVecVec(mean2D, vTemp, vSigmaPoint2D);
                descriptor3D->vSigmaPoint1a = GetCorresponding3DPoint(vSigmaPoint2D, pointcloud);
 
                Math2d::MulVecScalar(
                    descriptor2D->vEigenvector1, sqrtf(descriptor2D->fEigenvalue1), vTemp);
                Math2d::AddVecVec(mean2D, vTemp, vSigmaPoint2D);
                descriptor3D->vSigmaPoint1b = GetCorresponding3DPoint(vSigmaPoint2D, pointcloud);
 
                Math2d::MulVecScalar(
                    descriptor2D->vEigenvector2, -sqrtf(descriptor2D->fEigenvalue2), vTemp);
                Math2d::AddVecVec(mean2D, vTemp, vSigmaPoint2D);
                descriptor3D->vSigmaPoint2a = GetCorresponding3DPoint(vSigmaPoint2D, pointcloud);
 
                Math2d::MulVecScalar(
                    descriptor2D->vEigenvector2, sqrtf(descriptor2D->fEigenvalue2), vTemp);
                Math2d::AddVecVec(mean2D, vTemp, vSigmaPoint2D);
                descriptor3D->vSigmaPoint2b = GetCorresponding3DPoint(vSigmaPoint2D, pointcloud);
 
                aAllMSERs.push_back(descriptor3D);
            }
 
#endif
        }
 
 
        //**************************************************************************************************************
        // get 3D points from the depth map
        //**************************************************************************************************************
 
#pragma omp section
        {
            const float fIntensityFactor = 1.0f / 255.0f;
            const size_t width = pointcloud->width;
            const size_t height = pointcloud->height;
 
            CByteImage* leftImageGaussFiltered = new CByteImage(pImageLeftColor);
            ::ImageProcessor::GaussianSmooth3x3(pImageLeftColor, leftImageGaussFiltered);
 
            for (size_t i = 0; i < height; i += nDisparityPointDistance)
            {
                for (size_t j = 0; j < width; j += nDisparityPointDistance)
                {
                    const pcl::PointXYZRGBA point3D = pointcloud->at(i * width + j);
 
                    if (point3D.z > 0)
                    {
                        if (point3D.z < OLP_MAX_OBJECT_DISTANCE)
                        {
                            // create point descriptor
                            CHypothesisPoint* pNewPoint = new CHypothesisPoint();
                            pNewPoint->ePointType = CHypothesisPoint::eDepthMapPoint;
                            Vec3d vPoint3D = {point3D.x, point3D.y, point3D.z};
                            Math3d::SetVec(pNewPoint->vPosition, vPoint3D);
                            Math3d::SetVec(pNewPoint->vOldPosition, vPoint3D);
                            pNewPoint->pMSERDescriptor = NULL;
                            pNewPoint->pFeatureDescriptors = new CSIFTFeatureArray();
                            pNewPoint->fMembershipProbability = 0;
 
                            Vec2d vPoint2D;
                            calibration->CameraToImageCoordinates(vPoint3D, vPoint2D, false);
                            int u = (int)(vPoint2D.x + 0.5f);
                            int v = (int)(vPoint2D.y + 0.5f);
                            u = (u < 0) ? 0
                                        : ((u > leftImageGaussFiltered->width - 1)
                                               ? leftImageGaussFiltered->width - 1
                                               : u);
                            v = (v < 0) ? 0
                                        : ((v > leftImageGaussFiltered->height - 1)
                                               ? leftImageGaussFiltered->height - 1
                                               : v);
                            float r, g, b;
                            const float weight = 1.0f;
                            r = weight * leftImageGaussFiltered
                                             ->pixels[3 * (v * leftImageGaussFiltered->width + u)] +
                                (1.0f - weight) * point3D.r;
                            g = weight *
                                    leftImageGaussFiltered
                                        ->pixels[3 * (v * leftImageGaussFiltered->width + u) + 1] +
                                (1.0f - weight) * point3D.g;
                            b = weight *
                                    leftImageGaussFiltered
                                        ->pixels[3 * (v * leftImageGaussFiltered->width + u) + 2] +
                                (1.0f - weight) * point3D.b;
 
                            const float fIntensity = (r + g + b) / 3.0f;
                            const float normalizationFactor = 1.0f / (fIntensity + 3.0f);
                            pNewPoint->fIntensity = fIntensity * fIntensityFactor;
                            pNewPoint->fColorR = normalizationFactor * (r + 1);
                            pNewPoint->fColorG = normalizationFactor * (g + 1);
                            pNewPoint->fColorB = normalizationFactor * (b + 1);
 
                            aPointsFromDepthImage.push_back(pNewPoint);
                        }
                    }
                }
            }
 
            delete leftImageGaussFiltered;
        }
    }
 
    gettimeofday(&tEnd, 0);
    long tTimeDiff =
        (1000 * tEnd.tv_sec + tEnd.tv_usec / 1000) - (1000 * tStart.tv_sec + tStart.tv_usec / 1000);
    ARMARX_VERBOSE_S << "Time for GetAllFeaturePoints(): " << tTimeDiff << " ms";
}
 
#endif
 
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
CFeatureCalculation::GetPointsFromDisparity(const CByteImage* pImageLeftColor,
                                            const CByteImage* pImageRightColor,
                                            const CByteImage* pImageLeftGrey,
                                            const CByteImage* pImageRightGrey,
                                            CStereoCalibration* pStereoCalibration,
                                            const int nDisparityPointDistance,
                                            std::vector<CHypothesisPoint*>& aPointsFromDisparity,
                                            CByteImage* pDisparityImage,
                                            std::vector<Vec3d>* pAll3DPoints)
{
    // 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;
 
#pragma omp parallel sections
    {
#pragma omp section
        {
            const CByteImage* ppOriginalImages[2];
            ppOriginalImages[0] = pImageLeftGrey;
            ppOriginalImages[1] = pImageRightGrey;
            pRectifiedImageLeftGrey =
                new CByteImage(OLP_IMG_WIDTH, OLP_IMG_HEIGHT, CByteImage::eGrayScale);
            pRectifiedImageRightGrey =
                new CByteImage(OLP_IMG_WIDTH, OLP_IMG_HEIGHT, CByteImage::eGrayScale);
            CByteImage* ppRectifiedImages[2];
            ppRectifiedImages[0] = pRectifiedImageLeftGrey;
            ppRectifiedImages[1] = pRectifiedImageRightGrey;
 
            CRectification* pRectification = new CRectification(true, false);
            pRectification->Init(pStereoCalibration);
            pRectification->Rectify(ppOriginalImages, ppRectifiedImages);
 
            pRectifiedIplImageLeft = IplImageAdaptor::Adapt(pRectifiedImageLeftGrey);
            pRectifiedIplImageRight = IplImageAdaptor::Adapt(pRectifiedImageRightGrey);
            bGreyImagesReady = true;
 
            // half size
            pRectifiedImageLeftGreyHalfsize =
                new CByteImage(OLP_IMG_WIDTH / 2, OLP_IMG_HEIGHT / 2, CByteImage::eGrayScale);
            pRectifiedImageRightGreyHalfsize =
                new CByteImage(OLP_IMG_WIDTH / 2, OLP_IMG_HEIGHT / 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(OLP_IMG_WIDTH / 4, OLP_IMG_HEIGHT / 4, CByteImage::eGrayScale);
            pRectifiedImageRightGreyQuartersize =
                new CByteImage(OLP_IMG_WIDTH / 4, OLP_IMG_HEIGHT / 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(OLP_IMG_WIDTH, OLP_IMG_HEIGHT, CByteImage::eRGB24);
            CByteImage* pRectifiedImageRightColor =
                new CByteImage(OLP_IMG_WIDTH, OLP_IMG_HEIGHT, CByteImage::eRGB24);
            CByteImage* ppRectifiedImages[2];
            ppRectifiedImages[0] = pRectifiedImageLeftColor;
            ppRectifiedImages[1] = pRectifiedImageRightColor;
 
            CRectification* pRectification = new CRectification(true, false);
            pRectification->Init(pStereoCalibration);
            pRectification->Rectify(ppOriginalImages, ppRectifiedImages);
 
            pRectifiedImageLeftColorGaussFiltered =
                new CByteImage(OLP_IMG_WIDTH, OLP_IMG_HEIGHT, 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 = 8 * 3 * nSADWindowSize * nSADWindowSize; // 400
            const int nPenaltyDispDiffBiggerOne = 32 * 3 * nSADWindowSize * nSADWindowSize; // 600
 
            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 = 8 * 3 * nSADWindowSize * nSADWindowSize; // 400
            const int nPenaltyDispDiffBiggerOne = 32 * 3 * nSADWindowSize * nSADWindowSize; // 600
 
            doStereoSGBM(1,
                         4 * 16,
                         nSADWindowSize,
                         nPenaltyDispDiffOne,
                         nPenaltyDispDiffBiggerOne,
                         4,
                         mLeftImg,
                         mRightImg,
                         mDispImg);
        }
 
 
#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 = 8 * 3 * nSADWindowSize * nSADWindowSize; // 400
            const int nPenaltyDispDiffBiggerOne = 32 * 3 * nSADWindowSize * nSADWindowSize; // 600
 
            doStereoSGBM(1,
                         4 * 16,
                         nSADWindowSize,
                         nPenaltyDispDiffOne,
                         nPenaltyDispDiffBiggerOne,
                         4,
                         mLeftImg,
                         mRightImg,
                         mDispImg);
        }
    }
 
 
    CStereoMatcher* pStereoMatcher = new CStereoMatcher();
    CStereoCalibration* pStereoCalibrationCopy = new CStereoCalibration(*pStereoCalibration);
    pStereoMatcher->InitCameraParameters(pStereoCalibrationCopy, false);
    const int nDispMin = pStereoMatcher->GetDisparityEstimate(4000);
    const int nDispMax = pStereoMatcher->GetDisparityEstimate(200);
    Vec2d vImgPointLeft, vImgPointRight;
    Vec3d vPoint3D;
    const float fIntensityFactor = 1.0f / (3 * 255);
 
 
    // combine disparities
    float pDisparities[OLP_IMG_WIDTH * OLP_IMG_HEIGHT];
#pragma omp parallel for schedule(static, 80)
    for (int i = 0; i < OLP_IMG_HEIGHT; i++)
    {
        for (int j = 0; j < OLP_IMG_WIDTH; 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 * OLP_IMG_WIDTH + j] = fDisp;
        }
    }
 
 
    // fill holes
    if (false)
    {
        int pDisparitiesCopy[OLP_IMG_WIDTH * OLP_IMG_HEIGHT];
 
        for (int i = 0; i < OLP_IMG_HEIGHT * OLP_IMG_WIDTH; i++)
        {
            pDisparitiesCopy[i] = pDisparities[i];
        }
 
        const int nMargin = 10;
        const int nHoleFillingAveragingRadius = 5;
 
        for (int i = nMargin; i < OLP_IMG_HEIGHT - nMargin; i++)
        {
            for (int j = nMargin; j < OLP_IMG_WIDTH - nMargin; j++)
            {
                if (pDisparitiesCopy[i * OLP_IMG_WIDTH + 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) * OLP_IMG_WIDTH + (j + l)] > 0)
                            {
                                nSum += pDisparitiesCopy[(i + k) * OLP_IMG_WIDTH + (j + l)];
                                nNumValidDisparities++;
                            }
                        }
                    }
 
                    if (nNumValidDisparities > 0)
                    {
                        pDisparities[i * OLP_IMG_WIDTH + j] = nSum / nNumValidDisparities;
                    }
                }
            }
        }
    }
 
 
    // visualize
    CByteImage* pRectifiedDisparityImage =
        new CByteImage(OLP_IMG_WIDTH, OLP_IMG_HEIGHT, CByteImage::eGrayScale);
 
    for (int i = 0; i < OLP_IMG_HEIGHT; i++)
    {
        for (int j = 0; j < OLP_IMG_WIDTH; j++)
        {
            int nDisp = pDisparities[i * OLP_IMG_WIDTH + j];
            pRectifiedDisparityImage->pixels[i * OLP_IMG_WIDTH + j] = (nDisp > 255) ? 255 : nDisp;
        }
    }
 
    // get unrectified disparity image
    ImageProcessor::Zero(pDisparityImage);
    Vec2d vRectPos, vUnRectPos;
    Mat3d mRectificationHomography = pStereoCalibration->rectificationHomographyLeft;
 
    for (int i = 0; i < OLP_IMG_HEIGHT; i++)
    {
        for (int j = 0; j < OLP_IMG_WIDTH; j++)
        {
            vRectPos.x = j;
            vRectPos.y = i;
            Math2d::ApplyHomography(mRectificationHomography, vRectPos, vUnRectPos);
 
            if (0 <= vUnRectPos.y && vUnRectPos.y < OLP_IMG_HEIGHT && 0 <= vUnRectPos.x &&
                vUnRectPos.x < OLP_IMG_WIDTH)
            {
                pDisparityImage->pixels[(int)vUnRectPos.y * OLP_IMG_WIDTH + (int)vUnRectPos.x] =
                    pRectifiedDisparityImage->pixels[i * OLP_IMG_WIDTH + j];
            }
        }
    }
 
    COLPTools::FillHolesGray(pDisparityImage, pRectifiedDisparityImage, 1);
    COLPTools::FillHolesGray(pRectifiedDisparityImage, pDisparityImage, 1);
    delete pRectifiedDisparityImage;
 
 
    // get smoothed disparity
    float* pSmoothedDisparity = new float[OLP_IMG_WIDTH * OLP_IMG_HEIGHT];
    ;
 
    if (true)
    {
        float* pSmoothedDisparity1 = new float[OLP_IMG_WIDTH * OLP_IMG_HEIGHT];
        float* pSmoothedDisparity2 = new float[OLP_IMG_WIDTH * OLP_IMG_HEIGHT];
        float* pSmoothedDisparity3 = new float[OLP_IMG_WIDTH * OLP_IMG_HEIGHT];
        float* pSmoothedDisparity4 = new float[OLP_IMG_WIDTH * OLP_IMG_HEIGHT];
#pragma omp parallel sections
        {
#pragma omp section
            {
                CalculateSmoothedDisparityImage(pDisparities, pSmoothedDisparity1, 0);
            }
 
#pragma omp section
            {
                CalculateSmoothedDisparityImage(pDisparities, pSmoothedDisparity2, 1);
            }
 
#pragma omp section
            {
                CalculateSmoothedDisparityImage(pDisparities, pSmoothedDisparity3, 2);
            }
 
#pragma omp section
            {
                CalculateSmoothedDisparityImage(pDisparities, pSmoothedDisparity4, 4);
            }
        }
 
        for (int i = 0; i < OLP_IMG_WIDTH * OLP_IMG_HEIGHT; i++)
        {
            pSmoothedDisparity[i] = 0.25f * (pSmoothedDisparity1[i] + pSmoothedDisparity2[i] +
                                             pSmoothedDisparity3[i] + pSmoothedDisparity4[i]);
        }
 
        delete[] pSmoothedDisparity1;
        delete[] pSmoothedDisparity2;
        delete[] pSmoothedDisparity3;
        delete[] pSmoothedDisparity4;
    }
    else
    {
        for (int i = 0; i < OLP_IMG_WIDTH * OLP_IMG_HEIGHT; i++)
        {
            pSmoothedDisparity[i] = pDisparities[i];
        }
    }
 
 
    const float fDispMin = nDispMin;
    const float fDispMax = nDispMax;
    CDynamicArray afSIFTDescriptors(4);
 
    for (int i = 0; i < OLP_IMG_HEIGHT; i += nDisparityPointDistance)
    {
        for (int j = 0; j < OLP_IMG_WIDTH; j += nDisparityPointDistance)
        {
            //fDisp = m3.at<short>(i,j)/16;
            float fDisp = pSmoothedDisparity[i * OLP_IMG_WIDTH + j];
 
            if ((fDisp > fDispMin) && (fDisp < fDispMax))
            {
                //ARMARX_VERBOSE_S <<  << "disparity: %d   ", nDisp);
                vImgPointLeft.x = j;
                vImgPointLeft.y = i;
                vImgPointRight.x = j - fDisp;
                vImgPointRight.y = i;
 
                pStereoCalibration->Calculate3DPoint(
                    vImgPointLeft, vImgPointRight, vPoint3D, true, false);
                //ARMARX_VERBOSE_S <<  << "z: %.0f\n", vPoint3D.z);
 
                if (vPoint3D.z < OLP_MAX_OBJECT_DISTANCE)
                {
                    // create point descriptor
                    CHypothesisPoint* pNewPoint = new CHypothesisPoint();
                    pNewPoint->ePointType = CHypothesisPoint::eDepthMapPoint;
                    Math3d::SetVec(pNewPoint->vPosition, vPoint3D);
                    Math3d::SetVec(pNewPoint->vOldPosition, vPoint3D);
                    pNewPoint->pMSERDescriptor = NULL;
                    pNewPoint->pFeatureDescriptors = new CSIFTFeatureArray();
                    pNewPoint->fMembershipProbability = 0;
                    const float fIntensity =
                        pRectifiedImageLeftColorGaussFiltered->pixels[3 * (i * OLP_IMG_WIDTH + j)] +
                        pRectifiedImageLeftColorGaussFiltered
                            ->pixels[3 * (i * OLP_IMG_WIDTH + j) + 1] +
                        pRectifiedImageLeftColorGaussFiltered
                            ->pixels[3 * (i * OLP_IMG_WIDTH + j) + 2] +
                        3;
                    pNewPoint->fIntensity = (fIntensity - 3) * fIntensityFactor;
                    pNewPoint->fColorR = (pRectifiedImageLeftColorGaussFiltered
                                              ->pixels[3 * (i * OLP_IMG_WIDTH + j)] +
                                          1) /
                                         fIntensity;
                    pNewPoint->fColorG = (pRectifiedImageLeftColorGaussFiltered
                                              ->pixels[3 * (i * OLP_IMG_WIDTH + j) + 1] +
                                          1) /
                                         fIntensity;
                    pNewPoint->fColorB = (pRectifiedImageLeftColorGaussFiltered
                                              ->pixels[3 * (i * OLP_IMG_WIDTH + j) + 2] +
                                          1) /
                                         fIntensity;
 
                    // create sift descriptor for point
                    m_pSIFTFeatureCalculator->CreateSIFTDescriptors(
                        pRectifiedImageLeftGrey, &afSIFTDescriptors, j, i, 1.0f, false, false);
 
                    if (afSIFTDescriptors.GetSize() > 0)
                    {
                        pNewPoint->pFeatureDescriptors->AddElement(
                            (CSIFTFeatureEntry*)((CSIFTFeatureEntry*)afSIFTDescriptors[0])
                                ->Clone());
                        (*pNewPoint->pFeatureDescriptors)[0]->point3d = vPoint3D;
                        // cleanup
                        CSIFTFeatureEntry* pFeature = (CSIFTFeatureEntry*)afSIFTDescriptors[0];
                        afSIFTDescriptors[0]->bDelete = false;
                        afSIFTDescriptors.Clear();
                        delete pFeature;
                    }
 
                    aPointsFromDisparity.push_back(pNewPoint);
                }
            }
            else
            {
                //ARMARX_VERBOSE_S <<  << "invalid disparity: %d\n", nDisp);
            }
        }
    }
 
    if (pAll3DPoints)
    {
        for (int i = 0; i < OLP_IMG_HEIGHT; i += nDisparityPointDistance)
        {
            for (int j = 0; j < OLP_IMG_WIDTH; j += nDisparityPointDistance)
            {
                float fDisp = pSmoothedDisparity[i * OLP_IMG_WIDTH + j];
                vImgPointLeft.x = j;
                vImgPointLeft.y = i;
                vImgPointRight.x = j - fDisp;
                vImgPointRight.y = i;
                pStereoCalibration->Calculate3DPoint(
                    vImgPointLeft, vImgPointRight, vPoint3D, true, false);
 
                if (Math3d::Length(vPoint3D) > 2 * OLP_MAX_OBJECT_DISTANCE)
                {
                    if (aPointsFromDisparity.size() > 0)
                    {
                        Math3d::SetVec(vPoint3D, aPointsFromDisparity.at(0)->vPosition);
                    }
                }
 
                pAll3DPoints->push_back(vPoint3D);
            }
        }
    }
 
 
    delete pRectifiedImageLeftGrey;
    delete pRectifiedImageRightGrey;
    delete pRectifiedImageLeftGreyHalfsize;
    delete pRectifiedImageRightGreyHalfsize;
    delete pRectifiedImageLeftGreyQuartersize;
    delete pRectifiedImageRightGreyQuartersize;
    delete pRectifiedImageLeftColorGaussFiltered;
    delete pStereoMatcher;
    delete pStereoCalibrationCopy;
    cvReleaseImageHeader(&pRectifiedIplImageLeft);
    cvReleaseImageHeader(&pRectifiedIplImageRight);
    delete[] pSmoothedDisparity;
}
 
void
CFeatureCalculation::CalculateSmoothedDisparityImage(float* pInputDisparity,
                                                     float* pSmoothedDisparity,
                                                     const int nRadius)
{
    for (int i = 0; i < OLP_IMG_WIDTH * OLP_IMG_HEIGHT; i++)
    {
        pSmoothedDisparity[i] = pInputDisparity[i];
    }
 
    for (int i = nRadius; i < OLP_IMG_HEIGHT - nRadius; i++)
    {
        for (int j = nRadius; j < OLP_IMG_WIDTH - nRadius; j++)
        {
            int nIndex = i * OLP_IMG_WIDTH + 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 * OLP_IMG_WIDTH + k;
 
                        if (pInputDisparity[nTempIndex] != 0)
                        {
                            fSum += pInputDisparity[nTempIndex];
                            nNumPixels++;
                        }
                    }
                }
 
                pSmoothedDisparity[nIndex] = fSum / (float)nNumPixels;
            }
        }
    }
}
 
Vec3d
CFeatureCalculation::GetCorresponding3DPoint(
    const Vec2d point2D,
    const pcl::PointCloud<pcl::PointXYZRGBA>::Ptr pointcloud)
{
    Vec3d result;
    const int width = pointcloud->width;
    const int height = pointcloud->height;
    const int x = (point2D.x < 0) ? 0 : ((point2D.x > width - 1) ? width - 1 : point2D.x);
    const int y = (point2D.y < 0) ? 0 : ((point2D.y > height - 1) ? height - 1 : point2D.y);
    const int index = y * width + x;
    pcl::PointXYZRGBA point3D = pointcloud->at(index);
    result.x = point3D.x;
    result.y = point3D.y;
    result.z = point3D.z;
    return result;
}