Subversion Repositories seema-scanner

//

// Two Frequency Phase Shifting using the Heterodyne Principle

//

// This implementation follows "Reich, Ritter, Thesing, White light heterodyne

// principle for 3D-measurement", SPIE (1997).

//

// Different from the paper, it uses only two different frequencies.

//

// The number of periods in the primary frequency can be chosen freely, but

// small changes can have a considerable impact on quality. The number of

// phase shifts can be chosen freely (min. 3), and higher values reduce the

// effects of image noise.They also allow us to filter bad points based on

// energy at non-primary frequencies.

//

#include "AlgorithmPhaseShiftTwoFreq.h"

#include <math.h>

#include "cvtools.h"

#include "algorithmtools.h"

#include <omp.h>

static unsigned int nStepsPrimary = 16; // number of shifts/steps in primary

static unsigned int nStepsSecondary = 8; // number of shifts/steps in secondary

static float nPeriodsPrimary = 40; // primary period

AlgorithmPhaseShiftTwoFreq::AlgorithmPhaseShiftTwoFreq(unsigned int _screenCols,

                                                       unsigned int _screenRows)

    : Algorithm(_screenCols, _screenRows) {

    // Set N

    N = 2+nStepsPrimary+nStepsSecondary;

    // Determine the secondary (wider) period to fulfill the heterodyne condition

    float nPeriodsSecondary = nPeriodsPrimary + 1;

    // all on pattern

    cv::Mat allOn(1, screenCols, CV_8UC3, cv::Scalar::all(255));

    patterns.push_back(allOn);

    // all off pattern

    cv::Mat allOff(1, screenCols, CV_8UC3, cv::Scalar::all(0));

    patterns.push_back(allOff);

    // Primary encoding patterns

    for(unsigned int i=0; i<nStepsPrimary; i++){

        float phase = 2.0*CV_PI/nStepsPrimary * i;

        float pitch = screenCols/nPeriodsPrimary;

        cv::Mat patternI(1,1,CV_8U);

        patternI = computePhaseVector(screenCols, phase, pitch);

        patterns.push_back(patternI.t());

    }

    // Secondary encoding patterns

    for(unsigned int i=0; i<nStepsSecondary; i++){

        float phase = 2.0*CV_PI/nStepsSecondary * i;

        float pitch = screenCols/nPeriodsSecondary;

        cv::Mat patternI(1,1,CV_8U);

        patternI = computePhaseVector(screenCols, phase, pitch);

        patterns.push_back(patternI.t());

    }

}

cv::Mat AlgorithmPhaseShiftTwoFreq::getEncodingPattern(unsigned int depth){

    return patterns[depth];

}

struct StereoRectifyier {

    cv::Mat map0X, map0Y, map1X, map1Y;

    cv::Mat R0, R1, P0, P1, QRect;

};

static void getStereoRectifyier(const SMCalibrationParameters &calibration,

                                const cv::Size& frameSize,

                                StereoRectifyier& stereoRect);

static void determineAmplitudePhaseEnergy(std::vector<cv::Mat>& frames,

                                    cv::Mat& amplitude,

                                    cv::Mat& phase,

                                    cv::Mat& energy);

static void collectPhases(const cv::Mat& phasePrimary,

                          const cv::Mat& phaseSecondary,

                          cv::Mat& phase);

static void matchPhaseMaps(const cv::Mat& occlusion0, const cv::Mat& occlusion1,

                           const cv::Mat& phase0, const cv::Mat& phase1,

                           std::vector<cv::Vec2f>& q0, std::vector<cv::Vec2f>& q1);

static void triangulate(const StereoRectifyier& stereoRect,

                        const std::vector<cv::Vec2f>& q0,

                        const std::vector<cv::Vec2f>& q1,

                        std::vector<cv::Point3f>& Q);

void AlgorithmPhaseShiftTwoFreq::

    get3DPoints(const SMCalibrationParameters & calibration,

                const std::vector<cv::Mat>& frames0,

                const std::vector<cv::Mat>& frames1,

                std::vector<cv::Point3f>& Q,

                std::vector<cv::Vec3b>& color){

    assert(frames0.size() == N);

    assert(frames1.size() == N);

    StereoRectifyier stereoRect;

    getStereoRectifyier(calibration,

                        cv::Size(frames0[0].cols, frames0[0].rows),

                        stereoRect);

    // // Erode occlusion masks

    // cv::Mat strel = cv::getStructuringElement(cv::MORPH_ELLIPSE, cv::Size(5,5));

    cv::Mat up0, up1;

    cv::Mat occlusion0, occlusion1;

    cv::Mat color0, color1;

    #pragma omp parallel sections

    {

        #pragma omp section

        {

            // Gray-scale and remap/rectify

            std::vector<cv::Mat> frames0Rect(N);

            for(unsigned int i=0; i<N; i++){

                cv::Mat temp;

                cv::cvtColor(frames0[i], temp, CV_BayerBG2GRAY);

                cv::remap(temp, frames0Rect[i],

                          stereoRect.map0X, stereoRect.map0Y,

                          CV_INTER_LINEAR);

            }

            cv::cvtColor(frames0[0], color0, CV_BayerBG2RGB);

            cv::remap(color0, color0,

                      stereoRect.map1X, stereoRect.map1Y,

                      CV_INTER_LINEAR);

            // Occlusion masks

            cv::subtract(frames0Rect[0], frames0Rect[1], occlusion0);

            occlusion0 = (occlusion0 > 25) & (occlusion0 < 250);

            // Decode camera0

            std::vector<cv::Mat> frames0Primary(frames0Rect.begin()+2,

                                                frames0Rect.begin()+2+nStepsPrimary);

            std::vector<cv::Mat> frames0Secondary(frames0Rect.begin()+2+nStepsPrimary,

                                                  frames0Rect.end());

            frames0Rect.clear();

            cv::Mat amplitude0Primary, amplitude0Secondary;

            cv::Mat up0Primary, up0Secondary;

            cv::Mat energy0Primary, energy0Secondary;

            determineAmplitudePhaseEnergy(frames0Primary,

                                          amplitude0Primary,

                                          up0Primary,

                                          energy0Primary);

            determineAmplitudePhaseEnergy(frames0Secondary,

                                          amplitude0Secondary,

                                          up0Secondary,

                                          energy0Secondary);

            collectPhases(up0Primary, up0Secondary, up0);

            // Threshold on energy at primary frequency

            occlusion0 = occlusion0 & (amplitude0Primary > 5.0*nStepsPrimary);

            // Threshold on energy ratios

            occlusion0 = occlusion0 & (amplitude0Primary > 0.25*energy0Primary);

            occlusion0 = occlusion0 & (amplitude0Secondary > 0.25*energy0Secondary);

            // // Erode occlusion masks

            // cv::erode(occlusion0, occlusion0, strel);

            // Threshold on gradient of phase

            cv::Mat edges0;

            cv::Sobel(up0, edges0, -1, 1, 1, 5);

            occlusion0 = occlusion0 & (abs(edges0) < 10);

            #ifdef SM_DEBUG

                cvtools::writeMat(up0Primary, "up0Primary.mat", "up0Primary");

                cvtools::writeMat(up0Secondary, "up0Secondary.mat", "up0Secondary");

                cvtools::writeMat(up0, "up0.mat", "up0");

                cvtools::writeMat(amplitude0Primary,

                                  "amplitude0Primary.mat", "amplitude0Primary");

                cvtools::writeMat(amplitude0Secondary,

                                  "amplitude0Secondary.mat", "amplitude0Secondary");

                cvtools::writeMat(energy0Primary,

                                  "energy0Primary.mat", "energy0Primary");

                cvtools::writeMat(energy0Secondary,

                                  "energy0Secondary.mat", "energy0Secondary");

                cvtools::writeMat(edges0, "edges0.mat", "edges0");

                cvtools::writeMat(occlusion0, "occlusion0.mat", "occlusion0");

                cvtools::writeMat(color0, "color0.mat", "color0");

            #endif

        }

        #pragma omp section

        {

            // Gray-scale and remap

            std::vector<cv::Mat> frames1Rect(N);

            for(unsigned int i=0; i<N; i++){

                cv::Mat temp;

                cv::cvtColor(frames1[i], temp, CV_BayerBG2GRAY);

                cv::remap(temp, frames1Rect[i],

                          stereoRect.map1X, stereoRect.map1Y,

                          CV_INTER_LINEAR);

            }

            cv::cvtColor(frames1[0], color1, CV_BayerBG2RGB);

            cv::remap(color1, color1,

                      stereoRect.map1X, stereoRect.map1Y,

                      CV_INTER_LINEAR);

            // Occlusion masks

            cv::subtract(frames1Rect[0], frames1Rect[1], occlusion1);

            occlusion1 = (occlusion1 > 25) & (occlusion1 < 250);

            // Decode camera1

            std::vector<cv::Mat> frames1Primary(frames1Rect.begin()+2,

                                                frames1Rect.begin()+2+nStepsPrimary);

            std::vector<cv::Mat> frames1Secondary(frames1Rect.begin()+2+nStepsPrimary,

                                                  frames1Rect.end());

            frames1Rect.clear();

            cv::Mat amplitude1Primary, amplitude1Secondary;

            cv::Mat up1Primary, up1Secondary;

            cv::Mat energy1Primary, energy1Secondary;

            determineAmplitudePhaseEnergy(frames1Primary,

                                          amplitude1Primary,

                                          up1Primary,

                                          energy1Primary);

            determineAmplitudePhaseEnergy(frames1Secondary,

                                          amplitude1Secondary,

                                          up1Secondary,

                                          energy1Secondary);

            collectPhases(up1Primary, up1Secondary, up1);

            // Threshold on energy at primary frequency

            occlusion1 = occlusion1 & (amplitude1Primary > 5.0*nStepsPrimary);

            // Threshold on energy ratios

            occlusion1 = occlusion1 & (amplitude1Primary > 0.25*energy1Primary);

            occlusion1 = occlusion1 & (amplitude1Secondary > 0.25*energy1Secondary);

            // // Erode occlusion masks

            // cv::erode(occlusion1, occlusion1, strel);

            // Threshold on gradient of phase

            cv::Mat edges1;

            cv::Sobel(up1, edges1, -1, 1, 1, 5);

            occlusion1 = occlusion1 & (abs(edges1) < 10);

            #ifdef SM_DEBUG

                cvtools::writeMat(up1Primary, "up1Primary.mat", "up1Primary");

                cvtools::writeMat(up1Secondary, "up1Secondary.mat", "up1Secondary");

                cvtools::writeMat(up1, "up1.mat", "up1");

                cvtools::writeMat(amplitude1Primary,

                                  "amplitude1Primary.mat", "amplitude1Primary");

                cvtools::writeMat(amplitude1Secondary,

                                  "amplitude1Secondary.mat", "amplitude1Secondary");

                cvtools::writeMat(energy1Primary,

                                  "energy1Primary.mat", "energy1Primary");

                cvtools::writeMat(energy1Secondary,

                                  "energy1Secondary.mat", "energy1Secondary");

                cvtools::writeMat(edges1, "edges1.mat", "edges1");

                cvtools::writeMat(occlusion1, "occlusion1.mat", "occlusion1");

                cvtools::writeMat(color1, "color1.mat", "color1");

            #endif

        }

    }

    // Match phase maps

    // camera0 against camera1

    std::vector<cv::Vec2f> q0, q1;

    matchPhaseMaps(occlusion0, occlusion1, up0, up1, q0, q1);

    size_t nMatches = q0.size();

    if(nMatches < 1){

        Q.resize(0);

        color.resize(0);

        return;

    }

    else {

        // Retrieve color information

        color.resize(nMatches);

        for(unsigned int i=0; i<nMatches; i++){

            cv::Vec3b c0 = color0.at<cv::Vec3b>(int(q0[i][1]), int(q0[i][0]));

            cv::Vec3b c1 = color1.at<cv::Vec3b>(int(q1[i][1]), int(q1[i][0]));

            color[i] = 0.5*c0 + 0.5*c1;

        }

    }

    // Triangulate points

    triangulate(stereoRect, q0, q1, Q);

}

void getStereoRectifyier(const SMCalibrationParameters &calibration,

                         const cv::Size& frameSize,

                         StereoRectifyier& stereoRect){

    // cv::stereoRectify segfaults unless R is double precision

    cv::Mat R, T;

    cv::Mat(calibration.R1).convertTo(R, CV_64F);

    cv::Mat(calibration.T1).convertTo(T, CV_64F);

    cv::stereoRectify(calibration.K0, calibration.k0,

                      calibration.K1, calibration.k1,

                      frameSize, R, T,

                      stereoRect.R0, stereoRect.R1,

                      stereoRect.P0, stereoRect.P1,

                      stereoRect.QRect, 0);

    // Interpolation maps (lens distortion and rectification)

    cv::initUndistortRectifyMap(calibration.K0, calibration.k0,

                                stereoRect.R0, stereoRect.P0,

                                frameSize, CV_32F,

                                stereoRect.map0X, stereoRect.map0Y);

    cv::initUndistortRectifyMap(calibration.K1, calibration.k1,

                                stereoRect.R1, stereoRect.P1,

                                frameSize, CV_32F,

                                stereoRect.map1X, stereoRect.map1Y);

}

void determineAmplitudePhaseEnergy(std::vector<cv::Mat>& frames,

                                   cv::Mat& amplitude,

                                   cv::Mat& phase,

                                   cv::Mat& energy) {

    std::vector<cv::Mat> fourier = getDFTComponents(frames);

    cv::phase(fourier[2], -fourier[3], phase);

    // Signal energy at unit frequency

    cv::magnitude(fourier[2], -fourier[3], amplitude);

    // Collected signal energy at higher frequencies

    energy = cv::Mat(phase.rows, phase.cols, CV_32F, cv::Scalar(0.0));

    for(unsigned int i=0; i<frames.size()-1; i++){

        cv::Mat magnitude;

        cv::magnitude(fourier[i*2 + 2], fourier[i*2 + 3], magnitude);

        cv::add(energy, magnitude, energy, cv::noArray(), CV_32F);

    }

    frames.clear();

}

void collectPhases(const cv::Mat& phasePrimary,

                   const cv::Mat& phaseSecondary,

                   cv::Mat& phase) {

    cv::Mat phaseEquivalent = phaseSecondary - phasePrimary;

    phaseEquivalent = cvtools::modulo(phaseEquivalent, 2.0*CV_PI);

    phase = unwrapWithCue(phasePrimary, phaseEquivalent, nPeriodsPrimary);

    phase *= phasePrimary.cols/(2.0*CV_PI);

}

void matchPhaseMaps(const cv::Mat& occlusion0, const cv::Mat& occlusion1,

                    const cv::Mat& phase0, const cv::Mat& phase1,

                    std::vector<cv::Vec2f>& q0, std::vector<cv::Vec2f>& q1) {

    #pragma omp parallel for

    for(int row=0; row<occlusion0.rows; row++){

        for(int col=0; col<occlusion0.cols; col++){

            if(!occlusion0.at<char>(row,col))

                continue;

            float phase0i = phase0.at<float>(row,col);

            for(int col1=0; col1<phase1.cols-1; col1++){

                if(!occlusion1.at<char>(row,col1) || !occlusion1.at<char>(row,col1+1))

                    continue;

                float phase1Left = phase1.at<float>(row,col1);

                float phase1Right = phase1.at<float>(row,col1+1);

                bool match = (phase1Left <= phase0i)

                              && (phase0i <= phase1Right)

                              && (phase0i-phase1Left < 1.0)

                              && (phase1Right-phase0i < 1.0)

                              && (phase1Right-phase1Left > 0.1);

                if(match){

                    float col1i = col1 + (phase0i-phase1Left)/(phase1Right-phase1Left);

                    #pragma omp critical

                    {

                    q0.push_back(cv::Point2f(col, row));

                    q1.push_back(cv::Point2f(col1i, row));

                    }

                    break;

                }

            }

        }

    }

}

void triangulate(const StereoRectifyier& stereoRect,

                 const std::vector<cv::Vec2f>& q0,

                 const std::vector<cv::Vec2f>& q1,

                 std::vector<cv::Point3f>& Q) {

    // cv::Mat QMatHomogenous, QMat;

    // cv::triangulatePoints(P0, P1, q0, q1, QMatHomogenous);

    // cvtools::convertMatFromHomogeneous(QMatHomogenous, QMat);

    // // Undo rectification

    // cv::Mat R0Inv;

    // cv::Mat(R0.t()).convertTo(R0Inv, CV_32F);

    // QMat = R0Inv*QMat;

    // cvtools::matToPoints3f(QMat, Q);

    // Triangulate by means of disparity projection

    Q.resize(q0.size());

    cv::Matx44f QRectx = cv::Matx44f(stereoRect.QRect);

    cv::Matx33f R0invx = cv::Matx33f(cv::Mat(stereoRect.R0.t()));

    #pragma omp parallel for

    for(unsigned int i=0; i < q0.size(); i++){

        float disparity = q0[i][0] - q1[i][0];

        cv::Vec4f Qih = QRectx*cv::Vec4f(q0[i][0], q0[i][1], disparity, 1.0);

        float winv = float(1.0) / Qih[3];

        Q[i] = R0invx * cv::Point3f(Qih[0]*winv, Qih[1]*winv, Qih[2]*winv);

    }

}