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 First and Second 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-First 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 nStepsFirst = 8; // number of shifts/steps in First
static unsigned int nStepsSecond = 8; // number of shifts/steps in Second
static unsigned int nStepsThird = 8; // number of shifts/steps in Third
static float nPeriodsFirst = 24; // First period
static float nPeriodsSecond = 30; // First period
 
AlgorithmPhaseShiftThreeFreq::AlgorithmPhaseShiftThreeFreq(unsigned int _screenCols, unsigned int _screenRows) : Algorithm(_screenCols, _screenRows){
 
    // Set N
    N = 2+nStepsFirst+nStepsSecond+nStepsThird;
 
    // Determine the third number of periods to fulfill the heterodyne condition
    float nPeriodsThird = 1 + 2*nPeriodsSecond - nPeriodsFirst;
 
    // 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;
 
    // First encoding patterns
    for(unsigned int i=0; i<nStepsFirst; i++){
        float phase = 2.0*pi/nStepsFirst * i;
        float pitch = screenCols/nPeriodsFirst;
        cv::Mat patternI(1,1,CV_8U);
        patternI = computePhaseVector(screenCols, phase, pitch);
        patterns.push_back(patternI.t());
    }
 
    // Second encoding patterns
    for(unsigned int i=0; i<nStepsSecond; i++){
        float phase = 2.0*pi/nStepsSecond * i;
        float pitch = screenCols/nPeriodsSecond;
        cv::Mat patternI(1,1,CV_8U);
        patternI = computePhaseVector(screenCols, phase, pitch);
        patterns.push_back(patternI.t());
    }
    // Third encoding patterns
    for(unsigned int i=0; i<nStepsThird; i++){
        float phase = 2.0*pi/nStepsThird * i;
        float pitch = screenCols/nPeriodsThird;
        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;
 
    float nPeriodsThird = 1 + 2*nPeriodsSecond - nPeriodsFirst;
 
 
    // 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> frames0First(frames0Rect.begin()+2, frames0Rect.begin()+2+nStepsFirst);
    std::vector<cv::Mat> frames0Second(frames0Rect.begin()+2+nStepsFirst, frames0Rect.end()-nStepsThird);
    std::vector<cv::Mat> frames0Third(frames0Rect.end()-nStepsThird, frames0Rect.end());
 
    std::vector<cv::Mat> F0First = getDFTComponents(frames0First);
    cv::Mat up0First;
    cv::phase(F0First[2], -F0First[3], up0First);
    std::vector<cv::Mat> F0Second = getDFTComponents(frames0Second);
    cv::Mat up0Second;
    cv::phase(F0Second[2], -F0Second[3], up0Second);
    std::vector<cv::Mat> F0Third = getDFTComponents(frames0Third);
    cv::Mat up0Third;
    cv::phase(F0Third[2], -F0Third[3], up0Third);
 
    cv::Mat up0FS = up0Second - up0First;
    up0FS = cvtools::modulo(up0FS, 2.0*pi);
 
    cv::Mat up0ST = up0Third - up0Second;
    up0ST = cvtools::modulo(up0ST, 2.0*pi);
 
    cv::Mat up0FST = up0ST - up0FS;
    up0FST = cvtools::modulo(up0FST, 2.0*pi);
 
    cv::Mat up0F = unwrapWithCue(up0First, up0FST, nPeriodsFirst);
    cv::Mat up0S = unwrapWithCue(up0Second, up0FST, nPeriodsSecond);
    cv::Mat up0T = unwrapWithCue(up0Third, up0FST, nPeriodsThird);
    cv::Mat up0Mean = 1.0/3.0 * (up0F + up0S + up0T);
    cv::Mat up0Range = cv::max(up0F, cv::max(up0S, up0T)) - cv::min(up0F, cv::min(up0S, up0T));
    cv::Mat up0 = up0Mean * screenCols/(2.0*pi);
 
 
 
    cv::Mat amplitude0;
    cv::magnitude(F0First[2], -F0First[3], amplitude0);
 
    // Decode camera1
    std::vector<cv::Mat> frames1First(frames1Rect.begin()+2, frames1Rect.begin()+2+nStepsFirst);
    std::vector<cv::Mat> frames1Second(frames1Rect.begin()+2+nStepsFirst, frames1Rect.end()-nStepsThird);
    std::vector<cv::Mat> frames1Third(frames1Rect.end()-nStepsThird, frames1Rect.end());
 
    std::vector<cv::Mat> F1First = getDFTComponents(frames1First);
    cv::Mat up1First;
    cv::phase(F1First[2], -F1First[3], up1First);
    std::vector<cv::Mat> F1Second = getDFTComponents(frames1Second);
    cv::Mat up1Second;
    cv::phase(F1Second[2], -F1Second[3], up1Second);
    std::vector<cv::Mat> F1Third = getDFTComponents(frames1Third);
    cv::Mat up1Third;
    cv::phase(F1Third[2], -F1Third[3], up1Third);
 
    cv::Mat up1FS = up1Second - up1First;
    up1FS = cvtools::modulo(up1FS, 2.0*pi);
 
    cv::Mat up1ST = up1Third - up1Second;
    up1ST = cvtools::modulo(up1ST, 2.0*pi);
 
    cv::Mat up1FST = up1ST - up1FS;
    up1FST = cvtools::modulo(up1FST, 2.0*pi);
 
    cv::Mat up1F = unwrapWithCue(up1First, up1FST, nPeriodsFirst);
    cv::Mat up1S = unwrapWithCue(up1Second, up1FST, nPeriodsSecond);
    cv::Mat up1T = unwrapWithCue(up1Third, up1FST, nPeriodsThird);
    cv::Mat up1Mean = 1.0/3.0 * (up1F + up1S + up1T);
    cv::Mat up1Range = cv::max(up1F, cv::max(up1S, up1T)) - cv::min(up1F, cv::min(up1S, up1T));
    cv::Mat up1 = up1Mean * screenCols/(2.0*pi);
 
 
 
    cv::Mat amplitude1;
    cv::magnitude(F1First[2], -F1First[3], amplitude1);
 
    #ifdef QT_DEBUG
        cvtools::writeMat(up0First, "up0First.mat", "up0First");
        cvtools::writeMat(up0Second, "up0Second.mat", "up0Second");
        cvtools::writeMat(up0Third, "up0Third.mat", "up0Third");
        cvtools::writeMat(up0FS, "up0FS.mat", "up0FS");
        cvtools::writeMat(up0ST, "up0ST.mat", "up0ST");
        cvtools::writeMat(up0FST, "up0FST.mat", "up0FST");
        cvtools::writeMat(up0Mean, "up0Mean.mat", "up0Mean");
        cvtools::writeMat(up0Range, "up0Range.mat", "up0Range");
        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 > 25) & (occlusion0 < 250);
    cv::subtract(frames1Rect[0], frames1Rect[1], occlusion1);
    occlusion1 = (occlusion1 > 25) & (occlusion1 < 250);
 
    // Threshold on range
    occlusion0 = occlusion0 & (up0Range < 0.05);
    occlusion1 = occlusion1 & (up1Range < 0.05);
 
//    // Erode occlusion masks
//    cv::Mat strel = cv::getStructuringElement(cv::MORPH_ELLIPSE, cv::Size(5,5));
//    cv::erode(occlusion0, occlusion0, strel);
//    cv::erode(occlusion1, occlusion1, strel);
 
    // 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) && (up1Right-up1Left > 0.1)){
 
                    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);
 
}
 

Rev 189	Rev 190
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 First and Second frequencies can be chosen freely, but small changes can have a considerable impact on quality.`	6	`// The number of periods in the First and Second 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-First 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-First 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 nStepsFirst = 8; // number of shifts/steps in First`	21	`static unsigned int nStepsFirst = 8; // number of shifts/steps in First`
22	`static unsigned int nStepsSecond = 8; // number of shifts/steps in Second`	22	`static unsigned int nStepsSecond = 8; // number of shifts/steps in Second`
23	`static unsigned int nStepsThird = 8; // number of shifts/steps in Third`	23	`static unsigned int nStepsThird = 8; // number of shifts/steps in Third`
24	`static float nPeriodsFirst = 24; // First period`	24	`static float nPeriodsFirst = 24; // First period`
25	`static float nPeriodsSecond = 30; // First period`	25	`static float nPeriodsSecond = 30; // First 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+nStepsFirst+nStepsSecond+nStepsThird;`	30	`N = 2+nStepsFirst+nStepsSecond+nStepsThird;`
31		31
32	`// Determine the Third period to fulfill the heterodyne condition`	32	`// Determine the third number of periods to fulfill the heterodyne condition`
33	`float nPeriodsThird = 1 + 2*nPeriodsSecond - nPeriodsFirst;`	33	`float nPeriodsThird = 1 + 2*nPeriodsSecond - nPeriodsFirst;`
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));`
41	`patterns.push_back(allOff);`	41	`patterns.push_back(allOff);`
42		42
43	`// Precompute encoded patterns`	43	`// Precompute encoded patterns`
44	`const float pi = M_PI;`	44	`const float pi = M_PI;`
45		45
46	`// First encoding patterns`	46	`// First encoding patterns`
47	`for(unsigned int i=0; i<nStepsFirst; i++){`	47	`for(unsigned int i=0; i<nStepsFirst; i++){`
48	`float phase = 2.0pi/nStepsFirst i;`	48	`float phase = 2.0pi/nStepsFirst i;`
49	`float pitch = screenCols/nPeriodsFirst;`	49	`float pitch = screenCols/nPeriodsFirst;`
50	`cv::Mat patternI(1,1,CV_8U);`	50	`cv::Mat patternI(1,1,CV_8U);`
51	`patternI = computePhaseVector(screenCols, phase, pitch);`	51	`patternI = computePhaseVector(screenCols, phase, pitch);`
52	`patterns.push_back(patternI.t());`	52	`patterns.push_back(patternI.t());`
53	`}`	53	`}`
54		54
55	`// Second encoding patterns`	55	`// Second encoding patterns`
56	`for(unsigned int i=0; i<nStepsSecond; i++){`	56	`for(unsigned int i=0; i<nStepsSecond; i++){`
57	`float phase = 2.0pi/nStepsSecond i;`	57	`float phase = 2.0pi/nStepsSecond i;`
58	`float pitch = screenCols/nPeriodsSecond;`	58	`float pitch = screenCols/nPeriodsSecond;`
59	`cv::Mat patternI(1,1,CV_8U);`	59	`cv::Mat patternI(1,1,CV_8U);`
60	`patternI = computePhaseVector(screenCols, phase, pitch);`	60	`patternI = computePhaseVector(screenCols, phase, pitch);`
61	`patterns.push_back(patternI.t());`	61	`patterns.push_back(patternI.t());`
62	`}`	62	`}`
63	`// Third encoding patterns`	63	`// Third encoding patterns`
64	`for(unsigned int i=0; i<nStepsThird; i++){`	64	`for(unsigned int i=0; i<nStepsThird; i++){`
65	`float phase = 2.0pi/nStepsThird i;`	65	`float phase = 2.0pi/nStepsThird i;`
66	`float pitch = screenCols/nPeriodsThird;`	66	`float pitch = screenCols/nPeriodsThird;`
67	`cv::Mat patternI(1,1,CV_8U);`	67	`cv::Mat patternI(1,1,CV_8U);`
68	`patternI = computePhaseVector(screenCols, phase, pitch);`	68	`patternI = computePhaseVector(screenCols, phase, pitch);`
69	`patterns.push_back(patternI.t());`	69	`patterns.push_back(patternI.t());`
70	`}`	70	`}`
71		71
72	`}`	72	`}`
73		73
74	`cv::Mat AlgorithmPhaseShiftThreeFreq::getEncodingPattern(unsigned int depth){`	74	`cv::Mat AlgorithmPhaseShiftThreeFreq::getEncodingPattern(unsigned int depth){`
75	`return patterns[depth];`	75	`return patterns[depth];`
76	`}`	76	`}`
77		77
78	`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){`	78	`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){`
79		79
80	`const float pi = M_PI;`	80	`const float pi = M_PI;`
81		81
82	`assert(frames0.size() == N);`	82	`assert(frames0.size() == N);`
83	`assert(frames1.size() == N);`	83	`assert(frames1.size() == N);`
84		84
85	`int frameRows = frames0[0].rows;`	85	`int frameRows = frames0[0].rows;`
86	`int frameCols = frames0[0].cols;`	86	`int frameCols = frames0[0].cols;`
87		87
-		88	`float nPeriodsThird = 1 + 2*nPeriodsSecond - nPeriodsFirst;`
-		89
-		90
88	`// Rectifying homographies (rotation+projections)`	91	`// Rectifying homographies (rotation+projections)`
89	`cv::Size frameSize(frameCols, frameRows);`	92	`cv::Size frameSize(frameCols, frameRows);`
90	`cv::Mat R, T;`	93	`cv::Mat R, T;`
91	`// stereoRectify segfaults unless R is double precision`	94	`// stereoRectify segfaults unless R is double precision`
92	`cv::Mat(calibration.R1).convertTo(R, CV_64F);`	95	`cv::Mat(calibration.R1).convertTo(R, CV_64F);`
93	`cv::Mat(calibration.T1).convertTo(T, CV_64F);`	96	`cv::Mat(calibration.T1).convertTo(T, CV_64F);`
94	`cv::Mat R0, R1, P0, P1, QRect;`	97	`cv::Mat R0, R1, P0, P1, QRect;`
95	`cv::stereoRectify(calibration.K0, calibration.k0, calibration.K1, calibration.k1, frameSize, R, T, R0, R1, P0, P1, QRect, 0);`	98	`cv::stereoRectify(calibration.K0, calibration.k0, calibration.K1, calibration.k1, frameSize, R, T, R0, R1, P0, P1, QRect, 0);`
96		99
97	`// Interpolation maps (lens distortion and rectification)`	100	`// Interpolation maps (lens distortion and rectification)`
98	`cv::Mat map0X, map0Y, map1X, map1Y;`	101	`cv::Mat map0X, map0Y, map1X, map1Y;`
99	`cv::initUndistortRectifyMap(calibration.K0, calibration.k0, R0, P0, frameSize, CV_32F, map0X, map0Y);`	102	`cv::initUndistortRectifyMap(calibration.K0, calibration.k0, R0, P0, frameSize, CV_32F, map0X, map0Y);`
100	`cv::initUndistortRectifyMap(calibration.K1, calibration.k1, R1, P1, frameSize, CV_32F, map1X, map1Y);`	103	`cv::initUndistortRectifyMap(calibration.K1, calibration.k1, R1, P1, frameSize, CV_32F, map1X, map1Y);`
101		104
102		105
103	`// Gray-scale and remap`	106	`// Gray-scale and remap`
104	`std::vector<cv::Mat> frames0Rect(N);`	107	`std::vector<cv::Mat> frames0Rect(N);`
105	`std::vector<cv::Mat> frames1Rect(N);`	108	`std::vector<cv::Mat> frames1Rect(N);`
106	`for(unsigned int i=0; i<N; i++){`	109	`for(unsigned int i=0; i<N; i++){`
107	`cv::Mat temp;`	110	`cv::Mat temp;`
108	`cv::cvtColor(frames0[i], temp, CV_BayerBG2GRAY);`	111	`cv::cvtColor(frames0[i], temp, CV_BayerBG2GRAY);`
109	`cv::remap(temp, frames0Rect[i], map0X, map0Y, CV_INTER_LINEAR);`	112	`cv::remap(temp, frames0Rect[i], map0X, map0Y, CV_INTER_LINEAR);`
110	`cv::cvtColor(frames1[i], temp, CV_BayerBG2GRAY);`	113	`cv::cvtColor(frames1[i], temp, CV_BayerBG2GRAY);`
111	`cv::remap(temp, frames1Rect[i], map1X, map1Y, CV_INTER_LINEAR);`	114	`cv::remap(temp, frames1Rect[i], map1X, map1Y, CV_INTER_LINEAR);`
112	`}`	115	`}`
113		116
114	`// Decode camera0`	117	`// Decode camera0`
115	`std::vector<cv::Mat> frames0First(frames0Rect.begin()+2, frames0Rect.begin()+2+nStepsFirst);`	118	`std::vector<cv::Mat> frames0First(frames0Rect.begin()+2, frames0Rect.begin()+2+nStepsFirst);`
116	`std::vector<cv::Mat> frames0Second(frames0Rect.begin()+2+nStepsFirst, frames0Rect.end()-nStepsThird);`	119	`std::vector<cv::Mat> frames0Second(frames0Rect.begin()+2+nStepsFirst, frames0Rect.end()-nStepsThird);`
117	`std::vector<cv::Mat> frames0Third(frames0Rect.end()-nStepsThird, frames0Rect.end());`	120	`std::vector<cv::Mat> frames0Third(frames0Rect.end()-nStepsThird, frames0Rect.end());`
118		121
119	`std::vector<cv::Mat> F0First = getDFTComponents(frames0First);`	122	`std::vector<cv::Mat> F0First = getDFTComponents(frames0First);`
120	`cv::Mat up0First;`	123	`cv::Mat up0First;`
121	`cv::phase(F0First[2], -F0First[3], up0First);`	124	`cv::phase(F0First[2], -F0First[3], up0First);`
122	`std::vector<cv::Mat> F0Second = getDFTComponents(frames0Second);`	125	`std::vector<cv::Mat> F0Second = getDFTComponents(frames0Second);`
123	`cv::Mat up0Second;`	126	`cv::Mat up0Second;`
124	`cv::phase(F0Second[2], -F0Second[3], up0Second);`	127	`cv::phase(F0Second[2], -F0Second[3], up0Second);`
125	`std::vector<cv::Mat> F0Third = getDFTComponents(frames0Third);`	128	`std::vector<cv::Mat> F0Third = getDFTComponents(frames0Third);`
126	`cv::Mat up0Third;`	129	`cv::Mat up0Third;`
127	`cv::phase(F0Third[2], -F0Third[3], up0Third);`	130	`cv::phase(F0Third[2], -F0Third[3], up0Third);`
128		131
129	`cv::Mat up0EquivalentFS = up0First - up0Second;`	132	`cv::Mat up0FS = up0Second - up0First;`
130	`up0EquivalentFS = cvtools::modulo(up0EquivalentFS, 2.0*pi);`	133	`up0FS = cvtools::modulo(up0FS, 2.0*pi);`
131		134
132	`cv::Mat up0EquivalentST = up0Second - up0Third;`	135	`cv::Mat up0ST = up0Third - up0Second;`
133	`up0EquivalentST = cvtools::modulo(up0EquivalentST, 2.0*pi);`	136	`up0ST = cvtools::modulo(up0ST, 2.0*pi);`
134		137
135	`cv::Mat up0EquivalentFST = up0EquivalentFS - up0EquivalentST;`	138	`cv::Mat up0FST = up0ST - up0FS;`
136	`up0EquivalentFST = cvtools::modulo(up0EquivalentFST, 2.0*pi);`	139	`up0FST = cvtools::modulo(up0FST, 2.0*pi);`
137		140
-		141	`cv::Mat up0F = unwrapWithCue(up0First, up0FST, nPeriodsFirst);`
-		142	`cv::Mat up0S = unwrapWithCue(up0Second, up0FST, nPeriodsSecond);`
138	`// TODO: we should use backward phase unwrapping (Song Zhang term)...`	143	`cv::Mat up0T = unwrapWithCue(up0Third, up0FST, nPeriodsThird);`
-		144	`cv::Mat up0Mean = 1.0/3.0 * (up0F + up0S + up0T);`
-		145	`cv::Mat up0Range = cv::max(up0F, cv::max(up0S, up0T)) - cv::min(up0F, cv::min(up0S, up0T));`
-		146	`cv::Mat up0 = up0Mean * screenCols/(2.0*pi);`
139		147
140	`cv::Mat up0 = unwrapWithCue(up0First, up0EquivalentFST, (float)screenCols/nPeriodsFirst);`	-
141	`up0 = screenCols/(2.0pi);`	-
142	`cv::Mat amplitude0;`	148	`cv::Mat amplitude0;`
143	`cv::magnitude(F0First[2], -F0First[3], amplitude0);`	149	`cv::magnitude(F0First[2], -F0First[3], amplitude0);`
144		150
145	`// Decode camera1`	151	`// Decode camera1`
146	`std::vector<cv::Mat> frames1First(frames1Rect.begin()+2, frames1Rect.begin()+2+nStepsFirst);`	152	`std::vector<cv::Mat> frames1First(frames1Rect.begin()+2, frames1Rect.begin()+2+nStepsFirst);`
147	`std::vector<cv::Mat> frames1Second(frames1Rect.begin()+2+nStepsFirst, frames1Rect.end()-nStepsThird);`	153	`std::vector<cv::Mat> frames1Second(frames1Rect.begin()+2+nStepsFirst, frames1Rect.end()-nStepsThird);`
148	`std::vector<cv::Mat> frames1Third(frames1Rect.end()-nStepsThird, frames1Rect.end());`	154	`std::vector<cv::Mat> frames1Third(frames1Rect.end()-nStepsThird, frames1Rect.end());`
149		155
150	`std::vector<cv::Mat> F1First = getDFTComponents(frames1First);`	156	`std::vector<cv::Mat> F1First = getDFTComponents(frames1First);`
151	`cv::Mat up1First;`	157	`cv::Mat up1First;`
152	`cv::phase(F1First[2], -F1First[3], up1First);`	158	`cv::phase(F1First[2], -F1First[3], up1First);`
153	`std::vector<cv::Mat> F1Second = getDFTComponents(frames1Second);`	159	`std::vector<cv::Mat> F1Second = getDFTComponents(frames1Second);`
154	`cv::Mat up1Second;`	160	`cv::Mat up1Second;`
155	`cv::phase(F1Second[2], -F1Second[3], up1Second);`	161	`cv::phase(F1Second[2], -F1Second[3], up1Second);`
156	`std::vector<cv::Mat> F1Third = getDFTComponents(frames1Third);`	162	`std::vector<cv::Mat> F1Third = getDFTComponents(frames1Third);`
157	`cv::Mat up1Third;`	163	`cv::Mat up1Third;`
158	`cv::phase(F1Third[2], -F1Third[3], up1Third);`	164	`cv::phase(F1Third[2], -F1Third[3], up1Third);`
159		165
160	`cv::Mat up1EquivalentFS = up1First - up1Second;`	166	`cv::Mat up1FS = up1Second - up1First;`
161	`up1EquivalentFS = cvtools::modulo(up1EquivalentFS, 2.0*pi);`	167	`up1FS = cvtools::modulo(up1FS, 2.0*pi);`
162		168
163	`cv::Mat up1EquivalentST = up1Second - up1Third;`	169	`cv::Mat up1ST = up1Third - up1Second;`
164	`up1EquivalentST = cvtools::modulo(up1EquivalentST, 2.0*pi);`	170	`up1ST = cvtools::modulo(up1ST, 2.0*pi);`
165		171
166	`cv::Mat up1EquivalentFST = up1EquivalentFS - up1EquivalentST;`	172	`cv::Mat up1FST = up1ST - up1FS;`
167	`up1EquivalentFST = cvtools::modulo(up1EquivalentFST, 2.0*pi);`	173	`up1FST = cvtools::modulo(up1FST, 2.0*pi);`
-		174
-		175	`cv::Mat up1F = unwrapWithCue(up1First, up1FST, nPeriodsFirst);`
-		176	`cv::Mat up1S = unwrapWithCue(up1Second, up1FST, nPeriodsSecond);`
-		177	`cv::Mat up1T = unwrapWithCue(up1Third, up1FST, nPeriodsThird);`
-		178	`cv::Mat up1Mean = 1.0/3.0 * (up1F + up1S + up1T);`
-		179	`cv::Mat up1Range = cv::max(up1F, cv::max(up1S, up1T)) - cv::min(up1F, cv::min(up1S, up1T));`
-		180	`cv::Mat up1 = up1Mean * screenCols/(2.0*pi);`
168		181
169	`cv::Mat up1 = unwrapWithCue(up1First, up1EquivalentFST, (float)screenCols/nPeriodsFirst);`	-
170	`up1 = screenCols/(2.0pi);`	-
171	`cv::Mat amplitude1;`	182	`cv::Mat amplitude1;`
172	`cv::magnitude(F1First[2], -F1First[3], amplitude1);`	183	`cv::magnitude(F1First[2], -F1First[3], amplitude1);`
173		184
174	`#ifdef QT_DEBUG`	185	`#ifdef QT_DEBUG`
175	`cvtools::writeMat(up0First, "up0First.mat", "up0First");`	186	`cvtools::writeMat(up0First, "up0First.mat", "up0First");`
176	`cvtools::writeMat(up0Second, "up0Second.mat", "up0Second");`	187	`cvtools::writeMat(up0Second, "up0Second.mat", "up0Second");`
177	`cvtools::writeMat(up0Third, "up0Third.mat", "up0Third");`	188	`cvtools::writeMat(up0Third, "up0Third.mat", "up0Third");`
178	`cvtools::writeMat(up0EquivalentFS, "up0EquivalentFS.mat", "up0EquivalentFS");`	189	`cvtools::writeMat(up0FS, "up0FS.mat", "up0FS");`
179	`cvtools::writeMat(up0EquivalentST, "up0EquivalentST.mat", "up0EquivalentST");`	190	`cvtools::writeMat(up0ST, "up0ST.mat", "up0ST");`
180	`cvtools::writeMat(up0EquivalentFST, "up0EquivalentFST.mat", "up0EquivalentFST");`	191	`cvtools::writeMat(up0FST, "up0FST.mat", "up0FST");`
-		192	`cvtools::writeMat(up0Mean, "up0Mean.mat", "up0Mean");`
-		193	`cvtools::writeMat(up0Range, "up0Range.mat", "up0Range");`
181	`cvtools::writeMat(up0, "up0.mat", "up0");`	194	`cvtools::writeMat(up0, "up0.mat", "up0");`
182	`cvtools::writeMat(up1, "up1.mat", "up1");`	195	`cvtools::writeMat(up1, "up1.mat", "up1");`
183	`cvtools::writeMat(amplitude0, "amplitude0.mat", "amplitude0");`	196	`cvtools::writeMat(amplitude0, "amplitude0.mat", "amplitude0");`
184		197
185	`cvtools::writeMat(amplitude0, "amplitude0.mat", "amplitude0");`	198	`cvtools::writeMat(amplitude0, "amplitude0.mat", "amplitude0");`
186	`cvtools::writeMat(amplitude1, "amplitude1.mat", "amplitude1");`	199	`cvtools::writeMat(amplitude1, "amplitude1.mat", "amplitude1");`
187	`#endif`	200	`#endif`
188		201
189	`// color debayer and remap`	202	`// color debayer and remap`
190	`cv::Mat color0, color1;`	203	`cv::Mat color0, color1;`
191	`cv::cvtColor(frames0[0], color0, CV_BayerBG2RGB);`	204	`cv::cvtColor(frames0[0], color0, CV_BayerBG2RGB);`
192	`cv::remap(color0, color0, map0X, map0Y, CV_INTER_LINEAR);`	205	`cv::remap(color0, color0, map0X, map0Y, CV_INTER_LINEAR);`
193		206
194	`cv::cvtColor(frames1[0], color1, CV_BayerBG2RGB);`	207	`cv::cvtColor(frames1[0], color1, CV_BayerBG2RGB);`
195	`cv::remap(color1, color1, map1X, map1Y, CV_INTER_LINEAR);`	208	`cv::remap(color1, color1, map1X, map1Y, CV_INTER_LINEAR);`
196		209
197	`#ifdef QT_DEBUG`	210	`#ifdef QT_DEBUG`
198	`cvtools::writeMat(color0, "color0.mat", "color0");`	211	`cvtools::writeMat(color0, "color0.mat", "color0");`
199	`cvtools::writeMat(color1, "color1.mat", "color1");`	212	`cvtools::writeMat(color1, "color1.mat", "color1");`
200	`#endif`	213	`#endif`
201		214
202	`// Occlusion masks`	215	`// Occlusion masks`
203	`cv::Mat occlusion0, occlusion1;`	216	`cv::Mat occlusion0, occlusion1;`
204	`cv::subtract(frames0Rect[0], frames0Rect[1], occlusion0);`	217	`cv::subtract(frames0Rect[0], frames0Rect[1], occlusion0);`
205	`occlusion0 = (occlusion0 > 5) & (occlusion0 < 250);`	218	`occlusion0 = (occlusion0 > 25) & (occlusion0 < 250);`
206	`cv::subtract(frames1Rect[0], frames1Rect[1], occlusion1);`	219	`cv::subtract(frames1Rect[0], frames1Rect[1], occlusion1);`
207	`occlusion1 = (occlusion1 > 5) & (occlusion1 < 250);`	220	`occlusion1 = (occlusion1 > 25) & (occlusion1 < 250);`
-		221
-		222	`// Threshold on range`
-		223	`occlusion0 = occlusion0 & (up0Range < 0.05);`
-		224	`occlusion1 = occlusion1 & (up1Range < 0.05);`
-		225
-		226	`// // Erode occlusion masks`
-		227	`// cv::Mat strel = cv::getStructuringElement(cv::MORPH_ELLIPSE, cv::Size(5,5));`
-		228	`// cv::erode(occlusion0, occlusion0, strel);`
-		229	`// cv::erode(occlusion1, occlusion1, strel);`
208		230
209	`// Threshold on gradient of phase`	231	`// Threshold on gradient of phase`
210	`cv::Mat edges0;`	232	`cv::Mat edges0;`
211	`cv::Sobel(up0, edges0, -1, 1, 1, 5);`	233	`cv::Sobel(up0, edges0, -1, 1, 1, 5);`
212	`occlusion0 = occlusion0 & (abs(edges0) < 150);`	234	`occlusion0 = occlusion0 & (abs(edges0) < 150);`
213	`cv::Mat edges1;`	235	`cv::Mat edges1;`
214	`cv::Sobel(up1, edges1, -1, 1, 1, 5);`	236	`cv::Sobel(up1, edges1, -1, 1, 1, 5);`
215	`occlusion1 = occlusion1 & (abs(edges1) < 150);`	237	`occlusion1 = occlusion1 & (abs(edges1) < 150);`
216		238
217	`#ifdef QT_DEBUG`	239	`#ifdef QT_DEBUG`
218	`cvtools::writeMat(edges0, "edges0.mat", "edges0");`	240	`cvtools::writeMat(edges0, "edges0.mat", "edges0");`
219	`cvtools::writeMat(edges1, "edges1.mat", "edges1");`	241	`cvtools::writeMat(edges1, "edges1.mat", "edges1");`
220	`#endif`	242	`#endif`
221		243
222	`#ifdef QT_DEBUG`	244	`#ifdef QT_DEBUG`
223	`cvtools::writeMat(occlusion0, "occlusion0.mat", "occlusion0");`	245	`cvtools::writeMat(occlusion0, "occlusion0.mat", "occlusion0");`
224	`cvtools::writeMat(occlusion1, "occlusion1.mat", "occlusion1");`	246	`cvtools::writeMat(occlusion1, "occlusion1.mat", "occlusion1");`
225	`#endif`	247	`#endif`
226		248
227	`// Match phase maps`	249	`// Match phase maps`
228	`int frameRectRows = map0X.rows;`	250	`int frameRectRows = map0X.rows;`
229	`int frameRectCols = map0X.cols;`	251	`int frameRectCols = map0X.cols;`
230		252
231	`// camera0 against camera1`	253	`// camera0 against camera1`
232	`std::vector<cv::Vec2f> q0, q1;`	254	`std::vector<cv::Vec2f> q0, q1;`
233	`for(int row=0; row<frameRectRows; row++){`	255	`for(int row=0; row<frameRectRows; row++){`
234	`for(int col=0; col<frameRectCols; col++){`	256	`for(int col=0; col<frameRectCols; col++){`
235		257
236	`if(!occlusion0.at<char>(row,col))`	258	`if(!occlusion0.at<char>(row,col))`
237	`continue;`	259	`continue;`
238		260
239	`float up0i = up0.at<float>(row,col);`	261	`float up0i = up0.at<float>(row,col);`
240	`for(int col1=0; col1<up1.cols-1; col1++){`	262	`for(int col1=0; col1<up1.cols-1; col1++){`
241		263
242	`if(!occlusion1.at<char>(row,col1) \|\| !occlusion1.at<char>(row,col1+1))`	264	`if(!occlusion1.at<char>(row,col1) \|\| !occlusion1.at<char>(row,col1+1))`
243	`continue;`	265	`continue;`
244		266
245	`float up1Left = up1.at<float>(row,col1);`	267	`float up1Left = up1.at<float>(row,col1);`
246	`float up1Right = up1.at<float>(row,col1+1);`	268	`float up1Right = up1.at<float>(row,col1+1);`
247		269
248	`if((up1Left <= up0i) && (up0i <= up1Right) && (up0i-up1Left < 1.0) && (up1Right-up0i < 1.0) && (up1Right-up1Left > 0.1)){`	270	`if((up1Left <= up0i) && (up0i <= up1Right) && (up0i-up1Left < 1.0) && (up1Right-up0i < 1.0) && (up1Right-up1Left > 0.1)){`
249		271
250	`float col1i = col1 + (up0i-up1Left)/(up1Right-up1Left);`	272	`float col1i = col1 + (up0i-up1Left)/(up1Right-up1Left);`
251		273
252	`q0.push_back(cv::Point2f(col, row));`	274	`q0.push_back(cv::Point2f(col, row));`
253	`q1.push_back(cv::Point2f(col1i, row));`	275	`q1.push_back(cv::Point2f(col1i, row));`
254		276
255	`break;`	277	`break;`
256	`}`	278	`}`
257	`}`	279	`}`
258	`}`	280	`}`
259	`}`	281	`}`
260		282
261	`int nMatches = q0.size();`	283	`int nMatches = q0.size();`
262		284
263	`if(nMatches < 1){`	285	`if(nMatches < 1){`
264	`Q.resize(0);`	286	`Q.resize(0);`
265	`color.resize(0);`	287	`color.resize(0);`
266		288
267	`return;`	289	`return;`
268	`}`	290	`}`
269		291
270	`// Retrieve color information`	292	`// Retrieve color information`
271	`color.resize(nMatches);`	293	`color.resize(nMatches);`
272	`for(int i=0; i<nMatches; i++){`	294	`for(int i=0; i<nMatches; i++){`
273		295
274	`cv::Vec3b c0 = color0.at<cv::Vec3b>(q0[i][1], q0[i][0]);`	296	`cv::Vec3b c0 = color0.at<cv::Vec3b>(q0[i][1], q0[i][0]);`
275	`cv::Vec3b c1 = color1.at<cv::Vec3b>(q1[i][1], q1[i][0]);`	297	`cv::Vec3b c1 = color1.at<cv::Vec3b>(q1[i][1], q1[i][0]);`
276		298
277	`color[i] = 0.5c0 + 0.5c1;`	299	`color[i] = 0.5c0 + 0.5c1;`
278	`}`	300	`}`
279		301
280	`// Triangulate points`	302	`// Triangulate points`
281	`cv::Mat QMatHomogenous, QMat;`	303	`cv::Mat QMatHomogenous, QMat;`
282	`cv::triangulatePoints(P0, P1, q0, q1, QMatHomogenous);`	304	`cv::triangulatePoints(P0, P1, q0, q1, QMatHomogenous);`
283	`cvtools::convertMatFromHomogeneous(QMatHomogenous, QMat);`	305	`cvtools::convertMatFromHomogeneous(QMatHomogenous, QMat);`
284		306
285	`// Undo rectification`	307	`// Undo rectification`
286	`cv::Mat R0Inv;`	308	`cv::Mat R0Inv;`
287	`cv::Mat(R0.t()).convertTo(R0Inv, CV_32F);`	309	`cv::Mat(R0.t()).convertTo(R0Inv, CV_32F);`
288	`QMat = R0Inv*QMat;`	310	`QMat = R0Inv*QMat;`
289		311
290	`cvtools::matToPoints3f(QMat, Q);`	312	`cvtools::matToPoints3f(QMat, Q);`
291		313
292	`}`	314	`}`
293		315

Subversion Repositories seema-scanner

(root)/src/algorithm/AlgorithmPhaseShiftThreeFreq.cpp – Rev 189 → 190