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

 //
 // Embedded Phase Shifting
 //
 // This implementation follows "Moreno, Son, Taubin: Embedded Phase Shifting: Robust Phase Shifting with Embedded Signals, CVPR 2015"
 //
 //
 #include "AlgorithmPhaseShiftEmbedded.h"
 #include <math.h>
 #include "cvtools.h"
 #include "algorithmtools.h"
 // Number of frequencies
-static const int M = 3;
+static const int M = 4;
-// Embedded frequencies
+// Embedded periods (product of these must be greater than screenCols)
-static const float Fm[M] = {1.0/16, 1.0/128, 1.0/1024};
+static const float Tm[M] = {16, 8, 8, 8};
 // Number of patterns at each frequency
-static const int Nm[M] = {3, 3, 2};
+static const int Nm[M] = {3, 3, 3, 3};
 AlgorithmPhaseShiftEmbedded::AlgorithmPhaseShiftEmbedded(unsigned int _screenCols, unsigned int _screenRows) : Algorithm(_screenCols, _screenRows){
     // Set N
     N = 2;
     for(int m=0; m<M; m++)
         N += Nm[m];
     // all on pattern
     cv::Mat allOn(1, screenCols, CV_8UC3, cv::Scalar::all(255));
     patterns.push_back(allOn);
     // all off pattern
     cv::Mat allOff(1, screenCols, CV_8UC3, cv::Scalar::all(0));
     patterns.push_back(allOff);
     // Precompute encoded patterns
     const float pi = M_PI;
+    // Compute embedded frequencies
+    float Fm[M];
+    for(int m=0; m<M; m++){
+        Fm[m] = 1.0;
+        for(int i=0; i<=m; i++)
+            Fm[m] *= 1.0/Tm[i];
+    }
+    // Compute pattern frequencies
+    float fm[M];
+    for(int m=0; m<M; m++)
+        fm[m] = Fm[0];
+    for(int m=1; m<M; m++)
+        fm[m] += Fm[m];
+    for(int m=0; m<M; m++)
+        std::cout << fm[m] << std::endl;
     // Encoding patterns
     for(int m=0; m<M; m++){
         int nSteps = Nm[m];
-        float frequency = Fm[m];
+        float frequency = fm[m];
         for(unsigned int i=0; i<nSteps; i++){
-            float phase = 2.0*pi/nSteps * i;
+            float phase = 2.0*pi/std::max(nSteps, 3) * i;
             float pitch = 1.0/frequency;
             cv::Mat patternI(1,1,CV_8U);
             patternI = computePhaseVector(screenCols, phase, pitch);
             patterns.push_back(patternI.t());
+        }
+    }
+}
 cv::Mat AlgorithmPhaseShiftEmbedded::getEncodingPattern(unsigned int depth){
     return patterns[depth];
+}
-static void decodeEmbeddedPS(const std::vector<cv::Mat> &frames, cv::Mat &up, cv::Mat &upRange){
+static void decodeEmbeddedPS(const std::vector<cv::Mat> &frames, cv::Mat &up, cv::Mat &upRange, float screenCols){
     const int N = frames.size();
     // Construct shift matrix
-    cv::Mat A(N, 1 + 2*N, CV_32F);
+    cv::Mat A(N, 1 + 2*M, CV_32F);
+    A.col(0).setTo(1.0);
     int rowBegin = 0;
     for(int m=0; m<M; m++){
         int nSteps = Nm[m];
         cv::Mat Am(nSteps, 2, CV_32F);
         for(unsigned int i=0; i<nSteps; i++){
-            float phase = 2.0*CV_PI/nSteps * i;
+            float phase = 2.0*CV_PI/std::max(nSteps, 3) * i;
             Am.at<float>(i, 0) = std::cos(phase);
             Am.at<float>(i, 1) = -std::sin(phase);
+        }
         // Copy into the A matrix
         Am.copyTo(A.rowRange(rowBegin, rowBegin+nSteps).colRange(1+2*m, 1+2*(m+1)));
         rowBegin += nSteps;
+    }
+    //std::cout << A << std::endl << std::endl;
     int frameRows = frames[0].rows;
     int frameCols = frames[0].cols;
     // DC-offset
     cv::Mat O(frameRows, frameCols, CV_32F);
     // Relative phase maps
     std::vector<cv::Mat> phim;
     for(int i=0; i<N; i++)
         phim.push_back(cv::Mat(frameRows, frameCols, CV_32F));
     // Solve for relative phase values
     for(int row=0; row<frameRows; row++){
         for(int col=0; col<frameCols; col++){
             // Measurement vector
             cv::Mat r(N, 1, CV_32F);
             for(int i=0; i<N; i++)
-                r.at<float>(i) = frames[i].at<float>(row, col);
+                r.at<float>(i) = frames[i].at<uchar>(row, col);
             // Solve
             cv::Mat u; //[o, a cos1, a sin1, a cos2, a sin2, ...]
-            cv::solve(A, r, u);
+            cv::solve(A, r, u, cv::DECOMP_SVD);
             for(int m=0; m<M; m++)
                 phim[m].at<float>(row, col) = std::atan2(u.at<float>(m*2+1), u.at<float>(m*2+2));
             O.at<float>(row, col) = u.at<float>(0);
+        }
+    }
+    #if 0
+        for(int i=0; i<N; i++)
+            cvtools::writeMat(frames[i], QString("frames_%1.mat").arg(i).toStdString().c_str());
+        cvtools::writeMat(O, "O.mat");
+        for(int m=0; m<M; m++)
+            cvtools::writeMat(phim[m], QString("phim_%1.mat").arg(m).toStdString().c_str());
+    #endif
-    // Determine phase cues
+    // Determine phase cue sequence
     std::vector<cv::Mat> Phim(M);
     Phim[0] = phim[0];
-    for(int i=1; i<M; i++)
+    for(int m=1; m<M; m++){
-        cv::subtract(phim[i], phim[0], Phim[i]);
+        cv::subtract(phim[m], phim[0], Phim[m]);
+        Phim[m] = cvtools::modulo(Phim[m], 2.0*CV_PI);
+    }
     // Note: Phim[1] is the cue of highest quality
+    #if 0
-    std::vector<cv::Mat> upm;
+        for(int m=0; m<M; m++)
+            cvtools::writeMat(Phim[m], QString("Phim_%1.mat").arg(m).toStdString().c_str());
+    #endif
+    // Compute embedded frequencies
+    float Fm[M];
-    for(int m=0; m<M; m++)
+    for(int m=0; m<M; m++){
+        Fm[m] = 1.0;
+        for(int i=0; i<=m; i++)
-        upm[m] = unwrapWithCue(phim[m], Phim[1], 1.0/Fm[1]);
+            Fm[m] *= 1.0/Tm[i];
+    }
+    // Unwrap phase cue sequence
+    cv::Mat upCue = Phim[M-1];
+    for(int m=M-2; m>0; m--){
+        upCue = unwrapWithCue(Phim[m], upCue, screenCols*Fm[m]);
+        #if 1
+                cvtools::writeMat(upCue, "upCue.mat", "upCue");
+        #endif
+    }
+    // Unwrap high frequency patterns
+    std::vector<cv::Mat> upm(M);
+    for(int m=0; m<M; m++){
+        upm[m] = unwrapWithCue(phim[m], upCue, 1.0/Fm[1]);
+    }
+    #if 1
+        for(int m=0; m<M; m++)
+            cvtools::writeMat(upm[m], QString("upm_%1.mat").arg(m).toStdString().c_str());
+    #endif
     // Determine range of phases (for outlier detection)
     cv::Mat upMin = upm[0];
     cv::Mat upMax = upm[0];
     for(int m=1; m<M; m++){
         upMin = cv::min(upMin, upm[m]);
         upMax = cv::max(upMax, upm[m]);
+    }
     upRange = upMax-upMin;
     // Return average of phase maps
     up = upm[0];
     for(int m=1; m<M; m++)
         up += upm[m];
     up /= M;
+}
 void AlgorithmPhaseShiftEmbedded::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){
     assert(frames0.size() == N);
     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);
     int frameRectRows = map0X.rows;
     int frameRectCols = map0X.cols;
     // Gray-scale and remap
     std::vector<cv::Mat> frames0Rect(N);
     std::vector<cv::Mat> frames1Rect(N);
     for(unsigned int i=0; i<N; i++){
         cv::Mat temp;
         cv::cvtColor(frames0[i], temp, CV_BayerBG2GRAY);
         cv::remap(temp, frames0Rect[i], map0X, map0Y, CV_INTER_LINEAR);
         cv::cvtColor(frames1[i], temp, CV_BayerBG2GRAY);
         cv::remap(temp, frames1Rect[i], map1X, map1Y, CV_INTER_LINEAR);
+    }
     // Decode camera 0
     std::vector<cv::Mat> frames0Patterns(frames0Rect.begin()+2, frames0Rect.end());
     cv::Mat up0, up0Range;
-    decodeEmbeddedPS(frames0Patterns, up0, up0Range);
+    decodeEmbeddedPS(frames0Patterns, up0, up0Range, screenCols);
     up0 *= screenCols;
     #ifdef QT_DEBUG
         cvtools::writeMat(up0, "up0.mat", "up0");
         cvtools::writeMat(up0Range, "up0Range.mat", "up0Range");
     #endif
     // Decode camera 1
     std::vector<cv::Mat> frames1Patterns(frames1Rect.begin()+2, frames1Rect.end());
     cv::Mat up1, up1Range;
-    decodeEmbeddedPS(frames1Patterns, up1, up1Range);
+    decodeEmbeddedPS(frames1Patterns, up1, up1Range, screenCols);
     up1 *= screenCols;
     #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 & (amplitude0 > 5.0*nStepsPrimary);
 //    occlusion1 = occlusion1 & (amplitude1 > 5.0*nStepsPrimary);
 //    // 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) < 150);
     cv::Mat edges1;
     cv::Sobel(up1, edges1, -1, 1, 1, 5);
     occlusion1 = occlusion1 & (abs(edges1) < 150);
     #ifdef QT_DEBUG
         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++){
         for(int col=0; col<frameRectCols; col++){
             if(!occlusion0.at<char>(row,col))
                 continue;
             float up0i = up0.at<float>(row,col);
             for(int col1=0; col1<up1.cols-1; col1++){
                 if(!occlusion1.at<char>(row,col1) || !occlusion1.at<char>(row,col1+1))
                     continue;
                 float up1Left = up1.at<float>(row,col1);
                 float up1Right = up1.at<float>(row,col1+1);
                 if((up1Left <= up0i) && (up0i <= up1Right) && (up0i-up1Left < 1.0) && (up1Right-up0i < 1.0) && (up1Right-up1Left > 0.1)){
                     float col1i = col1 + (up0i-up1Left)/(up1Right-up1Left);
                     q0.push_back(cv::Point2f(col, row));
                     q1.push_back(cv::Point2f(col1i, row));
                     break;
+                }
+            }
+        }
+    }
     int nMatches = q0.size();
     if(nMatches < 1){
         Q.resize(0);
         color.resize(0);
         return;
+    }
     // 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);
     // Undo rectification
     cv::Mat R0Inv;
     cv::Mat(R0.t()).convertTo(R0Inv, CV_32F);
     QMat = R0Inv*QMat;
     cvtools::matToPoints3f(QMat, Q);
+}

Subversion Repositories seema-scanner

(root)/src/algorithm/AlgorithmPhaseShiftEmbedded.cpp – Rev 192 → 193