Subversion Repositories seema-scanner

Rev

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