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

 #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){
     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);
         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);
     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);
         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;
                 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);
         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 @ 257 – Rev 230 → 231