Subversion Repositories seema-scanner

Rev

Rev 187 | Rev 189 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 187 Rev 188
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");