Line 1... |
Line 1... |
1 |
//
|
1 |
//
|
2 |
// Three Frequency Phase Shifting using the Heterodyne Principle
|
2 |
// Three Frequency Phase Shifting using the Heterodyne Principle
|
3 |
//
|
3 |
//
|
4 |
// This implementation follows "Reich, Ritter, Thesing, White light heterodyne principle for 3D-measurement", SPIE (1997)
|
4 |
// This implementation follows "Reich, Ritter, Thesing, White light heterodyne principle for 3D-measurement", SPIE (1997)
|
5 |
//
|
5 |
//
|
6 |
// The number of periods in the primary and secondary frequencies can be chosen freely, but small changes can have a considerable impact on quality.
|
6 |
// The number of periods in the First and Second frequencies can be chosen freely, but small changes can have a considerable impact on quality.
|
7 |
// The number of phase shifts can be chosen freely (min. 3), and higher values reduce the effects of image noise. They also allow us to filter bad points based on energy at non-primary frequencies.
|
7 |
// The number of phase shifts can be chosen freely (min. 3), and higher values reduce the effects of image noise. They also allow us to filter bad points based on energy at non-First frequencies.
|
8 |
//
|
8 |
//
|
9 |
|
9 |
|
10 |
|
10 |
|
11 |
#include "AlgorithmPhaseShiftThreeFreq.h"
|
11 |
#include "AlgorithmPhaseShiftThreeFreq.h"
|
12 |
#include <math.h>
|
12 |
#include <math.h>
|
Line 16... |
Line 16... |
16 |
|
16 |
|
17 |
#ifndef M_PI
|
17 |
#ifndef M_PI
|
18 |
#define M_PI 3.14159265358979323846
|
18 |
#define M_PI 3.14159265358979323846
|
19 |
#endif
|
19 |
#endif
|
20 |
|
20 |
|
21 |
static unsigned int nStepsPrimary = 8; // number of shifts/steps in primary
|
21 |
static unsigned int nStepsFirst = 8; // number of shifts/steps in First
|
22 |
static unsigned int nStepsSecondary = 8; // number of shifts/steps in secondary
|
22 |
static unsigned int nStepsSecond = 8; // number of shifts/steps in Second
|
23 |
static unsigned int nStepsTertiary = 8; // number of shifts/steps in tertiary
|
23 |
static unsigned int nStepsThird = 8; // number of shifts/steps in Third
|
24 |
static float nPeriodsPrimary = 24; // primary period
|
24 |
static float nPeriodsFirst = 24; // First period
|
25 |
static float nPeriodsSecondary = 30; // primary period
|
25 |
static float nPeriodsSecond = 30; // First period
|
26 |
|
26 |
|
27 |
AlgorithmPhaseShiftThreeFreq::AlgorithmPhaseShiftThreeFreq(unsigned int _screenCols, unsigned int _screenRows) : Algorithm(_screenCols, _screenRows){
|
27 |
AlgorithmPhaseShiftThreeFreq::AlgorithmPhaseShiftThreeFreq(unsigned int _screenCols, unsigned int _screenRows) : Algorithm(_screenCols, _screenRows){
|
28 |
|
28 |
|
29 |
// Set N
|
29 |
// Set N
|
30 |
N = 2+nStepsPrimary+nStepsSecondary+nStepsTertiary;
|
30 |
N = 2+nStepsFirst+nStepsSecond+nStepsThird;
|
31 |
|
31 |
|
32 |
// Determine the tertiary period to fulfill the heterodyne condition
|
32 |
// Determine the Third period to fulfill the heterodyne condition
|
33 |
float nPeriodsTertiary = (screenCols*nPeriodsPrimary*nPeriodsSecondary)/(nPeriodsPrimary*nPeriodsSecondary+2*screenCols*nPeriodsPrimary-screenCols*nPeriodsSecondary);
|
33 |
float nPeriodsThird = 1 + 2*nPeriodsSecond - nPeriodsFirst;
|
34 |
|
34 |
|
35 |
// all on pattern
|
35 |
// all on pattern
|
36 |
cv::Mat allOn(1, screenCols, CV_8UC3, cv::Scalar::all(255));
|
36 |
cv::Mat allOn(1, screenCols, CV_8UC3, cv::Scalar::all(255));
|
37 |
patterns.push_back(allOn);
|
37 |
patterns.push_back(allOn);
|
38 |
|
38 |
|
Line 41... |
Line 41... |
41 |
patterns.push_back(allOff);
|
41 |
patterns.push_back(allOff);
|
42 |
|
42 |
|
43 |
// Precompute encoded patterns
|
43 |
// Precompute encoded patterns
|
44 |
const float pi = M_PI;
|
44 |
const float pi = M_PI;
|
45 |
|
45 |
|
46 |
// Primary encoding patterns
|
46 |
// First encoding patterns
|
47 |
for(unsigned int i=0; i<nStepsPrimary; i++){
|
47 |
for(unsigned int i=0; i<nStepsFirst; i++){
|
48 |
float phase = 2.0*pi/nStepsPrimary * i;
|
48 |
float phase = 2.0*pi/nStepsFirst * i;
|
49 |
float pitch = screenCols/nPeriodsPrimary;
|
49 |
float pitch = screenCols/nPeriodsFirst;
|
50 |
cv::Mat patternI(1,1,CV_8U);
|
50 |
cv::Mat patternI(1,1,CV_8U);
|
51 |
patternI = computePhaseVector(screenCols, phase, pitch);
|
51 |
patternI = computePhaseVector(screenCols, phase, pitch);
|
52 |
patterns.push_back(patternI.t());
|
52 |
patterns.push_back(patternI.t());
|
53 |
}
|
53 |
}
|
54 |
|
54 |
|
55 |
// Secondary encoding patterns
|
55 |
// Second encoding patterns
|
56 |
for(unsigned int i=0; i<nStepsSecondary; i++){
|
56 |
for(unsigned int i=0; i<nStepsSecond; i++){
|
57 |
float phase = 2.0*pi/nStepsSecondary * i;
|
57 |
float phase = 2.0*pi/nStepsSecond * i;
|
58 |
float pitch = screenCols/nPeriodsSecondary;
|
58 |
float pitch = screenCols/nPeriodsSecond;
|
59 |
cv::Mat patternI(1,1,CV_8U);
|
59 |
cv::Mat patternI(1,1,CV_8U);
|
60 |
patternI = computePhaseVector(screenCols, phase, pitch);
|
60 |
patternI = computePhaseVector(screenCols, phase, pitch);
|
61 |
patterns.push_back(patternI.t());
|
61 |
patterns.push_back(patternI.t());
|
62 |
}
|
62 |
}
|
63 |
// Tertiary encoding patterns
|
63 |
// Third encoding patterns
|
64 |
for(unsigned int i=0; i<nStepsTertiary; i++){
|
64 |
for(unsigned int i=0; i<nStepsThird; i++){
|
65 |
float phase = 2.0*pi/nStepsTertiary * i;
|
65 |
float phase = 2.0*pi/nStepsThird * i;
|
66 |
float pitch = screenCols/nPeriodsTertiary;
|
66 |
float pitch = screenCols/nPeriodsThird;
|
67 |
cv::Mat patternI(1,1,CV_8U);
|
67 |
cv::Mat patternI(1,1,CV_8U);
|
68 |
patternI = computePhaseVector(screenCols, phase, pitch);
|
68 |
patternI = computePhaseVector(screenCols, phase, pitch);
|
69 |
patterns.push_back(patternI.t());
|
69 |
patterns.push_back(patternI.t());
|
70 |
}
|
70 |
}
|
71 |
|
71 |
|
Line 110... |
Line 110... |
110 |
cv::cvtColor(frames1[i], temp, CV_BayerBG2GRAY);
|
110 |
cv::cvtColor(frames1[i], temp, CV_BayerBG2GRAY);
|
111 |
cv::remap(temp, frames1Rect[i], map1X, map1Y, CV_INTER_LINEAR);
|
111 |
cv::remap(temp, frames1Rect[i], map1X, map1Y, CV_INTER_LINEAR);
|
112 |
}
|
112 |
}
|
113 |
|
113 |
|
114 |
// Decode camera0
|
114 |
// Decode camera0
|
115 |
std::vector<cv::Mat> frames0Primary(frames0Rect.begin()+2, frames0Rect.begin()+2+nStepsPrimary);
|
115 |
std::vector<cv::Mat> frames0First(frames0Rect.begin()+2, frames0Rect.begin()+2+nStepsFirst);
|
116 |
std::vector<cv::Mat> frames0Secondary(frames0Rect.begin()+2+nStepsPrimary, frames0Rect.end()-nStepsTertiary);
|
116 |
std::vector<cv::Mat> frames0Second(frames0Rect.begin()+2+nStepsFirst, frames0Rect.end()-nStepsThird);
|
117 |
std::vector<cv::Mat> frames0Tertiary(frames0Rect.end()-nStepsTertiary, frames0Rect.end());
|
117 |
std::vector<cv::Mat> frames0Third(frames0Rect.end()-nStepsThird, frames0Rect.end());
|
118 |
|
118 |
|
119 |
std::vector<cv::Mat> F0Primary = getDFTComponents(frames0Primary);
|
119 |
std::vector<cv::Mat> F0First = getDFTComponents(frames0First);
|
120 |
cv::Mat up0Primary;
|
120 |
cv::Mat up0First;
|
121 |
cv::phase(F0Primary[2], -F0Primary[3], up0Primary);
|
121 |
cv::phase(F0First[2], -F0First[3], up0First);
|
122 |
std::vector<cv::Mat> F0Secondary = getDFTComponents(frames0Secondary);
|
122 |
std::vector<cv::Mat> F0Second = getDFTComponents(frames0Second);
|
123 |
cv::Mat up0Secondary;
|
123 |
cv::Mat up0Second;
|
124 |
cv::phase(F0Secondary[2], -F0Secondary[3], up0Secondary);
|
124 |
cv::phase(F0Second[2], -F0Second[3], up0Second);
|
125 |
std::vector<cv::Mat> F0Tertiary = getDFTComponents(frames0Tertiary);
|
125 |
std::vector<cv::Mat> F0Third = getDFTComponents(frames0Third);
|
126 |
cv::Mat up0Tertiary;
|
126 |
cv::Mat up0Third;
|
127 |
cv::phase(F0Tertiary[2], -F0Tertiary[3], up0Tertiary);
|
127 |
cv::phase(F0Third[2], -F0Third[3], up0Third);
|
128 |
|
128 |
|
129 |
cv::Mat up0EquivalentPS = up0Primary - up0Secondary;
|
129 |
cv::Mat up0EquivalentPS = up0First - up0Second;
|
130 |
up0EquivalentPS = cvtools::modulo(up0EquivalentPS, 2.0*pi);
|
130 |
up0EquivalentPS = cvtools::modulo(up0EquivalentPS, 2.0*pi);
|
131 |
|
131 |
|
132 |
cv::Mat up0EquivalentPT = up0Primary - up0Tertiary;
|
132 |
cv::Mat up0EquivalentPT = up0First - up0Third;
|
133 |
up0EquivalentPT = cvtools::modulo(up0EquivalentPT, 2.0*pi);
|
133 |
up0EquivalentPT = cvtools::modulo(up0EquivalentPT, 2.0*pi);
|
134 |
|
134 |
|
135 |
cv::Mat up0Equivalent = up0EquivalentPS - up0EquivalentPT;
|
135 |
cv::Mat up0EquivalentPST = up0EquivalentPS - up0EquivalentPT;
|
136 |
up0Equivalent = cvtools::modulo(up0Equivalent, 2.0*pi);
|
136 |
up0EquivalentPST = cvtools::modulo(up0EquivalentPST, 2.0*pi);
|
137 |
|
137 |
|
- |
|
138 |
// TODO: we should use backward phase unwrapping (Song Zhang term)...
|
- |
|
139 |
|
138 |
cv::Mat up0 = unwrapWithCue(up0Primary, up0Equivalent, (float)screenCols/nPeriodsPrimary);
|
140 |
cv::Mat up0 = unwrapWithCue(up0First, up0EquivalentPST, (float)screenCols/nPeriodsFirst);
|
139 |
up0 *= screenCols/(2.0*pi);
|
141 |
up0 *= screenCols/(2.0*pi);
|
140 |
cv::Mat amplitude0;
|
142 |
cv::Mat amplitude0;
|
141 |
cv::magnitude(F0Primary[2], -F0Primary[3], amplitude0);
|
143 |
cv::magnitude(F0First[2], -F0First[3], amplitude0);
|
142 |
|
144 |
|
143 |
// Decode camera1
|
145 |
// Decode camera1
|
144 |
std::vector<cv::Mat> frames1Primary(frames1Rect.begin()+2, frames1Rect.begin()+2+nStepsPrimary);
|
146 |
std::vector<cv::Mat> frames1First(frames1Rect.begin()+2, frames1Rect.begin()+2+nStepsFirst);
|
145 |
std::vector<cv::Mat> frames1Secondary(frames1Rect.begin()+2+nStepsPrimary, frames1Rect.end()-nStepsTertiary);
|
147 |
std::vector<cv::Mat> frames1Second(frames1Rect.begin()+2+nStepsFirst, frames1Rect.end()-nStepsThird);
|
146 |
std::vector<cv::Mat> frames1Tertiary(frames1Rect.end()-nStepsTertiary, frames1Rect.end());
|
148 |
std::vector<cv::Mat> frames1Third(frames1Rect.end()-nStepsThird, frames1Rect.end());
|
147 |
|
149 |
|
148 |
std::vector<cv::Mat> F1Primary = getDFTComponents(frames1Primary);
|
150 |
std::vector<cv::Mat> F1First = getDFTComponents(frames1First);
|
149 |
cv::Mat up1Primary;
|
151 |
cv::Mat up1First;
|
150 |
cv::phase(F1Primary[2], -F1Primary[3], up1Primary);
|
152 |
cv::phase(F1First[2], -F1First[3], up1First);
|
151 |
std::vector<cv::Mat> F1Secondary = getDFTComponents(frames1Secondary);
|
153 |
std::vector<cv::Mat> F1Second = getDFTComponents(frames1Second);
|
152 |
cv::Mat up1Secondary;
|
154 |
cv::Mat up1Second;
|
153 |
cv::phase(F1Secondary[2], -F1Secondary[3], up1Secondary);
|
155 |
cv::phase(F1Second[2], -F1Second[3], up1Second);
|
154 |
std::vector<cv::Mat> F1Tertiary = getDFTComponents(frames1Tertiary);
|
156 |
std::vector<cv::Mat> F1Third = getDFTComponents(frames1Third);
|
155 |
cv::Mat up1Tertiary;
|
157 |
cv::Mat up1Third;
|
156 |
cv::phase(F1Tertiary[2], -F1Tertiary[3], up1Tertiary);
|
158 |
cv::phase(F1Third[2], -F1Third[3], up1Third);
|
157 |
|
159 |
|
158 |
cv::Mat up1EquivalentPS = up1Primary - up1Secondary;
|
160 |
cv::Mat up1EquivalentPS = up1First - up1Second;
|
159 |
up1EquivalentPS = cvtools::modulo(up1EquivalentPS, 2.0*pi);
|
161 |
up1EquivalentPS = cvtools::modulo(up1EquivalentPS, 2.0*pi);
|
160 |
|
162 |
|
161 |
cv::Mat up1EquivalentST = up1Secondary - up1Tertiary;
|
163 |
cv::Mat up1EquivalentST = up1Second - up1Third;
|
162 |
up1EquivalentST = cvtools::modulo(up1EquivalentST, 2.0*pi);
|
164 |
up1EquivalentST = cvtools::modulo(up1EquivalentST, 2.0*pi);
|
163 |
|
165 |
|
164 |
cv::Mat up1Equivalent = up1EquivalentPS - up1EquivalentST;
|
166 |
cv::Mat up1EquivalentPST = up1EquivalentPS - up1EquivalentST;
|
165 |
up1Equivalent = cvtools::modulo(up1Equivalent, 2.0*pi);
|
167 |
up1EquivalentPST = cvtools::modulo(up1EquivalentPST, 2.0*pi);
|
166 |
|
- |
|
167 |
// TODO: we should use backward phase unwrapping (Song Zhang term)...
|
- |
|
168 |
|
168 |
|
169 |
cv::Mat up1 = unwrapWithCue(up1Primary, up1Equivalent, (float)screenCols/nPeriodsPrimary);
|
169 |
cv::Mat up1 = unwrapWithCue(up1First, up1EquivalentPST, (float)screenCols/nPeriodsFirst);
|
170 |
up1 *= screenCols/(2.0*pi);
|
170 |
up1 *= screenCols/(2.0*pi);
|
171 |
cv::Mat amplitude1;
|
171 |
cv::Mat amplitude1;
|
172 |
cv::magnitude(F1Primary[2], -F1Primary[3], amplitude1);
|
172 |
cv::magnitude(F1First[2], -F1First[3], amplitude1);
|
173 |
|
173 |
|
174 |
#ifdef QT_DEBUG
|
174 |
#ifdef QT_DEBUG
|
175 |
cvtools::writeMat(up0Primary, "up0Primary.mat", "up0Primary");
|
175 |
cvtools::writeMat(up0First, "up0First.mat", "up0First");
|
176 |
cvtools::writeMat(up0Secondary, "up0Secondary.mat", "up0Secondary");
|
176 |
cvtools::writeMat(up0Second, "up0Second.mat", "up0Second");
|
177 |
cvtools::writeMat(up0Tertiary, "up0Tertiary.mat", "up0Tertiary");
|
177 |
cvtools::writeMat(up0Third, "up0Third.mat", "up0Third");
|
178 |
cvtools::writeMat(up0EquivalentPS, "up0EquivalentPS.mat", "up0EquivalentPS");
|
178 |
cvtools::writeMat(up0EquivalentPS, "up0EquivalentPS.mat", "up0EquivalentPS");
|
179 |
cvtools::writeMat(up0EquivalentPT, "up0EquivalentPT.mat", "up0EquivalentPT");
|
179 |
cvtools::writeMat(up0EquivalentPT, "up0EquivalentPT.mat", "up0EquivalentPT");
|
180 |
cvtools::writeMat(up0Equivalent, "up0Equivalent.mat", "up0Equivalent");
|
180 |
cvtools::writeMat(up0EquivalentPST, "up0EquivalentPST.mat", "up0EquivalentPST");
|
181 |
cvtools::writeMat(up0, "up0.mat", "up0");
|
181 |
cvtools::writeMat(up0, "up0.mat", "up0");
|
182 |
cvtools::writeMat(up1, "up1.mat", "up1");
|
182 |
cvtools::writeMat(up1, "up1.mat", "up1");
|
183 |
cvtools::writeMat(amplitude0, "amplitude0.mat", "amplitude0");
|
183 |
cvtools::writeMat(amplitude0, "amplitude0.mat", "amplitude0");
|
184 |
|
184 |
|
185 |
cvtools::writeMat(amplitude0, "amplitude0.mat", "amplitude0");
|
185 |
cvtools::writeMat(amplitude0, "amplitude0.mat", "amplitude0");
|