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

 //
 // Two Frequency Phase Shifting using the Heterodyne Principle
 //
 // This implementation follows "Reich, Ritter, Thesing, White light heterodyne principle for 3D-measurement", SPIE (1997)
 // Different from the paper, it uses only two different frequencies.
 //
 // The number of periods in the primary frequency can be chosen freely, but small changes can have a considerable impact on quality.
 // The number of phase shifts can be chosen freely (min. 3), and higher values reduce the effects of image noise. They also allow us to filter bad points based on energy at non-primary frequencies.
 //
 #include "AlgorithmPhaseShiftTwoFreq.h"
 #include <math.h>
 #include "cvtools.h"
 #include "algorithmtools.h"
+#include <omp.h>
 static unsigned int nStepsPrimary = 16; // number of shifts/steps in primary
 static unsigned int nStepsSecondary = 8; // number of shifts/steps in secondary
 static float nPeriodsPrimary = 40; // primary period
 AlgorithmPhaseShiftTwoFreq::AlgorithmPhaseShiftTwoFreq(unsigned int _screenCols, unsigned int _screenRows) : Algorithm(_screenCols, _screenRows){
     // Set N
     N = 2+nStepsPrimary+nStepsSecondary;
     // Determine the secondary (wider) period to fulfill the heterodyne condition
     float nPeriodsSecondary = nPeriodsPrimary + 1;
     // 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);
     // Primary encoding patterns
     for(unsigned int i=0; i<nStepsPrimary; i++){
         float phase = 2.0*CV_PI/nStepsPrimary * i;
         float pitch = screenCols/nPeriodsPrimary;
         cv::Mat patternI(1,1,CV_8U);
         patternI = computePhaseVector(screenCols, phase, pitch);
         patterns.push_back(patternI.t());
+    }
     // Secondary encoding patterns
     for(unsigned int i=0; i<nStepsSecondary; i++){
         float phase = 2.0*CV_PI/nStepsSecondary * i;
         float pitch = screenCols/nPeriodsSecondary;
         cv::Mat patternI(1,1,CV_8U);
         patternI = computePhaseVector(screenCols, phase, pitch);
         patterns.push_back(patternI.t());
+    }
+}
 cv::Mat AlgorithmPhaseShiftTwoFreq::getEncodingPattern(unsigned int depth){
     return patterns[depth];
+}
 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;
+    cv::Mat R0, P0, P1, QRect;
-{
+    {
     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  R1, QRect;
+    cv::Mat  R1;
     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::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;
     cv::Mat occlusion0;
-{
-        // Gray-scale and remap
-        std::vector<cv::Mat> frames0Rect(N);
-        for(unsigned int i=0; i<N; i++){
-            cv::Mat temp;
+    cv::Mat up1;
-            cv::cvtColor(frames0[i], temp, CV_BayerBG2GRAY);
-            cv::remap(temp, frames0Rect[i], map0X, map0Y, CV_INTER_LINEAR);
-        }
-        // Occlusion masks
+    cv::Mat occlusion1;
-        cv::subtract(frames0Rect[0], frames0Rect[1], occlusion0);
-        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());
+    #pragma omp parallel sections
+    {
+    #pragma omp section
+    {
+    // Gray-scale and remap
+    std::vector<cv::Mat> frames0Rect(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);
+    }
+    // Occlusion masks
+    cv::subtract(frames0Rect[0], frames0Rect[1], occlusion0);
+    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());
-        frames0Rect.clear();
+    frames0Rect.clear();
     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);
     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::magnitude(F0Secondary[2], -F0Secondary[3], amplitude0Secondary);
     // Collected signal energy at higher frequencies
     cv::Mat energy0Primary(frameRectRows, frameRectCols, CV_32F, cv::Scalar(0.0));
     for(unsigned int i=0; i<nStepsPrimary-1; i++){
         cv::Mat magnitude;
         cv::magnitude(F0Primary[i*2 + 2], F0Primary[i*2 + 3], magnitude);
         cv::add(energy0Primary, magnitude, energy0Primary, cv::noArray(), CV_32F);
+    }
     cv::Mat energy0Secondary(frameRectRows, frameRectCols, CV_32F, cv::Scalar(0.0));
     for(unsigned int i=0; i<nStepsSecondary-1; i++){
         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 & (amplitude0Primary > 5.0*nStepsPrimary);
     // Threshold on energy ratios
     occlusion0 = occlusion0 & (amplitude0Primary > 0.25*energy0Primary);
     occlusion0 = occlusion0 & (amplitude0Secondary > 0.25*energy0Secondary);
     #ifdef SM_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;
+    #pragma omp section
+    {
-        // Gray-scale and remap
+    // Gray-scale and remap
-        std::vector<cv::Mat> frames1Rect(N);
+    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
+    for(unsigned int i=0; i<N; i++){
-        cv::subtract(frames1Rect[0], frames1Rect[1], occlusion1);
-        occlusion1 = (occlusion1 > 25) & (occlusion1 < 250);
-        // Decode camera1
+        cv::Mat temp;
-        std::vector<cv::Mat> frames1Primary(frames1Rect.begin()+2, frames1Rect.begin()+2+nStepsPrimary);
+        cv::cvtColor(frames1[i], temp, CV_BayerBG2GRAY);
-        std::vector<cv::Mat> frames1Secondary(frames1Rect.begin()+2+nStepsPrimary, frames1Rect.end());
+        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
+    std::vector<cv::Mat> frames1Primary(frames1Rect.begin()+2, frames1Rect.begin()+2+nStepsPrimary);
+    std::vector<cv::Mat> frames1Secondary(frames1Rect.begin()+2+nStepsPrimary, frames1Rect.end());
-        frames1Rect.clear();
+    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);
     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::magnitude(F1Secondary[2], -F1Secondary[3], amplitude1Secondary);
     // Collected signal energy at higher frequencies
     cv::Mat energy1Primary(frameRectRows, frameRectCols, CV_32F, cv::Scalar(0.0));
     for(unsigned int i=0; i<nStepsPrimary-1; i++){
         cv::Mat magnitude;
         cv::magnitude(F1Primary[i*2 + 2], F1Primary[i*2 + 3], magnitude);
         cv::add(energy1Primary, magnitude, energy1Primary, cv::noArray(), CV_32F);
+    }
     cv::Mat energy1Secondary(frameRectRows, frameRectCols, CV_32F, cv::Scalar(0.0));
     for(unsigned int i=0; i<nStepsSecondary-1; i++){
         cv::Mat magnitude;
         cv::magnitude(F1Secondary[i*2 + 2], F1Secondary[i*2 + 3], magnitude);
         cv::add(energy1Secondary, magnitude, energy1Secondary, cv::noArray(), CV_32F);
+    }
     // Threshold on energy at primary frequency
     occlusion1 = occlusion1 & (amplitude1Primary > 5.0*nStepsPrimary);
     // Threshold on energy ratios
     occlusion1 = occlusion1 & (amplitude1Primary > 0.25*energy1Primary);
     occlusion1 = occlusion1 & (amplitude1Secondary > 0.25*energy1Secondary);
     #ifdef SM_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);
     cv::Mat edges1;
     cv::Sobel(up1, edges1, -1, 1, 1, 5);
     occlusion1 = occlusion1 & (abs(edges1) < 10);
     #ifdef SM_DEBUG
         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;
+    #pragma omp parallel for
     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);
+                    #pragma omp critical
+                    {
                     q0.push_back(cv::Point2f(col, row));
                     q1.push_back(cv::Point2f(col1i, row));
+                    }
                     break;
+                }
+            }
+        }
+    }
-    int nMatches = q0.size();
+    size_t nMatches = q0.size();
     if(nMatches < 1){
         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 SM_DEBUG
         cvtools::writeMat(color0, "color0.mat", "color0");
         cvtools::writeMat(color1, "color1.mat", "color1");
     #endif
     // Retrieve color information
     color.resize(nMatches);
-    for(int i=0; i<nMatches; i++){
+    for(unsigned int i=0; i<nMatches; i++){
-        cv::Vec3b c0 = color0.at<cv::Vec3b>(q0[i][1], q0[i][0]);
+        cv::Vec3b c0 = color0.at<cv::Vec3b>(int(q0[i][1]), int(q0[i][0]));
-        cv::Vec3b c1 = color1.at<cv::Vec3b>(q1[i][1], q1[i][0]);
+        cv::Vec3b c1 = color1.at<cv::Vec3b>(int(q1[i][1]), int(q1[i][0]));
         color[i] = 0.5*c0 + 0.5*c1;
+    }
+}
-    // Triangulate points
+//    // Triangulate points
-    cv::Mat QMatHomogenous, QMat;
+//    cv::Mat QMatHomogenous, QMat;
-    cv::triangulatePoints(P0, P1, q0, q1, QMatHomogenous);
+//    cv::triangulatePoints(P0, P1, q0, q1, QMatHomogenous);
-    cvtools::convertMatFromHomogeneous(QMatHomogenous, QMat);
+//    cvtools::convertMatFromHomogeneous(QMatHomogenous, QMat);
-    // Undo rectification
+//    // Undo rectification
-    cv::Mat R0Inv;
+//    cv::Mat R0Inv;
-    cv::Mat(R0.t()).convertTo(R0Inv, CV_32F);
+//    cv::Mat(R0.t()).convertTo(R0Inv, CV_32F);
-    QMat = R0Inv*QMat;
+//    QMat = R0Inv*QMat;
-    cvtools::matToPoints3f(QMat, Q);
+//    cvtools::matToPoints3f(QMat, Q);
-//    // Triangulate by means of disparity projection
+    // Triangulate by means of disparity projection
-//    Q.resize(q0.size());
+    Q.resize(q0.size());
+    cv::Matx44f QRectx = cv::Matx44f(QRect);
+    cv::Matx33f R0invx = cv::Matx33f(cv::Mat(R0.t()));
+    #pragma omp parallel for
-//    for(int i=0; i<q0.size(); i++){
+    for(unsigned int i=0; i<q0.size(); i++){
+        float disparity = q0[i][0]-q1[i][0];
-//        cv::Vec4d Qih = QRect*cv::Vec4d(x, y, q1[i]-q0[i], 1.0);
+        cv::Vec4f Qih = QRectx*cv::Vec4f(q0[i][0], q0[i][1], disparity, 1.0);
+        float winv = float(1.0)/Qih[3];
-//        Q[i] = cv::Point3f(Qih[0]/Qih[3], Qih[1]/Qih[3], Qih[2]/Qih[3]);
+        Q[i] = R0invx * cv::Point3f(Qih[0]*winv, Qih[1]*winv, Qih[2]*winv);
-//    }
+    }
+}

Subversion Repositories seema-scanner

(root)/src/algorithm/AlgorithmPhaseShiftTwoFreq.cpp – Rev 230 → 231