WebSVN – seema-scanner – Diff – /src/algorithm/AlgorithmPhaseShiftThreeFreq.cpp


//
// Three Frequency Phase Shifting using the Heterodyne Principle
//
// This implementation follows "Reich, Ritter, Thesing, White light heterodyne principle for 3D-measurement", SPIE (1997)
//
// The number of periods in the primary and secondary frequencies 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 "AlgorithmPhaseShiftThreeFreq.h"
#include <math.h>
 
#include "cvtools.h"
#include "algorithmtools.h"
 
#ifndef M_PI
    #define M_PI 3.14159265358979323846
#endif
 
static unsigned int nStepsPrimary = 8; // number of shifts/steps in primary
static unsigned int nStepsSecondary = 8; // number of shifts/steps in secondary
static unsigned int nStepsTertiary = 8; // number of shifts/steps in tertiary
static float nPeriodsPrimary = 24; // primary period
static float nPeriodsSecondary = 30; // primary period
 
AlgorithmPhaseShiftThreeFreq::AlgorithmPhaseShiftThreeFreq(unsigned int _screenCols, unsigned int _screenRows) : Algorithm(_screenCols, _screenRows){
 
    // Set N
    N = 2+nStepsPrimary+nStepsSecondary+nStepsTertiary;
 
    // Determine the tertiary period to fulfill the heterodyne condition
    float nPeriodsTertiary = (screenCols*nPeriodsPrimary*nPeriodsSecondary)/(nPeriodsPrimary*nPeriodsSecondary+2*screenCols*nPeriodsPrimary-screenCols*nPeriodsSecondary);
 
    // 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);
 
    // Precompute encoded patterns
    const float pi = M_PI;
 
    // Primary encoding patterns
    for(unsigned int i=0; i<nStepsPrimary; i++){
        float phase = 2.0*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*pi/nStepsSecondary * i;
        float pitch = screenCols/nPeriodsSecondary;
        cv::Mat patternI(1,1,CV_8U);
        patternI = computePhaseVector(screenCols, phase, pitch);
        patterns.push_back(patternI.t());
    }
    // Tertiary encoding patterns
    for(unsigned int i=0; i<nStepsTertiary; i++){
        float phase = 2.0*pi/nStepsTertiary * i;
        float pitch = screenCols/nPeriodsTertiary;
        cv::Mat patternI(1,1,CV_8U);
        patternI = computePhaseVector(screenCols, phase, pitch);
        patterns.push_back(patternI.t());
    }
 
}
 
cv::Mat AlgorithmPhaseShiftThreeFreq::getEncodingPattern(unsigned int depth){
    return patterns[depth];
}
 
void AlgorithmPhaseShiftThreeFreq::get3DPoints(SMCalibrationParameters calibration, const std::vector<cv::Mat>& frames0, const std::vector<cv::Mat>& frames1, std::vector<cv::Point3f>& Q, std::vector<cv::Vec3b>& color){
 
    const float pi = M_PI;
 
    assert(frames0.size() == N);
    assert(frames1.size() == N);
 
    int frameRows = frames0[0].rows;
    int frameCols = frames0[0].cols;
 
    // Rectifying homographies (rotation+projections)
    cv::Size frameSize(frameCols, frameRows);
    cv::Mat R, T;
    // stereoRectify segfaults unless R is double precision
    cv::Mat(calibration.R1).convertTo(R, CV_64F);
    cv::Mat(calibration.T1).convertTo(T, CV_64F);
    cv::Mat R0, R1, P0, P1, QRect;
    cv::stereoRectify(calibration.K0, calibration.k0, calibration.K1, calibration.k1, frameSize, R, T, R0, R1, P0, P1, QRect, 0);
 
    // Interpolation maps (lens distortion and rectification)
    cv::Mat map0X, map0Y, map1X, map1Y;
    cv::initUndistortRectifyMap(calibration.K0, calibration.k0, R0, P0, frameSize, CV_32F, map0X, map0Y);
    cv::initUndistortRectifyMap(calibration.K1, calibration.k1, R1, P1, frameSize, CV_32F, map1X, map1Y);
 
 
    // Gray-scale and remap
    std::vector<cv::Mat> frames0Rect(N);
    std::vector<cv::Mat> frames1Rect(N);
    for(unsigned int i=0; i<N; i++){
        cv::Mat temp;
        cv::cvtColor(frames0[i], temp, CV_BayerBG2GRAY);
        cv::remap(temp, frames0Rect[i], map0X, map0Y, CV_INTER_LINEAR);
        cv::cvtColor(frames1[i], temp, CV_BayerBG2GRAY);
        cv::remap(temp, frames1Rect[i], map1X, map1Y, CV_INTER_LINEAR);
    }
 
    // 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()-nStepsTertiary);
    std::vector<cv::Mat> frames0Tertiary(frames0Rect.end()-nStepsTertiary, frames0Rect.end());
 
    std::vector<cv::Mat> F0Primary = getDFTComponents(frames0Primary);
    cv::Mat up0Primary;
    cv::phase(F0Primary[2], -F0Primary[3], up0Primary);
    std::vector<cv::Mat> F0Secondary = getDFTComponents(frames0Secondary);
    cv::Mat up0Secondary;
    cv::phase(F0Secondary[2], -F0Secondary[3], up0Secondary);
    std::vector<cv::Mat> F0Tertiary = getDFTComponents(frames0Tertiary);
    cv::Mat up0Tertiary;
    cv::phase(F0Tertiary[2], -F0Tertiary[3], up0Tertiary);
 
    cv::Mat up0EquivalentPS = up0Primary - up0Secondary;
    up0EquivalentPS = cvtools::modulo(up0EquivalentPS, 2.0*pi);
 
    cv::Mat up0EquivalentPT = up0Primary - up0Tertiary;
    up0EquivalentPT = cvtools::modulo(up0EquivalentPT, 2.0*pi);
 
    cv::Mat up0Equivalent = up0EquivalentPS - up0EquivalentPT;
    up0Equivalent = cvtools::modulo(up0Equivalent, 2.0*pi);
 
    cv::Mat up0 = unwrapWithCue(up0Primary, up0Equivalent, (float)screenCols/nPeriodsPrimary);
    up0 *= screenCols/(2.0*pi);
    cv::Mat amplitude0;
    cv::magnitude(F0Primary[2], -F0Primary[3], amplitude0);
 
    // 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()-nStepsTertiary);
    std::vector<cv::Mat> frames1Tertiary(frames1Rect.end()-nStepsTertiary, frames1Rect.end());
 
    std::vector<cv::Mat> F1Primary = getDFTComponents(frames1Primary);
    cv::Mat up1Primary;
    cv::phase(F1Primary[2], -F1Primary[3], up1Primary);
    std::vector<cv::Mat> F1Secondary = getDFTComponents(frames1Secondary);
    cv::Mat up1Secondary;
    cv::phase(F1Secondary[2], -F1Secondary[3], up1Secondary);
    std::vector<cv::Mat> F1Tertiary = getDFTComponents(frames1Tertiary);
    cv::Mat up1Tertiary;
    cv::phase(F1Tertiary[2], -F1Tertiary[3], up1Tertiary);
 
    cv::Mat up1EquivalentPS = up1Primary - up1Secondary;
    up1EquivalentPS = cvtools::modulo(up1EquivalentPS, 2.0*pi);
 
    cv::Mat up1EquivalentST = up1Secondary - up1Tertiary;
    up1EquivalentST = cvtools::modulo(up1EquivalentST, 2.0*pi);
 
    cv::Mat up1Equivalent = up1EquivalentPS - up1EquivalentST;
    up1Equivalent = cvtools::modulo(up1Equivalent, 2.0*pi);
 
    // TODO: we should use backward phase unwrapping (Song Zhang term)...
 
    cv::Mat up1 = unwrapWithCue(up1Primary, up1Equivalent, (float)screenCols/nPeriodsPrimary);
    up1 *= screenCols/(2.0*pi);
    cv::Mat amplitude1;
    cv::magnitude(F1Primary[2], -F1Primary[3], amplitude1);
 
    #ifdef QT_DEBUG
        cvtools::writeMat(up0Primary, "up0Primary.mat", "up0Primary");
        cvtools::writeMat(up0Secondary, "up0Secondary.mat", "up0Secondary");
        cvtools::writeMat(up0Tertiary, "up0Tertiary.mat", "up0Tertiary");
        cvtools::writeMat(up0EquivalentPS, "up0EquivalentPS.mat", "up0EquivalentPS");
        cvtools::writeMat(up0EquivalentPT, "up0EquivalentPT.mat", "up0EquivalentPT");
        cvtools::writeMat(up0Equivalent, "up0Equivalent.mat", "up0Equivalent");
        cvtools::writeMat(up0, "up0.mat", "up0");
        cvtools::writeMat(up1, "up1.mat", "up1");
        cvtools::writeMat(amplitude0, "amplitude0.mat", "amplitude0");
 
        cvtools::writeMat(amplitude0, "amplitude0.mat", "amplitude0");
        cvtools::writeMat(amplitude1, "amplitude1.mat", "amplitude1");
    #endif
 
    // color debayer and remap
    cv::Mat color0, color1;
    cv::cvtColor(frames0[0], color0, CV_BayerBG2RGB);
    cv::remap(color0, color0, map0X, map0Y, CV_INTER_LINEAR);
 
    cv::cvtColor(frames1[0], color1, CV_BayerBG2RGB);
    cv::remap(color1, color1, map1X, map1Y, CV_INTER_LINEAR);
 
    #ifdef QT_DEBUG
        cvtools::writeMat(color0, "color0.mat", "color0");
        cvtools::writeMat(color1, "color1.mat", "color1");
    #endif
 
    // Occlusion masks
    cv::Mat occlusion0, occlusion1;
    cv::subtract(frames0Rect[0], frames0Rect[1], occlusion0);
    occlusion0 = (occlusion0 > 5) & (occlusion0 < 250);
    cv::subtract(frames1Rect[0], frames1Rect[1], occlusion1);
    occlusion1 = (occlusion1 > 5) & (occlusion1 < 250);
 
 
 
 
 
 
 
 
    // Threshold on gradient of phase
    cv::Mat edges0;
    cv::Sobel(up0, edges0, -1, 1, 1, 5);
    occlusion0 = occlusion0 & (abs(edges0) < 150);
    cv::Mat edges1;
    cv::Sobel(up1, edges1, -1, 1, 1, 5);
    occlusion1 = occlusion1 & (abs(edges1) < 150);
 
    #ifdef QT_DEBUG
        cvtools::writeMat(edges0, "edges0.mat", "edges0");
        cvtools::writeMat(edges1, "edges1.mat", "edges1");
    #endif
 
    #ifdef QT_DEBUG
        cvtools::writeMat(occlusion0, "occlusion0.mat", "occlusion0");
        cvtools::writeMat(occlusion1, "occlusion1.mat", "occlusion1");
    #endif
 
    // Match phase maps
    int frameRectRows = map0X.rows;
    int frameRectCols = map0X.cols;
 
    // camera0 against camera1
    std::vector<cv::Vec2f> q0, q1;
    for(int row=0; row<frameRectRows; row++){
        for(int col=0; col<frameRectCols; col++){
 
            if(!occlusion0.at<char>(row,col))
                continue;
 
            float up0i = up0.at<float>(row,col);
            for(int col1=0; col1<up1.cols-1; col1++){
 
                if(!occlusion1.at<char>(row,col1) || !occlusion1.at<char>(row,col1+1))
                    continue;
 
                float up1Left = up1.at<float>(row,col1);
                float up1Right = up1.at<float>(row,col1+1);
 
                if((up1Left <= up0i) && (up0i <= up1Right) && (up0i-up1Left < 1.0) && (up1Right-up0i < 1.0)){
 
                    float col1i = col1 + (up0i-up1Left)/(up1Right-up1Left);
 
                    q0.push_back(cv::Point2f(col, row));
                    q1.push_back(cv::Point2f(col1i, row));
 
                    break;
                }
            }
        }
    }
 
    int nMatches = q0.size();
 
    if(nMatches < 1){
        Q.resize(0);
        color.resize(0);
 
        return;
    }
 
    // Retrieve color information
    color.resize(nMatches);
    for(int i=0; i<nMatches; i++){
 
        cv::Vec3b c0 = color0.at<cv::Vec3b>(q0[i][1], q0[i][0]);
        cv::Vec3b c1 = color1.at<cv::Vec3b>(q1[i][1], q1[i][0]);
 
        color[i] = 0.5*c0 + 0.5*c1;
    }
 
    // Triangulate points
    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);
 
}
 

Subversion Repositories seema-scanner

(root)/src/algorithm/AlgorithmPhaseShiftThreeFreq.cpp – Rev 185 → 187

Rev 185		Rev 187
1	`//`	1	`//`
2	`// Three Frequency Phase Shifting using the Heterodyne Principle`	2	`// Three Frequency Phase Shifting using the Heterodyne Principle`
3	`//`	3	`//`
4	`// This implementation follows "Reich, Ritter, Thesing, White light heterodyne principle for 3D-measurement", SPIE (1997)`	4	`// This implementation follows "Reich, Ritter, Thesing, White light heterodyne principle for 3D-measurement", SPIE (1997)`
5	`//`	5	`//`
6	`// The number of periods in the primary and secondary frequencies can be chosen freely, but small changes can have a considerable impact on quality.`	6	`// The number of periods in the primary and secondary frequencies can be chosen freely, but small changes can have a considerable impact on quality.`
7	`// 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.`	7	`// 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.`
8	`//`	8	`//`
9		9
10		10
11	`#include "AlgorithmPhaseShiftThreeFreq.h"`	11	`#include "AlgorithmPhaseShiftThreeFreq.h"`
12	`#include <math.h>`	12	`#include <math.h>`
13		13
14	`#include "cvtools.h"`	14	`#include "cvtools.h"`
15	`#include "algorithmtools.h"`	15	`#include "algorithmtools.h"`
16		16
17	`#ifndef M_PI`	17	`#ifndef M_PI`
18	`#define M_PI 3.14159265358979323846`	18	`#define M_PI 3.14159265358979323846`
19	`#endif`	19	`#endif`
20		20
21	`static unsigned int nStepsPrimary = 8; // number of shifts/steps in primary`	21	`static unsigned int nStepsPrimary = 8; // number of shifts/steps in primary`
22	`static unsigned int nStepsSecondary = 8; // number of shifts/steps in secondary`	22	`static unsigned int nStepsSecondary = 8; // number of shifts/steps in secondary`
23	`static unsigned int nStepsTertiary = 8; // number of shifts/steps in tertiary`	23	`static unsigned int nStepsTertiary = 8; // number of shifts/steps in tertiary`
24	`static float nPeriodsPrimary = 24; // primary period`	24	`static float nPeriodsPrimary = 24; // primary period`
25	`static float nPeriodsSecondary = 30; // primary period`	25	`static float nPeriodsSecondary = 30; // primary period`
26		26
27	`AlgorithmPhaseShiftThreeFreq::AlgorithmPhaseShiftThreeFreq(unsigned int _screenCols, unsigned int _screenRows) : Algorithm(_screenCols, _screenRows){`	27	`AlgorithmPhaseShiftThreeFreq::AlgorithmPhaseShiftThreeFreq(unsigned int _screenCols, unsigned int _screenRows) : Algorithm(_screenCols, _screenRows){`
28		28
29	`// Set N`	29	`// Set N`
30	`N = 2+nStepsPrimary+nStepsSecondary+nStepsTertiary;`	30	`N = 2+nStepsPrimary+nStepsSecondary+nStepsTertiary;`
31		31
32	`// Determine the tertiary period to fulfill the heterodyne condition`	32	`// Determine the tertiary period to fulfill the heterodyne condition`
33	`float nPeriodsTertiary = (screenColsnPeriodsPrimarynPeriodsSecondary)/(nPeriodsPrimarynPeriodsSecondary+2screenColsnPeriodsPrimary-screenColsnPeriodsSecondary);`	33	`float nPeriodsTertiary = (screenColsnPeriodsPrimarynPeriodsSecondary)/(nPeriodsPrimarynPeriodsSecondary+2screenColsnPeriodsPrimary-screenColsnPeriodsSecondary);`
34		34
35	`// all on pattern`	35	`// all on pattern`
36	`cv::Mat allOn(1, screenCols, CV_8UC3, cv::Scalar::all(255));`	36	`cv::Mat allOn(1, screenCols, CV_8UC3, cv::Scalar::all(255));`
37	`patterns.push_back(allOn);`	37	`patterns.push_back(allOn);`
38		38
39	`// all off pattern`	39	`// all off pattern`
40	`cv::Mat allOff(1, screenCols, CV_8UC3, cv::Scalar::all(0));`	40	`cv::Mat allOff(1, screenCols, CV_8UC3, cv::Scalar::all(0));`