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


#include "AlgorithmPhaseShiftThreeFreq.h"
#include <math.h>
 
#include "cvtools.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 = 16; // number of shifts/steps in secondary
static unsigned int nStepsTertiary = 16; // number of shifts/steps in tertiary
static float periodPrimary = 24; // primary period
static float periodSecondary = 30; // 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;
}
 
AlgorithmPhaseShiftThreeFreq::AlgorithmPhaseShiftThreeFreq(unsigned int _screenCols, unsigned int _screenRows) : Algorithm(_screenCols, _screenRows){
 
    // Set N
    N = 2+nStepsPrimary+nStepsSecondary+nStepsTertiary;
 
    // Determine the tertiary period to fulfill the heterodyne condition
    float periodTertiary = (screenCols*periodPrimary*periodSecondary)/(periodPrimary*periodSecondary+2*screenCols*periodPrimary-screenCols*periodSecondary);
 
    // 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);
 
    // Precompute encoded patterns
    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;
        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 = periodSecondary;
        cv::Mat patternI(1,1,CV_8U);
        patternI = computePhaseVector(screenCols, phase, pitch);
        patterns.push_back(patternI.t());
    }
    // Tertiary encoding patterns
    for(unsigned int i=0; i<nStepsTertiary; i++){
        float phase = 2.0*pi/nStepsTertiary * i;
        float pitch = periodTertiary;
        cv::Mat patternI(1,1,CV_8U);
        patternI = computePhaseVector(screenCols, phase, pitch);
        patterns.push_back(patternI.t());
    }
 
}
 
cv::Mat AlgorithmPhaseShiftThreeFreq::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 AlgorithmPhaseShiftThreeFreq::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);
    assert(frames1.size() == N);
 
    int frameRows = frames0[0].rows;
    int frameCols = frames0[0].cols;
 
    // Gray-scale everything
    std::vector<cv::Mat> frames0Gray(N);
    std::vector<cv::Mat> frames1Gray(N);
    for(int i=0; i<N; i++){
        cv::cvtColor(frames0[i], frames0Gray[i], CV_BayerBG2GRAY);
        cv::cvtColor(frames1[i], frames1Gray[i], CV_BayerBG2GRAY);
    }
 
    // Decode camera0
    std::vector<cv::Mat> frames0Primary(frames0Gray.begin()+2, frames0Gray.begin()+2+nStepsPrimary);
    std::vector<cv::Mat> frames0Secondary(frames0Gray.begin()+2+nStepsPrimary, frames0Gray.end()-nStepsTertiary);
    std::vector<cv::Mat> frames0Tertiary(frames0Gray.end()-nStepsTertiary, frames0Gray.end());
 
    std::vector<cv::Mat> F0Primary = getDFTComponents(frames0Primary);
    cv::Mat up0Primary;
    cv::phase(F0Primary[2], -F0Primary[3], up0Primary);
    std::vector<cv::Mat> F0Secondary = getDFTComponents(frames0Secondary);
    cv::Mat up0Secondary;
    cv::phase(F0Secondary[2], -F0Secondary[3], up0Secondary);
    std::vector<cv::Mat> F0Tertiary = getDFTComponents(frames0Tertiary);
    cv::Mat up0Tertiary;
    cv::phase(F0Tertiary[2], -F0Tertiary[3], up0Tertiary);
 
    cv::Mat up0EquivalentPS = up0Primary - up0Secondary;
    up0EquivalentPS = cvtools::modulo(up0EquivalentPS, 2.0*pi);
 
    cv::Mat up0EquivalentST = up0Secondary - up0Tertiary;
    up0EquivalentST = cvtools::modulo(up0EquivalentST, 2.0*pi);
 
    cv::Mat up0Equivalent = up0EquivalentPS - up0EquivalentST;
    up0Equivalent = cvtools::modulo(up0Equivalent, 2.0*pi);
 
 
 
    cv::Mat up0 = unwrapWithCue(up0Primary, up0Equivalent, (float)screenCols/periodPrimary);
    up0 *= screenCols/(2.0*pi);
    cv::Mat amplitude0;
    cv::magnitude(F0Primary[2], -F0Primary[3], amplitude0);
 
    // Decode camera1
    std::vector<cv::Mat> frames1Primary(frames1Gray.begin()+2, frames1Gray.begin()+2+nStepsPrimary);
    std::vector<cv::Mat> frames1Secondary(frames1Gray.begin()+2+nStepsPrimary, frames1Gray.end()-nStepsTertiary);
    std::vector<cv::Mat> frames1Tertiary(frames1Gray.end()-nStepsTertiary, frames1Gray.end());
 
    std::vector<cv::Mat> F1Primary = getDFTComponents(frames1Primary);
    cv::Mat up1Primary;
    cv::phase(F1Primary[2], -F1Primary[3], up1Primary);
    std::vector<cv::Mat> F1Secondary = getDFTComponents(frames1Secondary);
    cv::Mat up1Secondary;
    cv::phase(F1Secondary[2], -F1Secondary[3], up1Secondary);
    std::vector<cv::Mat> F1Tertiary = getDFTComponents(frames1Tertiary);
    cv::Mat up1Tertiary;
    cv::phase(F1Tertiary[2], -F1Tertiary[3], up1Tertiary);
 
    cv::Mat up1EquivalentPS = up1Primary - up1Secondary;
    up1EquivalentPS = cvtools::modulo(up1EquivalentPS, 2.0*pi);
 
    cv::Mat up1EquivalentST = up1Secondary - up1Tertiary;
    up1EquivalentST = cvtools::modulo(up1EquivalentST, 2.0*pi);
 
    cv::Mat up1Equivalent = up1EquivalentPS - up1EquivalentST;
    up1Equivalent = cvtools::modulo(up1Equivalent, 2.0*pi);
 
    cv::Mat up1 = unwrapWithCue(up1Primary, up1Equivalent, (float)screenCols/periodPrimary);
    up1 *= screenCols/(2.0*pi);
    cv::Mat amplitude1;
    cv::magnitude(F1Primary[2], -F1Primary[3], amplitude1);
 
//cvtools::writeMat(up0Primary, "up0Primary.mat", "up0Primary");
//cvtools::writeMat(up0Secondary, "up0Secondary.mat", "up0Secondary");
//cvtools::writeMat(up0Tertiary, "up0Tertiary.mat", "up0Tertiary");
//cvtools::writeMat(up0Equivalent, "up0Equivalent.mat", "up0Equivalent");
cvtools::writeMat(up0Equivalent2, "up0Equivalent2.mat", "up0Equivalent2");
//cvtools::writeMat(up0EquivalentPS, "up0EquivalentPS.mat", "up0EquivalentPS");
//cvtools::writeMat(up0EquivalentST, "up0EquivalentST.mat", "up0EquivalentST");
//cvtools::writeMat(up0, "up0.mat", "up0");
//cvtools::writeMat(up1, "up1.mat", "up1");
//cvtools::writeMat(amplitude0, "amplitude0.mat", "amplitude0");
 
    // 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 R0, R1, P0, P1, QRect;
    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::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);
 
    // Phase remaps
    cv::Mat up0Rect, up1Rect;
    cv::remap(up0, up0Rect, map0X, map0Y, CV_INTER_LINEAR);
    cv::remap(up1, up1Rect, map1X, map1Y, CV_INTER_LINEAR);
 
    // amplitude remaps
    cv::Mat amplitude0Rect, amplitude1Rect;
    cv::remap(amplitude0, amplitude0Rect, map0X, map0Y, CV_INTER_LINEAR);
    cv::remap(amplitude1, amplitude1Rect, map1X, map1Y, CV_INTER_LINEAR);
 
//cvtools::writeMat(up0Rect, "up0Rect.mat", "up0Rect");
//cvtools::writeMat(up1Rect, "up1Rect.mat", "up1Rect");
 
    // color debayer and remap
    cv::Mat color0Rect, color1Rect;
//    frames0[0].convertTo(color0Rect, CV_8UC1, 1.0/256.0);
    cv::cvtColor(frames0[0], color0Rect, CV_BayerBG2RGB);
    cv::remap(color0Rect, color0Rect, map0X, map0Y, CV_INTER_LINEAR);
 
//    frames1[0].convertTo(color1Rect, CV_8UC1, 1.0/256.0);
    cv::cvtColor(frames1[0], color1Rect, CV_BayerBG2RGB);
    cv::remap(color1Rect, color1Rect, map1X, map1Y, CV_INTER_LINEAR);
 
//cvtools::writeMat(frames0Rect[18], "frames0Rect_18.mat", "frames0Rect_18");
//cvtools::writeMat(frames0Rect[19], "frames0Rect_19.mat", "frames0Rect_19");
 
//cvtools::writeMat(color0Rect, "color0Rect.mat", "color0Rect");
//cvtools::writeMat(color1Rect, "color1Rect.mat", "color1Rect");
 
    // On/off remaps
    cv::Mat frames0OnRect, frames0OffRect;
    cv::remap(frames0Gray[0], frames0OnRect, map0X, map0Y, CV_INTER_LINEAR);
    cv::remap(frames0Gray[1], frames0OffRect, map0X, map0Y, CV_INTER_LINEAR);
 
    cv::Mat frames1OnRect, frames1OffRect;
    cv::remap(frames1Gray[0], frames1OnRect, map1X, map1Y, CV_INTER_LINEAR);
    cv::remap(frames1Gray[1], frames1OffRect, map1X, map1Y, CV_INTER_LINEAR);
 
    // Occlusion masks
    cv::Mat occlusion0Rect, occlusion1Rect;
    cv::subtract(frames0OnRect, frames0OffRect, occlusion0Rect);
    occlusion0Rect = (occlusion0Rect > 25) & (occlusion0Rect < 250);
    cv::subtract(frames1OnRect, frames1OffRect, occlusion1Rect);
    occlusion1Rect = (occlusion1Rect > 25) & (occlusion1Rect < 250);
 
    // Threshold on energy at primary frequency
    occlusion0Rect = occlusion0Rect & (amplitude0Rect > 5.0*nStepsPrimary);
    occlusion1Rect = occlusion1Rect & (amplitude1Rect > 5.0*nStepsPrimary);
 
//cvtools::writeMat(occlusion0Rect, "occlusion0Rect.mat", "occlusion0Rect");
//cvtools::writeMat(occlusion1Rect, "occlusion1Rect.mat", "occlusion1Rect");
 
    // Erode occlusion masks
    cv::Mat strel = cv::getStructuringElement(cv::MORPH_ELLIPSE, cv::Size(5,5));
    cv::erode(occlusion0Rect, occlusion0Rect, strel);
    cv::erode(occlusion1Rect, occlusion1Rect, strel);
 
    // Threshold on gradient of phase
    cv::Mat edges0;
    cv::Sobel(up0Rect, edges0, -1, 1, 1, 5);
    occlusion0Rect = occlusion0Rect & (abs(edges0) < 150);
 
    cv::Mat edges1;
    cv::Sobel(up1Rect, edges1, -1, 1, 1, 5);
    occlusion1Rect = occlusion1Rect & (abs(edges1) < 150);
 
//cvtools::writeMat(edges0, "edges0.mat", "edges0");
//cvtools::writeMat(edges1, "edges1.mat", "edges1");
 
    // Match phase maps
    int frameRectRows = map0X.rows;
    int frameRectCols = map0X.cols;
 
    // camera0 against camera1
    std::vector<cv::Vec2f> q0Rect, q1Rect;
    for(int row=0; row<frameRectRows; row++){
        for(int col=0; col<frameRectCols; col++){
 
            if(!occlusion0Rect.at<char>(row,col))
                continue;
 
            float up0i = up0Rect.at<float>(row,col);
            for(int col1=0; col1<up1Rect.cols-1; col1++){
 
                if(!occlusion1Rect.at<char>(row,col1) || !occlusion1Rect.at<char>(row,col1+1))
                    continue;
 
                float up1Left = up1Rect.at<float>(row,col1);
                float up1Right = up1Rect.at<float>(row,col1+1);
 
                if((up1Left <= up0i) && (up0i <= up1Right) && (up0i-up1Left < 0.5) && (up1Right-up0i < 0.5)){
 
                    float col1i = col1 + (up0i-up1Left)/(up1Right-up1Left);
 
                    q0Rect.push_back(cv::Point2f(col, row));
                    q1Rect.push_back(cv::Point2f(col1i, row));
 
                    break;
                }
            }
        }
    }
 
//    // camera1 against camera0
//    for(int row=0; row<frameRectRows; row++){
//        for(int col=0; col<frameRectCols; col++){
 
//            if(!occlusion1Rect.at<char>(row,col))
//                continue;
 
//            float up1i = up1Rect.at<float>(row,col);
//            for(int col0=0; col0<up0Rect.cols-1; col0++){
 
//                if(!occlusion0Rect.at<char>(row,col0) || !occlusion0Rect.at<char>(row,col0+1))
//                    continue;
 
//                float up0Left = up0Rect.at<float>(row,col0);
//                float up0Right = up0Rect.at<float>(row,col0+1);
 
//                if((up0Left <= up1i) && (up1i <= up0Right) && (up1i-up0Left < 1) && (up0Right-up1i < 1)){
 
//                    float col0i = col0 + (up1i-up0Left)/(up0Right-up0Left);
 
//                    q1Rect.push_back(cv::Point2f(col, row));
//                    q0Rect.push_back(cv::Point2f(col0i, row));
 
//                    break;
//                }
//            }
//        }
//    }
 
    int nMatches = q0Rect.size();
 
    if(nMatches < 1){
        Q.resize(0);
        color.resize(0);
 
        return;
    }
 
    // Retrieve color information
    color.resize(nMatches);
    for(int i=0; i<nMatches; i++){
 
        cv::Vec3b c0 = color0Rect.at<cv::Vec3b>(q0Rect[i][1], q0Rect[i][0]);
        cv::Vec3b c1 = color1Rect.at<cv::Vec3b>(q1Rect[i][1], q1Rect[i][0]);
 
        color[i] = 0.5*c0 + 0.5*c1;
    }
 
    // Triangulate points
    cv::Mat QMatHomogenous, QMat;
    cv::triangulatePoints(P0, P1, q0Rect, q1Rect, 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);
 
}
 

Subversion Repositories seema-scanner

(root)/src/algorithm/AlgorithmPhaseShiftThreeFreq.cpp – Rev 129 → 130

Rev 129		Rev 130
1	`#include "AlgorithmPhaseShiftThreeFreq.h"`	1	`#include "AlgorithmPhaseShiftThreeFreq.h"`
2	`#include <math.h>`	2	`#include <math.h>`
3		3
4	`#include "cvtools.h"`	4	`#include "cvtools.h"`
5		5
6	`#ifndef M_PI`	6	`#ifndef M_PI`
7	`#define M_PI 3.14159265358979323846`	7	`#define M_PI 3.14159265358979323846`
8	`#endif`	8	`#endif`
9		9
10	`static unsigned int nStepsPrimary = 16; // number of shifts/steps in primary`	10	`static unsigned int nStepsPrimary = 16; // number of shifts/steps in primary`
11	`static unsigned int nStepsSecondary = 16; // number of shifts/steps in secondary`	11	`static unsigned int nStepsSecondary = 16; // number of shifts/steps in secondary`
12	`static unsigned int nStepsTertiary = 16; // number of shifts/steps in tertiary`	12	`static unsigned int nStepsTertiary = 16; // number of shifts/steps in tertiary`
13	`static float periodPrimary = 24; // primary period`	13	`static float periodPrimary = 24; // primary period`
14	`static float periodSecondary = 30; // primary period`	14	`static float periodSecondary = 30; // primary period`
15		15
16	`// Algorithm`	16	`// Algorithm`
17	`static cv::Mat computePhaseVector(unsigned int length, float phase, float pitch){`	17	`static cv::Mat computePhaseVector(unsigned int length, float phase, float pitch){`
18		18
19	`cv::Mat phaseVector(length, 1, CV_8UC3);`	19	`cv::Mat phaseVector(length, 1, CV_8UC3);`
20	`//phaseVector.setTo(0);`	20	`//phaseVector.setTo(0);`
21		21
22	`const float pi = M_PI;`	22	`const float pi = M_PI;`
23		23
24	`// Loop through vector`	24	`// Loop through vector`
25	`for(int i=0; i<phaseVector.rows; i++){`	25	`for(int i=0; i<phaseVector.rows; i++){`
26	`// Amplitude of channels`	26	`// Amplitude of channels`
27	`float amp = 0.5(1+cos(2pi*i/pitch - phase));`	27	`float amp = 0.5(1+cos(2pi*i/pitch - phase));`
28	`phaseVector.at<cv::Vec3b>(i, 0) = cv::Vec3b(255.0amp,255.0amp,255.0*amp);`	28	`phaseVector.at<cv::Vec3b>(i, 0) = cv::Vec3b(255.0amp,255.0amp,255.0*amp);`
29	`}`	29	`}`
30		30
31	`return phaseVector;`	31	`return phaseVector;`
32	`}`	32	`}`
33		33
34	`AlgorithmPhaseShiftThreeFreq::AlgorithmPhaseShiftThreeFreq(unsigned int _screenCols, unsigned int _screenRows) : Algorithm(_screenCols, _screenRows){`	34	`AlgorithmPhaseShiftThreeFreq::AlgorithmPhaseShiftThreeFreq(unsigned int _screenCols, unsigned int _screenRows) : Algorithm(_screenCols, _screenRows){`
35		35
36	`// Set N`	36	`// Set N`
37	`N = 2+nStepsPrimary+nStepsSecondary+nStepsTertiary;`	37	`N = 2+nStepsPrimary+nStepsSecondary+nStepsTertiary;`
38		38
39	`// Determine the tertiary period to fulfill the heterodyne condition`	39	`// Determine the tertiary period to fulfill the heterodyne condition`
40	`float periodTertiary = (screenColsperiodPrimaryperiodSecondary)/(periodPrimaryperiodSecondary+2screenColsperiodPrimary-screenColsperiodSecondary);`	40	`float periodTertiary = (screenColsperiodPrimaryperiodSecondary)/(periodPrimaryperiodSecondary+2screenColsperiodPrimary-screenColsperiodSecondary);`