| Line 6... |
Line 6... |
| 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 = 16; // number of shifts/steps in primary
|
10 |
static unsigned int nStepsPrimary = 16; // number of shifts/steps in primary
|
| 11 |
static unsigned int nStepsSecondary = 6; // number of shifts/steps in secondary
|
11 |
static unsigned int nStepsSecondary = 16; // number of shifts/steps in secondary
|
| 12 |
static unsigned int nStepsTertiary = 6; // number of shifts/steps in secondary
|
12 |
static unsigned int nStepsTertiary = 16; // number of shifts/steps in tertiary
|
| 13 |
static float periodPrimary = 32; // primary period
|
13 |
static float periodPrimary = 24; // primary period
|
| - |
|
14 |
static float periodSecondary = 30; // primary period
|
| 14 |
|
15 |
|
| 15 |
// Algorithm
|
16 |
// Algorithm
|
| 16 |
static cv::Mat computePhaseVector(unsigned int length, float phase, float pitch){
|
17 |
static cv::Mat computePhaseVector(unsigned int length, float phase, float pitch){
|
| 17 |
|
18 |
|
| 18 |
cv::Mat phaseVector(length, 1, CV_8UC3);
|
19 |
cv::Mat phaseVector(length, 1, CV_8UC3);
|
| Line 33... |
Line 34... |
| 33 |
AlgorithmPhaseShiftThreeFreq::AlgorithmPhaseShiftThreeFreq(unsigned int _screenCols, unsigned int _screenRows) : Algorithm(_screenCols, _screenRows){
|
34 |
AlgorithmPhaseShiftThreeFreq::AlgorithmPhaseShiftThreeFreq(unsigned int _screenCols, unsigned int _screenRows) : Algorithm(_screenCols, _screenRows){
|
| 34 |
|
35 |
|
| 35 |
// Set N
|
36 |
// Set N
|
| 36 |
N = 2+nStepsPrimary+nStepsSecondary+nStepsTertiary;
|
37 |
N = 2+nStepsPrimary+nStepsSecondary+nStepsTertiary;
|
| 37 |
|
38 |
|
| 38 |
// Determine the secondary (wider) period
|
39 |
// Determine the tertiary period to fulfill the heterodyne condition
|
| 39 |
float pSecondary = (screenCols*periodPrimary)/(screenCols-periodPrimary);
|
40 |
float periodTertiary = (screenCols*periodPrimary*periodSecondary)/(periodPrimary*periodSecondary+2*screenCols*periodPrimary-screenCols*periodSecondary);
|
| 40 |
|
41 |
|
| 41 |
// all on pattern
|
42 |
// all on pattern
|
| 42 |
cv::Mat allOn(1, screenCols, CV_8UC3, cv::Scalar::all(255));
|
43 |
cv::Mat allOn(1, screenCols, CV_8UC3, cv::Scalar::all(255));
|
| 43 |
patterns.push_back(allOn);
|
44 |
patterns.push_back(allOn);
|
| 44 |
|
45 |
|
| Line 59... |
Line 60... |
| 59 |
}
|
60 |
}
|
| 60 |
|
61 |
|
| 61 |
// Secondary encoding patterns
|
62 |
// Secondary encoding patterns
|
| 62 |
for(unsigned int i=0; i<nStepsSecondary; i++){
|
63 |
for(unsigned int i=0; i<nStepsSecondary; i++){
|
| 63 |
float phase = 2.0*pi/nStepsSecondary * i;
|
64 |
float phase = 2.0*pi/nStepsSecondary * i;
|
| 64 |
float pitch = pSecondary;
|
65 |
float pitch = periodSecondary;
|
| 65 |
cv::Mat patternI(1,1,CV_8U);
|
66 |
cv::Mat patternI(1,1,CV_8U);
|
| 66 |
patternI = computePhaseVector(screenCols, phase, pitch);
|
67 |
patternI = computePhaseVector(screenCols, phase, pitch);
|
| 67 |
patterns.push_back(patternI.t());
|
68 |
patterns.push_back(patternI.t());
|
| 68 |
}
|
69 |
}
|
| 69 |
// Tertiary encoding patterns
|
70 |
// Tertiary encoding patterns
|
| 70 |
for(unsigned int i=0; i<nStepsTertiary; i++){
|
71 |
for(unsigned int i=0; i<nStepsTertiary; i++){
|
| 71 |
float phase = 2.0*pi/nStepsTertiary * i;
|
72 |
float phase = 2.0*pi/nStepsTertiary * i;
|
| 72 |
float pitch = pTertiary;
|
73 |
float pitch = periodTertiary;
|
| 73 |
cv::Mat patternI(1,1,CV_8U);
|
74 |
cv::Mat patternI(1,1,CV_8U);
|
| 74 |
patternI = computePhaseVector(screenCols, phase, pitch);
|
75 |
patternI = computePhaseVector(screenCols, phase, pitch);
|
| 75 |
patterns.push_back(patternI.t());
|
76 |
patterns.push_back(patternI.t());
|
| 76 |
}
|
77 |
}
|
| 77 |
|
78 |
|
| Line 164... |
Line 165... |
| 164 |
|
165 |
|
| 165 |
// Decode camera0
|
166 |
// Decode camera0
|
| 166 |
std::vector<cv::Mat> frames0Primary(frames0Gray.begin()+2, frames0Gray.begin()+2+nStepsPrimary);
|
167 |
std::vector<cv::Mat> frames0Primary(frames0Gray.begin()+2, frames0Gray.begin()+2+nStepsPrimary);
|
| 167 |
std::vector<cv::Mat> frames0Secondary(frames0Gray.begin()+2+nStepsPrimary, frames0Gray.end()-nStepsTertiary);
|
168 |
std::vector<cv::Mat> frames0Secondary(frames0Gray.begin()+2+nStepsPrimary, frames0Gray.end()-nStepsTertiary);
|
| 168 |
std::vector<cv::Mat> frames0Tertiary(frames0Gray.end()-nStepsTertiary, frames0Gray.end());
|
169 |
std::vector<cv::Mat> frames0Tertiary(frames0Gray.end()-nStepsTertiary, frames0Gray.end());
|
| 169 |
std::vector<cv::Mat> F0Primary = getDFTComponents(frames0Primary);
|
- |
|
| 170 |
|
170 |
|
| - |
|
171 |
std::vector<cv::Mat> F0Primary = getDFTComponents(frames0Primary);
|
| 171 |
cv::Mat up0Primary;
|
172 |
cv::Mat up0Primary;
|
| 172 |
cv::phase(F0Primary[2], -F0Primary[3], up0Primary);
|
173 |
cv::phase(F0Primary[2], -F0Primary[3], up0Primary);
|
| 173 |
std::vector<cv::Mat> F0Secondary = getDFTComponents(frames0Secondary);
|
174 |
std::vector<cv::Mat> F0Secondary = getDFTComponents(frames0Secondary);
|
| 174 |
cv::Mat up0Secondary;
|
175 |
cv::Mat up0Secondary;
|
| 175 |
cv::phase(F0Secondary[2], -F0Secondary[3], up0Secondary);
|
176 |
cv::phase(F0Secondary[2], -F0Secondary[3], up0Secondary);
|
| Line 184... |
Line 185... |
| 184 |
up0EquivalentST = cvtools::modulo(up0EquivalentST, 2.0*pi);
|
185 |
up0EquivalentST = cvtools::modulo(up0EquivalentST, 2.0*pi);
|
| 185 |
|
186 |
|
| 186 |
cv::Mat up0Equivalent = up0EquivalentPS - up0EquivalentST;
|
187 |
cv::Mat up0Equivalent = up0EquivalentPS - up0EquivalentST;
|
| 187 |
up0Equivalent = cvtools::modulo(up0Equivalent, 2.0*pi);
|
188 |
up0Equivalent = cvtools::modulo(up0Equivalent, 2.0*pi);
|
| 188 |
|
189 |
|
| - |
|
190 |
cv::Mat up0Equivalent2 = unwrapWithCue(up0EquivalentPS, up0Equivalent, (float)screenCols/periodPrimary);
|
| - |
|
191 |
|
| 189 |
cv::Mat up0 = unwrapWithCue(up0Primary, up0Equivalent, (float)screenCols/periodPrimary);
|
192 |
cv::Mat up0 = unwrapWithCue(up0Primary, up0Equivalent2, (float)screenCols/periodPrimary);
|
| 190 |
up0 *= screenCols/(2.0*pi);
|
193 |
up0 *= screenCols/(2.0*pi);
|
| 191 |
cv::Mat amplitude0;
|
194 |
cv::Mat amplitude0;
|
| 192 |
cv::magnitude(F0Primary[2], -F0Primary[3], amplitude0);
|
195 |
cv::magnitude(F0Primary[2], -F0Primary[3], amplitude0);
|
| 193 |
|
196 |
|
| 194 |
// Decode camera1
|
197 |
// Decode camera1
|
| 195 |
std::vector<cv::Mat> frames1Primary(frames1Gray.begin()+2, frames1Gray.begin()+2+nStepsPrimary);
|
198 |
std::vector<cv::Mat> frames1Primary(frames1Gray.begin()+2, frames1Gray.begin()+2+nStepsPrimary);
|
| 196 |
std::vector<cv::Mat> frames1Secondary(frames1Gray.begin()+2+nStepsPrimary, frames1Gray.end());
|
199 |
std::vector<cv::Mat> frames1Secondary(frames1Gray.begin()+2+nStepsPrimary, frames1Gray.end()-nStepsTertiary);
|
| - |
|
200 |
std::vector<cv::Mat> frames1Tertiary(frames1Gray.end()-nStepsTertiary, frames1Gray.end());
|
| - |
|
201 |
|
| 197 |
std::vector<cv::Mat> F1Primary = getDFTComponents(frames1Primary);
|
202 |
std::vector<cv::Mat> F1Primary = getDFTComponents(frames1Primary);
|
| 198 |
cv::Mat up1Primary;
|
203 |
cv::Mat up1Primary;
|
| 199 |
cv::phase(F1Primary[2], -F1Primary[3], up1Primary);
|
204 |
cv::phase(F1Primary[2], -F1Primary[3], up1Primary);
|
| 200 |
std::vector<cv::Mat> F1Secondary = getDFTComponents(frames1Secondary);
|
205 |
std::vector<cv::Mat> F1Secondary = getDFTComponents(frames1Secondary);
|
| 201 |
cv::Mat up1Secondary;
|
206 |
cv::Mat up1Secondary;
|
| Line 216... |
Line 221... |
| 216 |
cv::Mat up1 = unwrapWithCue(up1Primary, up1Equivalent, (float)screenCols/periodPrimary);
|
221 |
cv::Mat up1 = unwrapWithCue(up1Primary, up1Equivalent, (float)screenCols/periodPrimary);
|
| 217 |
up1 *= screenCols/(2.0*pi);
|
222 |
up1 *= screenCols/(2.0*pi);
|
| 218 |
cv::Mat amplitude1;
|
223 |
cv::Mat amplitude1;
|
| 219 |
cv::magnitude(F1Primary[2], -F1Primary[3], amplitude1);
|
224 |
cv::magnitude(F1Primary[2], -F1Primary[3], amplitude1);
|
| 220 |
|
225 |
|
| 221 |
cvtools::writeMat(up0Primary, "up0Primary.mat", "up0Primary");
|
226 |
//cvtools::writeMat(up0Primary, "up0Primary.mat", "up0Primary");
|
| 222 |
cvtools::writeMat(up0Secondary, "up0Secondary.mat", "up0Secondary");
|
227 |
//cvtools::writeMat(up0Secondary, "up0Secondary.mat", "up0Secondary");
|
| - |
|
228 |
//cvtools::writeMat(up0Tertiary, "up0Tertiary.mat", "up0Tertiary");
|
| 223 |
cvtools::writeMat(up0Equivalent, "up0Equivalent.mat", "up0Equivalent");
|
229 |
//cvtools::writeMat(up0Equivalent, "up0Equivalent.mat", "up0Equivalent");
|
| - |
|
230 |
cvtools::writeMat(up0Equivalent2, "up0Equivalent2.mat", "up0Equivalent2");
|
| - |
|
231 |
//cvtools::writeMat(up0EquivalentPS, "up0EquivalentPS.mat", "up0EquivalentPS");
|
| - |
|
232 |
//cvtools::writeMat(up0EquivalentST, "up0EquivalentST.mat", "up0EquivalentST");
|
| 224 |
cvtools::writeMat(up0, "up0.mat", "up0");
|
233 |
//cvtools::writeMat(up0, "up0.mat", "up0");
|
| - |
|
234 |
//cvtools::writeMat(up1, "up1.mat", "up1");
|
| 225 |
cvtools::writeMat(amplitude0, "amplitude0.mat", "amplitude0");
|
235 |
//cvtools::writeMat(amplitude0, "amplitude0.mat", "amplitude0");
|
| 226 |
|
236 |
|
| 227 |
// Rectifying homographies (rotation+projections)
|
237 |
// Rectifying homographies (rotation+projections)
|
| 228 |
cv::Size frameSize(frameCols, frameRows);
|
238 |
cv::Size frameSize(frameCols, frameRows);
|
| 229 |
cv::Mat R, T;
|
239 |
cv::Mat R, T;
|
| 230 |
// stereoRectify segfaults unless R is double precision
|
240 |
// stereoRectify segfaults unless R is double precision
|