Subversion Repositories seema-scanner

Rev

Rev 121 | Rev 129 | Go to most recent revision | Only display areas with differences | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

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