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

     assert(frames1.size() == N);
     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::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);
-    // Gray-scale everything
+    // gray-scale and remap
-    std::vector<cv::Mat> frames0Gray(N);
+    std::vector<cv::Mat> frames0Rect(N);
-    std::vector<cv::Mat> frames1Gray(N);
+    std::vector<cv::Mat> frames1Rect(N);
     for(unsigned int i=0; i<N; i++){
+        cv::Mat temp;
-        cv::cvtColor(frames0[i], frames0Gray[i], CV_BayerBG2GRAY);
+        cv::cvtColor(frames0[i], temp, CV_BayerBG2GRAY);
+        cv::remap(temp, frames0Rect[i], map0X, map0Y, CV_INTER_LINEAR);
-        cv::cvtColor(frames1[i], frames1Gray[i], CV_BayerBG2GRAY);
+        cv::cvtColor(frames1[i], temp, CV_BayerBG2GRAY);
+        cv::remap(temp, frames1Rect[i], map1X, map1Y, CV_INTER_LINEAR);
+    }
     // Decode camera0
-    std::vector<cv::Mat> frames0Primary(frames0Gray.begin()+2, frames0Gray.begin()+2+nStepsPrimary);
+    std::vector<cv::Mat> frames0Primary(frames0Rect.begin()+2, frames0Rect.begin()+2+nStepsPrimary);
-    std::vector<cv::Mat> frames0Secondary(frames0Gray.begin()+2+nStepsPrimary, frames0Gray.end());
+    std::vector<cv::Mat> frames0Secondary(frames0Rect.begin()+2+nStepsPrimary, frames0Rect.end());
     std::vector<cv::Mat> F0Primary = getDFTComponents(frames0Primary);
     cv::Mat up0Primary;
     cv::phase(F0Primary[2], -F0Primary[3], up0Primary);
     //cv::Mat up0Secondary = getPhase(frames0Secondary[0], frames0Secondary[1], frames0Secondary[2]);
     std::vector<cv::Mat> F0Secondary = getDFTComponents(frames0Secondary);
     up0 *= screenCols/(2.0*pi);
     cv::Mat amplitude0;
     cv::magnitude(F0Primary[2], -F0Primary[3], amplitude0);
     // Decode camera1
-    std::vector<cv::Mat> frames1Primary(frames1Gray.begin()+2, frames1Gray.begin()+2+nStepsPrimary);
+    std::vector<cv::Mat> frames1Primary(frames1Rect.begin()+2, frames1Rect.begin()+2+nStepsPrimary);
-    std::vector<cv::Mat> frames1Secondary(frames1Gray.begin()+2+nStepsPrimary, frames1Gray.end());
+    std::vector<cv::Mat> frames1Secondary(frames1Rect.begin()+2+nStepsPrimary, frames1Rect.end());
     std::vector<cv::Mat> F1Primary = getDFTComponents(frames1Primary);
     cv::Mat up1Primary;
     cv::phase(F1Primary[2], -F1Primary[3], up1Primary);
     //cv::Mat up1Secondary = getPhase(frames1Secondary[0], frames1Secondary[1], frames1Secondary[2]);
     std::vector<cv::Mat> F1Secondary = getDFTComponents(frames1Secondary);
 //cvtools::writeMat(up0Secondary, "up0Secondary.mat", "up0Secondary");
 //cvtools::writeMat(up0Equivalent, "up0Equivalent.mat", "up0Equivalent");
 //cvtools::writeMat(up0, "up0.mat", "up0");
 //cvtools::writeMat(amplitude0, "amplitude0.mat", "amplitude0");
-    // 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::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);
-    // Phase remaps
-    cv::Mat up0Rect, up1Rect;
-    cv::remap(up0, up0Rect, map0X, map0Y, CV_INTER_LINEAR);
-    cv::remap(up1, up1Rect, map1X, map1Y, CV_INTER_LINEAR);
-    // amplitude remaps
-    cv::Mat amplitude0Rect, amplitude1Rect;
-    cv::remap(amplitude0, amplitude0Rect, map0X, map0Y, CV_INTER_LINEAR);
-    cv::remap(amplitude1, amplitude1Rect, map1X, map1Y, CV_INTER_LINEAR);
-//cvtools::writeMat(up0Rect, "up0Rect.mat", "up0Rect");
-//cvtools::writeMat(up1Rect, "up1Rect.mat", "up1Rect");
     // color debayer and remap
-    cv::Mat color0Rect, color1Rect;
+    cv::Mat color0, color1;
 //    frames0[0].convertTo(color0Rect, CV_8UC1, 1.0/256.0);
-    cv::cvtColor(frames0[0], color0Rect, CV_BayerBG2RGB);
+    cv::cvtColor(frames0[0], color0, CV_BayerBG2RGB);
-    cv::remap(color0Rect, color0Rect, map0X, map0Y, CV_INTER_LINEAR);
+    cv::remap(color0, color0, map0X, map0Y, CV_INTER_LINEAR);
 //    frames1[0].convertTo(color1Rect, CV_8UC1, 1.0/256.0);
-    cv::cvtColor(frames1[0], color1Rect, CV_BayerBG2RGB);
+    cv::cvtColor(frames1[0], color1, CV_BayerBG2RGB);
-    cv::remap(color1Rect, color1Rect, map1X, map1Y, CV_INTER_LINEAR);
+    cv::remap(color1, color1, map1X, map1Y, CV_INTER_LINEAR);
-//cvtools::writeMat(frames0Rect[18], "frames0Rect_18.mat", "frames0Rect_18");
-//cvtools::writeMat(frames0Rect[19], "frames0Rect_19.mat", "frames0Rect_19");
-//cvtools::writeMat(color0Rect, "color0Rect.mat", "color0Rect");
+//cvtools::writeMat(color0, "color0.mat", "color0");
-//cvtools::writeMat(color1Rect, "color1Rect.mat", "color1Rect");
+//cvtools::writeMat(color1, "color1.mat", "color1");
-    // On/off remaps
-    cv::Mat frames0OnRect, frames0OffRect;
-    cv::remap(frames0Gray[0], frames0OnRect, map0X, map0Y, CV_INTER_LINEAR);
-    cv::remap(frames0Gray[1], frames0OffRect, map0X, map0Y, CV_INTER_LINEAR);
-    cv::Mat frames1OnRect, frames1OffRect;
-    cv::remap(frames1Gray[0], frames1OnRect, map1X, map1Y, CV_INTER_LINEAR);
-    cv::remap(frames1Gray[1], frames1OffRect, map1X, map1Y, CV_INTER_LINEAR);
     // Occlusion masks
-    cv::Mat occlusion0Rect, occlusion1Rect;
+    cv::Mat occlusion0, occlusion1;
-    cv::subtract(frames0OnRect, frames0OffRect, occlusion0Rect);
+    cv::subtract(frames0Rect[0], frames0Rect[1], occlusion0);
-    occlusion0Rect = (occlusion0Rect > 25) & (occlusion0Rect < 250);
+    occlusion0 = (occlusion0 > 25) & (occlusion0 < 250);
-    cv::subtract(frames1OnRect, frames1OffRect, occlusion1Rect);
+    cv::subtract(frames1Rect[0], frames1Rect[1], occlusion1);
-    occlusion1Rect = (occlusion1Rect > 25) & (occlusion1Rect < 250);
+    occlusion1 = (occlusion1 > 25) & (occlusion1 < 250);
     // Threshold on energy at primary frequency
-    occlusion0Rect = occlusion0Rect & (amplitude0Rect > 5.0*nStepsPrimary);
+    occlusion0 = occlusion0 & (amplitude0 > 5.0*nStepsPrimary);
-    occlusion1Rect = occlusion1Rect & (amplitude1Rect > 5.0*nStepsPrimary);
+    occlusion1 = occlusion1 & (amplitude1 > 5.0*nStepsPrimary);
-//cvtools::writeMat(occlusion0Rect, "occlusion0Rect.mat", "occlusion0Rect");
+//cvtools::writeMat(occlusion1, "occlusion1.mat", "occlusion1");
-//cvtools::writeMat(occlusion1Rect, "occlusion1Rect.mat", "occlusion1Rect");
+//cvtools::writeMat(occlusion1, "occlusion1.mat", "occlusion1");
     // Erode occlusion masks
     cv::Mat strel = cv::getStructuringElement(cv::MORPH_ELLIPSE, cv::Size(5,5));
-    cv::erode(occlusion0Rect, occlusion0Rect, strel);
+    cv::erode(occlusion0, occlusion0, strel);
-    cv::erode(occlusion1Rect, occlusion1Rect, strel);
+    cv::erode(occlusion1, occlusion1, strel);
     // Threshold on gradient of phase
     cv::Mat edges0;
-    cv::Sobel(up0Rect, edges0, -1, 1, 1, 5);
+    cv::Sobel(up0, edges0, -1, 1, 1, 5);
-    occlusion0Rect = occlusion0Rect & (abs(edges0) < 150);
+    occlusion0 = occlusion0 & (abs(edges0) < 150);
     cv::Mat edges1;
-    cv::Sobel(up1Rect, edges1, -1, 1, 1, 5);
+    cv::Sobel(up1, edges1, -1, 1, 1, 5);
-    occlusion1Rect = occlusion1Rect & (abs(edges1) < 150);
+    occlusion1 = occlusion1 & (abs(edges1) < 150);
 //cvtools::writeMat(edges0, "edges0.mat", "edges0");
 //cvtools::writeMat(edges1, "edges1.mat", "edges1");
     // Match phase maps
     int frameRectRows = map0X.rows;
     int frameRectCols = map0X.cols;
     // camera0 against camera1
-    std::vector<cv::Vec2f> q0Rect, q1Rect;
+    std::vector<cv::Vec2f> q0, q1;
     for(int row=0; row<frameRectRows; row++){
         for(int col=0; col<frameRectCols; col++){
-            if(!occlusion0Rect.at<char>(row,col))
+            if(!occlusion0.at<char>(row,col))
                 continue;
-            float up0i = up0Rect.at<float>(row,col);
+            float up0i = up0.at<float>(row,col);
-            for(int col1=0; col1<up1Rect.cols-1; col1++){
+            for(int col1=0; col1<up1.cols-1; col1++){
-                if(!occlusion1Rect.at<char>(row,col1) || !occlusion1Rect.at<char>(row,col1+1))
+                if(!occlusion1.at<char>(row,col1) || !occlusion1.at<char>(row,col1+1))
                     continue;
-                float up1Left = up1Rect.at<float>(row,col1);
+                float up1Left = up1.at<float>(row,col1);
-                float up1Right = up1Rect.at<float>(row,col1+1);
+                float up1Right = up1.at<float>(row,col1+1);
                 if((up1Left <= up0i) && (up0i <= up1Right) && (up0i-up1Left < 1) && (up1Right-up0i < 1)){
                     float col1i = col1 + (up0i-up1Left)/(up1Right-up1Left);
-                    q0Rect.push_back(cv::Point2f(col, row));
+                    q0.push_back(cv::Point2f(col, row));
-                    q1Rect.push_back(cv::Point2f(col1i, row));
+                    q1.push_back(cv::Point2f(col1i, row));
                     break;
+                }
+            }
+        }
+    }
-//    // camera1 against camera0
-//    for(int row=0; row<frameRectRows; row++){
-//        for(int col=0; col<frameRectCols; col++){
-//            if(!occlusion1Rect.at<char>(row,col))
-//                continue;
-//            float up1i = up1Rect.at<float>(row,col);
-//            for(int col0=0; col0<up0Rect.cols-1; col0++){
-//                if(!occlusion0Rect.at<char>(row,col0) || !occlusion0Rect.at<char>(row,col0+1))
-//                    continue;
-//                float up0Left = up0Rect.at<float>(row,col0);
-//                float up0Right = up0Rect.at<float>(row,col0+1);
-//                if((up0Left <= up1i) && (up1i <= up0Right) && (up1i-up0Left < 1) && (up0Right-up1i < 1)){
-//                    float col0i = col0 + (up1i-up0Left)/(up0Right-up0Left);
-//                    q1Rect.push_back(cv::Point2f(col, row));
-//                    q0Rect.push_back(cv::Point2f(col0i, row));
-//                    break;
-//                }
-//            }
-//        }
-//    }
-    int nMatches = q0Rect.size();
+    int nMatches = q0.size();
     if(nMatches < 1){
         Q.resize(0);
         color.resize(0);
     // Retrieve color information
     color.resize(nMatches);
     for(int i=0; i<nMatches; i++){
-        cv::Vec3b c0 = color0Rect.at<cv::Vec3b>(q0Rect[i][1], q0Rect[i][0]);
+        cv::Vec3b c0 = color0.at<cv::Vec3b>(q0[i][1], q0[i][0]);
-        cv::Vec3b c1 = color1Rect.at<cv::Vec3b>(q1Rect[i][1], q1Rect[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, q0Rect, q1Rect, QMatHomogenous);
+    cv::triangulatePoints(P0, P1, q0, q1, QMatHomogenous);
     cvtools::convertMatFromHomogeneous(QMatHomogenous, QMat);
     // Undo rectification
     cv::Mat R0Inv;
     cv::Mat(R0.t()).convertTo(R0Inv, CV_32F);

Subversion Repositories seema-scanner

(root)/src/algorithm/AlgorithmPhaseShiftTwoFreq.cpp – Rev 167 → 178