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


#include "AlgorithmPhaseShift.h"
#include <math.h>
 
#include "cvtools.h"
 
#ifndef M_PI
    #define M_PI 3.14159265358979323846
#endif
 
static unsigned int nPhases = 40;
static unsigned int nSteps = 20;
 
// Algorithm
static cv::Mat computePhaseVector(unsigned int length, float phase, float pitch){
 
    cv::Mat phaseVector(length, 1, CV_8UC3);
    //phaseVector.setTo(0);
 
    const float pi = M_PI;
 
    // Loop through vector
    for(int i=0; i<phaseVector.rows; i++){
        // Amplitude of channels
        float amp = 0.5*(1+cos(2*pi*i/pitch - phase));
        phaseVector.at<cv::Vec3b>(i, 0) = cv::Vec3b(255.0*amp,255.0*amp,255.0*amp);
    }
 
    return phaseVector;
}
 
AlgorithmPhaseShift::AlgorithmPhaseShift(unsigned int _screenCols, unsigned int _screenRows) : Algorithm(_screenCols, _screenRows){
 
    // Set N
    N = 2+2*nSteps;
 
    // 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;
 
    // Horizontally encoding patterns
    for(unsigned int i=0; i<nSteps; i++){
        float phase = 2.0*pi/nSteps * i;
        float pitch = (float)screenCols/(float)nPhases;
        cv::Mat patternI(1,1,CV_8U);
        patternI = computePhaseVector(screenCols, phase, pitch);
        patterns.push_back(patternI.t());
    }
 
    // Phase cue patterns
    for(unsigned int i=0; i<nSteps; i++){
        float phase = 2.0*pi/nSteps * i;
        float pitch = screenCols;
        cv::Mat patternI(1,1,CV_8U);
        patternI = computePhaseVector(screenCols, phase, pitch);
        patterns.push_back(patternI.t());
    }
 
 
}
 
cv::Mat AlgorithmPhaseShift::getEncodingPattern(unsigned int depth){
    return patterns[depth];
}
 
 
// Absolute phase from 3 frames
cv::Mat getPhase(const cv::Mat I1, const cv::Mat I2, const cv::Mat I3){
 
    cv::Mat_<float> I1_(I1);
    cv::Mat_<float> I2_(I2);
    cv::Mat_<float> I3_(I3);
 
    cv::Mat phase;
 
    // One call approach
    cv::phase(2.0*I1_-I3_-I2_, sqrt(3.0)*(I2_-I3_), phase);
    return phase;
 
}
 
// Phase unwrapping by means of a phase cue
cv::Mat unwrapWithCue(const cv::Mat up, const cv::Mat upCue, unsigned int nPhases){
 
    const float pi = M_PI;
 
    // Determine number of jumps
    cv::Mat P = (upCue*nPhases-up)/(2*pi);
 
    // Round to integers
    P.convertTo(P, CV_8U);
    P.convertTo(P, CV_32F);
 
    // Add to phase
    cv::Mat upUnwrapped = up + P*2*pi;
 
    // Scale to range [0; 2pi]
    upUnwrapped *= 1.0/nPhases;
 
    return upUnwrapped;
}
 
// Absolute phase and magnitude from N frames
std::vector<cv::Mat> getDFTComponents(const std::vector<cv::Mat> frames){
 
    unsigned int N = frames.size();
 
//    std::vector<cv::Mat> framesReverse = frames;
//    std::reverse(framesReverse.begin(), framesReverse.end());
 
    // DFT approach
    cv::Mat I;
    cv::merge(frames, I);
    unsigned int w = I.cols;
    unsigned int h = I.rows;
    I = I.reshape(1, h*w);
    I.convertTo(I, CV_32F);
    cv::Mat fI;
    cv::dft(I, fI, cv::DFT_ROWS + cv::DFT_COMPLEX_OUTPUT);
    fI = fI.reshape(N*2, h);
 
    std::vector<cv::Mat> fIcomp;
    cv::split(fI, fIcomp);
 
    return fIcomp;
 
}
 
void AlgorithmPhaseShift::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;
 
    // Gray-scale everything
    std::vector<cv::Mat> frames0Gray(N);
    std::vector<cv::Mat> frames1Gray(N);
    for(int i=0; i<N; i++){
        cv::cvtColor(frames0[i], frames0Gray[i], CV_RGB2GRAY);
        cv::cvtColor(frames1[i], frames1Gray[i], CV_RGB2GRAY);
    }
 
    // Decode camera0
    std::vector<cv::Mat> frames0Enc(frames0Gray.begin()+2, frames0Gray.begin()+2+nSteps);
    std::vector<cv::Mat> frames0Cue(frames0Gray.begin()+2+nSteps, frames0Gray.begin()+2+nSteps+nSteps);
    std::vector<cv::Mat> F0Enc = getDFTComponents(frames0Enc);
    cv::Mat up0;
    cv::phase(F0Enc[2], -F0Enc[3], up0);
cvtools::writeMat(up0, "up0.mat", "up0");
    std::vector<cv::Mat> F0Cue = getDFTComponents(frames0Cue);
    cv::Mat up0Cue;
    cv::phase(F0Cue[2], -F0Cue[3], up0Cue);
    up0 = unwrapWithCue(up0, up0Cue, nPhases);
    up0 *= screenCols/(2*pi);
 
    // Decode camera1
    std::vector<cv::Mat> frames1Enc(frames1Gray.begin()+2, frames1Gray.begin()+2+nSteps);
    std::vector<cv::Mat> frames1Cue(frames1Gray.begin()+2+nSteps, frames1Gray.begin()+2+nSteps+nSteps);
    std::vector<cv::Mat> f1Icomp = getDFTComponents(frames1Enc);
    cv::Mat up1;
    cv::phase(f1Icomp[2], -f1Icomp[3], up1);
cvtools::writeMat(up1, "up1.mat", "up1");
    std::vector<cv::Mat> F1Cue = getDFTComponents(frames1Cue);
    cv::Mat up1Cue;
    cv::phase(F1Cue[2], -F1Cue[3], up1Cue);
    up1 = unwrapWithCue(up1, up1Cue, nPhases);
    up1 *= screenCols/(2*pi);
 
cvtools::writeMat(up0Cue, "up0Cue.mat", "up0Cue");
cvtools::writeMat(up1Cue, "up1Cue.mat", "up1Cue");
 
    // 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);
 
    // Phase remaps
    cv::Mat up0Rect, up1Rect;
    cv::remap(up0, up0Rect, map0X, map0Y, CV_INTER_CUBIC);
    cv::remap(up1, up1Rect, map1X, map1Y, CV_INTER_CUBIC);
 
cvtools::writeMat(up0Rect, "up0Rect.mat", "up0Rect");
cvtools::writeMat(up1Rect, "up1Rect.mat", "up1Rect");
 
    // Color remaps
    cv::Mat color0Rect, color1Rect;
    cv::remap(frames0[0], color0Rect, map0X, map0Y, CV_INTER_CUBIC);
    cv::remap(frames1[0], color1Rect, map1X, map1Y, CV_INTER_CUBIC);
 
cvtools::writeMat(color0Rect, "color0Rect.mat", "color0Rect");
cvtools::writeMat(color1Rect, "color1Rect.mat", "color1Rect");
 
    // On/off remaps
    cv::Mat frames0OnRect, frames0OffRect;
    cv::remap(frames0Gray[0], frames0OnRect, map0X, map0Y, CV_INTER_CUBIC);
    cv::remap(frames0Gray[1], frames0OffRect, map0X, map0Y, CV_INTER_CUBIC);
 
    cv::Mat frames1OnRect, frames1OffRect;
    cv::remap(frames1Gray[0], frames1OnRect, map1X, map1Y, CV_INTER_CUBIC);
    cv::remap(frames1Gray[1], frames1OffRect, map1X, map1Y, CV_INTER_CUBIC);
 
    // Occlusion masks
    cv::Mat occlusion0Rect, occlusion1Rect;
    cv::subtract(frames0OnRect, frames0OffRect, occlusion0Rect);
    occlusion0Rect = occlusion0Rect > 50;
    cv::subtract(frames1OnRect, frames1OffRect, occlusion1Rect);
    occlusion1Rect = occlusion1Rect > 50;
 
cvtools::writeMat(occlusion0Rect, "occlusion0Rect.mat", "occlusion0Rect");
cvtools::writeMat(occlusion1Rect, "occlusion1Rect.mat", "occlusion1Rect");
 
    // Erode occlusion masks
    cv::Mat strel = cv::getStructuringElement(cv::MORPH_ELLIPSE, cv::Size(3,3));
    cv::erode(occlusion0Rect, occlusion0Rect, strel);
    cv::erode(occlusion1Rect, occlusion1Rect, strel);
 
    // Threshold on gradient of phase
    cv::Mat edges0;
    cv::Sobel(up0Rect, edges0, -1, 1, 1, 5);
    occlusion0Rect = occlusion0Rect & (abs(edges0) < 5);
 
    cv::Mat edges1;
    cv::Sobel(up1Rect, edges1, -1, 1, 1, 5);
    occlusion1Rect = occlusion1Rect & (abs(edges1) < 5);
 
cvtools::writeMat(edges0, "edges0.mat", "edges0");
cvtools::writeMat(edges1, "edges1.mat", "edges1");
 
    // Match phase maps
    int frameRectRows = map0X.rows;
    int frameRectCols = map0X.cols;
 
    // camera0 against camera1
    std::vector<cv::Vec2f> q0Rect, q1Rect;
    for(int row=0; row<frameRectRows; row++){
 
        for(int col=0; col<frameRectCols; col++){
 
            if(!occlusion0Rect.at<char>(row,col))
                continue;
 
            float up0i = up0Rect.at<float>(row,col);
            for(int col1=0; col1<up1Rect.cols-1; col1++){
 
                if(!occlusion1Rect.at<char>(row,col1) || !occlusion1Rect.at<char>(row,col1+1))
                    continue;
 
                float up1Left = up1Rect.at<float>(row,col1);
                float up1Right = up1Rect.at<float>(row,col1+1);
 
                if((up1Left <= up0i) && (up0i <= up1Right)){
 
                    float col1i = col1 + (up0i-up1Left)/(up1Right-up1Left);
 
                    q0Rect.push_back(cv::Point2f(col, row));
                    q1Rect.push_back(cv::Point2f(col1i, row));
 
                    break;
                }
 
            }
 
        }
 
    }
 
    // camera1 against camera0
    //[...]
 
    int nMatches = q0Rect.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 = color0Rect.at<cv::Vec3b>(q0Rect[i][1], q0Rect[i][0]);
        cv::Vec3b c1 = color1Rect.at<cv::Vec3b>(q1Rect[i][1], q1Rect[i][0]);
 
        color[i] = 0.5*c0 + 0.5*c1;
    }
 
    // Triangulate points
    cv::Mat QMatHomogenous, QMat;
    cv::triangulatePoints(P0, P1, q0Rect, q1Rect, 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 71	Rev 72
1	`#include "AlgorithmPhaseShift.h"`	1	`#include "AlgorithmPhaseShift.h"`
2	`#include <math.h>`	2	`#include <math.h>`
3		3
4	`#include "cvtools.h"`	4	`#include "cvtools.h"`
5		5
6	`#ifndef M_PI`	6	`#ifndef M_PI`
7	`#define M_PI 3.14159265358979323846`	7	`#define M_PI 3.14159265358979323846`
8	`#endif`	8	`#endif`
9		9
10	`static unsigned int nPhases = 40;`	10	`static unsigned int nPhases = 40;`
11	`static unsigned int nSteps = 20;`	11	`static unsigned int nSteps = 20;`
12		12
13	`// Algorithm`	13	`// Algorithm`
14	`static cv::Mat computePhaseVector(unsigned int length, float phase, float pitch){`	14	`static cv::Mat computePhaseVector(unsigned int length, float phase, float pitch){`
15		15
16	`cv::Mat phaseVector(length, 1, CV_8UC3);`	16	`cv::Mat phaseVector(length, 1, CV_8UC3);`
17	`//phaseVector.setTo(0);`	17	`//phaseVector.setTo(0);`
18		18
19	`const float pi = M_PI;`	19	`const float pi = M_PI;`
20		20
21	`// Loop through vector`	21	`// Loop through vector`
22	`for(int i=0; i<phaseVector.rows; i++){`	22	`for(int i=0; i<phaseVector.rows; i++){`
23	`// Amplitude of channels`	23	`// Amplitude of channels`
24	`float amp = 0.5(1+cos(2pi*i/pitch - phase));`	24	`float amp = 0.5(1+cos(2pi*i/pitch - phase));`
25	`phaseVector.at<cv::Vec3b>(i, 0) = cv::Vec3b(255.0amp,255.0amp,255.0*amp);`	25	`phaseVector.at<cv::Vec3b>(i, 0) = cv::Vec3b(255.0amp,255.0amp,255.0*amp);`
26	`}`	26	`}`
27		27
28	`return phaseVector;`	28	`return phaseVector;`
29	`}`	29	`}`
30		30
31	`AlgorithmPhaseShift::AlgorithmPhaseShift(unsigned int _screenCols, unsigned int _screenRows) : Algorithm(_screenCols, _screenRows){`	31	`AlgorithmPhaseShift::AlgorithmPhaseShift(unsigned int _screenCols, unsigned int _screenRows) : Algorithm(_screenCols, _screenRows){`
32		32
-		33	`// Set N`
-		34	`N = 2+2*nSteps;`
-		35
33	`// all on pattern`	36	`// all on pattern`
34	`cv::Mat allOn(1, screenCols, CV_8UC3, cv::Scalar::all(255));`	37	`cv::Mat allOn(1, screenCols, CV_8UC3, cv::Scalar::all(255));`
35	`patterns.push_back(allOn);`	38	`patterns.push_back(allOn);`
36		39
37	`// all off pattern`	40	`// all off pattern`
38	`cv::Mat allOff(1, screenCols, CV_8UC3, cv::Scalar::all(0));`	41	`cv::Mat allOff(1, screenCols, CV_8UC3, cv::Scalar::all(0));`
39	`patterns.push_back(allOff);`	42	`patterns.push_back(allOff);`
40		43
41	`// Set N`	-
42	`N = 2+nSteps+3;`	-
43		-
44	`// Precompute encoded patterns`	44	`// Precompute encoded patterns`
45	`const float pi = M_PI;`	45	`const float pi = M_PI;`
46		46
47	`// Horizontally encoding patterns`	47	`// Horizontally encoding patterns`
48	`for(unsigned int i=0; i<nSteps; i++){`	48	`for(unsigned int i=0; i<nSteps; i++){`
49	`float phase = 2.0pi/nSteps i;`	49	`float phase = 2.0pi/nSteps i;`
50	`float pitch = (float)screenCols/(float)nPhases;`	50	`float pitch = (float)screenCols/(float)nPhases;`
51	`cv::Mat patternI(1,1,CV_8U);`	51	`cv::Mat patternI(1,1,CV_8U);`
52	`patternI = computePhaseVector(screenCols, phase, pitch);`	52	`patternI = computePhaseVector(screenCols, phase, pitch);`
53	`patterns.push_back(patternI.t());`	53	`patterns.push_back(patternI.t());`
54	`}`	54	`}`
55		55
56	`// Phase cue patterns`	56	`// Phase cue patterns`
57	`for(unsigned int i=0; i<3; i++){`	57	`for(unsigned int i=0; i<nSteps; i++){`
58	`float phase = 2.0pi/3.0 i;`	58	`float phase = 2.0pi/nSteps i;`
59	`float pitch = screenCols;`	59	`float pitch = screenCols;`
60	`cv::Mat patternI;`	60	`cv::Mat patternI(1,1,CV_8U);`
61	`patternI = computePhaseVector(screenCols, phase, pitch);`	61	`patternI = computePhaseVector(screenCols, phase, pitch);`
62	`patterns.push_back(patternI.t());`	62	`patterns.push_back(patternI.t());`
63	`}`	63	`}`
64		64
-		65
65	`}`	66	`}`
66		67
67	`cv::Mat AlgorithmPhaseShift::getEncodingPattern(unsigned int depth){`	68	`cv::Mat AlgorithmPhaseShift::getEncodingPattern(unsigned int depth){`
68	`return patterns[depth];`	69	`return patterns[depth];`
69	`}`	70	`}`
70		71
71		72
72	`// Absolute phase from 3 frames`	73	`// Absolute phase from 3 frames`
73	`cv::Mat getPhase(const cv::Mat I1, const cv::Mat I2, const cv::Mat I3){`	74	`cv::Mat getPhase(const cv::Mat I1, const cv::Mat I2, const cv::Mat I3){`
74		75
75	`cv::Mat_<float> I1_(I1);`	76	`cv::Mat_<float> I1_(I1);`
76	`cv::Mat_<float> I2_(I2);`	77	`cv::Mat_<float> I2_(I2);`
77	`cv::Mat_<float> I3_(I3);`	78	`cv::Mat_<float> I3_(I3);`
78		79
79	`cv::Mat phase;`	80	`cv::Mat phase;`
80		81
81	`// One call approach`	82	`// One call approach`
82	`cv::phase(2.0I1_-I3_-I2_, sqrt(3.0)(I2_-I3_), phase);`	83	`cv::phase(2.0I1_-I3_-I2_, sqrt(3.0)(I2_-I3_), phase);`
83	`return phase;`	84	`return phase;`
84		85
85	`}`	86	`}`
86		87
87	`// Phase unwrapping by means of a phase cue`	88	`// Phase unwrapping by means of a phase cue`
88	`cv::Mat unwrapWithCue(const cv::Mat up, const cv::Mat upCue, unsigned int nPhases){`	89	`cv::Mat unwrapWithCue(const cv::Mat up, const cv::Mat upCue, unsigned int nPhases){`
89		90
90	`const float pi = M_PI;`	91	`const float pi = M_PI;`
91		92
92	`// Determine number of jumps`	93	`// Determine number of jumps`
93	`cv::Mat P = (upCuenPhases-up)/(2pi);`	94	`cv::Mat P = (upCuenPhases-up)/(2pi);`
94		95
95	`// Round to integers`	96	`// Round to integers`
96	`P.convertTo(P, CV_8U);`	97	`P.convertTo(P, CV_8U);`
97	`P.convertTo(P, CV_32F);`	98	`P.convertTo(P, CV_32F);`
98		99
99	`// Add to phase`	100	`// Add to phase`
100	`cv::Mat upUnwrapped = up + P2pi;`	101	`cv::Mat upUnwrapped = up + P2pi;`
101		102
102	`// Scale to range [0; 2pi]`	103	`// Scale to range [0; 2pi]`
103	`upUnwrapped *= 1.0/nPhases;`	104	`upUnwrapped *= 1.0/nPhases;`
104		105
105	`return upUnwrapped;`	106	`return upUnwrapped;`
106	`}`	107	`}`
107		108
108	`// Absolute phase and magnitude from N frames`	109	`// Absolute phase and magnitude from N frames`
109	`std::vector<cv::Mat> getDFTComponents(const std::vector<cv::Mat> frames){`	110	`std::vector<cv::Mat> getDFTComponents(const std::vector<cv::Mat> frames){`
110		111
111	`unsigned int N = frames.size();`	112	`unsigned int N = frames.size();`
112		113
113	`// std::vector<cv::Mat> framesReverse = frames;`	114	`// std::vector<cv::Mat> framesReverse = frames;`
114	`// std::reverse(framesReverse.begin(), framesReverse.end());`	115	`// std::reverse(framesReverse.begin(), framesReverse.end());`
115		116
116	`// DFT approach`	117	`// DFT approach`
117	`cv::Mat I;`	118	`cv::Mat I;`
118	`cv::merge(frames, I);`	119	`cv::merge(frames, I);`
119	`unsigned int w = I.cols;`	120	`unsigned int w = I.cols;`
120	`unsigned int h = I.rows;`	121	`unsigned int h = I.rows;`
121	`I = I.reshape(1, h*w);`	122	`I = I.reshape(1, h*w);`
122	`I.convertTo(I, CV_32F);`	123	`I.convertTo(I, CV_32F);`
123	`cv::Mat fI;`	124	`cv::Mat fI;`
124	`cv::dft(I, fI, cv::DFT_ROWS + cv::DFT_COMPLEX_OUTPUT);`	125	`cv::dft(I, fI, cv::DFT_ROWS + cv::DFT_COMPLEX_OUTPUT);`
125	`fI = fI.reshape(N*2, h);`	126	`fI = fI.reshape(N*2, h);`
126		127
127	`std::vector<cv::Mat> fIcomp;`	128	`std::vector<cv::Mat> fIcomp;`
128	`cv::split(fI, fIcomp);`	129	`cv::split(fI, fIcomp);`
129		130
130	`return fIcomp;`	131	`return fIcomp;`
131		132
132	`}`	133	`}`
133		134
134	`void AlgorithmPhaseShift::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){`	135	`void AlgorithmPhaseShift::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){`
135		136
136	`const float pi = M_PI;`	137	`const float pi = M_PI;`
137		138
138	`assert(frames0.size() == N);`	139	`assert(frames0.size() == N);`
139	`assert(frames1.size() == N);`	140	`assert(frames1.size() == N);`
140		141
141	`int frameRows = frames0[0].rows;`	142	`int frameRows = frames0[0].rows;`
142	`int frameCols = frames0[0].cols;`	143	`int frameCols = frames0[0].cols;`
143		144
144	`// Gray-scale`	145	`// Gray-scale everything`
145	`std::vector<cv::Mat> frames0Gray(N);`	146	`std::vector<cv::Mat> frames0Gray(N);`
146	`std::vector<cv::Mat> frames1Gray(N);`	147	`std::vector<cv::Mat> frames1Gray(N);`
147	`for(int i=0; i<N; i++){`	148	`for(int i=0; i<N; i++){`
148	`cv::cvtColor(frames0[i], frames0Gray[i], CV_RGB2GRAY);`	149	`cv::cvtColor(frames0[i], frames0Gray[i], CV_RGB2GRAY);`
149	`cv::cvtColor(frames1[i], frames1Gray[i], CV_RGB2GRAY);`	150	`cv::cvtColor(frames1[i], frames1Gray[i], CV_RGB2GRAY);`
150	`}`	151	`}`
151		152
152	`// Decode camera0`	153	`// Decode camera0`
153	`std::vector<cv::Mat> frames0Enc(frames0Gray.begin()+2, frames0Gray.begin()+2+nSteps);`	154	`std::vector<cv::Mat> frames0Enc(frames0Gray.begin()+2, frames0Gray.begin()+2+nSteps);`
154	`std::vector<cv::Mat> frames0Cue(frames0Gray.begin()+2+nSteps, frames0Gray.begin()+2+nSteps+3);`	155	`std::vector<cv::Mat> frames0Cue(frames0Gray.begin()+2+nSteps, frames0Gray.begin()+2+nSteps+nSteps);`
155	`std::vector<cv::Mat> f0Icomp = getDFTComponents(frames0Enc);`	156	`std::vector<cv::Mat> F0Enc = getDFTComponents(frames0Enc);`
156	`cv::Mat up0;`	157	`cv::Mat up0;`
157	`cv::phase(f0Icomp[2], -f0Icomp[3], up0);`	158	`cv::phase(F0Enc[2], -F0Enc[3], up0);`
158	`cvtools::writeMat(up0, "up0.mat", "up0");`	159	`cvtools::writeMat(up0, "up0.mat", "up0");`
-		160	`std::vector<cv::Mat> F0Cue = getDFTComponents(frames0Cue);`
-		161	`cv::Mat up0Cue;`
159	`cv::Mat up0Cue = getPhase(frames0Cue[0], frames0Cue[1], frames0Cue[2]);`	162	`cv::phase(F0Cue[2], -F0Cue[3], up0Cue);`
160	`up0 = unwrapWithCue(up0, up0Cue, nPhases);`	163	`up0 = unwrapWithCue(up0, up0Cue, nPhases);`
161	`up0 = screenCols/(2pi);`	164	`up0 = screenCols/(2pi);`
162		165
163	`// Decode camera1`	166	`// Decode camera1`
164	`std::vector<cv::Mat> frames1Enc(frames1Gray.begin()+2, frames1Gray.begin()+2+nSteps);`	167	`std::vector<cv::Mat> frames1Enc(frames1Gray.begin()+2, frames1Gray.begin()+2+nSteps);`
165	`std::vector<cv::Mat> frames1Cue(frames1Gray.begin()+2+nSteps, frames1Gray.begin()+2+nSteps+3);`	168	`std::vector<cv::Mat> frames1Cue(frames1Gray.begin()+2+nSteps, frames1Gray.begin()+2+nSteps+nSteps);`
166	`std::vector<cv::Mat> f1Icomp = getDFTComponents(frames1Enc);`	169	`std::vector<cv::Mat> f1Icomp = getDFTComponents(frames1Enc);`
167	`cv::Mat up1;`	170	`cv::Mat up1;`
168	`cv::phase(f1Icomp[2], -f1Icomp[3], up1);`	171	`cv::phase(f1Icomp[2], -f1Icomp[3], up1);`
169	`cvtools::writeMat(up1, "up1.mat", "up1");`	172	`cvtools::writeMat(up1, "up1.mat", "up1");`
-		173	`std::vector<cv::Mat> F1Cue = getDFTComponents(frames1Cue);`
-		174	`cv::Mat up1Cue;`
170	`cv::Mat up1Cue = getPhase(frames1Cue[0], frames1Cue[1], frames1Cue[2]);`	175	`cv::phase(F1Cue[2], -F1Cue[3], up1Cue);`
171	`up1 = unwrapWithCue(up1, up1Cue, nPhases);`	176	`up1 = unwrapWithCue(up1, up1Cue, nPhases);`
172	`up1 = screenCols/(2pi);`	177	`up1 = screenCols/(2pi);`
173		178
174	`cvtools::writeMat(up0Cue, "up0Cue.mat", "up0Cue");`	179	`cvtools::writeMat(up0Cue, "up0Cue.mat", "up0Cue");`
175	`cvtools::writeMat(up1Cue, "up1Cue.mat", "up1Cue");`	180	`cvtools::writeMat(up1Cue, "up1Cue.mat", "up1Cue");`
176		181
177	`// Rectifying homographies (rotation+projections)`	182	`// Rectifying homographies (rotation+projections)`
178	`cv::Size frameSize(frameCols, frameRows);`	183	`cv::Size frameSize(frameCols, frameRows);`
179	`cv::Mat R, T;`	184	`cv::Mat R, T;`
180	`// stereoRectify segfaults unless R is double precision`	185	`// stereoRectify segfaults unless R is double precision`
181	`cv::Mat(calibration.R1).convertTo(R, CV_64F);`	186	`cv::Mat(calibration.R1).convertTo(R, CV_64F);`
182	`cv::Mat(calibration.T1).convertTo(T, CV_64F);`	187	`cv::Mat(calibration.T1).convertTo(T, CV_64F);`
183	`cv::Mat R0, R1, P0, P1, QRect;`	188	`cv::Mat R0, R1, P0, P1, QRect;`
184	`cv::stereoRectify(calibration.K0, calibration.k0, calibration.K1, calibration.k1, frameSize, R, T, R0, R1, P0, P1, QRect, 0);`	189	`cv::stereoRectify(calibration.K0, calibration.k0, calibration.K1, calibration.k1, frameSize, R, T, R0, R1, P0, P1, QRect, 0);`
185		190
186	`// Interpolation maps (lens distortion and rectification)`	191	`// Interpolation maps (lens distortion and rectification)`
187	`cv::Mat map0X, map0Y, map1X, map1Y;`	192	`cv::Mat map0X, map0Y, map1X, map1Y;`
188	`cv::initUndistortRectifyMap(calibration.K0, calibration.k0, R0, P0, frameSize, CV_32F, map0X, map0Y);`	193	`cv::initUndistortRectifyMap(calibration.K0, calibration.k0, R0, P0, frameSize, CV_32F, map0X, map0Y);`
189	`cv::initUndistortRectifyMap(calibration.K1, calibration.k1, R1, P1, frameSize, CV_32F, map1X, map1Y);`	194	`cv::initUndistortRectifyMap(calibration.K1, calibration.k1, R1, P1, frameSize, CV_32F, map1X, map1Y);`
190		195
191	`// Phase remaps`	196	`// Phase remaps`
192	`cv::Mat up0Rect, up1Rect;`	197	`cv::Mat up0Rect, up1Rect;`
193	`cv::remap(up0, up0Rect, map0X, map0Y, CV_INTER_CUBIC);`	198	`cv::remap(up0, up0Rect, map0X, map0Y, CV_INTER_CUBIC);`
194	`cv::remap(up1, up1Rect, map1X, map1Y, CV_INTER_CUBIC);`	199	`cv::remap(up1, up1Rect, map1X, map1Y, CV_INTER_CUBIC);`
195		200
196	`cvtools::writeMat(up0Rect, "up0Rect.mat", "up0Rect");`	201	`cvtools::writeMat(up0Rect, "up0Rect.mat", "up0Rect");`
197	`cvtools::writeMat(up1Rect, "up1Rect.mat", "up1Rect");`	202	`cvtools::writeMat(up1Rect, "up1Rect.mat", "up1Rect");`
198		203
199	`// Color remaps`	204	`// Color remaps`
200	`cv::Mat color0Rect, color1Rect;`	205	`cv::Mat color0Rect, color1Rect;`
201	`cv::remap(frames0[0], color0Rect, map0X, map0Y, CV_INTER_CUBIC);`	206	`cv::remap(frames0[0], color0Rect, map0X, map0Y, CV_INTER_CUBIC);`
202	`cv::remap(frames1[0], color1Rect, map1X, map1Y, CV_INTER_CUBIC);`	207	`cv::remap(frames1[0], color1Rect, map1X, map1Y, CV_INTER_CUBIC);`
203		208
204	`cvtools::writeMat(color0Rect, "color0Rect.mat", "color0Rect");`	209	`cvtools::writeMat(color0Rect, "color0Rect.mat", "color0Rect");`
205	`cvtools::writeMat(color1Rect, "color1Rect.mat", "color1Rect");`	210	`cvtools::writeMat(color1Rect, "color1Rect.mat", "color1Rect");`
206		211
207	`// On/off remaps`	212	`// On/off remaps`
208	`cv::Mat frames0OnRect, frames0OffRect;`	213	`cv::Mat frames0OnRect, frames0OffRect;`
209	`cv::remap(frames0Gray[0], frames0OnRect, map0X, map0Y, CV_INTER_CUBIC);`	214	`cv::remap(frames0Gray[0], frames0OnRect, map0X, map0Y, CV_INTER_CUBIC);`
210	`cv::remap(frames0Gray[1], frames0OffRect, map0X, map0Y, CV_INTER_CUBIC);`	215	`cv::remap(frames0Gray[1], frames0OffRect, map0X, map0Y, CV_INTER_CUBIC);`
211		216
212	`cv::Mat frames1OnRect, frames1OffRect;`	217	`cv::Mat frames1OnRect, frames1OffRect;`
213	`cv::remap(frames1Gray[0], frames1OnRect, map1X, map1Y, CV_INTER_CUBIC);`	218	`cv::remap(frames1Gray[0], frames1OnRect, map1X, map1Y, CV_INTER_CUBIC);`
214	`cv::remap(frames1Gray[1], frames1OffRect, map1X, map1Y, CV_INTER_CUBIC);`	219	`cv::remap(frames1Gray[1], frames1OffRect, map1X, map1Y, CV_INTER_CUBIC);`
215		220
216	`// Occlusion masks`	221	`// Occlusion masks`
217	`cv::Mat occlusion0Rect, occlusion1Rect;`	222	`cv::Mat occlusion0Rect, occlusion1Rect;`
218	`cv::subtract(frames0OnRect, frames0OffRect, occlusion0Rect);`	223	`cv::subtract(frames0OnRect, frames0OffRect, occlusion0Rect);`
219	`occlusion0Rect = occlusion0Rect > 50;`	224	`occlusion0Rect = occlusion0Rect > 50;`
220	`cv::subtract(frames1OnRect, frames1OffRect, occlusion1Rect);`	225	`cv::subtract(frames1OnRect, frames1OffRect, occlusion1Rect);`
221	`occlusion1Rect = occlusion1Rect > 50;`	226	`occlusion1Rect = occlusion1Rect > 50;`
222		227
223	`cvtools::writeMat(occlusion0Rect, "occlusion0Rect.mat", "occlusion0Rect");`	228	`cvtools::writeMat(occlusion0Rect, "occlusion0Rect.mat", "occlusion0Rect");`
224	`cvtools::writeMat(occlusion1Rect, "occlusion1Rect.mat", "occlusion1Rect");`	229	`cvtools::writeMat(occlusion1Rect, "occlusion1Rect.mat", "occlusion1Rect");`
225		230
226	`// Erode occlusion masks`	231	`// Erode occlusion masks`
227	`cv::Mat strel = cv::getStructuringElement(cv::MORPH_ELLIPSE, cv::Size(3,3));`	232	`cv::Mat strel = cv::getStructuringElement(cv::MORPH_ELLIPSE, cv::Size(3,3));`
228	`cv::erode(occlusion0Rect, occlusion0Rect, strel);`	233	`cv::erode(occlusion0Rect, occlusion0Rect, strel);`
229	`cv::erode(occlusion1Rect, occlusion1Rect, strel);`	234	`cv::erode(occlusion1Rect, occlusion1Rect, strel);`
230		235
231	`// Threshold on gradient of phase`	236	`// Threshold on gradient of phase`
232	`cv::Mat edges0;`	237	`cv::Mat edges0;`
233	`cv::Sobel(up0Rect, edges0, -1, 1, 1, 5);`	238	`cv::Sobel(up0Rect, edges0, -1, 1, 1, 5);`
234	`occlusion0Rect = occlusion0Rect & (abs(edges0) < 5);`	239	`occlusion0Rect = occlusion0Rect & (abs(edges0) < 5);`
235		240
236	`cv::Mat edges1;`	241	`cv::Mat edges1;`
237	`cv::Sobel(up1Rect, edges1, -1, 1, 1, 5);`	242	`cv::Sobel(up1Rect, edges1, -1, 1, 1, 5);`
238	`occlusion1Rect = occlusion1Rect & (abs(edges1) < 5);`	243	`occlusion1Rect = occlusion1Rect & (abs(edges1) < 5);`
239		244
240	`cvtools::writeMat(edges0, "edges0.mat", "edges0");`	245	`cvtools::writeMat(edges0, "edges0.mat", "edges0");`
241	`cvtools::writeMat(edges1, "edges1.mat", "edges1");`	246	`cvtools::writeMat(edges1, "edges1.mat", "edges1");`
242		247
243	`// Match phase maps`	248	`// Match phase maps`
244	`int frameRectRows = map0X.rows;`	249	`int frameRectRows = map0X.rows;`
245	`int frameRectCols = map0X.cols;`	250	`int frameRectCols = map0X.cols;`
246		251
247	`// camera0 against camera1`	252	`// camera0 against camera1`
248	`std::vector<cv::Vec2f> q0Rect, q1Rect;`	253	`std::vector<cv::Vec2f> q0Rect, q1Rect;`
249	`for(int row=0; row<frameRectRows; row++){`	254	`for(int row=0; row<frameRectRows; row++){`
250		255
251	`for(int col=0; col<frameRectCols; col++){`	256	`for(int col=0; col<frameRectCols; col++){`
252		257
253	`if(!occlusion0Rect.at<char>(row,col))`	258	`if(!occlusion0Rect.at<char>(row,col))`
254	`continue;`	259	`continue;`
255		260
256	`float up0i = up0Rect.at<float>(row,col);`	261	`float up0i = up0Rect.at<float>(row,col);`
257	`for(int col1=0; col1<up1Rect.cols-1; col1++){`	262	`for(int col1=0; col1<up1Rect.cols-1; col1++){`
258		263
259	`if(!occlusion1Rect.at<char>(row,col1) \|\| !occlusion1Rect.at<char>(row,col1+1))`	264	`if(!occlusion1Rect.at<char>(row,col1) \|\| !occlusion1Rect.at<char>(row,col1+1))`
260	`continue;`	265	`continue;`
261		266
262	`float up1Left = up1Rect.at<float>(row,col1);`	267	`float up1Left = up1Rect.at<float>(row,col1);`
263	`float up1Right = up1Rect.at<float>(row,col1+1);`	268	`float up1Right = up1Rect.at<float>(row,col1+1);`
264		269
265	`if((up1Left <= up0i) && (up0i <= up1Right)){`	270	`if((up1Left <= up0i) && (up0i <= up1Right)){`
266		271
267	`float col1i = col1 + (up0i-up1Left)/(up1Right-up1Left);`	272	`float col1i = col1 + (up0i-up1Left)/(up1Right-up1Left);`
268		273
269	`q0Rect.push_back(cv::Point2f(col, row));`	274	`q0Rect.push_back(cv::Point2f(col, row));`
270	`q1Rect.push_back(cv::Point2f(col1i, row));`	275	`q1Rect.push_back(cv::Point2f(col1i, row));`
271		276
272	`break;`	277	`break;`
273	`}`	278	`}`
274		279
275	`}`	280	`}`
276		281
277	`}`	282	`}`
278		283
279	`}`	284	`}`
280		285
281	`// camera1 against camera0`	286	`// camera1 against camera0`
282	`//[...]`	287	`//[...]`
283		288
284	`int nMatches = q0Rect.size();`	289	`int nMatches = q0Rect.size();`
285		290
286	`if(nMatches < 1){`	291	`if(nMatches < 1){`
287	`Q.resize(0);`	292	`Q.resize(0);`
288	`color.resize(0);`	293	`color.resize(0);`
289		294
290	`return;`	295	`return;`
291	`}`	296	`}`
292		297
293	`// Retrieve color information`	298	`// Retrieve color information`
294	`color.resize(nMatches);`	299	`color.resize(nMatches);`
295	`for(int i=0; i<nMatches; i++){`	300	`for(int i=0; i<nMatches; i++){`
296		301
297	`cv::Vec3b c0 = color0Rect.at<cv::Vec3b>(q0Rect[i][1], q0Rect[i][0]);`	302	`cv::Vec3b c0 = color0Rect.at<cv::Vec3b>(q0Rect[i][1], q0Rect[i][0]);`
298	`cv::Vec3b c1 = color1Rect.at<cv::Vec3b>(q1Rect[i][1], q1Rect[i][0]);`	303	`cv::Vec3b c1 = color1Rect.at<cv::Vec3b>(q1Rect[i][1], q1Rect[i][0]);`
299		304
300	`color[i] = 0.5c0 + 0.5c1;`	305	`color[i] = 0.5c0 + 0.5c1;`
301	`}`	306	`}`
302		307
303	`// Triangulate points`	308	`// Triangulate points`
304	`cv::Mat QMatHomogenous, QMat;`	309	`cv::Mat QMatHomogenous, QMat;`
305	`cv::triangulatePoints(P0, P1, q0Rect, q1Rect, QMatHomogenous);`	310	`cv::triangulatePoints(P0, P1, q0Rect, q1Rect, QMatHomogenous);`
306	`cvtools::convertMatFromHomogeneous(QMatHomogenous, QMat);`	311	`cvtools::convertMatFromHomogeneous(QMatHomogenous, QMat);`
307		312
308	`// Undo rectification`	313	`// Undo rectification`
309	`cv::Mat R0Inv;`	314	`cv::Mat R0Inv;`
310	`cv::Mat(R0.t()).convertTo(R0Inv, CV_32F);`	315	`cv::Mat(R0.t()).convertTo(R0Inv, CV_32F);`
311	`QMat = R0Inv*QMat;`	316	`QMat = R0Inv*QMat;`
312		317
313	`cvtools::matToPoints3f(QMat, Q);`	318	`cvtools::matToPoints3f(QMat, Q);`
314		319
315	`}`	320	`}`
316		321

Subversion Repositories seema-scanner

(root)/src/algorithm/AlgorithmPhaseShift.cpp @ 72 – Rev 71 → 72