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);

            }

            // 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");

            #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);

            }

            // 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");

            #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;

    }

    {

        // color debayer and remap/rectify

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

        cv::remap(color1, color1, stereoRect.map1X, stereoRect.map1Y, CV_INTER_LINEAR);

        #ifdef SM_DEBUG

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

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

        #endif

        // 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);

    }

}