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];
+}
 struct StereoRectifyier {
     cv::Mat map0X, map0Y, map1X, map1Y;
     cv::Mat R0, R1, P0, P1, QRect;
 };
-static void getStereoRectifyier(const SMCalibrationParameters &calibration,
+void getStereoRectifier(const SMCalibrationParameters &calibration,
-                                const cv::Size& frameSize,
+                         const cv::Size& frameSize,
-                                StereoRectifyier& stereoRect);
+                         StereoRectifyier& stereoRect){
+    // cv::stereoRectify segfaults unless R is double precision
+    cv::Mat R, T;
+    cv::Mat(calibration.R1).convertTo(R, CV_64F);
+    cv::Mat(calibration.T1).convertTo(T, CV_64F);
+    cv::stereoRectify(calibration.K0, calibration.k0,
+                      calibration.K1, calibration.k1,
+                      frameSize, R, T,
+                      stereoRect.R0, stereoRect.R1,
+                      stereoRect.P0, stereoRect.P1,
+                      stereoRect.QRect, 0);
+    // Interpolation maps (lens distortion and rectification)
+    cv::initUndistortRectifyMap(calibration.K0, calibration.k0,
+                                stereoRect.R0, stereoRect.P0,
+                                frameSize, CV_32F,
+                                stereoRect.map0X, stereoRect.map0Y);
+    cv::initUndistortRectifyMap(calibration.K1, calibration.k1,
+                                stereoRect.R1, stereoRect.P1,
+                                frameSize, CV_32F,
+                                stereoRect.map1X, stereoRect.map1Y);
+}
-static void determineAmplitudePhaseEnergy(std::vector<cv::Mat>& frames,
+void determineAmplitudePhaseEnergy(std::vector<cv::Mat>& frames,
-                                    cv::Mat& amplitude,
+                                   cv::Mat& amplitude,
-                                    cv::Mat& phase,
+                                   cv::Mat& phase,
-                                    cv::Mat& energy);
+                                   cv::Mat& energy) {
+    std::vector<cv::Mat> fourier = getDFTComponents(frames);
+    cv::phase(fourier[2], -fourier[3], phase);
+    // Signal energy at unit frequency
+    cv::magnitude(fourier[2], -fourier[3], amplitude);
+    // Collected signal energy at higher frequencies
+    energy = cv::Mat(phase.rows, phase.cols, CV_32F, cv::Scalar(0.0));
+    for(unsigned int i=0; i<frames.size()-1; i++){
+        cv::Mat magnitude;
+        cv::magnitude(fourier[i*2 + 2], fourier[i*2 + 3], magnitude);
+        cv::add(energy, magnitude, energy, cv::noArray(), CV_32F);
+    }
+    frames.clear();
+}
-static void collectPhases(const cv::Mat& phasePrimary,
+void unWrapPhaseMap(const cv::Mat& phasePrimary,
-                          const cv::Mat& phaseSecondary,
+                   const cv::Mat& phaseSecondary,
-                          cv::Mat& phase);
+                   cv::Mat& phase) {
+    cv::Mat phaseEquivalent = phaseSecondary - phasePrimary;
+    phaseEquivalent = cvtools::modulo(phaseEquivalent, 2.0*CV_PI);
+    phase = unwrapWithCue(phasePrimary, phaseEquivalent, nPeriodsPrimary);
+    phase *= phasePrimary.cols/(2.0*CV_PI);
+}
-static void matchPhaseMaps(const cv::Mat& occlusion0, const cv::Mat& occlusion1,
+void matchPhaseMaps(const cv::Mat& occlusion0, const cv::Mat& occlusion1,
-                           const cv::Mat& phase0, const cv::Mat& phase1,
+                    const cv::Mat& phase0, const cv::Mat& phase1,
-                           std::vector<cv::Vec2f>& q0, std::vector<cv::Vec2f>& q1);
+                    std::vector<cv::Vec2f>& q0, std::vector<cv::Vec2f>& q1) {
-static void triangulate(const StereoRectifyier& stereoRect,
+    #pragma omp parallel for
-                        const std::vector<cv::Vec2f>& q0,
-                        const std::vector<cv::Vec2f>& q1,
+    for(int row=0; row<occlusion0.rows; row++){
-                        std::vector<cv::Point3f>& Q);
+        for(int col=0; col<occlusion0.cols; col++){
+            if(!occlusion0.at<char>(row,col))
+                continue;
+            float phase0i = phase0.at<float>(row,col);
+            for(int col1=0; col1<phase1.cols-1; col1++){
+                if(!occlusion1.at<char>(row,col1) || !occlusion1.at<char>(row,col1+1))
+                    continue;
+                float phase1Left = phase1.at<float>(row,col1);
+                float phase1Right = phase1.at<float>(row,col1+1);
+                bool match = (phase1Left <= phase0i)
+                              && (phase0i <= phase1Right)
+                              && (phase0i-phase1Left < 1.0)
+                              && (phase1Right-phase0i < 1.0)
+                              && (phase1Right-phase1Left > 0.1);
+                if(match){
+                    float col1i = col1 + (phase0i-phase1Left)/(phase1Right-phase1Left);
+                    #pragma omp critical
+                    {
+                    q0.push_back(cv::Point2f(col, row));
+                    q1.push_back(cv::Point2f(col1i, row));
+                    }
+                    break;
+                }
+            }
+        }
+    }
+}
+void triangulate(const StereoRectifyier& stereoRect,
+                 const std::vector<cv::Vec2f>& q0,
+                 const std::vector<cv::Vec2f>& q1,
+                 std::vector<cv::Point3f>& Q) {
+    // 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);
+    // Triangulate by means of disparity projection
+    Q.resize(q0.size());
+    cv::Matx44f QRectx = cv::Matx44f(stereoRect.QRect);
+    cv::Matx33f R0invx = cv::Matx33f(cv::Mat(stereoRect.R0.t()));
+    #pragma omp parallel for
+    for(unsigned int i=0; i < q0.size(); i++){
+        float disparity = q0[i][0] - q1[i][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] = R0invx * cv::Point3f(Qih[0]*winv, Qih[1]*winv, Qih[2]*winv);
+    }
+}
 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);
     StereoRectifyier stereoRect;
-    getStereoRectifyier(calibration,
+    getStereoRectifier(calibration,
                         cv::Size(frames0[0].cols, frames0[0].rows),
                         stereoRect);
     // // Erode occlusion masks
     // cv::Mat strel = cv::getStructuringElement(cv::MORPH_ELLIPSE, cv::Size(5,5));
     cv::Mat up0, up1;
     cv::Mat occlusion0, occlusion1;
     cv::Mat color0, color1;
     #pragma omp parallel sections
+    {
         #pragma omp section
+        {
             // Gray-scale and remap/rectify
             std::vector<cv::Mat> frames0Rect(N);
             for(unsigned int i=0; i<N; i++){
                 cv::Mat temp;
+                if(frames1[i].depth() == CV_8U)
-                cv::cvtColor(frames0[i], temp, CV_BayerBG2GRAY);
+                    cv::cvtColor(frames1[i], temp, CV_BayerBG2GRAY);
+                else
+                    temp = frames1[i];
                 cv::remap(temp, frames0Rect[i],
                           stereoRect.map0X, stereoRect.map0Y,
                           CV_INTER_LINEAR);
+            }
-            cv::cvtColor(frames0[0], color0, CV_BayerBG2RGB);
+            // If images are HDR (float), we need to convert to uchar
-            cv::remap(color0, color0,
+            frames0Rect[0].convertTo(color0, CV_8UC3);
-                      stereoRect.map0X, stereoRect.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();
             cv::Mat amplitude0Primary, amplitude0Secondary;
             cv::Mat up0Primary, up0Secondary;
             cv::Mat energy0Primary, energy0Secondary;
             determineAmplitudePhaseEnergy(frames0Primary,
                                           amplitude0Primary,
                                           up0Primary,
                                           energy0Primary);
             determineAmplitudePhaseEnergy(frames0Secondary,
                                           amplitude0Secondary,
                                           up0Secondary,
                                           energy0Secondary);
-            collectPhases(up0Primary, up0Secondary, up0);
+            unWrapPhaseMap(up0Primary, up0Secondary, up0);
             // 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);
             // // Erode occlusion masks
             // cv::erode(occlusion0, occlusion0, strel);
-            // Threshold on gradient of phase
+            // Threshold on vertical gradient of phase
             cv::Mat edges0;
             cv::Sobel(up0, edges0, -1, 1, 1, 5);
             occlusion0 = occlusion0 & (abs(edges0) < 10);
             #ifdef SM_DEBUG
                 cvtools::writeMat(up0Primary, "up0Primary.mat", "up0Primary");
                 cvtools::writeMat(up0Secondary, "up0Secondary.mat", "up0Secondary");
                 cvtools::writeMat(up0, "up0.mat", "up0");
                 cvtools::writeMat(amplitude0Primary,
                                   "amplitude0Primary.mat", "amplitude0Primary");
                 cvtools::writeMat(amplitude0Secondary,
                                   "amplitude0Secondary.mat", "amplitude0Secondary");
                 cvtools::writeMat(energy0Primary,
                                   "energy0Primary.mat", "energy0Primary");
                 cvtools::writeMat(energy0Secondary,
                                   "energy0Secondary.mat", "energy0Secondary");
                 cvtools::writeMat(edges0, "edges0.mat", "edges0");
                 cvtools::writeMat(occlusion0, "occlusion0.mat", "occlusion0");
                 cvtools::writeMat(color0, "color0.mat", "color0");
             #endif
+        }
         #pragma omp section
+        {
             // Gray-scale and remap
             std::vector<cv::Mat> frames1Rect(N);
             for(unsigned int i=0; i<N; i++){
                 cv::Mat temp;
+                if(frames1[i].depth() == CV_8U)
-                cv::cvtColor(frames1[i], temp, CV_BayerBG2GRAY);
+                    cv::cvtColor(frames1[i], temp, CV_BayerBG2GRAY);
+                else
+                    temp = frames1[i];
                 cv::remap(temp, frames1Rect[i],
                           stereoRect.map1X, stereoRect.map1Y,
                           CV_INTER_LINEAR);
+            }
-            cv::cvtColor(frames1[0], color1, CV_BayerBG2RGB);
+            // If images are HDR (float), we need to convert to uchar
-            cv::remap(color1, color1,
+            frames1Rect[0].convertTo(color1, CV_8UC3);
-                      stereoRect.map1X, stereoRect.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();
             cv::Mat amplitude1Primary, amplitude1Secondary;
             cv::Mat up1Primary, up1Secondary;
             cv::Mat energy1Primary, energy1Secondary;
             determineAmplitudePhaseEnergy(frames1Primary,
                                           amplitude1Primary,
                                           up1Primary,
                                           energy1Primary);
             determineAmplitudePhaseEnergy(frames1Secondary,
                                           amplitude1Secondary,
                                           up1Secondary,
                                           energy1Secondary);
-            collectPhases(up1Primary, up1Secondary, up1);
+            unWrapPhaseMap(up1Primary, up1Secondary, up1);
             // 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);
             // // Erode occlusion masks
             // cv::erode(occlusion1, occlusion1, strel);
-            // Threshold on gradient of phase
+            // Threshold on vertical gradient of phase
             cv::Mat edges1;
             cv::Sobel(up1, edges1, -1, 1, 1, 5);
             occlusion1 = occlusion1 & (abs(edges1) < 10);
             #ifdef SM_DEBUG
                 cvtools::writeMat(up1Primary, "up1Primary.mat", "up1Primary");
                 cvtools::writeMat(up1Secondary, "up1Secondary.mat", "up1Secondary");
                 cvtools::writeMat(up1, "up1.mat", "up1");
                 cvtools::writeMat(amplitude1Primary,
                                   "amplitude1Primary.mat", "amplitude1Primary");
                 cvtools::writeMat(amplitude1Secondary,
                                   "amplitude1Secondary.mat", "amplitude1Secondary");
                 cvtools::writeMat(energy1Primary,
                                   "energy1Primary.mat", "energy1Primary");
                 cvtools::writeMat(energy1Secondary,
                                   "energy1Secondary.mat", "energy1Secondary");
                 cvtools::writeMat(edges1, "edges1.mat", "edges1");
                 cvtools::writeMat(occlusion1, "occlusion1.mat", "occlusion1");
                 cvtools::writeMat(color1, "color1.mat", "color1");
             #endif
+        }
+    }
     // Match phase maps
     // camera0 against camera1
     std::vector<cv::Vec2f> q0, q1;
     matchPhaseMaps(occlusion0, occlusion1, up0, up1, q0, q1);
     size_t nMatches = q0.size();
     if(nMatches < 1){
         Q.resize(0);
         color.resize(0);
         return;
+    }
     else {
         // Retrieve color information
         color.resize(nMatches);
         for(unsigned int i=0; i<nMatches; i++){
             cv::Vec3b c0 = color0.at<cv::Vec3b>(int(q0[i][1]), int(q0[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(stereoRect, q0, q1, Q);
+}
-void getStereoRectifyier(const SMCalibrationParameters &calibration,
-                         const cv::Size& frameSize,
-                         StereoRectifyier& stereoRect){
-    // cv::stereoRectify segfaults unless R is double precision
-    cv::Mat R, T;
-    cv::Mat(calibration.R1).convertTo(R, CV_64F);
-    cv::Mat(calibration.T1).convertTo(T, CV_64F);
-    cv::stereoRectify(calibration.K0, calibration.k0,
-                      calibration.K1, calibration.k1,
-                      frameSize, R, T,
-                      stereoRect.R0, stereoRect.R1,
-                      stereoRect.P0, stereoRect.P1,
-                      stereoRect.QRect, 0);
-    // Interpolation maps (lens distortion and rectification)
-    cv::initUndistortRectifyMap(calibration.K0, calibration.k0,
-                                stereoRect.R0, stereoRect.P0,
-                                frameSize, CV_32F,
-                                stereoRect.map0X, stereoRect.map0Y);
-    cv::initUndistortRectifyMap(calibration.K1, calibration.k1,
-                                stereoRect.R1, stereoRect.P1,
-                                frameSize, CV_32F,
-                                stereoRect.map1X, stereoRect.map1Y);
-}
-void determineAmplitudePhaseEnergy(std::vector<cv::Mat>& frames,
-                                   cv::Mat& amplitude,
-                                   cv::Mat& phase,
-                                   cv::Mat& energy) {
-    std::vector<cv::Mat> fourier = getDFTComponents(frames);
-    cv::phase(fourier[2], -fourier[3], phase);
-    // Signal energy at unit frequency
-    cv::magnitude(fourier[2], -fourier[3], amplitude);
-    // Collected signal energy at higher frequencies
-    energy = cv::Mat(phase.rows, phase.cols, CV_32F, cv::Scalar(0.0));
-    for(unsigned int i=0; i<frames.size()-1; i++){
-        cv::Mat magnitude;
-        cv::magnitude(fourier[i*2 + 2], fourier[i*2 + 3], magnitude);
-        cv::add(energy, magnitude, energy, cv::noArray(), CV_32F);
-    }
-    frames.clear();
-}
-void collectPhases(const cv::Mat& phasePrimary,
-                   const cv::Mat& phaseSecondary,
-                   cv::Mat& phase) {
-    cv::Mat phaseEquivalent = phaseSecondary - phasePrimary;
-    phaseEquivalent = cvtools::modulo(phaseEquivalent, 2.0*CV_PI);
-    phase = unwrapWithCue(phasePrimary, phaseEquivalent, nPeriodsPrimary);
-    phase *= phasePrimary.cols/(2.0*CV_PI);
-}
-void matchPhaseMaps(const cv::Mat& occlusion0, const cv::Mat& occlusion1,
-                    const cv::Mat& phase0, const cv::Mat& phase1,
-                    std::vector<cv::Vec2f>& q0, std::vector<cv::Vec2f>& q1) {
-    #pragma omp parallel for
-    for(int row=0; row<occlusion0.rows; row++){
-        for(int col=0; col<occlusion0.cols; col++){
-            if(!occlusion0.at<char>(row,col))
-                continue;
-            float phase0i = phase0.at<float>(row,col);
-            for(int col1=0; col1<phase1.cols-1; col1++){
-                if(!occlusion1.at<char>(row,col1) || !occlusion1.at<char>(row,col1+1))
-                    continue;
-                float phase1Left = phase1.at<float>(row,col1);
-                float phase1Right = phase1.at<float>(row,col1+1);
-                bool match = (phase1Left <= phase0i)
-                              && (phase0i <= phase1Right)
-                              && (phase0i-phase1Left < 1.0)
-                              && (phase1Right-phase0i < 1.0)
-                              && (phase1Right-phase1Left > 0.1);
-                if(match){
-                    float col1i = col1 + (phase0i-phase1Left)/(phase1Right-phase1Left);
-                    #pragma omp critical
-                    {
-                    q0.push_back(cv::Point2f(col, row));
-                    q1.push_back(cv::Point2f(col1i, row));
-                    }
-                    break;
-                }
-            }
-        }
-    }
-}
-void triangulate(const StereoRectifyier& stereoRect,
-                 const std::vector<cv::Vec2f>& q0,
-                 const std::vector<cv::Vec2f>& q1,
-                 std::vector<cv::Point3f>& Q) {
-    // 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);
-    // Triangulate by means of disparity projection
-    Q.resize(q0.size());
-    cv::Matx44f QRectx = cv::Matx44f(stereoRect.QRect);
-    cv::Matx33f R0invx = cv::Matx33f(cv::Mat(stereoRect.R0.t()));
-    #pragma omp parallel for
-    for(unsigned int i=0; i < q0.size(); i++){
-        float disparity = q0[i][0] - q1[i][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] = R0invx * cv::Point3f(Qih[0]*winv, Qih[1]*winv, Qih[2]*winv);
-    }
-}

Subversion Repositories seema-scanner

(root)/src/algorithm/AlgorithmPhaseShiftTwoFreq.cpp/ – Rev 235 → 244