Subversion Repositories seema-scanner

Rev

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

Rev 235 Rev 244
Line 71... Line 71...
71 
 
 71 
 
72 
struct StereoRectifyier {
 72 
struct StereoRectifyier {
73 
    cv::Mat map0X, map0Y, map1X, map1Y;
 73 
    cv::Mat map0X, map0Y, map1X, map1Y;
74 
    cv::Mat R0, R1, P0, P1, QRect;
 74 
    cv::Mat R0, R1, P0, P1, QRect;
75 
};
 75 
};
76 
static void getStereoRectifyier(const SMCalibrationParameters &calibration,
 76 
void getStereoRectifier(const SMCalibrationParameters &calibration,
77 
                                const cv::Size& frameSize,
 77 
                         const cv::Size& frameSize,
78 
                                StereoRectifyier& stereoRect);
 78 
                         StereoRectifyier& stereoRect){
- 
 
 79 
 
- 
 
 80 
    // cv::stereoRectify segfaults unless R is double precision
- 
 
 81 
    cv::Mat R, T;
- 
 
 82 
    cv::Mat(calibration.R1).convertTo(R, CV_64F);
- 
 
 83 
    cv::Mat(calibration.T1).convertTo(T, CV_64F);
- 
 
 84 
 
- 
 
 85 
    cv::stereoRectify(calibration.K0, calibration.k0,
- 
 
 86 
                      calibration.K1, calibration.k1,
- 
 
 87 
                      frameSize, R, T,
- 
 
 88 
                      stereoRect.R0, stereoRect.R1,
- 
 
 89 
                      stereoRect.P0, stereoRect.P1,
- 
 
 90 
                      stereoRect.QRect, 0);
- 
 
 91 
 
- 
 
 92 
    // Interpolation maps (lens distortion and rectification)
- 
 
 93 
    cv::initUndistortRectifyMap(calibration.K0, calibration.k0,
- 
 
 94 
                                stereoRect.R0, stereoRect.P0,
- 
 
 95 
                                frameSize, CV_32F,
- 
 
 96 
                                stereoRect.map0X, stereoRect.map0Y);
- 
 
 97 
    cv::initUndistortRectifyMap(calibration.K1, calibration.k1,
- 
 
 98 
                                stereoRect.R1, stereoRect.P1,
- 
 
 99 
                                frameSize, CV_32F,
- 
 
 100 
                                stereoRect.map1X, stereoRect.map1Y);
- 
 
 101 
}
- 
 
 102 
 
79 
static void determineAmplitudePhaseEnergy(std::vector<cv::Mat>& frames,
 103 
void determineAmplitudePhaseEnergy(std::vector<cv::Mat>& frames,
80 
                                    cv::Mat& amplitude,
 104 
                                   cv::Mat& amplitude,
81 
                                    cv::Mat& phase,
 105 
                                   cv::Mat& phase,
82 
                                    cv::Mat& energy);
 106 
                                   cv::Mat& energy) {
- 
 
 107 
 
- 
 
 108 
    std::vector<cv::Mat> fourier = getDFTComponents(frames);
- 
 
 109 
 
- 
 
 110 
    cv::phase(fourier[2], -fourier[3], phase);
- 
 
 111 
 
- 
 
 112 
    // Signal energy at unit frequency
- 
 
 113 
    cv::magnitude(fourier[2], -fourier[3], amplitude);
- 
 
 114 
 
- 
 
 115 
    // Collected signal energy at higher frequencies
- 
 
 116 
    energy = cv::Mat(phase.rows, phase.cols, CV_32F, cv::Scalar(0.0));
- 
 
 117 
 
- 
 
 118 
    for(unsigned int i=0; i<frames.size()-1; i++){
- 
 
 119 
        cv::Mat magnitude;
- 
 
 120 
        cv::magnitude(fourier[i*2 + 2], fourier[i*2 + 3], magnitude);
- 
 
 121 
        cv::add(energy, magnitude, energy, cv::noArray(), CV_32F);
- 
 
 122 
    }
- 
 
 123 
 
- 
 
 124 
    frames.clear();
- 
 
 125 
}
- 
 
 126 
 
83 
static void collectPhases(const cv::Mat& phasePrimary,
 127 
void unWrapPhaseMap(const cv::Mat& phasePrimary,
84 
                          const cv::Mat& phaseSecondary,
 128 
                   const cv::Mat& phaseSecondary,
85 
                          cv::Mat& phase);
 129 
                   cv::Mat& phase) {
- 
 
 130 
    cv::Mat phaseEquivalent = phaseSecondary - phasePrimary;
- 
 
 131 
    phaseEquivalent = cvtools::modulo(phaseEquivalent, 2.0*CV_PI);
- 
 
 132 
    phase = unwrapWithCue(phasePrimary, phaseEquivalent, nPeriodsPrimary);
- 
 
 133 
    phase *= phasePrimary.cols/(2.0*CV_PI);
- 
 
 134 
}
- 
 
 135 
 
- 
 
 136 
 
86 
static void matchPhaseMaps(const cv::Mat& occlusion0, const cv::Mat& occlusion1,
 137 
void matchPhaseMaps(const cv::Mat& occlusion0, const cv::Mat& occlusion1,
87 
                           const cv::Mat& phase0, const cv::Mat& phase1,
 138 
                    const cv::Mat& phase0, const cv::Mat& phase1,
88 
                           std::vector<cv::Vec2f>& q0, std::vector<cv::Vec2f>& q1);
 139 
                    std::vector<cv::Vec2f>& q0, std::vector<cv::Vec2f>& q1) {
- 
 
 140 
 
89 
static void triangulate(const StereoRectifyier& stereoRect,
 141 
    #pragma omp parallel for
90 
                        const std::vector<cv::Vec2f>& q0,
 - 
 
91 
                        const std::vector<cv::Vec2f>& q1,
 142 
    for(int row=0; row<occlusion0.rows; row++){
92 
                        std::vector<cv::Point3f>& Q);
 143 
        for(int col=0; col<occlusion0.cols; col++){
93 
 
 144 
 
- 
 
 145 
            if(!occlusion0.at<char>(row,col))
- 
 
 146 
                continue;
- 
 
 147 
 
- 
 
 148 
            float phase0i = phase0.at<float>(row,col);
- 
 
 149 
            for(int col1=0; col1<phase1.cols-1; col1++){
- 
 
 150 
 
- 
 
 151 
                if(!occlusion1.at<char>(row,col1) || !occlusion1.at<char>(row,col1+1))
- 
 
 152 
                    continue;
- 
 
 153 
 
- 
 
 154 
                float phase1Left = phase1.at<float>(row,col1);
- 
 
 155 
                float phase1Right = phase1.at<float>(row,col1+1);
- 
 
 156 
 
- 
 
 157 
                bool match = (phase1Left <= phase0i)
- 
 
 158 
                              && (phase0i <= phase1Right)
- 
 
 159 
                              && (phase0i-phase1Left < 1.0)
- 
 
 160 
                              && (phase1Right-phase0i < 1.0)
- 
 
 161 
                              && (phase1Right-phase1Left > 0.1);
- 
 
 162 
 
- 
 
 163 
                if(match){
- 
 
 164 
 
- 
 
 165 
                    float col1i = col1 + (phase0i-phase1Left)/(phase1Right-phase1Left);
- 
 
 166 
 
- 
 
 167 
                    #pragma omp critical
- 
 
 168 
                    {
- 
 
 169 
                    q0.push_back(cv::Point2f(col, row));
- 
 
 170 
                    q1.push_back(cv::Point2f(col1i, row));
- 
 
 171 
                    }
- 
 
 172 
                    break;
- 
 
 173 
                }
- 
 
 174 
            }
- 
 
 175 
        }
- 
 
 176 
    }
- 
 
 177 
 
- 
 
 178 
}
- 
 
 179 
 
- 
 
 180 
void triangulate(const StereoRectifyier& stereoRect,
- 
 
 181 
                 const std::vector<cv::Vec2f>& q0,
- 
 
 182 
                 const std::vector<cv::Vec2f>& q1,
- 
 
 183 
                 std::vector<cv::Point3f>& Q) {
- 
 
 184 
 
- 
 
 185 
    // cv::Mat QMatHomogenous, QMat;
- 
 
 186 
    // cv::triangulatePoints(P0, P1, q0, q1, QMatHomogenous);
- 
 
 187 
    // cvtools::convertMatFromHomogeneous(QMatHomogenous, QMat);
- 
 
 188 
 
- 
 
 189 
    // // Undo rectification
- 
 
 190 
    // cv::Mat R0Inv;
- 
 
 191 
    // cv::Mat(R0.t()).convertTo(R0Inv, CV_32F);
- 
 
 192 
    // QMat = R0Inv*QMat;
- 
 
 193 
 
- 
 
 194 
    // cvtools::matToPoints3f(QMat, Q);
- 
 
 195 
 
- 
 
 196 
 
- 
 
 197 
    // Triangulate by means of disparity projection
- 
 
 198 
    Q.resize(q0.size());
- 
 
 199 
    cv::Matx44f QRectx = cv::Matx44f(stereoRect.QRect);
- 
 
 200 
    cv::Matx33f R0invx = cv::Matx33f(cv::Mat(stereoRect.R0.t()));
- 
 
 201 
 
- 
 
 202 
    #pragma omp parallel for
- 
 
 203 
    for(unsigned int i=0; i < q0.size(); i++){
- 
 
 204 
        float disparity = q0[i][0] - q1[i][0];
- 
 
 205 
        cv::Vec4f Qih = QRectx*cv::Vec4f(q0[i][0], q0[i][1], disparity, 1.0);
- 
 
 206 
        float winv = float(1.0) / Qih[3];
- 
 
 207 
        Q[i] = R0invx * cv::Point3f(Qih[0]*winv, Qih[1]*winv, Qih[2]*winv);
- 
 
 208 
    }
- 
 
 209 
}
94 
 
 210 
 
95 
void AlgorithmPhaseShiftTwoFreq::
 211 
void AlgorithmPhaseShiftTwoFreq::
96 
    get3DPoints(const SMCalibrationParameters & calibration,
 212 
    get3DPoints(const SMCalibrationParameters & calibration,
97 
                const std::vector<cv::Mat>& frames0,
 213 
                const std::vector<cv::Mat>& frames0,
98 
                const std::vector<cv::Mat>& frames1,
 214 
                const std::vector<cv::Mat>& frames1,
Line 101... Line 217...
101 
 
 217 
 
102 
    assert(frames0.size() == N);
 218 
    assert(frames0.size() == N);
103 
    assert(frames1.size() == N);
 219 
    assert(frames1.size() == N);
104 
 
 220 
 
105 
    StereoRectifyier stereoRect;
 221 
    StereoRectifyier stereoRect;
106 
    getStereoRectifyier(calibration,
 222 
    getStereoRectifier(calibration,
107 
                        cv::Size(frames0[0].cols, frames0[0].rows),
 223 
                        cv::Size(frames0[0].cols, frames0[0].rows),
108 
                        stereoRect);
 224 
                        stereoRect);
109 
 
 225 
 
110 
    // // Erode occlusion masks
 226 
    // // Erode occlusion masks
111 
    // cv::Mat strel = cv::getStructuringElement(cv::MORPH_ELLIPSE, cv::Size(5,5));
 227 
    // cv::Mat strel = cv::getStructuringElement(cv::MORPH_ELLIPSE, cv::Size(5,5));
Line 122... Line 238...
122 
            // Gray-scale and remap/rectify
 238 
            // Gray-scale and remap/rectify
123 
            std::vector<cv::Mat> frames0Rect(N);
 239 
            std::vector<cv::Mat> frames0Rect(N);
124 
 
 240 
 
125 
            for(unsigned int i=0; i<N; i++){
 241 
            for(unsigned int i=0; i<N; i++){
126 
                cv::Mat temp;
 242 
                cv::Mat temp;
- 
 
 243 
                if(frames1[i].depth() == CV_8U)
127 
                cv::cvtColor(frames0[i], temp, CV_BayerBG2GRAY);
 244 
                    cv::cvtColor(frames1[i], temp, CV_BayerBG2GRAY);
- 
 
 245 
                else
- 
 
 246 
                    temp = frames1[i];
128 
                cv::remap(temp, frames0Rect[i],
 247 
                cv::remap(temp, frames0Rect[i],
129 
                          stereoRect.map0X, stereoRect.map0Y,
 248 
                          stereoRect.map0X, stereoRect.map0Y,
130 
                          CV_INTER_LINEAR);
 249 
                          CV_INTER_LINEAR);
131 
            }
 250 
            }
132 
 
 251 
 
133 
            cv::cvtColor(frames0[0], color0, CV_BayerBG2RGB);
 252 
            // If images are HDR (float), we need to convert to uchar
134 
            cv::remap(color0, color0,
 253 
            frames0Rect[0].convertTo(color0, CV_8UC3);
135 
                      stereoRect.map0X, stereoRect.map0Y,
 - 
 
136 
                      CV_INTER_LINEAR);
 - 
 
137 
 
 254 
 
138 
            // Occlusion masks
 255 
            // Occlusion masks
139 
            cv::subtract(frames0Rect[0], frames0Rect[1], occlusion0);
 256 
            cv::subtract(frames0Rect[0], frames0Rect[1], occlusion0);
140 
            occlusion0 = (occlusion0 > 25) & (occlusion0 < 250);
 257 
            occlusion0 = (occlusion0 > 25) & (occlusion0 < 250);
141 
 
 258 
 
Line 157... Line 274...
157 
            determineAmplitudePhaseEnergy(frames0Secondary,
 274 
            determineAmplitudePhaseEnergy(frames0Secondary,
158 
                                          amplitude0Secondary,
 275 
                                          amplitude0Secondary,
159 
                                          up0Secondary,
 276 
                                          up0Secondary,
160 
                                          energy0Secondary);
 277 
                                          energy0Secondary);
161 
 
 278 
 
162 
            collectPhases(up0Primary, up0Secondary, up0);
 279 
            unWrapPhaseMap(up0Primary, up0Secondary, up0);
163 
 
 280 
 
164 
            // Threshold on energy at primary frequency
 281 
            // Threshold on energy at primary frequency
165 
            occlusion0 = occlusion0 & (amplitude0Primary > 5.0*nStepsPrimary);
 282 
            occlusion0 = occlusion0 & (amplitude0Primary > 5.0*nStepsPrimary);
166 
            // Threshold on energy ratios
 283 
            // Threshold on energy ratios
167 
            occlusion0 = occlusion0 & (amplitude0Primary > 0.25*energy0Primary);
 284 
            occlusion0 = occlusion0 & (amplitude0Primary > 0.25*energy0Primary);
168 
            occlusion0 = occlusion0 & (amplitude0Secondary > 0.25*energy0Secondary);
 285 
            occlusion0 = occlusion0 & (amplitude0Secondary > 0.25*energy0Secondary);
169 
 
 286 
 
170 
            // // Erode occlusion masks
 287 
            // // Erode occlusion masks
171 
            // cv::erode(occlusion0, occlusion0, strel);
 288 
            // cv::erode(occlusion0, occlusion0, strel);
172 
 
 289 
 
173 
            // Threshold on gradient of phase
 290 
            // Threshold on vertical gradient of phase
174 
            cv::Mat edges0;
 291 
            cv::Mat edges0;
175 
            cv::Sobel(up0, edges0, -1, 1, 1, 5);
 292 
            cv::Sobel(up0, edges0, -1, 1, 1, 5);
176 
            occlusion0 = occlusion0 & (abs(edges0) < 10);
 293 
            occlusion0 = occlusion0 & (abs(edges0) < 10);
177 
 
 294 
 
178 
            #ifdef SM_DEBUG
 295 
            #ifdef SM_DEBUG
Line 199... Line 316...
199 
            // Gray-scale and remap
 316 
            // Gray-scale and remap
200 
            std::vector<cv::Mat> frames1Rect(N);
 317 
            std::vector<cv::Mat> frames1Rect(N);
201 
 
 318 
 
202 
            for(unsigned int i=0; i<N; i++){
 319 
            for(unsigned int i=0; i<N; i++){
203 
                cv::Mat temp;
 320 
                cv::Mat temp;
- 
 
 321 
                if(frames1[i].depth() == CV_8U)
204 
                cv::cvtColor(frames1[i], temp, CV_BayerBG2GRAY);
 322 
                    cv::cvtColor(frames1[i], temp, CV_BayerBG2GRAY);
- 
 
 323 
                else
- 
 
 324 
                    temp = frames1[i];
205 
                cv::remap(temp, frames1Rect[i],
 325 
                cv::remap(temp, frames1Rect[i],
206 
                          stereoRect.map1X, stereoRect.map1Y,
 326 
                          stereoRect.map1X, stereoRect.map1Y,
207 
                          CV_INTER_LINEAR);
 327 
                          CV_INTER_LINEAR);
208 
            }
 328 
            }
209 
 
 329 
 
210 
            cv::cvtColor(frames1[0], color1, CV_BayerBG2RGB);
 330 
            // If images are HDR (float), we need to convert to uchar
211 
            cv::remap(color1, color1,
 331 
            frames1Rect[0].convertTo(color1, CV_8UC3);
212 
                      stereoRect.map1X, stereoRect.map1Y,
 - 
 
213 
                      CV_INTER_LINEAR);
 - 
 
214 
 
 332 
 
215 
            // Occlusion masks
 333 
            // Occlusion masks
216 
            cv::subtract(frames1Rect[0], frames1Rect[1], occlusion1);
 334 
            cv::subtract(frames1Rect[0], frames1Rect[1], occlusion1);
217 
            occlusion1 = (occlusion1 > 25) & (occlusion1 < 250);
 335 
            occlusion1 = (occlusion1 > 25) & (occlusion1 < 250);
218 
 
 336 
 
Line 234... Line 352...
234 
            determineAmplitudePhaseEnergy(frames1Secondary,
 352 
            determineAmplitudePhaseEnergy(frames1Secondary,
235 
                                          amplitude1Secondary,
 353 
                                          amplitude1Secondary,
236 
                                          up1Secondary,
 354 
                                          up1Secondary,
237 
                                          energy1Secondary);
 355 
                                          energy1Secondary);
238 
 
 356 
 
239 
            collectPhases(up1Primary, up1Secondary, up1);
 357 
            unWrapPhaseMap(up1Primary, up1Secondary, up1);
240 
 
 358 
 
241 
            // Threshold on energy at primary frequency
 359 
            // Threshold on energy at primary frequency
242 
            occlusion1 = occlusion1 & (amplitude1Primary > 5.0*nStepsPrimary);
 360 
            occlusion1 = occlusion1 & (amplitude1Primary > 5.0*nStepsPrimary);
243 
            // Threshold on energy ratios
 361 
            // Threshold on energy ratios
244 
            occlusion1 = occlusion1 & (amplitude1Primary > 0.25*energy1Primary);
 362 
            occlusion1 = occlusion1 & (amplitude1Primary > 0.25*energy1Primary);
Line 246... Line 364...
246 
 
 364 
 
247 
            // // Erode occlusion masks
 365 
            // // Erode occlusion masks
248 
            // cv::erode(occlusion1, occlusion1, strel);
 366 
            // cv::erode(occlusion1, occlusion1, strel);
249 
 
 367 
 
250 
 
 368 
 
251 
            // Threshold on gradient of phase
 369 
            // Threshold on vertical gradient of phase
252 
            cv::Mat edges1;
 370 
            cv::Mat edges1;
253 
            cv::Sobel(up1, edges1, -1, 1, 1, 5);
 371 
            cv::Sobel(up1, edges1, -1, 1, 1, 5);
254 
            occlusion1 = occlusion1 & (abs(edges1) < 10);
 372 
            occlusion1 = occlusion1 & (abs(edges1) < 10);
255 
 
 373 
 
256 
            #ifdef SM_DEBUG
 374 
            #ifdef SM_DEBUG
Line 303... Line 421...
303 
    // Triangulate points
 421 
    // Triangulate points
304 
    triangulate(stereoRect, q0, q1, Q);
 422 
    triangulate(stereoRect, q0, q1, Q);
305 
 
 423 
 
306 
}
 424 
}
307 
 
 425 
 
308 
void getStereoRectifyier(const SMCalibrationParameters &calibration,
 - 
 
309 
                         const cv::Size& frameSize,
 - 
 
310 
                         StereoRectifyier& stereoRect){
 - 
 
311 
 
 - 
 
312 
    // cv::stereoRectify segfaults unless R is double precision
 - 
 
313 
    cv::Mat R, T;
 - 
 
314 
    cv::Mat(calibration.R1).convertTo(R, CV_64F);
 - 
 
315 
    cv::Mat(calibration.T1).convertTo(T, CV_64F);
 - 
 
316 
 
 - 
 
317 
    cv::stereoRectify(calibration.K0, calibration.k0,
 - 
 
318 
                      calibration.K1, calibration.k1,
 - 
 
319 
                      frameSize, R, T,
 - 
 
320 
                      stereoRect.R0, stereoRect.R1,
 - 
 
321 
                      stereoRect.P0, stereoRect.P1,
 - 
 
322 
                      stereoRect.QRect, 0);
 - 
 
323 
 
 - 
 
324 
    // Interpolation maps (lens distortion and rectification)
 - 
 
325 
    cv::initUndistortRectifyMap(calibration.K0, calibration.k0,
 - 
 
326 
                                stereoRect.R0, stereoRect.P0,
 - 
 
327 
                                frameSize, CV_32F,
 - 
 
328 
                                stereoRect.map0X, stereoRect.map0Y);
 - 
 
329 
    cv::initUndistortRectifyMap(calibration.K1, calibration.k1,
 - 
 
330 
                                stereoRect.R1, stereoRect.P1,
 - 
 
331 
                                frameSize, CV_32F,
 - 
 
332 
                                stereoRect.map1X, stereoRect.map1Y);
 - 
 
333 
}
 - 
 
334 
 
 - 
 
335 
void determineAmplitudePhaseEnergy(std::vector<cv::Mat>& frames,
 - 
 
336 
                                   cv::Mat& amplitude,
 - 
 
337 
                                   cv::Mat& phase,
 - 
 
338 
                                   cv::Mat& energy) {
 - 
 
339 
 
 - 
 
340 
    std::vector<cv::Mat> fourier = getDFTComponents(frames);
 - 
 
341 
 
 - 
 
342 
    cv::phase(fourier[2], -fourier[3], phase);
 - 
 
343 
 
 - 
 
344 
    // Signal energy at unit frequency
 - 
 
345 
    cv::magnitude(fourier[2], -fourier[3], amplitude);
 - 
 
346 
 
 - 
 
347 
    // Collected signal energy at higher frequencies
 - 
 
348 
    energy = cv::Mat(phase.rows, phase.cols, CV_32F, cv::Scalar(0.0));
 - 
 
349 
 
 - 
 
350 
    for(unsigned int i=0; i<frames.size()-1; i++){
 - 
 
351 
        cv::Mat magnitude;
 - 
 
352 
        cv::magnitude(fourier[i*2 + 2], fourier[i*2 + 3], magnitude);
 - 
 
353 
        cv::add(energy, magnitude, energy, cv::noArray(), CV_32F);
 - 
 
354 
    }
 - 
 
355 
 
 - 
 
356 
    frames.clear();
 - 
 
357 
}
 - 
 
358 
 
 - 
 
359 
void collectPhases(const cv::Mat& phasePrimary,
 - 
 
360 
                   const cv::Mat& phaseSecondary,
 - 
 
361 
                   cv::Mat& phase) {
 - 
 
362 
    cv::Mat phaseEquivalent = phaseSecondary - phasePrimary;
 - 
 
363 
    phaseEquivalent = cvtools::modulo(phaseEquivalent, 2.0*CV_PI);
 - 
 
364 
    phase = unwrapWithCue(phasePrimary, phaseEquivalent, nPeriodsPrimary);
 - 
 
365 
    phase *= phasePrimary.cols/(2.0*CV_PI);
 - 
 
366 
}
 - 
 
367 
 
 - 
 
368 
 
 - 
 
369 
void matchPhaseMaps(const cv::Mat& occlusion0, const cv::Mat& occlusion1,
 - 
 
370 
                    const cv::Mat& phase0, const cv::Mat& phase1,
 - 
 
371 
                    std::vector<cv::Vec2f>& q0, std::vector<cv::Vec2f>& q1) {
 - 
 
372 
 
 - 
 
373 
    #pragma omp parallel for
 - 
 
374 
    for(int row=0; row<occlusion0.rows; row++){
 - 
 
375 
        for(int col=0; col<occlusion0.cols; col++){
 - 
 
376 
 
 - 
 
377 
            if(!occlusion0.at<char>(row,col))
 - 
 
378 
                continue;
 - 
 
379 
 
 - 
 
380 
            float phase0i = phase0.at<float>(row,col);
 - 
 
381 
            for(int col1=0; col1<phase1.cols-1; col1++){
 - 
 
382 
 
 - 
 
383 
                if(!occlusion1.at<char>(row,col1) || !occlusion1.at<char>(row,col1+1))
 - 
 
384 
                    continue;
 - 
 
385 
 
 - 
 
386 
                float phase1Left = phase1.at<float>(row,col1);
 - 
 
387 
                float phase1Right = phase1.at<float>(row,col1+1);
 - 
 
388 
 
 - 
 
389 
                bool match = (phase1Left <= phase0i)
 - 
 
390 
                              && (phase0i <= phase1Right)
 - 
 
391 
                              && (phase0i-phase1Left < 1.0)
 - 
 
392 
                              && (phase1Right-phase0i < 1.0)
 - 
 
393 
                              && (phase1Right-phase1Left > 0.1);
 - 
 
394 
 
 - 
 
395 
                if(match){
 - 
 
396 
 
 - 
 
397 
                    float col1i = col1 + (phase0i-phase1Left)/(phase1Right-phase1Left);
 - 
 
398 
 
 - 
 
399 
                    #pragma omp critical
 - 
 
400 
                    {
 - 
 
401 
                    q0.push_back(cv::Point2f(col, row));
 - 
 
402 
                    q1.push_back(cv::Point2f(col1i, row));
 - 
 
403 
                    }
 - 
 
404 
                    break;
 - 
 
405 
                }
 - 
 
406 
            }
 - 
 
407 
        }
 - 
 
408 
    }
 - 
 
409 
 
 - 
 
410 
}
 - 
 
411 
 
 - 
 
412 
void triangulate(const StereoRectifyier& stereoRect,
 - 
 
413 
                 const std::vector<cv::Vec2f>& q0,
 - 
 
414 
                 const std::vector<cv::Vec2f>& q1,
 - 
 
415 
                 std::vector<cv::Point3f>& Q) {
 - 
 
416 
 
 - 
 
417 
    // cv::Mat QMatHomogenous, QMat;
 - 
 
418 
    // cv::triangulatePoints(P0, P1, q0, q1, QMatHomogenous);
 - 
 
419 
    // cvtools::convertMatFromHomogeneous(QMatHomogenous, QMat);
 - 
 
420 
 
 - 
 
421 
    // // Undo rectification
 - 
 
422 
    // cv::Mat R0Inv;
 - 
 
423 
    // cv::Mat(R0.t()).convertTo(R0Inv, CV_32F);
 - 
 
424 
    // QMat = R0Inv*QMat;
 - 
 
425 
 
 - 
 
426 
    // cvtools::matToPoints3f(QMat, Q);
 - 
 
427 
 
 - 
 
428 
 
 - 
 
429 
    // Triangulate by means of disparity projection
 - 
 
430 
    Q.resize(q0.size());
 - 
 
431 
    cv::Matx44f QRectx = cv::Matx44f(stereoRect.QRect);
 - 
 
432 
    cv::Matx33f R0invx = cv::Matx33f(cv::Mat(stereoRect.R0.t()));
 - 
 
433 
 
 - 
 
434 
    #pragma omp parallel for
 - 
 
435 
    for(unsigned int i=0; i < q0.size(); i++){
 - 
 
436 
        float disparity = q0[i][0] - q1[i][0];
 - 
 
437 
        cv::Vec4f Qih = QRectx*cv::Vec4f(q0[i][0], q0[i][1], disparity, 1.0);
 - 
 
438 
        float winv = float(1.0) / Qih[3];
 - 
 
439 
        Q[i] = R0invx * cv::Point3f(Qih[0]*winv, Qih[1]*winv, Qih[2]*winv);
 - 
 
440 
    }
 - 
 
441 
}
 -