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

Subversion Repositories seema-scanner

(root)/src/algorithm/AlgorithmPhaseShift.cpp @ 76 – Rev 74 → 76