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,
+                                const cv::Size& frameSize,
+                                StereoRectifyier& stereoRect);
+static void determineAmplitudePhaseEnergy(std::vector<cv::Mat>& frames,
+                                    cv::Mat& amplitude, cv::Mat& phase, cv::Mat& energy);
+static void collectPhases(const cv::Mat& phasePrimary, const cv::Mat& phaseSecondary,
+                          cv::Mat& phase);
+static 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);
+static void triangulate(const StereoRectifyier& stereoRect,
+                        const std::vector<cv::Vec2f>& q0,
+                        const std::vector<cv::Vec2f>& q1,
+                        std::vector<cv::Point3f>& Q);
 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;
+    StereoRectifyier stereoRect;
+    getStereoRectifyier(calibration, cv::Size(frames0[0].cols, frames0[0].rows), stereoRect);
-    cv::Mat R0, P0, P1, QRect;
+    // // 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
+    {
-    int frameRows = frames0[0].rows;
+        #pragma omp section
-    int frameCols = frames0[0].cols;
+        {
-    // Rectifying homographies (rotation+projections)
+            // Gray-scale and remap/rectify
-    cv::Size frameSize(frameCols, frameRows);
+            std::vector<cv::Mat> frames0Rect(N);
-    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;
-    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)
+            for(unsigned int i=0; i<N; i++){
+                cv::Mat temp;
-    cv::initUndistortRectifyMap(calibration.K0, calibration.k0, R0, P0, frameSize, CV_32F, map0X, map0Y);
+                cv::cvtColor(frames0[i], temp, CV_BayerBG2GRAY);
-    cv::initUndistortRectifyMap(calibration.K1, calibration.k1, R1, P1, frameSize, CV_32F, map1X, map1Y);
+                cv::remap(temp, frames0Rect[i], 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);
+            // 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
+            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");
+            #endif
-    int frameRectRows = map0X.rows;
+        }
-    int frameRectCols = map0X.cols;
+        #pragma omp section
+        {
-    cv::Mat up0;
-    cv::Mat occlusion0;
+            // Gray-scale and remap
-    cv::Mat up1;
-    cv::Mat occlusion1;
+            std::vector<cv::Mat> frames1Rect(N);
-    #pragma omp parallel sections
+            for(unsigned int i=0; i<N; i++){
-    {
-    #pragma omp section
+                cv::Mat temp;
+                cv::cvtColor(frames1[i], temp, CV_BayerBG2GRAY);
+                cv::remap(temp, frames1Rect[i], stereoRect.map1X, stereoRect.map1Y, CV_INTER_LINEAR);
-    {
+            }
+            // Occlusion masks
+            cv::subtract(frames1Rect[0], frames1Rect[1], occlusion1);
+            occlusion1 = (occlusion1 > 25) & (occlusion1 < 250);
-    // Gray-scale and remap
+            // 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);
+            // 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
-    std::vector<cv::Mat> frames0Rect(N);
+            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");
+            #endif
-    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();
-    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;
+    // Match phase maps
-        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++){
+    // camera0 against camera1
-        cv::Mat magnitude;
+    std::vector<cv::Vec2f> q0, q1;
-        cv::magnitude(F0Secondary[i*2 + 2], F0Secondary[i*2 + 3], magnitude);
+    matchPhaseMaps(occlusion0, occlusion1, up0, up1, q0, q1);
-        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
+    size_t nMatches = q0.size();
-    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
+    if(nMatches < 1){
+        Q.resize(0);
+        color.resize(0);
+        return;
+    }
-    #pragma omp section
+    {
+        // color debayer and remap/rectify
+        cv::cvtColor(frames1[0], color1, CV_BayerBG2RGB);
+        cv::remap(color1, color1, stereoRect.map1X, stereoRect.map1Y, CV_INTER_LINEAR);
-    // Gray-scale and remap
+        #ifdef SM_DEBUG
+            cvtools::writeMat(color0, "color0.mat", "color0");
-    std::vector<cv::Mat> frames1Rect(N);
+            cvtools::writeMat(color1, "color1.mat", "color1");
+        #endif
-    for(unsigned int i=0; i<N; i++){
+        // Retrieve color information
-        cv::Mat temp;
+        color.resize(nMatches);
-        cv::cvtColor(frames1[i], temp, CV_BayerBG2GRAY);
+        for(unsigned int i=0; i<nMatches; i++){
+            cv::Vec3b c0 = color0.at<cv::Vec3b>(int(q0[i][1]), int(q0[i][0]));
-        cv::remap(temp, frames1Rect[i], map1X, map1Y, CV_INTER_LINEAR);
+            cv::Vec3b c1 = color1.at<cv::Vec3b>(int(q1[i][1]), int(q1[i][0]));
+            color[i] = 0.5*c0 + 0.5*c1;
+        }
+    }
-    // Occlusion masks
+    // Triangulate points
-    cv::subtract(frames1Rect[0], frames1Rect[1], occlusion1);
+    triangulate(stereoRect, q0, q1, Q);
-    occlusion1 = (occlusion1 > 25) & (occlusion1 < 250);
-    // Decode camera1
+}
-    std::vector<cv::Mat> frames1Primary(frames1Rect.begin()+2, frames1Rect.begin()+2+nStepsPrimary);
+void getStereoRectifyier(const SMCalibrationParameters &calibration, const cv::Size& frameSize, StereoRectifyier& stereoRect){
-    std::vector<cv::Mat> frames1Secondary(frames1Rect.begin()+2+nStepsPrimary, frames1Rect.end());
+    // cv::stereoRectify segfaults unless R is double precision
-    frames1Rect.clear();
+    cv::Mat R, T;
+    cv::Mat(calibration.R1).convertTo(R, CV_64F);
+    cv::Mat(calibration.T1).convertTo(T, CV_64F);
-    std::vector<cv::Mat> F1Primary = getDFTComponents(frames1Primary);
+    cv::stereoRectify(calibration.K0, calibration.k0,
-    frames1Primary.clear();
+                      calibration.K1, calibration.k1,
-    cv::Mat up1Primary;
+                      frameSize, R, T,
+                      stereoRect.R0, stereoRect.R1,
-    cv::phase(F1Primary[2], -F1Primary[3], up1Primary);
+                      stereoRect.P0, stereoRect.P1,
+                      stereoRect.QRect, 0);
-    std::vector<cv::Mat> F1Secondary = getDFTComponents(frames1Secondary);
+    // Interpolation maps (lens distortion and rectification)
-    frames1Secondary.clear();
+    cv::initUndistortRectifyMap(calibration.K0, calibration.k0,
+                                stereoRect.R0, stereoRect.P0,
+                                frameSize, CV_32F,
+                                stereoRect.map0X, stereoRect.map0Y);
-    cv::Mat up1Secondary;
+    cv::initUndistortRectifyMap(calibration.K1, calibration.k1,
+                                stereoRect.R1, stereoRect.P1,
+                                frameSize, CV_32F,
-    cv::phase(F1Secondary[2], -F1Secondary[3], up1Secondary);
+                                stereoRect.map1X, stereoRect.map1Y);
+}
-    cv::Mat up1Equivalent = up1Secondary - up1Primary;
+void determineAmplitudePhaseEnergy(std::vector<cv::Mat>& frames,
-    up1Equivalent = cvtools::modulo(up1Equivalent, 2.0*CV_PI);
+                                   cv::Mat& amplitude, cv::Mat& phase, cv::Mat& energy) {
-    up1 = unwrapWithCue(up1Primary, up1Equivalent, nPeriodsPrimary);
+    std::vector<cv::Mat> fourier = getDFTComponents(frames);
-    up1 *= screenCols/(2.0*CV_PI);
+    cv::phase(fourier[2], -fourier[3], phase);
     // Signal energy at unit frequency
-    cv::Mat amplitude1Primary, amplitude1Secondary;
-    cv::magnitude(F1Primary[2], -F1Primary[3], amplitude1Primary);
+    cv::magnitude(fourier[2], -fourier[3], amplitude);
-    cv::magnitude(F1Secondary[2], -F1Secondary[3], amplitude1Secondary);
     // Collected signal energy at higher frequencies
-    cv::Mat energy1Primary(frameRectRows, frameRectCols, CV_32F, cv::Scalar(0.0));
+    energy = cv::Mat(phase.rows, phase.cols, 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++){
+    for(unsigned int i=0; i<frames.size()-1; i++){
         cv::Mat magnitude;
-        cv::magnitude(F1Secondary[i*2 + 2], F1Secondary[i*2 + 3], magnitude);
+        cv::magnitude(fourier[i*2 + 2], fourier[i*2 + 3], magnitude);
-        cv::add(energy1Secondary, magnitude, energy1Secondary, cv::noArray(), CV_32F);
+        cv::add(energy, magnitude, energy, 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
+    frames.clear();
-        cvtools::writeMat(up1, "up1.mat", "up1");
-    #endif
+}
-    }
-    }
-//    // Erode occlusion masks
-//    cv::Mat strel = cv::getStructuringElement(cv::MORPH_ELLIPSE, cv::Size(5,5));
+void collectPhases(const cv::Mat& phasePrimary, const cv::Mat& phaseSecondary,
-//    cv::erode(occlusion0, occlusion0, strel);
-//    cv::erode(occlusion1, occlusion1, strel);
-    // Threshold on gradient of phase
+                   cv::Mat& phase) {
-    cv::Mat edges0;
-    cv::Sobel(up0, edges0, -1, 1, 1, 5);
+    cv::Mat phaseEquivalent = phaseSecondary - phasePrimary;
-    occlusion0 = occlusion0 & (abs(edges0) < 10);
+    phaseEquivalent = cvtools::modulo(phaseEquivalent, 2.0*CV_PI);
-    cv::Mat edges1;
-    cv::Sobel(up1, edges1, -1, 1, 1, 5);
+    phase = unwrapWithCue(phasePrimary, phaseEquivalent, nPeriodsPrimary);
-    occlusion1 = occlusion1 & (abs(edges1) < 10);
+    phase *= phasePrimary.cols/(2.0*CV_PI);
+}
-    #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
+void matchPhaseMaps(const cv::Mat& occlusion0, const cv::Mat& occlusion1,
-    // camera0 against camera1
+                    const cv::Mat& phase0, const cv::Mat& phase1,
-    std::vector<cv::Vec2f> q0, q1;
+                    std::vector<cv::Vec2f>& q0, std::vector<cv::Vec2f>& q1) {
     #pragma omp parallel for
-    for(int row=0; row<frameRectRows; row++){
+    for(int row=0; row<occlusion0.rows; row++){
-        for(int col=0; col<frameRectCols; col++){
+        for(int col=0; col<occlusion0.cols; col++){
             if(!occlusion0.at<char>(row,col))
                 continue;
-            float up0i = up0.at<float>(row,col);
+            float phase0i = phase0.at<float>(row,col);
-            for(int col1=0; col1<up1.cols-1; col1++){
+            for(int col1=0; col1<phase1.cols-1; col1++){
                 if(!occlusion1.at<char>(row,col1) || !occlusion1.at<char>(row,col1+1))
                     continue;
-                float up1Left = up1.at<float>(row,col1);
+                float phase1Left = phase1.at<float>(row,col1);
-                float up1Right = up1.at<float>(row,col1+1);
+                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((up1Left <= up0i) && (up0i <= up1Right) && (up0i-up1Left < 1.0) && (up1Right-up0i < 1.0) && (up1Right-up1Left > 0.1)){
+                if(match){
-                    float col1i = col1 + (up0i-up1Left)/(up1Right-up1Left);
+                    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;
+                }
+            }
+        }
+    }
-    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(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;
-    }
+}
+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;
-//    // 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);
+    // cvtools::matToPoints3f(QMat, Q);
     // Triangulate by means of disparity projection
     Q.resize(q0.size());
-    cv::Matx44f QRectx = cv::Matx44f(QRect);
+    cv::Matx44f QRectx = cv::Matx44f(stereoRect.QRect);
-    cv::Matx33f R0invx = cv::Matx33f(cv::Mat(R0.t()));
+    cv::Matx33f R0invx = cv::Matx33f(cv::Mat(stereoRect.R0.t()));
     #pragma omp parallel for
-    for(unsigned int i=0; i<q0.size(); i++){
+    for(unsigned int i=0; i < q0.size(); i++){
-        float disparity = q0[i][0]-q1[i][0];
+        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];
+        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 231 → 233