WebSVN – seema-scanner – Diff – /src/algorithm/AlgorithmPhaseShiftTwoFreq.cpp

 cv::Mat AlgorithmPhaseShiftTwoFreq::getEncodingPattern(unsigned int depth){
     return patterns[depth];
+}
-void AlgorithmPhaseShiftTwoFreq::get3DPoints(SMCalibrationParameters calibration, const std::vector<cv::Mat>& frames0, const std::vector<cv::Mat>& frames1, std::vector<cv::Point3f>& Q, std::vector<cv::Vec3b>& color){
+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){
     assert(frames0.size() == N);
     assert(frames1.size() == N);
+    cv::Mat map0X, map0Y, map1X, map1Y;
+    cv::Mat R0, P0, P1;
+{
     int frameRows = frames0[0].rows;
     int frameCols = frames0[0].cols;
     // Rectifying homographies (rotation+projections)
     cv::Size frameSize(frameCols, frameRows);
     cv::Mat R, T;
     // stereoRectify segfaults unless R is double precision
     cv::Mat(calibration.R1).convertTo(R, CV_64F);
     cv::Mat(calibration.T1).convertTo(T, CV_64F);
-    cv::Mat R0, R1, P0, P1, QRect;
+    cv::Mat  R1, QRect;
     cv::stereoRectify(calibration.K0, calibration.k0, calibration.K1, calibration.k1, frameSize, R, T, R0, R1, P0, P1, QRect, 0);
     // Interpolation maps (lens distortion and rectification)
-    cv::Mat map0X, map0Y, map1X, map1Y;
     cv::initUndistortRectifyMap(calibration.K0, calibration.k0, R0, P0, frameSize, CV_32F, map0X, map0Y);
     cv::initUndistortRectifyMap(calibration.K1, calibration.k1, R1, P1, frameSize, CV_32F, map1X, map1Y);
+    }
     int frameRectRows = map0X.rows;
     int frameRectCols = map0X.cols;
+    cv::Mat up0;
-    // Gray-scale and remap
+    cv::Mat occlusion0;
+{
-    std::vector<cv::Mat> frames0Rect(N);
+        // Gray-scale and remap
-    std::vector<cv::Mat> frames1Rect(N);
+        std::vector<cv::Mat> frames0Rect(N);
-    for(unsigned int i=0; i<N; i++){
+        for(unsigned int i=0; i<N; i++){
-        cv::Mat temp;
+            cv::Mat temp;
-        cv::cvtColor(frames0[i], temp, CV_BayerBG2GRAY);
+            cv::cvtColor(frames0[i], temp, CV_BayerBG2GRAY);
-        cv::remap(temp, frames0Rect[i], map0X, map0Y, CV_INTER_LINEAR);
+            cv::remap(temp, frames0Rect[i], map0X, map0Y, CV_INTER_LINEAR);
+        }
+        // Occlusion masks
-        cv::cvtColor(frames1[i], temp, CV_BayerBG2GRAY);
+        cv::subtract(frames0Rect[0], frames0Rect[1], occlusion0);
-        cv::remap(temp, frames1Rect[i], map1X, map1Y, CV_INTER_LINEAR);
+        occlusion0 = (occlusion0 > 25) & (occlusion0 < 250);
-    }
+        // Decode camera0
+        std::vector<cv::Mat> frames0Primary(frames0Rect.begin()+2, frames0Rect.begin()+2+nStepsPrimary);
+        std::vector<cv::Mat> frames0Secondary(frames0Rect.begin()+2+nStepsPrimary, frames0Rect.end());
-    // Decode camera0
+        frames0Rect.clear();
-    std::vector<cv::Mat> frames0Primary(frames0Rect.begin()+2, frames0Rect.begin()+2+nStepsPrimary);
-    std::vector<cv::Mat> frames0Secondary(frames0Rect.begin()+2+nStepsPrimary, frames0Rect.end());
     std::vector<cv::Mat> F0Primary = getDFTComponents(frames0Primary);
+    frames0Primary.clear();
     cv::Mat up0Primary;
     cv::phase(F0Primary[2], -F0Primary[3], up0Primary);
     std::vector<cv::Mat> F0Secondary = getDFTComponents(frames0Secondary);
+    frames0Secondary.clear();
     cv::Mat up0Secondary;
     cv::phase(F0Secondary[2], -F0Secondary[3], up0Secondary);
     cv::Mat up0Equivalent = up0Secondary - up0Primary;
     up0Equivalent = cvtools::modulo(up0Equivalent, 2.0*CV_PI);
-    cv::Mat up0 = unwrapWithCue(up0Primary, up0Equivalent, nPeriodsPrimary);
+    up0 = unwrapWithCue(up0Primary, up0Equivalent, nPeriodsPrimary);
     up0 *= screenCols/(2.0*CV_PI);
     // Signal energy at unit frequency
     cv::Mat amplitude0Primary, amplitude0Secondary;
     cv::magnitude(F0Primary[2], -F0Primary[3], amplitude0Primary);
         cv::Mat magnitude;
         cv::magnitude(F0Secondary[i*2 + 2], F0Secondary[i*2 + 3], magnitude);
         cv::add(energy0Secondary, magnitude, energy0Secondary, cv::noArray(), CV_32F);
+    }
+    // Threshold on energy at primary frequency
+    occlusion0 = occlusion0 & (amplitude0 > 5.0*nStepsPrimary);
+    // Threshold on energy ratios
+    occlusion0 = occlusion0 & (amplitude0 > 0.85*energy0Primary);
+    occlusion0 = occlusion0 & (amplitude0 > 0.85*energy0Secondary);
     #ifdef QT_DEBUG
         cvtools::writeMat(up0Primary, "up0Primary.mat", "up0Primary");
         cvtools::writeMat(up0Secondary, "up0Secondary.mat", "up0Secondary");
         cvtools::writeMat(up0Equivalent, "up0Equivalent.mat", "up0Equivalent");
         cvtools::writeMat(up0, "up0.mat", "up0");
         cvtools::writeMat(amplitude0Primary, "amplitude0Primary.mat", "amplitude0Primary");
         cvtools::writeMat(energy0Primary, "energy0Primary.mat", "energy0Primary");
         cvtools::writeMat(energy0Secondary, "energy0Secondary.mat", "energy0Secondary");
     #endif
+}
+    cv::Mat up1;
+    cv::Mat occlusion1;
+    {
+        // Gray-scale and remap
+        std::vector<cv::Mat> frames1Rect(N);
+        for(unsigned int i=0; i<N; i++){
+            cv::Mat temp;
+            cv::cvtColor(frames1[i], temp, CV_BayerBG2GRAY);
+            cv::remap(temp, frames1Rect[i], map1X, map1Y, CV_INTER_LINEAR);
+        }
+        // Occlusion masks
+        cv::subtract(frames1Rect[0], frames1Rect[1], occlusion1);
+        occlusion1 = (occlusion1 > 25) & (occlusion1 < 250);
-    // Decode camera1
+        // Decode camera1
-    std::vector<cv::Mat> frames1Primary(frames1Rect.begin()+2, frames1Rect.begin()+2+nStepsPrimary);
+        std::vector<cv::Mat> frames1Primary(frames1Rect.begin()+2, frames1Rect.begin()+2+nStepsPrimary);
-    std::vector<cv::Mat> frames1Secondary(frames1Rect.begin()+2+nStepsPrimary, frames1Rect.end());
+        std::vector<cv::Mat> frames1Secondary(frames1Rect.begin()+2+nStepsPrimary, frames1Rect.end());
+        frames1Rect.clear();
     std::vector<cv::Mat> F1Primary = getDFTComponents(frames1Primary);
+    frames1Primary.clear();
     cv::Mat up1Primary;
     cv::phase(F1Primary[2], -F1Primary[3], up1Primary);
     std::vector<cv::Mat> F1Secondary = getDFTComponents(frames1Secondary);
+    frames1Secondary.clear();
     cv::Mat up1Secondary;
     cv::phase(F1Secondary[2], -F1Secondary[3], up1Secondary);
     cv::Mat up1Equivalent = up1Secondary - up1Primary;
     up1Equivalent = cvtools::modulo(up1Equivalent, 2.0*CV_PI);
-    cv::Mat up1 = unwrapWithCue(up1Primary, up1Equivalent, nPeriodsPrimary);
+    up1 = unwrapWithCue(up1Primary, up1Equivalent, nPeriodsPrimary);
     up1 *= screenCols/(2.0*CV_PI);
     // Signal energy at unit frequency
     cv::Mat amplitude1Primary, amplitude1Secondary;
     cv::magnitude(F1Primary[2], -F1Primary[3], amplitude1Primary);
         cv::Mat magnitude;
         cv::magnitude(F1Secondary[i*2 + 2], F1Secondary[i*2 + 3], magnitude);
         cv::add(energy1Secondary, magnitude, energy1Secondary, cv::noArray(), CV_32F);
+    }
-    #ifdef QT_DEBUG
-        cvtools::writeMat(up1, "up1.mat", "up1");
-    #endif
-    // color debayer and remap
-    cv::Mat color0, color1;
-    cv::cvtColor(frames0[0], color0, CV_BayerBG2RGB);
-    cv::remap(color0, color0, map0X, map0Y, CV_INTER_LINEAR);
-    cv::cvtColor(frames1[0], color1, CV_BayerBG2RGB);
-    cv::remap(color1, color1, map1X, map1Y, CV_INTER_LINEAR);
-    #ifdef QT_DEBUG
-        cvtools::writeMat(color0, "color0.mat", "color0");
-        cvtools::writeMat(color1, "color1.mat", "color1");
-    #endif
-    // Occlusion masks
-    cv::Mat occlusion0, occlusion1;
-    cv::subtract(frames0Rect[0], frames0Rect[1], occlusion0);
-    occlusion0 = (occlusion0 > 25) & (occlusion0 < 250);
-    cv::subtract(frames1Rect[0], frames1Rect[1], occlusion1);
-    occlusion1 = (occlusion1 > 25) & (occlusion1 < 250);
     // Threshold on energy at primary frequency
-    occlusion0 = occlusion0 & (amplitude0Primary > 5.0*nStepsPrimary);
-    occlusion1 = occlusion1 & (amplitude1Primary > 5.0*nStepsPrimary);
+    occlusion1 = occlusion1 & (amplitude1 > 5.0*nStepsPrimary);
     // Threshold on energy ratios
-    occlusion0 = occlusion0 & (amplitude0Primary > 0.85*energy0Primary);
+    occlusion1 = occlusion1 & (amplitude1 > 0.85*energy1Primary);
-    occlusion0 = occlusion0 & (amplitude0Secondary > 0.85*energy0Secondary);
     occlusion1 = occlusion1 & (amplitude1Primary > 0.85*energy1Primary);
     occlusion1 = occlusion1 & (amplitude1Secondary > 0.85*energy1Secondary);
+    #ifdef QT_DEBUG
+        cvtools::writeMat(up1, "up1.mat", "up1");
+    #endif
+}
 //    // Erode occlusion masks
 //    cv::Mat strel = cv::getStructuringElement(cv::MORPH_ELLIPSE, cv::Size(5,5));
 //    cv::erode(occlusion0, occlusion0, strel);
 //    cv::erode(occlusion1, occlusion1, strel);
+{
     // Threshold on gradient of phase
     cv::Mat edges0;
     cv::Sobel(up0, edges0, -1, 1, 1, 5);
     occlusion0 = occlusion0 & (abs(edges0) < 10);
         cvtools::writeMat(edges0, "edges0.mat", "edges0");
         cvtools::writeMat(edges1, "edges1.mat", "edges1");
         cvtools::writeMat(occlusion0, "occlusion0.mat", "occlusion0");
         cvtools::writeMat(occlusion1, "occlusion1.mat", "occlusion1");
     #endif
+}
     // Match phase maps
     // camera0 against camera1
     std::vector<cv::Vec2f> q0, q1;
     for(int row=0; row<frameRectRows; row++){
         Q.resize(0);
         color.resize(0);
         return;
+    }
+{
+    // color debayer and remap
+    cv::Mat color0, color1;
+    cv::cvtColor(frames0[0], color0, CV_BayerBG2RGB);
+    cv::remap(color0, color0, map0X, map0Y, CV_INTER_LINEAR);
+    cv::cvtColor(frames1[0], color1, CV_BayerBG2RGB);
+    cv::remap(color1, color1, map1X, map1Y, CV_INTER_LINEAR);
+    #ifdef QT_DEBUG
+        cvtools::writeMat(color0, "color0.mat", "color0");
+        cvtools::writeMat(color1, "color1.mat", "color1");
+    #endif
-    // Retrieve color information
+        // Retrieve color information
     color.resize(nMatches);
     for(int i=0; i<nMatches; i++){
         cv::Vec3b c0 = color0.at<cv::Vec3b>(q0[i][1], q0[i][0]);
         cv::Vec3b c1 = color1.at<cv::Vec3b>(q1[i][1], q1[i][0]);
         color[i] = 0.5*c0 + 0.5*c1;
+    }
+}
     // Triangulate points
     cv::Mat QMatHomogenous, QMat;
     cv::triangulatePoints(P0, P1, q0, q1, QMatHomogenous);
     cvtools::convertMatFromHomogeneous(QMatHomogenous, QMat);

Subversion Repositories seema-scanner

(root)/src/algorithm/AlgorithmPhaseShiftTwoFreq.cpp – Rev 200 → 207