Subversion Repositories seema-scanner

Rev

Rev 231 | Rev 234 | Go to most recent revision | Show entire file | Regard whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 231 Rev 233
Line 59... Line 59...
59 
 
 59 
 
60 
cv::Mat AlgorithmPhaseShiftTwoFreq::getEncodingPattern(unsigned int depth){
 60 
cv::Mat AlgorithmPhaseShiftTwoFreq::getEncodingPattern(unsigned int depth){
61 
    return patterns[depth];
 61 
    return patterns[depth];
62 
}
 62 
}
63 
 
 63 
 
64 
void AlgorithmPhaseShiftTwoFreq::get3DPoints(const SMCalibrationParameters & calibration, const std::vector<cv::Mat>& frames0, const std::vector<cv::Mat>& frames1, std::vector<cv::Point3f>& Q, std::vector<cv::Vec3b>& color){
 - 
 
65 
 
 - 
 
66 
    assert(frames0.size() == N);
 - 
 
67 
    assert(frames1.size() == N);
 - 
 
68 
 
 64 
 
- 
 
 65 
struct StereoRectifyier {
69 
    cv::Mat map0X, map0Y, map1X, map1Y;
 66 
    cv::Mat map0X, map0Y, map1X, map1Y;
70 
    cv::Mat R0, P0, P1, QRect;
 67 
    cv::Mat R0, R1, P0, P1, QRect;
- 
 
 68 
};
- 
 
 69 
static void getStereoRectifyier(const SMCalibrationParameters &calibration,
- 
 
 70 
                                const cv::Size& frameSize,
- 
 
 71 
                                StereoRectifyier& stereoRect);
- 
 
 72 
static void determineAmplitudePhaseEnergy(std::vector<cv::Mat>& frames,
- 
 
 73 
                                    cv::Mat& amplitude, cv::Mat& phase, cv::Mat& energy);
- 
 
 74 
static void collectPhases(const cv::Mat& phasePrimary, const cv::Mat& phaseSecondary,
- 
 
 75 
                          cv::Mat& phase);
- 
 
 76 
static void matchPhaseMaps(const cv::Mat& occlusion0, const cv::Mat& occlusion1,
- 
 
 77 
                           const cv::Mat& phase0, const cv::Mat& phase1,
- 
 
 78 
                           std::vector<cv::Vec2f>& q0, std::vector<cv::Vec2f>& q1);
- 
 
 79 
static void triangulate(const StereoRectifyier& stereoRect,
- 
 
 80 
                        const std::vector<cv::Vec2f>& q0,
- 
 
 81 
                        const std::vector<cv::Vec2f>& q1,
- 
 
 82 
                        std::vector<cv::Point3f>& Q);
71 
 
 83 
 
72 
    {
 - 
 
73 
    int frameRows = frames0[0].rows;
 - 
 
74 
    int frameCols = frames0[0].cols;
 - 
 
75 
 
 84 
 
76 
    // Rectifying homographies (rotation+projections)
 - 
 
77 
    cv::Size frameSize(frameCols, frameRows);
 - 
 
78 
    cv::Mat R, T;
 - 
 
79 
    // stereoRectify segfaults unless R is double precision
 - 
 
80 
    cv::Mat(calibration.R1).convertTo(R, CV_64F);
 - 
 
81 
    cv::Mat(calibration.T1).convertTo(T, CV_64F);
 - 
 
82 
    cv::Mat  R1;
 - 
 
83 
    cv::stereoRectify(calibration.K0, calibration.k0, calibration.K1, calibration.k1, frameSize, R, T, R0, R1, P0, P1, QRect, 0);
 85 
void AlgorithmPhaseShiftTwoFreq::get3DPoints(const SMCalibrationParameters & calibration, const std::vector<cv::Mat>& frames0, const std::vector<cv::Mat>& frames1, std::vector<cv::Point3f>& Q, std::vector<cv::Vec3b>& color){
84 
 
 86 
 
85 
    // Interpolation maps (lens distortion and rectification)
 87 
    assert(frames0.size() == N);
86 
    cv::initUndistortRectifyMap(calibration.K0, calibration.k0, R0, P0, frameSize, CV_32F, map0X, map0Y);
 - 
 
87 
    cv::initUndistortRectifyMap(calibration.K1, calibration.k1, R1, P1, frameSize, CV_32F, map1X, map1Y);
 88 
    assert(frames1.size() == N);
88 
    }
 - 
 
89 
 
 89 
 
90 
    int frameRectRows = map0X.rows;
 90 
    StereoRectifyier stereoRect;
91 
    int frameRectCols = map0X.cols;
 91 
    getStereoRectifyier(calibration, cv::Size(frames0[0].cols, frames0[0].rows), stereoRect);
92 
 
 92 
 
93 
    cv::Mat up0;
 93 
    // // Erode occlusion masks
- 
 
 94 
    // cv::Mat strel = cv::getStructuringElement(cv::MORPH_ELLIPSE, cv::Size(5,5));
- 
 
 95 
 
94 
    cv::Mat occlusion0;
 96 
    cv::Mat up0, up1;
95 
    cv::Mat up1;
 97 
    cv::Mat occlusion0, occlusion1;
96 
    cv::Mat occlusion1;
 98 
    cv::Mat color0, color1;
97 
 
 99 
 
98 
    #pragma omp parallel sections
 100 
    #pragma omp parallel sections
99 
    {
 101 
    {
100 
    #pragma omp section
 102 
        #pragma omp section
101 
    {
 103 
        {
102 
 
 104 
 
103 
    // Gray-scale and remap
 105 
            // Gray-scale and remap/rectify
104 
    std::vector<cv::Mat> frames0Rect(N);
 106 
            std::vector<cv::Mat> frames0Rect(N);
105 
 
 107 
 
106 
    for(unsigned int i=0; i<N; i++){
 108 
            for(unsigned int i=0; i<N; i++){
107 
        cv::Mat temp;
 109 
                cv::Mat temp;
108 
        cv::cvtColor(frames0[i], temp, CV_BayerBG2GRAY);
 110 
                cv::cvtColor(frames0[i], temp, CV_BayerBG2GRAY);
109 
        cv::remap(temp, frames0Rect[i], map0X, map0Y, CV_INTER_LINEAR);
 111 
                cv::remap(temp, frames0Rect[i], stereoRect.map0X, stereoRect.map0Y, CV_INTER_LINEAR);
110 
    }
 112 
            }
- 
 
 113 
 
111 
    // Occlusion masks
 114 
            // Occlusion masks
112 
    cv::subtract(frames0Rect[0], frames0Rect[1], occlusion0);
 115 
            cv::subtract(frames0Rect[0], frames0Rect[1], occlusion0);
113 
    occlusion0 = (occlusion0 > 25) & (occlusion0 < 250);
 116 
            occlusion0 = (occlusion0 > 25) & (occlusion0 < 250);
114 
 
 117 
 
115 
    // Decode camera0
 118 
            // Decode camera0
116 
    std::vector<cv::Mat> frames0Primary(frames0Rect.begin()+2, frames0Rect.begin()+2+nStepsPrimary);
 119 
            std::vector<cv::Mat> frames0Primary(frames0Rect.begin()+2, frames0Rect.begin()+2+nStepsPrimary);
117 
    std::vector<cv::Mat> frames0Secondary(frames0Rect.begin()+2+nStepsPrimary, frames0Rect.end());
 120 
            std::vector<cv::Mat> frames0Secondary(frames0Rect.begin()+2+nStepsPrimary, frames0Rect.end());
118 
 
 121 
 
119 
    frames0Rect.clear();
 122 
            frames0Rect.clear();
120 
 
 123 
 
121 
    std::vector<cv::Mat> F0Primary = getDFTComponents(frames0Primary);
 - 
 
122 
    frames0Primary.clear();
 - 
 
123 
    cv::Mat up0Primary;
 - 
 
124 
    cv::phase(F0Primary[2], -F0Primary[3], up0Primary);
 - 
 
125 
 
 - 
 
126 
    std::vector<cv::Mat> F0Secondary = getDFTComponents(frames0Secondary);
 - 
 
127 
    frames0Secondary.clear();
 - 
 
128 
    cv::Mat up0Secondary;
 - 
 
129 
    cv::phase(F0Secondary[2], -F0Secondary[3], up0Secondary);
 - 
 
130 
 
 - 
 
131 
    cv::Mat up0Equivalent = up0Secondary - up0Primary;
 - 
 
132 
    up0Equivalent = cvtools::modulo(up0Equivalent, 2.0*CV_PI);
 - 
 
133 
    up0 = unwrapWithCue(up0Primary, up0Equivalent, nPeriodsPrimary);
 - 
 
134 
    up0 *= screenCols/(2.0*CV_PI);
 - 
 
135 
 
 - 
 
136 
    // Signal energy at unit frequency
 - 
 
137 
    cv::Mat amplitude0Primary, amplitude0Secondary;
 124 
            cv::Mat amplitude0Primary, amplitude0Secondary;
- 
 
 125 
            cv::Mat up0Primary, up0Secondary;
138 
    cv::magnitude(F0Primary[2], -F0Primary[3], amplitude0Primary);
 126 
            cv::Mat energy0Primary, energy0Secondary;
- 
 
 127 
            determineAmplitudePhaseEnergy(frames0Primary,
- 
 
 128 
                                          amplitude0Primary, up0Primary, energy0Primary);
- 
 
 129 
            determineAmplitudePhaseEnergy(frames0Secondary,
139 
    cv::magnitude(F0Secondary[2], -F0Secondary[3], amplitude0Secondary);
 130 
                                          amplitude0Secondary, up0Secondary, energy0Secondary);
140 
 
 131 
 
141 
    // Collected signal energy at higher frequencies
 - 
 
142 
    cv::Mat energy0Primary(frameRectRows, frameRectCols, CV_32F, cv::Scalar(0.0));
 - 
 
143 
    for(unsigned int i=0; i<nStepsPrimary-1; i++){
 - 
 
144 
        cv::Mat magnitude;
 - 
 
145 
        cv::magnitude(F0Primary[i*2 + 2], F0Primary[i*2 + 3], magnitude);
 - 
 
146 
        cv::add(energy0Primary, magnitude, energy0Primary, cv::noArray(), CV_32F);
 - 
 
147 
    }
 - 
 
148 
 
 - 
 
149 
    cv::Mat energy0Secondary(frameRectRows, frameRectCols, CV_32F, cv::Scalar(0.0));
 - 
 
150 
    for(unsigned int i=0; i<nStepsSecondary-1; i++){
 132 
            collectPhases(up0Primary, up0Secondary, up0);
151 
        cv::Mat magnitude;
 - 
 
152 
        cv::magnitude(F0Secondary[i*2 + 2], F0Secondary[i*2 + 3], magnitude);
 - 
 
153 
        cv::add(energy0Secondary, magnitude, energy0Secondary, cv::noArray(), CV_32F);
 - 
 
154 
    }
 - 
 
155 
 
 133 
 
156 
    // Threshold on energy at primary frequency
 134 
            // Threshold on energy at primary frequency
157 
    occlusion0 = occlusion0 & (amplitude0Primary > 5.0*nStepsPrimary);
 135 
            occlusion0 = occlusion0 & (amplitude0Primary > 5.0*nStepsPrimary);
158 
    // Threshold on energy ratios
 136 
            // Threshold on energy ratios
159 
    occlusion0 = occlusion0 & (amplitude0Primary > 0.25*energy0Primary);
 137 
            occlusion0 = occlusion0 & (amplitude0Primary > 0.25*energy0Primary);
160 
    occlusion0 = occlusion0 & (amplitude0Secondary > 0.25*energy0Secondary);
 138 
            occlusion0 = occlusion0 & (amplitude0Secondary > 0.25*energy0Secondary);
161 
 
 139 
 
- 
 
 140 
            // // Erode occlusion masks
- 
 
 141 
            // cv::erode(occlusion0, occlusion0, strel);
- 
 
 142 
 
- 
 
 143 
            // Threshold on gradient of phase
- 
 
 144 
            cv::Mat edges0;
- 
 
 145 
            cv::Sobel(up0, edges0, -1, 1, 1, 5);
- 
 
 146 
            occlusion0 = occlusion0 & (abs(edges0) < 10);
- 
 
 147 
 
162 
    #ifdef SM_DEBUG
 148 
            #ifdef SM_DEBUG
163 
        cvtools::writeMat(up0Primary, "up0Primary.mat", "up0Primary");
 149 
                cvtools::writeMat(up0Primary, "up0Primary.mat", "up0Primary");
164 
        cvtools::writeMat(up0Secondary, "up0Secondary.mat", "up0Secondary");
 150 
                cvtools::writeMat(up0Secondary, "up0Secondary.mat", "up0Secondary");
165 
        cvtools::writeMat(up0Equivalent, "up0Equivalent.mat", "up0Equivalent");
 - 
 
166 
        cvtools::writeMat(up0, "up0.mat", "up0");
 151 
                cvtools::writeMat(up0, "up0.mat", "up0");
167 
        cvtools::writeMat(amplitude0Primary, "amplitude0Primary.mat", "amplitude0Primary");
 152 
                cvtools::writeMat(amplitude0Primary, "amplitude0Primary.mat", "amplitude0Primary");
- 
 
 153 
                cvtools::writeMat(amplitude0Secondary, "amplitude0Secondary.mat", "amplitude0Secondary");
168 
        cvtools::writeMat(energy0Primary, "energy0Primary.mat", "energy0Primary");
 154 
                cvtools::writeMat(energy0Primary, "energy0Primary.mat", "energy0Primary");
169 
        cvtools::writeMat(energy0Secondary, "energy0Secondary.mat", "energy0Secondary");
 155 
                cvtools::writeMat(energy0Secondary, "energy0Secondary.mat", "energy0Secondary");
- 
 
 156 
                cvtools::writeMat(edges0, "edges0.mat", "edges0");
- 
 
 157 
                cvtools::writeMat(occlusion0, "occlusion0.mat", "occlusion0");
170 
    #endif
 158 
            #endif
171 
 
 159 
 
172 
 
 - 
 
173 
    }
 160 
        }
174 
    #pragma omp section
 161 
        #pragma omp section
175 
    {
 162 
        {
176 
 
 163 
 
177 
    // Gray-scale and remap
 164 
            // Gray-scale and remap
178 
    std::vector<cv::Mat> frames1Rect(N);
 165 
            std::vector<cv::Mat> frames1Rect(N);
179 
 
 166 
 
180 
    for(unsigned int i=0; i<N; i++){
 167 
            for(unsigned int i=0; i<N; i++){
181 
        cv::Mat temp;
 168 
                cv::Mat temp;
182 
        cv::cvtColor(frames1[i], temp, CV_BayerBG2GRAY);
 169 
                cv::cvtColor(frames1[i], temp, CV_BayerBG2GRAY);
183 
        cv::remap(temp, frames1Rect[i], map1X, map1Y, CV_INTER_LINEAR);
 170 
                cv::remap(temp, frames1Rect[i], stereoRect.map1X, stereoRect.map1Y, CV_INTER_LINEAR);
184 
    }
 171 
            }
185 
 
 172 
 
186 
    // Occlusion masks
 173 
            // Occlusion masks
187 
    cv::subtract(frames1Rect[0], frames1Rect[1], occlusion1);
 174 
            cv::subtract(frames1Rect[0], frames1Rect[1], occlusion1);
188 
    occlusion1 = (occlusion1 > 25) & (occlusion1 < 250);
 175 
            occlusion1 = (occlusion1 > 25) & (occlusion1 < 250);
Line 191... Line 178...
191 
    std::vector<cv::Mat> frames1Primary(frames1Rect.begin()+2, frames1Rect.begin()+2+nStepsPrimary);
 178 
            std::vector<cv::Mat> frames1Primary(frames1Rect.begin()+2, frames1Rect.begin()+2+nStepsPrimary);
192 
    std::vector<cv::Mat> frames1Secondary(frames1Rect.begin()+2+nStepsPrimary, frames1Rect.end());
 179 
            std::vector<cv::Mat> frames1Secondary(frames1Rect.begin()+2+nStepsPrimary, frames1Rect.end());
193 
 
 180 
 
194 
    frames1Rect.clear();
 181 
            frames1Rect.clear();
195 
 
 182 
 
196 
    std::vector<cv::Mat> F1Primary = getDFTComponents(frames1Primary);
 - 
 
197 
    frames1Primary.clear();
 - 
 
198 
    cv::Mat up1Primary;
 - 
 
199 
    cv::phase(F1Primary[2], -F1Primary[3], up1Primary);
 - 
 
200 
 
 - 
 
201 
    std::vector<cv::Mat> F1Secondary = getDFTComponents(frames1Secondary);
 - 
 
202 
    frames1Secondary.clear();
 - 
 
203 
    cv::Mat up1Secondary;
 - 
 
204 
    cv::phase(F1Secondary[2], -F1Secondary[3], up1Secondary);
 - 
 
205 
 
 - 
 
206 
    cv::Mat up1Equivalent = up1Secondary - up1Primary;
 - 
 
207 
    up1Equivalent = cvtools::modulo(up1Equivalent, 2.0*CV_PI);
 - 
 
208 
    up1 = unwrapWithCue(up1Primary, up1Equivalent, nPeriodsPrimary);
 - 
 
209 
    up1 *= screenCols/(2.0*CV_PI);
 - 
 
210 
 
 - 
 
211 
    // Signal energy at unit frequency
 - 
 
212 
    cv::Mat amplitude1Primary, amplitude1Secondary;
 183 
            cv::Mat amplitude1Primary, amplitude1Secondary;
213 
    cv::magnitude(F1Primary[2], -F1Primary[3], amplitude1Primary);
 184 
            cv::Mat up1Primary, up1Secondary;
214 
    cv::magnitude(F1Secondary[2], -F1Secondary[3], amplitude1Secondary);
 185 
            cv::Mat energy1Primary, energy1Secondary;
215 
 
 - 
 
216 
    // Collected signal energy at higher frequencies
 186 
            determineAmplitudePhaseEnergy(frames1Primary,
217 
    cv::Mat energy1Primary(frameRectRows, frameRectCols, CV_32F, cv::Scalar(0.0));
 187 
                                          amplitude1Primary, up1Primary, energy1Primary);
218 
    for(unsigned int i=0; i<nStepsPrimary-1; i++){
 188 
            determineAmplitudePhaseEnergy(frames1Secondary,
219 
        cv::Mat magnitude;
 - 
 
220 
        cv::magnitude(F1Primary[i*2 + 2], F1Primary[i*2 + 3], magnitude);
 - 
 
221 
        cv::add(energy1Primary, magnitude, energy1Primary, cv::noArray(), CV_32F);
 189 
                                          amplitude1Secondary, up1Secondary, energy1Secondary);
222 
    }
 - 
 
223 
 
 190 
 
224 
    cv::Mat energy1Secondary(frameRectRows, frameRectCols, CV_32F, cv::Scalar(0.0));
 - 
 
225 
    for(unsigned int i=0; i<nStepsSecondary-1; i++){
 191 
            collectPhases(up1Primary, up1Secondary, up1);
226 
        cv::Mat magnitude;
 - 
 
227 
        cv::magnitude(F1Secondary[i*2 + 2], F1Secondary[i*2 + 3], magnitude);
 - 
 
228 
        cv::add(energy1Secondary, magnitude, energy1Secondary, cv::noArray(), CV_32F);
 - 
 
229 
    }
 - 
 
230 
 
 192 
 
231 
    // Threshold on energy at primary frequency
 193 
            // Threshold on energy at primary frequency
232 
    occlusion1 = occlusion1 & (amplitude1Primary > 5.0*nStepsPrimary);
 194 
            occlusion1 = occlusion1 & (amplitude1Primary > 5.0*nStepsPrimary);
233 
    // Threshold on energy ratios
 195 
            // Threshold on energy ratios
234 
    occlusion1 = occlusion1 & (amplitude1Primary > 0.25*energy1Primary);
 196 
            occlusion1 = occlusion1 & (amplitude1Primary > 0.25*energy1Primary);
235 
    occlusion1 = occlusion1 & (amplitude1Secondary > 0.25*energy1Secondary);
 197 
            occlusion1 = occlusion1 & (amplitude1Secondary > 0.25*energy1Secondary);
236 
 
 198 
 
237 
    #ifdef SM_DEBUG
 - 
 
238 
        cvtools::writeMat(up1, "up1.mat", "up1");
 - 
 
239 
    #endif
 - 
 
240 
 
 - 
 
241 
    }
 - 
 
242 
    }
 - 
 
243 
 
 - 
 
244 
//    // Erode occlusion masks
 199 
            // // Erode occlusion masks
245 
//    cv::Mat strel = cv::getStructuringElement(cv::MORPH_ELLIPSE, cv::Size(5,5));
 - 
 
246 
//    cv::erode(occlusion0, occlusion0, strel);
 - 
 
247 
//    cv::erode(occlusion1, occlusion1, strel);
 200 
            // cv::erode(occlusion1, occlusion1, strel);
248 
 
 201 
 
249 
    // Threshold on gradient of phase
 - 
 
250 
    cv::Mat edges0;
 - 
 
251 
    cv::Sobel(up0, edges0, -1, 1, 1, 5);
 - 
 
252 
    occlusion0 = occlusion0 & (abs(edges0) < 10);
 - 
 
253 
 
 202 
 
- 
 
 203 
            // Threshold on gradient of phase
254 
    cv::Mat edges1;
 204 
            cv::Mat edges1;
255 
    cv::Sobel(up1, edges1, -1, 1, 1, 5);
 205 
            cv::Sobel(up1, edges1, -1, 1, 1, 5);
256 
    occlusion1 = occlusion1 & (abs(edges1) < 10);
 206 
            occlusion1 = occlusion1 & (abs(edges1) < 10);
257 
 
 207 
 
258 
    #ifdef SM_DEBUG
 208 
            #ifdef SM_DEBUG
- 
 
 209 
                cvtools::writeMat(up1Primary, "up1Primary.mat", "up1Primary");
- 
 
 210 
                cvtools::writeMat(up1Secondary, "up1Secondary.mat", "up1Secondary");
259 
        cvtools::writeMat(edges0, "edges0.mat", "edges0");
 211 
                cvtools::writeMat(up1, "up1.mat", "up1");
- 
 
 212 
                cvtools::writeMat(amplitude1Primary, "amplitude1Primary.mat", "amplitude1Primary");
- 
 
 213 
                cvtools::writeMat(amplitude1Secondary, "amplitude1Secondary.mat", "amplitude1Secondary");
- 
 
 214 
                cvtools::writeMat(energy1Primary, "energy1Primary.mat", "energy1Primary");
- 
 
 215 
                cvtools::writeMat(energy1Secondary, "energy1Secondary.mat", "energy1Secondary");
260 
        cvtools::writeMat(edges1, "edges1.mat", "edges1");
 216 
                cvtools::writeMat(edges1, "edges1.mat", "edges1");
261 
        cvtools::writeMat(occlusion0, "occlusion0.mat", "occlusion0");
 - 
 
262 
        cvtools::writeMat(occlusion1, "occlusion1.mat", "occlusion1");
 217 
                cvtools::writeMat(occlusion1, "occlusion1.mat", "occlusion1");
263 
    #endif
 218 
            #endif
264 
 
 219 
 
- 
 
 220 
        }
- 
 
 221 
    }
- 
 
 222 
 
- 
 
 223 
 
265 
    // Match phase maps
 224 
    // Match phase maps
266 
 
 225 
 
267 
    // camera0 against camera1
 226 
    // camera0 against camera1
268 
    std::vector<cv::Vec2f> q0, q1;
 227 
    std::vector<cv::Vec2f> q0, q1;
269 
 
 - 
 
270 
    #pragma omp parallel for
 - 
 
271 
    for(int row=0; row<frameRectRows; row++){
 - 
 
272 
        for(int col=0; col<frameRectCols; col++){
 - 
 
273 
 
 - 
 
274 
            if(!occlusion0.at<char>(row,col))
 - 
 
275 
                continue;
 - 
 
276 
 
 - 
 
277 
            float up0i = up0.at<float>(row,col);
 - 
 
278 
            for(int col1=0; col1<up1.cols-1; col1++){
 - 
 
279 
 
 - 
 
280 
                if(!occlusion1.at<char>(row,col1) || !occlusion1.at<char>(row,col1+1))
 228 
    matchPhaseMaps(occlusion0, occlusion1, up0, up1, q0, q1);
281 
                    continue;
 - 
 
282 
 
 - 
 
283 
                float up1Left = up1.at<float>(row,col1);
 - 
 
284 
                float up1Right = up1.at<float>(row,col1+1);
 - 
 
285 
 
 - 
 
286 
                if((up1Left <= up0i) && (up0i <= up1Right) && (up0i-up1Left < 1.0) && (up1Right-up0i < 1.0) && (up1Right-up1Left > 0.1)){
 - 
 
287 
 
 - 
 
288 
                    float col1i = col1 + (up0i-up1Left)/(up1Right-up1Left);
 - 
 
289 
 
 - 
 
290 
                    #pragma omp critical
 - 
 
291 
                    {
 - 
 
292 
                    q0.push_back(cv::Point2f(col, row));
 - 
 
293 
                    q1.push_back(cv::Point2f(col1i, row));
 - 
 
294 
                    }
 - 
 
295 
                    break;
 - 
 
296 
                }
 - 
 
297 
            }
 - 
 
298 
        }
 - 
 
299 
    }
 - 
 
300 
 
 - 
 
301 
 
 229 
 
302 
    size_t nMatches = q0.size();
 230 
    size_t nMatches = q0.size();
303 
 
 231 
 
304 
    if(nMatches < 1){
 232 
    if(nMatches < 1){
305 
        Q.resize(0);
 233 
        Q.resize(0);
306 
        color.resize(0);
 234 
        color.resize(0);
307 
 
 235 
 
308 
        return;
 236 
        return;
309 
    }
 237 
    }
310 
{
 238 
    {
311 
    // color debayer and remap
 239 
        // color debayer and remap/rectify
312 
    cv::Mat color0, color1;
 - 
 
313 
    cv::cvtColor(frames0[0], color0, CV_BayerBG2RGB);
 - 
 
314 
    cv::remap(color0, color0, map0X, map0Y, CV_INTER_LINEAR);
 - 
 
315 
 
 240 
 
316 
    cv::cvtColor(frames1[0], color1, CV_BayerBG2RGB);
 241 
        cv::cvtColor(frames1[0], color1, CV_BayerBG2RGB);
317 
    cv::remap(color1, color1, map1X, map1Y, CV_INTER_LINEAR);
 242 
        cv::remap(color1, color1, stereoRect.map1X, stereoRect.map1Y, CV_INTER_LINEAR);
318 
 
 243 
 
319 
    #ifdef SM_DEBUG
 244 
        #ifdef SM_DEBUG
320 
        cvtools::writeMat(color0, "color0.mat", "color0");
 245 
            cvtools::writeMat(color0, "color0.mat", "color0");
321 
        cvtools::writeMat(color1, "color1.mat", "color1");
 246 
            cvtools::writeMat(color1, "color1.mat", "color1");
322 
    #endif
 247 
        #endif
Line 330... Line 255...
330 
 
 255 
 
331 
        color[i] = 0.5*c0 + 0.5*c1;
 256 
            color[i] = 0.5*c0 + 0.5*c1;
332 
    }
 257 
        }
333 
}
 258 
    }
334 
 
 259 
 
- 
 
 260 
    // Triangulate points
- 
 
 261 
    triangulate(stereoRect, q0, q1, Q);
- 
 
 262 
 
- 
 
 263 
}
- 
 
 264 
 
- 
 
 265 
void getStereoRectifyier(const SMCalibrationParameters &calibration, const cv::Size& frameSize, StereoRectifyier& stereoRect){
- 
 
 266 
 
- 
 
 267 
    // cv::stereoRectify segfaults unless R is double precision
- 
 
 268 
    cv::Mat R, T;
- 
 
 269 
    cv::Mat(calibration.R1).convertTo(R, CV_64F);
- 
 
 270 
    cv::Mat(calibration.T1).convertTo(T, CV_64F);
- 
 
 271 
 
- 
 
 272 
    cv::stereoRectify(calibration.K0, calibration.k0,
- 
 
 273 
                      calibration.K1, calibration.k1,
- 
 
 274 
                      frameSize, R, T,
- 
 
 275 
                      stereoRect.R0, stereoRect.R1,
- 
 
 276 
                      stereoRect.P0, stereoRect.P1,
- 
 
 277 
                      stereoRect.QRect, 0);
- 
 
 278 
 
- 
 
 279 
    // Interpolation maps (lens distortion and rectification)
- 
 
 280 
    cv::initUndistortRectifyMap(calibration.K0, calibration.k0,
- 
 
 281 
                                stereoRect.R0, stereoRect.P0,
- 
 
 282 
                                frameSize, CV_32F,
- 
 
 283 
                                stereoRect.map0X, stereoRect.map0Y);
- 
 
 284 
    cv::initUndistortRectifyMap(calibration.K1, calibration.k1,
- 
 
 285 
                                stereoRect.R1, stereoRect.P1,
- 
 
 286 
                                frameSize, CV_32F,
- 
 
 287 
                                stereoRect.map1X, stereoRect.map1Y);
- 
 
 288 
}
- 
 
 289 
 
- 
 
 290 
void determineAmplitudePhaseEnergy(std::vector<cv::Mat>& frames,
- 
 
 291 
                                   cv::Mat& amplitude, cv::Mat& phase, cv::Mat& energy) {
- 
 
 292 
 
- 
 
 293 
    std::vector<cv::Mat> fourier = getDFTComponents(frames);
- 
 
 294 
 
- 
 
 295 
    cv::phase(fourier[2], -fourier[3], phase);
- 
 
 296 
 
- 
 
 297 
    // Signal energy at unit frequency
- 
 
 298 
    cv::magnitude(fourier[2], -fourier[3], amplitude);
- 
 
 299 
 
- 
 
 300 
    // Collected signal energy at higher frequencies
- 
 
 301 
    energy = cv::Mat(phase.rows, phase.cols, CV_32F, cv::Scalar(0.0));
- 
 
 302 
 
- 
 
 303 
    for(unsigned int i=0; i<frames.size()-1; i++){
- 
 
 304 
        cv::Mat magnitude;
- 
 
 305 
        cv::magnitude(fourier[i*2 + 2], fourier[i*2 + 3], magnitude);
- 
 
 306 
        cv::add(energy, magnitude, energy, cv::noArray(), CV_32F);
- 
 
 307 
    }
- 
 
 308 
 
- 
 
 309 
    frames.clear();
- 
 
 310 
}
- 
 
 311 
 
- 
 
 312 
void collectPhases(const cv::Mat& phasePrimary, const cv::Mat& phaseSecondary,
- 
 
 313 
                   cv::Mat& phase) {
- 
 
 314 
    cv::Mat phaseEquivalent = phaseSecondary - phasePrimary;
- 
 
 315 
    phaseEquivalent = cvtools::modulo(phaseEquivalent, 2.0*CV_PI);
- 
 
 316 
    phase = unwrapWithCue(phasePrimary, phaseEquivalent, nPeriodsPrimary);
- 
 
 317 
    phase *= phasePrimary.cols/(2.0*CV_PI);
- 
 
 318 
}
- 
 
 319 
 
- 
 
 320 
 
- 
 
 321 
void matchPhaseMaps(const cv::Mat& occlusion0, const cv::Mat& occlusion1,
- 
 
 322 
                    const cv::Mat& phase0, const cv::Mat& phase1,
- 
 
 323 
                    std::vector<cv::Vec2f>& q0, std::vector<cv::Vec2f>& q1) {
- 
 
 324 
 
- 
 
 325 
    #pragma omp parallel for
- 
 
 326 
    for(int row=0; row<occlusion0.rows; row++){
- 
 
 327 
        for(int col=0; col<occlusion0.cols; col++){
- 
 
 328 
 
- 
 
 329 
            if(!occlusion0.at<char>(row,col))
- 
 
 330 
                continue;
- 
 
 331 
 
- 
 
 332 
            float phase0i = phase0.at<float>(row,col);
- 
 
 333 
            for(int col1=0; col1<phase1.cols-1; col1++){
- 
 
 334 
 
- 
 
 335 
                if(!occlusion1.at<char>(row,col1) || !occlusion1.at<char>(row,col1+1))
- 
 
 336 
                    continue;
- 
 
 337 
 
- 
 
 338 
                float phase1Left = phase1.at<float>(row,col1);
- 
 
 339 
                float phase1Right = phase1.at<float>(row,col1+1);
- 
 
 340 
 
- 
 
 341 
                bool match = (phase1Left <= phase0i)
- 
 
 342 
                              && (phase0i <= phase1Right)
- 
 
 343 
                              && (phase0i-phase1Left < 1.0)
- 
 
 344 
                              && (phase1Right-phase0i < 1.0)
- 
 
 345 
                              && (phase1Right-phase1Left > 0.1);
- 
 
 346 
 
- 
 
 347 
                if(match){
- 
 
 348 
 
- 
 
 349 
                    float col1i = col1 + (phase0i-phase1Left)/(phase1Right-phase1Left);
- 
 
 350 
 
- 
 
 351 
                    #pragma omp critical
- 
 
 352 
                    {
- 
 
 353 
                    q0.push_back(cv::Point2f(col, row));
- 
 
 354 
                    q1.push_back(cv::Point2f(col1i, row));
- 
 
 355 
                    }
- 
 
 356 
                    break;
- 
 
 357 
                }
- 
 
 358 
            }
- 
 
 359 
        }
- 
 
 360 
    }
- 
 
 361 
 
- 
 
 362 
}
- 
 
 363 
 
- 
 
 364 
void triangulate(const StereoRectifyier& stereoRect,
- 
 
 365 
                 const std::vector<cv::Vec2f>& q0,
- 
 
 366 
                 const std::vector<cv::Vec2f>& q1,
- 
 
 367 
                 std::vector<cv::Point3f>& Q) {
335 
 
 368 
 
336 
//    // Triangulate points
 - 
 
337 
//    cv::Mat QMatHomogenous, QMat;
 369 
    // cv::Mat QMatHomogenous, QMat;
338 
//    cv::triangulatePoints(P0, P1, q0, q1, QMatHomogenous);
 370 
    // cv::triangulatePoints(P0, P1, q0, q1, QMatHomogenous);
339 
//    cvtools::convertMatFromHomogeneous(QMatHomogenous, QMat);
 371 
    // cvtools::convertMatFromHomogeneous(QMatHomogenous, QMat);
340 
 
 372 
 
341 
//    // Undo rectification
 373 
    // // Undo rectification
Line 346... Line 378...
346 
//    cvtools::matToPoints3f(QMat, Q);
 378 
    // cvtools::matToPoints3f(QMat, Q);
347 
 
 379 
 
348 
 
 380 
 
349 
    // Triangulate by means of disparity projection
 381 
    // Triangulate by means of disparity projection
350 
    Q.resize(q0.size());
 382 
    Q.resize(q0.size());
351 
    cv::Matx44f QRectx = cv::Matx44f(QRect);
 383 
    cv::Matx44f QRectx = cv::Matx44f(stereoRect.QRect);
352 
    cv::Matx33f R0invx = cv::Matx33f(cv::Mat(R0.t()));
 384 
    cv::Matx33f R0invx = cv::Matx33f(cv::Mat(stereoRect.R0.t()));
353 
 
 385 
 
354 
    #pragma omp parallel for
 386 
    #pragma omp parallel for
355 
    for(unsigned int i=0; i<q0.size(); i++){
 387 
    for(unsigned int i=0; i < q0.size(); i++){
356 
        float disparity = q0[i][0]-q1[i][0];
 388 
        float disparity = q0[i][0] - q1[i][0];
357 
        cv::Vec4f Qih = QRectx*cv::Vec4f(q0[i][0], q0[i][1], disparity, 1.0);
 389 
        cv::Vec4f Qih = QRectx*cv::Vec4f(q0[i][0], q0[i][1], disparity, 1.0);
358 
        float winv = float(1.0)/Qih[3];
 390 
        float winv = float(1.0) / Qih[3];
359 
        Q[i] = R0invx * cv::Point3f(Qih[0]*winv, Qih[1]*winv, Qih[2]*winv);
 391 
        Q[i] = R0invx * cv::Point3f(Qih[0]*winv, Qih[1]*winv, Qih[2]*winv);
360 
    }
 392 
    }
361 
 
 - 
 
362 
}
 393 
}
363 
 
 -