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 (higher frequency) can in principle be chosen freely, but small changes can have a considerable impact on quality.
+// 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"
 #ifndef M_PI
     #define M_PI 3.14159265358979323846
 #endif
 static unsigned int nStepsPrimary = 16; // number of shifts/steps in primary
 static unsigned int nStepsSecondary = 8; // number of shifts/steps in secondary
-static float periodPrimary = 48; // primary period
+static float nPeriodsPrimary = 48; // primary period
-// Algorithm
-static cv::Mat computePhaseVector(unsigned int length, float phase, float pitch){
-    cv::Mat phaseVector(length, 1, CV_8UC3);
-    //phaseVector.setTo(0);
-    const float pi = M_PI;
-    // Loop through vector
-    for(int i=0; i<phaseVector.rows; i++){
-        // Amplitude of channels
-        float amp = 0.5*(1+cos(2*pi*i/pitch - phase));
-        phaseVector.at<cv::Vec3b>(i, 0) = cv::Vec3b(255.0*amp,255.0*amp,255.0*amp);
-    }
-    return phaseVector;
-}
 AlgorithmPhaseShiftTwoFreq::AlgorithmPhaseShiftTwoFreq(unsigned int _screenCols, unsigned int _screenRows) : Algorithm(_screenCols, _screenRows){
     // Set N
     N = 2+nStepsPrimary+nStepsSecondary;
     // Determine the secondary (wider) period
-    float pSecondary = (screenCols*periodPrimary)/(screenCols-periodPrimary);
+    float nPeriodsSecondary = (screenCols*nPeriodsPrimary)/(screenCols-nPeriodsPrimary);
     // all on pattern
     cv::Mat allOn(1, screenCols, CV_8UC3, cv::Scalar::all(255));
     patterns.push_back(allOn);
     const float pi = M_PI;
     // Primary encoding patterns
     for(unsigned int i=0; i<nStepsPrimary; i++){
         float phase = 2.0*pi/nStepsPrimary * i;
-        float pitch = periodPrimary;
+        float pitch = 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*pi/nStepsSecondary * i;
-        float pitch = pSecondary;
+        float pitch = 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];
+}
-//// Absolute phase from 3 frames
-//static cv::Mat getPhase(const cv::Mat I1, const cv::Mat I2, const cv::Mat I3){
-//    cv::Mat_<float> I1_(I1);
-//    cv::Mat_<float> I2_(I2);
-//    cv::Mat_<float> I3_(I3);
-//    cv::Mat phase;
-//    // One call approach
-//    cv::phase(2.0*I1_-I3_-I2_, sqrt(3.0)*(I2_-I3_), phase);
-//    return phase;
-//}
-// Phase unwrapping by means of a phase cue
-static cv::Mat unwrapWithCue(const cv::Mat up, const cv::Mat upCue, float nPhases){
-    const float pi = M_PI;
-    // Determine number of jumps
-    cv::Mat P = (upCue*nPhases-up)/(2.0*pi);
-    // Round to integers
-    P.convertTo(P, CV_8U);
-    P.convertTo(P, CV_32F);
-    // Add to phase
-    cv::Mat upUnwrapped = up + P*2*pi;
-    // Scale to range [0; 2pi]
-    upUnwrapped *= 1.0/nPhases;
-    return upUnwrapped;
-}
-// Absolute phase and magnitude from N frames
-static std::vector<cv::Mat> getDFTComponents(const std::vector<cv::Mat> frames){
-    unsigned int N = frames.size();
-//    std::vector<cv::Mat> framesReverse = frames;
-//    std::reverse(framesReverse.begin(), framesReverse.end());
-    // DFT approach
-    cv::Mat I;
-    cv::merge(frames, I);
-    unsigned int w = I.cols;
-    unsigned int h = I.rows;
-    I = I.reshape(1, h*w);
-    I.convertTo(I, CV_32F);
-    cv::Mat fI;
-    cv::dft(I, fI, cv::DFT_ROWS + cv::DFT_COMPLEX_OUTPUT);
-    fI = fI.reshape(N*2, h);
-    std::vector<cv::Mat> fIcomp;
-    cv::split(fI, fIcomp);
-    return fIcomp;
-}
 void AlgorithmPhaseShiftTwoFreq::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){
     const float pi = M_PI;
     assert(frames0.size() == N);
     // 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;
+    }
     // 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());
     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);
     cv::Mat up0Secondary;
     cv::phase(F0Secondary[2], -F0Secondary[3], up0Secondary);
     cv::Mat up0Equivalent = up0Primary - up0Secondary;
     up0Equivalent = cvtools::modulo(up0Equivalent, 2.0*pi);
-    cv::Mat up0 = unwrapWithCue(up0Primary, up0Equivalent, (float)screenCols/periodPrimary);
+    cv::Mat up0 = unwrapWithCue(up0Primary, up0Equivalent, (float)screenCols/nPeriodsPrimary);
     up0 *= screenCols/(2.0*pi);
     cv::Mat amplitude0;
     cv::magnitude(F0Primary[2], -F0Primary[3], amplitude0);
+    // 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);
+    }
     // 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());
     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);
     cv::Mat up1Secondary;
     cv::phase(F1Secondary[2], -F1Secondary[3], up1Secondary);
     cv::Mat up1Equivalent = up1Primary - up1Secondary;
     up1Equivalent = cvtools::modulo(up1Equivalent, 2.0*pi);
-    cv::Mat up1 = unwrapWithCue(up1Primary, up1Equivalent, (float)screenCols/periodPrimary);
+    cv::Mat up1 = unwrapWithCue(up1Primary, up1Equivalent, (float)screenCols/nPeriodsPrimary);
     up1 *= screenCols/(2.0*pi);
     cv::Mat amplitude1;
     cv::magnitude(F1Primary[2], -F1Primary[3], amplitude1);
+    #ifdef Q_DEBUG
-cvtools::writeMat(up0Primary, "up0Primary.mat", "up0Primary");
+        cvtools::writeMat(up0Primary, "up0Primary.mat", "up0Primary");
-cvtools::writeMat(up0Secondary, "up0Secondary.mat", "up0Secondary");
+        cvtools::writeMat(up0Secondary, "up0Secondary.mat", "up0Secondary");
-cvtools::writeMat(up0Equivalent, "up0Equivalent.mat", "up0Equivalent");
+        cvtools::writeMat(up0Equivalent, "up0Equivalent.mat", "up0Equivalent");
-cvtools::writeMat(up0, "up0.mat", "up0");
+        cvtools::writeMat(up0, "up0.mat", "up0");
-cvtools::writeMat(amplitude0, "amplitude0.mat", "amplitude0");
+        cvtools::writeMat(amplitude0, "amplitude0.mat", "amplitude0");
+    #endif
+    // 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);
+    }
     // color debayer and remap
     cv::Mat color0, color1;
-//    frames0[0].convertTo(color0Rect, CV_8UC1, 1.0/256.0);
     cv::cvtColor(frames0[0], color0, CV_BayerBG2RGB);
     cv::remap(color0, color0, map0X, map0Y, CV_INTER_LINEAR);
-//    frames1[0].convertTo(color1Rect, CV_8UC1, 1.0/256.0);
     cv::cvtColor(frames1[0], color1, CV_BayerBG2RGB);
     cv::remap(color1, color1, map1X, map1Y, CV_INTER_LINEAR);
+    #ifdef Q_DEBUG
-//cvtools::writeMat(color0, "color0.mat", "color0");
+        cvtools::writeMat(color0, "color0.mat", "color0");
-//cvtools::writeMat(color1, "color1.mat", "color1");
+        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);
     // Threshold on energy at primary frequency
     occlusion0 = occlusion0 & (amplitude0 > 5.0*nStepsPrimary);
     occlusion1 = occlusion1 & (amplitude1 > 5.0*nStepsPrimary);
+    // Threshold on energy ratios
+    occlusion0 = occlusion0 & (amplitude0 > 0.85*energy0Primary);
+    occlusion0 = occlusion0 & (amplitude0 > 0.85*energy0Secondary);
-//cvtools::writeMat(occlusion1, "occlusion1.mat", "occlusion1");
+    occlusion1 = occlusion1 & (amplitude1 > 0.85*energy1Primary);
-//cvtools::writeMat(occlusion1, "occlusion1.mat", "occlusion1");
+    occlusion1 = occlusion1 & (amplitude1 > 0.85*energy1Secondary);
-    // Erode occlusion masks
+//    // Erode occlusion masks
-    cv::Mat strel = cv::getStructuringElement(cv::MORPH_ELLIPSE, cv::Size(5,5));
+//    cv::Mat strel = cv::getStructuringElement(cv::MORPH_ELLIPSE, cv::Size(5,5));
-    cv::erode(occlusion0, occlusion0, strel);
+//    cv::erode(occlusion0, occlusion0, strel);
-    cv::erode(occlusion1, occlusion1, strel);
+//    cv::erode(occlusion1, occlusion1, strel);
-    // Threshold on gradient of phase
+//    // Threshold on gradient of phase
-    cv::Mat edges0;
+//    cv::Mat edges0;
-    cv::Sobel(up0, edges0, -1, 1, 1, 5);
+//    cv::Sobel(up0, edges0, -1, 1, 1, 5);
-    occlusion0 = occlusion0 & (abs(edges0) < 150);
+//    occlusion0 = occlusion0 & (abs(edges0) < 150);
-    cv::Mat edges1;
+//    cv::Mat edges1;
-    cv::Sobel(up1, edges1, -1, 1, 1, 5);
+//    cv::Sobel(up1, edges1, -1, 1, 1, 5);
-    occlusion1 = occlusion1 & (abs(edges1) < 150);
+//    occlusion1 = occlusion1 & (abs(edges1) < 150);
+    #ifdef Q_DEBUG
-//cvtools::writeMat(edges0, "edges0.mat", "edges0");
+        cvtools::writeMat(occlusion1, "occlusion1.mat", "occlusion1");
-//cvtools::writeMat(edges1, "edges1.mat", "edges1");
+        cvtools::writeMat(occlusion1, "occlusion1.mat", "occlusion1");
+    #endif
     // Match phase maps
-    int frameRectRows = map0X.rows;
-    int frameRectCols = map0X.cols;
     // camera0 against camera1
     std::vector<cv::Vec2f> q0, q1;
     for(int row=0; row<frameRectRows; row++){
         for(int col=0; col<frameRectCols; col++){

Subversion Repositories seema-scanner

(root)/src/algorithm/AlgorithmPhaseShiftTwoFreq.cpp – Rev 181 → 182