Subversion Repositories seema-scanner

Rev

Rev 236 | Go to most recent revision | Only display areas with differences | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 236 Rev 245
1 
//
 1 
//
2 
// Gray Code Structured Light
 2 
// Gray Code Structured Light
3 
//
 3 
//
4 
// This implementation closely follows Henrik Aanaes, "Lecture Notes on Computer Vision" (2014).
 4 
// This implementation closely follows Henrik Aanaes, "Lecture Notes on Computer Vision" (2014).
5 
//
 5 
//
6 
 
 6 
 
7 
#include "AlgorithmGrayCode.h"
 7 
#include "AlgorithmGrayCode.h"
8 
#include <cmath>
 8 
#include <cmath>
9 
#include "cvtools.h"
 9 
#include "cvtools.h"
10 
#include "algorithmtools.h"
 10 
#include "algorithmtools.h"
11 
 
 11 
 
12 
// Algorithm
 12 
// Algorithm
13 
AlgorithmGrayCode::AlgorithmGrayCode(unsigned int _screenCols, unsigned int _screenRows) : Algorithm(_screenCols, _screenRows){
 13 
AlgorithmGrayCode::AlgorithmGrayCode(unsigned int _screenCols, unsigned int _screenRows) : Algorithm(_screenCols, _screenRows){
14 
 
 14 
 
15 
    Nbits = ceilf(log2f((float)screenCols)) - 1;
 15 
    Nbits = ceilf(log2f((float)screenCols)) - 1;
16 
    N = 2 + Nbits*2;
 16 
    N = 2 + Nbits*2;
17 
 
 17 
 
18 
    // all on pattern
 18 
    // all on pattern
19 
    cv::Mat allOn(1, screenCols, CV_8UC3, cv::Scalar::all(255));
 19 
    cv::Mat allOn(1, screenCols, CV_8UC3, cv::Scalar::all(255));
20 
    patterns.push_back(allOn);
 20 
    patterns.push_back(allOn);
21 
 
 21 
 
22 
    // all off pattern
 22 
    // all off pattern
23 
    cv::Mat allOff(1, screenCols, CV_8UC3, cv::Scalar::all(0));
 23 
    cv::Mat allOff(1, screenCols, CV_8UC3, cv::Scalar::all(0));
24 
    patterns.push_back(allOff);
 24 
    patterns.push_back(allOff);
25 
 
 25 
 
26 
 
 26 
 
27 
    // horizontally encoding patterns
 27 
    // horizontally encoding patterns
28 
    for(unsigned int p=0; p<Nbits; p++){
 28 
    for(unsigned int p=0; p<Nbits; p++){
29 
        cv::Mat pattern(1, screenCols, CV_8UC3);
 29 
        cv::Mat pattern(1, screenCols, CV_8UC3);
30 
        cv::Mat patternInv(1, screenCols, CV_8UC3);
 30 
        cv::Mat patternInv(1, screenCols, CV_8UC3);
31 
 
 31 
 
32 
        for(unsigned int j=0; j<screenCols; j++){
 32 
        for(unsigned int j=0; j<screenCols; j++){
33 
 
 33 
 
34 
            unsigned int jGray = binaryToGray(j);
 34 
            unsigned int jGray = binaryToGray(j);
35 
            // Amplitude of channels
 35 
            // Amplitude of channels
36 
            int bit = (int)getBit(jGray, Nbits-p+1);
 36 
            int bit = (int)getBit(jGray, Nbits-p+1);
37 
            pattern.at<cv::Vec3b>(0,j) = cv::Vec3b(255.0*bit,255.0*bit,255.0*bit);
 37 
            pattern.at<cv::Vec3b>(0,j) = cv::Vec3b(255.0*bit,255.0*bit,255.0*bit);
38 
            int invBit = bit^1;
 38 
            int invBit = bit^1;
39 
            patternInv.at<cv::Vec3b>(0,j) = cv::Vec3b(255.0*invBit,255.0*invBit,255.0*invBit);
 39 
            patternInv.at<cv::Vec3b>(0,j) = cv::Vec3b(255.0*invBit,255.0*invBit,255.0*invBit);
40 
        }
 40 
        }
41 
        patterns.push_back(pattern);
 41 
        patterns.push_back(pattern);
42 
        patterns.push_back(patternInv);
 42 
        patterns.push_back(patternInv);
43 
    }
 43 
    }
44 
 
 44 
 
45 
 
 45 
 
46 
}
 46 
}
47 
 
 47 
 
48 
cv::Mat AlgorithmGrayCode::getEncodingPattern(unsigned int depth){
 48 
cv::Mat AlgorithmGrayCode::getEncodingPattern(unsigned int depth){
49 
    return patterns[depth];
 49 
    return patterns[depth];
50 
}
 50 
}
51 
 
 51 
 
52 
 
 52 
 
53 
bool sortingLarger(cv::Vec4i i,cv::Vec4i j){ return (i[3]<j[3]);}
 53 
bool sortingLarger(cv::Vec4i i,cv::Vec4i j){ return (i[3]<j[3]);}
54 
bool sortingEqual(cv::Vec4i i,cv::Vec4i j){ return (i[3]==j[3]);}
 54 
bool sortingEqual(cv::Vec4i i,cv::Vec4i j){ return (i[3]==j[3]);}
55 
void getEdgeLabels(const cv::Mat& scanLine, int Nbits, std::vector<cv::Vec4i>& edges){
 55 
void getEdgeLabels(const cv::Mat& scanLine, int Nbits, std::vector<cv::Vec4i>& edges){
56 
 
 56 
 
57 
    int nCols = scanLine.cols;
 57 
    int nCols = scanLine.cols;
58 
    const int *data = scanLine.ptr<const int>(0);
 58 
    const int *data = scanLine.ptr<const int>(0);
59 
 
 59 
 
60 
    int labelLeft;
 60 
    int labelLeft;
61 
    int labelRight = data[0];
 61 
    int labelRight = data[0];
62 
 
 62 
 
63 
    // collect edges
 63 
    // collect edges
64 
    for(int col=1; col<nCols; col++){
 64 
    for(int col=1; col<nCols; col++){
65 
 
 65 
 
66 
        labelLeft = labelRight;
 66 
        labelLeft = labelRight;
67 
        labelRight = data[col];
 67 
        labelRight = data[col];
68 
 
 68 
 
69 
        // labels need to be non-background, and differ in exactly one bit
 69 
        // labels need to be non-background, and differ in exactly one bit
70 
        if(labelLeft != -1 && labelRight != -1 && (grayToBinary(labelRight) == grayToBinary(labelLeft)+1)){
 70 
        if(labelLeft != -1 && labelRight != -1 && (grayToBinary(labelRight) == grayToBinary(labelLeft)+1)){
71 
            int orderingRelation = (labelLeft << Nbits) + labelRight;
 71 
            int orderingRelation = (labelLeft << Nbits) + labelRight;
72 
            // store left label column
 72 
            // store left label column
73 
            edges.push_back(cv::Vec4i(col-1, labelLeft, labelRight, orderingRelation));
 73 
            edges.push_back(cv::Vec4i(col-1, labelLeft, labelRight, orderingRelation));
74 
        }
 74 
        }
75 
    }
 75 
    }
76 
 
 76 
 
77 
    // sort
 77 
    // sort
78 
    std::sort(edges.begin(), edges.end(), sortingLarger);
 78 
    std::sort(edges.begin(), edges.end(), sortingLarger);
79 
 
 79 
 
80 
    // remove duplicates
 80 
    // remove duplicates
81 
    std::vector<cv::Vec4i>::iterator it;
 81 
    std::vector<cv::Vec4i>::iterator it;
82 
    it = std::unique(edges.begin(), edges.end(), sortingEqual);
 82 
    it = std::unique(edges.begin(), edges.end(), sortingEqual);
83 
    edges.resize(std::distance(edges.begin(),it));
 83 
    edges.resize(std::distance(edges.begin(),it));
84 
}
 84 
}
85 
 
 85 
 
86 
void AlgorithmGrayCode::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){
 86 
void AlgorithmGrayCode::get3DPoints(const SMCalibrationParameters &calibration, const std::vector<cv::Mat>& frames0, const std::vector<cv::Mat>& frames1, std::vector<cv::Point3f>& Q, std::vector<cv::Vec3f>& color){
87 
 
 87 
 
88 
    assert(frames0.size() == N);
 88 
    assert(frames0.size() == N);
89 
    assert(frames1.size() == N);
 89 
    assert(frames1.size() == N);
90 
 
 90 
 
91 
    int frameRows = frames0[0].rows;
 91 
    int frameRows = frames0[0].rows;
92 
    int frameCols = frames0[0].cols;
 92 
    int frameCols = frames0[0].cols;
93 
 
 93 
 
94 
    // rectifying homographies (rotation+projections)
 94 
    // rectifying homographies (rotation+projections)
95 
    cv::Size frameSize(frameCols, frameRows);
 95 
    cv::Size frameSize(frameCols, frameRows);
96 
    cv::Mat R, T;
 96 
    cv::Mat R, T;
97 
    // stereoRectify segfaults unless R is double precision
 97 
    // stereoRectify segfaults unless R is double precision
98 
    cv::Mat(calibration.R1).convertTo(R, CV_64F);
 98 
    cv::Mat(calibration.R1).convertTo(R, CV_64F);
99 
    cv::Mat(calibration.T1).convertTo(T, CV_64F);
 99 
    cv::Mat(calibration.T1).convertTo(T, CV_64F);
100 
    cv::Mat R0, R1, P0, P1, QRect;
 100 
    cv::Mat R0, R1, P0, P1, QRect;
101 
    cv::stereoRectify(calibration.K0, calibration.k0, calibration.K1, calibration.k1, frameSize, R, T, R0, R1, P0, P1, QRect, 0);
 101 
    cv::stereoRectify(calibration.K0, calibration.k0, calibration.K1, calibration.k1, frameSize, R, T, R0, R1, P0, P1, QRect, 0);
102 
 
 102 
 
103 
    // interpolation maps
 103 
    // interpolation maps
104 
    cv::Mat map0X, map0Y, map1X, map1Y;
 104 
    cv::Mat map0X, map0Y, map1X, map1Y;
105 
    cv::initUndistortRectifyMap(calibration.K0, calibration.k0, R0, P0, frameSize, CV_32F, map0X, map0Y);
 105 
    cv::initUndistortRectifyMap(calibration.K0, calibration.k0, R0, P0, frameSize, CV_32F, map0X, map0Y);
106 
    cv::initUndistortRectifyMap(calibration.K1, calibration.k1, R1, P1, frameSize, CV_32F, map1X, map1Y);
 106 
    cv::initUndistortRectifyMap(calibration.K1, calibration.k1, R1, P1, frameSize, CV_32F, map1X, map1Y);
107 
 
 107 
 
108 
    // gray-scale and remap
 108 
    // gray-scale and remap
109 
    std::vector<cv::Mat> frames0Rect(N);
 109 
    std::vector<cv::Mat> frames0Rect(N);
110 
    std::vector<cv::Mat> frames1Rect(N);
 110 
    std::vector<cv::Mat> frames1Rect(N);
111 
    for(unsigned int i=0; i<N; i++){
 111 
    for(unsigned int i=0; i<N; i++){
112 
        cv::Mat temp;
 112 
        cv::Mat temp;
113 
        cv::cvtColor(frames0[i], temp, CV_BayerBG2GRAY);
 113 
        cv::cvtColor(frames0[i], temp, CV_RGB2GRAY);
114 
        cv::remap(temp, frames0Rect[i], map0X, map0Y, CV_INTER_LINEAR);
 114 
        cv::remap(temp, frames0Rect[i], map0X, map0Y, CV_INTER_LINEAR);
115 
        cv::cvtColor(frames1[i], temp, CV_BayerBG2GRAY);
 115 
        cv::cvtColor(frames1[i], temp, CV_RGB2GRAY);
116 
        cv::remap(temp, frames1Rect[i], map1X, map1Y, CV_INTER_LINEAR);
 116 
        cv::remap(temp, frames1Rect[i], map1X, map1Y, CV_INTER_LINEAR);
117 
    }
 117 
    }
118 
 
 118 
 
119 
    #ifdef SM_DEBUG
 119 
    #ifdef QT_DEBUG
120 
        cvtools::writeMat(frames0Rect[0], "frames0Rect_0.mat", "frames0Rect_0");
 120 
        cvtools::writeMat(frames0Rect[0], "frames0Rect_0.mat", "frames0Rect_0");
121 
        cvtools::writeMat(frames0[0], "frames0_0.mat", "frames0_0");
 121 
        cvtools::writeMat(frames0[0], "frames0_0.mat", "frames0_0");
122 
 
 122 
 
123 
        cvtools::writeMat(frames0Rect[22], "frames0Rect_22.mat", "frames0Rect_22");
 123 
        cvtools::writeMat(frames0Rect[22], "frames0Rect_22.mat", "frames0Rect_22");
124 
        cvtools::writeMat(frames0Rect[23], "frames0Rect_23.mat", "frames0Rect_23");
 124 
        cvtools::writeMat(frames0Rect[23], "frames0Rect_23.mat", "frames0Rect_23");
125 
 
 125 
 
126 
        cv::imwrite("frames0[0].png", frames0[0]);
 126 
        cv::imwrite("frames0[0].png", frames0[0]);
127 
        cv::imwrite("frames0Rect[0].png", frames0Rect[0]);
 127 
        cv::imwrite("frames0Rect[0].png", frames0Rect[0]);
128 
 
 128 
 
129 
        cv::imwrite("frames1[0].png", frames1[0]);
 129 
        cv::imwrite("frames1[0].png", frames1[0]);
130 
        cv::imwrite("frames1Rect[0].png", frames1Rect[0]);
 130 
        cv::imwrite("frames1Rect[0].png", frames1Rect[0]);
131 
    #endif
 131 
    #endif
132 
 
 132 
 
133 
    // color debayer and remap
 133 
    // color remap
134 
    cv::Mat color0Rect, color1Rect;
 134 
    cv::Mat color0Rect, color1Rect;
135 
    cv::cvtColor(frames0[0], color0Rect, CV_BayerBG2RGB);
 - 
 
136 
    cv::remap(color0Rect, color0Rect, map0X, map0Y, CV_INTER_LINEAR);
 135 
    cv::remap(frames0[0], color0Rect, map0X, map0Y, CV_INTER_LINEAR);
137 
 
 - 
 
138 
    cv::cvtColor(frames1[0], color1Rect, CV_BayerBG2RGB);
 - 
 
139 
    cv::remap(color1Rect, color1Rect, map1X, map1Y, CV_INTER_LINEAR);
 136 
    cv::remap(frames1[0], color1Rect, map1X, map1Y, CV_INTER_LINEAR);
140 
 
 137 
 
141 
    int frameRectRows = frames0Rect[0].rows;
 138 
    int frameRectRows = frames0Rect[0].rows;
142 
    int frameRectCols = frames0Rect[0].cols;
 139 
    int frameRectCols = frames0Rect[0].cols;
143 
 
 140 
 
144 
    // occlusion masks
 141 
    // occlusion masks
145 
    cv::Mat occlusion0Rect, occlusion1Rect;
 142 
    cv::Mat occlusion0Rect, occlusion1Rect;
146 
    cv::subtract(frames0Rect[0], frames0Rect[1], occlusion0Rect);
 143 
    cv::subtract(frames0Rect[0], frames0Rect[1], occlusion0Rect);
147 
    occlusion0Rect = (occlusion0Rect > 10) & (occlusion0Rect < 250);
 144 
    occlusion0Rect = (occlusion0Rect > 0.1) & (occlusion0Rect < 0.99);
148 
    cv::subtract(frames1Rect[0], frames1Rect[1], occlusion1Rect);
 145 
    cv::subtract(frames1Rect[0], frames1Rect[1], occlusion1Rect);
149 
    occlusion1Rect = (occlusion1Rect > 10) & (occlusion1Rect < 250);
 146 
    occlusion1Rect = (occlusion1Rect > 0.1) & (occlusion1Rect < 0.99);
150 
 
 147 
 
151 
    // erode occlusion masks
 148 
    // erode occlusion masks
152 
    cv::Mat strel = cv::getStructuringElement(cv::MORPH_ELLIPSE, cv::Size(2,2));
 149 
    cv::Mat strel = cv::getStructuringElement(cv::MORPH_ELLIPSE, cv::Size(2,2));
153 
    cv::erode(occlusion0Rect, occlusion0Rect, strel);
 150 
    cv::erode(occlusion0Rect, occlusion0Rect, strel);
154 
    cv::erode(occlusion1Rect, occlusion1Rect, strel);
 151 
    cv::erode(occlusion1Rect, occlusion1Rect, strel);
155 
 
 152 
 
156 
//    // correct for projector inversion error
 153 
//    // correct for projector inversion error
157 
//    cv::Mat W;
 154 
//    cv::Mat W;
158 
//    cv::add(frames0Rect[0], frames0Rect[1], W, cv::noArray(), CV_32F);
 155 
//    cv::add(frames0Rect[0], frames0Rect[1], W, cv::noArray(), CV_32F);
159 
//    for(int i=2; i<N; i+=2){
 156 
//    for(int i=2; i<N; i+=2){
160 
//        cv::Mat S, E;
 157 
//        cv::Mat S, E;
161 
//        cv::add(frames0Rect[i], frames0Rect[i+1], S, cv::noArray(), CV_32F);
 158 
//        cv::add(frames0Rect[i], frames0Rect[i+1], S, cv::noArray(), CV_32F);
162 
//        cv::subtract(W, S, E, cv::noArray(), CV_32F);
 159 
//        cv::subtract(W, S, E, cv::noArray(), CV_32F);
163 
//        E *= 0.5;
 160 
//        E *= 0.5;
164 
//        cv::add(frames0Rect[i], E, frames0Rect[i], cv::noArray(), CV_16UC1);
 161 
//        cv::add(frames0Rect[i], E, frames0Rect[i], cv::noArray(), CV_16UC1);
165 
//        cv::add(frames0Rect[i+1], E, frames0Rect[i+1], cv::noArray(), CV_16UC1);
 162 
//        cv::add(frames0Rect[i+1], E, frames0Rect[i+1], cv::noArray(), CV_16UC1);
166 
//    }
 163 
//    }
167 
 
 164 
 
168 
//    // correct for texture modulation and ambient
 165 
//    // correct for texture modulation and ambient
169 
//    cv::Mat A0 = frames0Rect[1];
 166 
//    cv::Mat A0 = frames0Rect[1];
170 
//    cv::Mat M0 = frames0Rect[0]-frames0Rect[1];
 167 
//    cv::Mat M0 = frames0Rect[0]-frames0Rect[1];
171 
//    cv::divide(256.0, M0, M0, CV_32F);
 168 
//    cv::divide(256.0, M0, M0, CV_32F);
172 
//    cv::Mat A1 = frames1Rect[1];
 169 
//    cv::Mat A1 = frames1Rect[1];
173 
//    cv::Mat M1 = frames1Rect[0]-frames1Rect[1];
 170 
//    cv::Mat M1 = frames1Rect[0]-frames1Rect[1];
174 
//    cv::divide(256.0, M1, M1, CV_32F);
 171 
//    cv::divide(256.0, M1, M1, CV_32F);
175 
 
 172 
 
176 
//    for(int i=2; i<N; i++){
 173 
//    for(int i=2; i<N; i++){
177 
//        cv::multiply(frames0Rect[i]-A0, M0, frames0Rect[i], 1.0, CV_8UC1);
 174 
//        cv::multiply(frames0Rect[i]-A0, M0, frames0Rect[i], 1.0, CV_8UC1);
178 
//        cv::multiply(frames1Rect[i]-A1, M1, frames1Rect[i], 1.0, CV_8UC1);
 175 
//        cv::multiply(frames1Rect[i]-A1, M1, frames1Rect[i], 1.0, CV_8UC1);
179 
//    }
 176 
//    }
180 
 
 177 
 
181 
    // decode patterns
 178 
    // decode patterns
182 
    cv::Mat code0Rect(frameRectRows, frameRectCols, CV_32S, cv::Scalar(0));
 179 
    cv::Mat code0Rect(frameRectRows, frameRectCols, CV_32S, cv::Scalar(0));
183 
    cv::Mat code1Rect(frameRectRows, frameRectCols, CV_32S, cv::Scalar(0));
 180 
    cv::Mat code1Rect(frameRectRows, frameRectCols, CV_32S, cv::Scalar(0));
184 
 
 181 
 
185 
    // into gray code
 182 
    // into gray code
186 
    for(unsigned int i=0; i<Nbits; i++){
 183 
    for(unsigned int i=0; i<Nbits; i++){
187 
        cv::Mat temp, bit0, bit1;
 184 
        cv::Mat temp, bit0, bit1;
188 
 
 185 
 
189 
        cv::compare(frames0Rect[i*2+2], frames0Rect[i*2+3], temp, cv::CMP_GT);
 186 
        cv::compare(frames0Rect[i*2+2], frames0Rect[i*2+3], temp, cv::CMP_GT);
190 
        temp.convertTo(bit0, CV_32S, 1.0/255.0);
 187 
        temp.convertTo(bit0, CV_32S, 1.0/255.0);
191 
        cv::add(code0Rect, bit0*twopowi(Nbits-i-1), code0Rect, cv::noArray(), CV_32S);
 188 
        cv::add(code0Rect, bit0*twopowi(Nbits-i-1), code0Rect, cv::noArray(), CV_32S);
192 
 
 189 
 
193 
        cv::compare(frames1Rect[i*2+2], frames1Rect[i*2+3], temp, cv::CMP_GT);
 190 
        cv::compare(frames1Rect[i*2+2], frames1Rect[i*2+3], temp, cv::CMP_GT);
194 
        temp.convertTo(bit1, CV_32S, 1.0/255.0);
 191 
        temp.convertTo(bit1, CV_32S, 1.0/255.0);
195 
        cv::add(code1Rect, bit1*twopowi(Nbits-i-1), code1Rect, cv::noArray(), CV_32S);
 192 
        cv::add(code1Rect, bit1*twopowi(Nbits-i-1), code1Rect, cv::noArray(), CV_32S);
196 
    }
 193 
    }
197 
 
 194 
 
198 
//cvtools::writeMat(code0Rect, "code0Rect.mat", "code0Rect");
 195 
//cvtools::writeMat(code0Rect, "code0Rect.mat", "code0Rect");
199 
//cvtools::writeMat(code1Rect, "code1Rect.mat", "code1Rect");
 196 
//cvtools::writeMat(code1Rect, "code1Rect.mat", "code1Rect");
200 
 
 197 
 
201 
 
 198 
 
202 
    #ifdef SM_DEBUG
 199 
    #ifdef QT_DEBUG
203 
        // convert to standard binary
 200 
        // convert to standard binary
204 
        cv::Mat code0Binary(code0Rect.rows, code0Rect.cols, CV_32F);
 201 
        cv::Mat code0Binary(code0Rect.rows, code0Rect.cols, CV_32F);
205 
        cv::Mat code1Binary(code1Rect.rows, code1Rect.cols, CV_32F);
 202 
        cv::Mat code1Binary(code1Rect.rows, code1Rect.cols, CV_32F);
206 
        for(int r=0; r<frameRectRows; r++){
 203 
        for(int r=0; r<frameRectRows; r++){
207 
            for(int c=0; c<frameRectCols; c++){
 204 
            for(int c=0; c<frameRectCols; c++){
208 
                if(code0Rect.at<int>(r,c) != -1)
 205 
                if(code0Rect.at<int>(r,c) != -1)
209 
                    code0Binary.at<float>(r,c) = grayToBinary(code0Rect.at<int>(r,c));
 206 
                    code0Binary.at<float>(r,c) = grayToBinary(code0Rect.at<int>(r,c));
210 
                if(code1Rect.at<int>(r,c) != -1)
 207 
                if(code1Rect.at<int>(r,c) != -1)
211 
                    code1Binary.at<float>(r,c) = grayToBinary(code1Rect.at<int>(r,c));
 208 
                    code1Binary.at<float>(r,c) = grayToBinary(code1Rect.at<int>(r,c));
212 
            }
 209 
            }
213 
        }
 210 
        }
214 
 
 211 
 
215 
        cvtools::writeMat(code0Binary, "code0Binary.mat", "code0Binary");
 212 
        cvtools::writeMat(code0Binary, "code0Binary.mat", "code0Binary");
216 
        cvtools::writeMat(code1Binary, "code1Binary.mat", "code1Binary");
 213 
        cvtools::writeMat(code1Binary, "code1Binary.mat", "code1Binary");
217 
    #endif
 214 
    #endif
218 
 
 215 
 
219 
//    // threshold on vertical discontinuities (due to imperfect rectification)
 216 
//    // threshold on vertical discontinuities (due to imperfect rectification)
220 
//    cv::Mat edges0;
 217 
//    cv::Mat edges0;
221 
//    cv::Sobel(code0Binary, edges0, -1, 0, 1, 5);
 218 
//    cv::Sobel(code0Binary, edges0, -1, 0, 1, 5);
222 
//    occlusion0Rect = occlusion0Rect & (abs(edges0) < 50);
 219 
//    occlusion0Rect = occlusion0Rect & (abs(edges0) < 50);
223 
 
 220 
 
224 
//    cv::Mat edges1;
 221 
//    cv::Mat edges1;
225 
//    cv::Sobel(code1Binary, edges1, -1, 0, 1, 5);
 222 
//    cv::Sobel(code1Binary, edges1, -1, 0, 1, 5);
226 
//    occlusion1Rect = occlusion1Rect & (abs(edges1) < 50);
 223 
//    occlusion1Rect = occlusion1Rect & (abs(edges1) < 50);
227 
 
 224 
 
228 
 
 225 
 
229 
    // set occluded pixels to -1
 226 
    // set occluded pixels to -1
230 
    for(int r=0; r<frameRectRows; r++){
 227 
    for(int r=0; r<frameRectRows; r++){
231 
        for(int c=0; c<frameRectCols; c++){
 228 
        for(int c=0; c<frameRectCols; c++){
232 
            if(occlusion0Rect.at<unsigned char>(r,c) == 0)
 229 
            if(occlusion0Rect.at<unsigned char>(r,c) == 0)
233 
                code0Rect.at<int>(r,c) = -1;
 230 
                code0Rect.at<int>(r,c) = -1;
234 
            if(occlusion1Rect.at<unsigned char>(r,c) == 0)
 231 
            if(occlusion1Rect.at<unsigned char>(r,c) == 0)
235 
                code1Rect.at<int>(r,c) = -1;
 232 
                code1Rect.at<int>(r,c) = -1;
236 
        }
 233 
        }
237 
    }
 234 
    }
238 
 
 235 
 
239 
    #ifdef SM_DEBUG
 236 
    #ifdef QT_DEBUG
240 
        cvtools::writeMat(code0Rect, "code0Rect.mat", "code0Rect");
 237 
        cvtools::writeMat(code0Rect, "code0Rect.mat", "code0Rect");
241 
        cvtools::writeMat*/(code1Rect, "code1Rect.mat", "code1Rect");
 238 
        cvtools::writeMat(code1Rect, "code1Rect.mat", "code1Rect");
242 
    #endif
 239 
    #endif
243 
 
 240 
 
244 
    // matching
 241 
    // matching
245 
    std::vector<cv::Vec2f> q0, q1;
 242 
    std::vector<cv::Vec2f> q0, q1;
246 
    for(int row=0; row<frameRectRows; row++){
 243 
    for(int row=0; row<frameRectRows; row++){
247 
 
 244 
 
248 
        // edge data structure containing [floor(column), labelLeft, labelRight, orderingRelation]
 245 
        // edge data structure containing [floor(column), labelLeft, labelRight, orderingRelation]
249 
        std::vector<cv::Vec4i> edges0, edges1;
 246 
        std::vector<cv::Vec4i> edges0, edges1;
250 
 
 247 
 
251 
        // sorted, unique edges
 248 
        // sorted, unique edges
252 
        getEdgeLabels(code0Rect.row(row), Nbits, edges0);
 249 
        getEdgeLabels(code0Rect.row(row), Nbits, edges0);
253 
        getEdgeLabels(code1Rect.row(row), Nbits, edges1);
 250 
        getEdgeLabels(code1Rect.row(row), Nbits, edges1);
254 
 
 251 
 
255 
        // match edges
 252 
        // match edges
256 
        std::vector<cv::Vec4i> matchedEdges0, matchedEdges1;
 253 
        std::vector<cv::Vec4i> matchedEdges0, matchedEdges1;
257 
        unsigned int i=0, j=0;
 254 
        unsigned int i=0, j=0;
258 
        while(i<edges0.size() && j<edges1.size()){
 255 
        while(i<edges0.size() && j<edges1.size()){
259 
 
 256 
 
260 
            if(edges0[i][3] == edges1[j][3]){
 257 
            if(edges0[i][3] == edges1[j][3]){
261 
                matchedEdges0.push_back(edges0[i]);
 258 
                matchedEdges0.push_back(edges0[i]);
262 
                matchedEdges1.push_back(edges1[j]);
 259 
                matchedEdges1.push_back(edges1[j]);
263 
                i += 1;
 260 
                i += 1;
264 
                j += 1;
 261 
                j += 1;
265 
            } else if(edges0[i][3] < edges1[j][3]){
 262 
            } else if(edges0[i][3] < edges1[j][3]){
266 
                i += 1;
 263 
                i += 1;
267 
            } else if(edges0[i][3] > edges1[j][3]){
 264 
            } else if(edges0[i][3] > edges1[j][3]){
268 
                j += 1;
 265 
                j += 1;
269 
            }
 266 
            }
270 
        }
 267 
        }
271 
 
 268 
 
272 
        // crude subpixel refinement
 269 
        // crude subpixel refinement
273 
        // finds the intersection of linear interpolants in the positive/negative pattern
 270 
        // finds the intersection of linear interpolants in the positive/negative pattern
274 
        for(unsigned int i=0; i<matchedEdges0.size(); i++){
 271 
        for(unsigned int i=0; i<matchedEdges0.size(); i++){
275 
 
 272 
 
276 
            int level = Nbits - leastSignificantBitSet(matchedEdges0[i][1]^matchedEdges0[i][2]);
 273 
            int level = Nbits - leastSignificantBitSet(matchedEdges0[i][1]^matchedEdges0[i][2]);
277 
 
 274 
 
278 
            // refine for camera 0
 275 
            // refine for camera 0
279 
            float c0 = matchedEdges0[i][0];
 276 
            float c0 = matchedEdges0[i][0];
280 
            float c1 = c0+1;
 277 
            float c1 = c0+1;
281 
 
 278 
 
282 
            float pos0 = frames0Rect[2*level+2].at<unsigned char>(row, c0);
 279 
            float pos0 = frames0Rect[2*level+2].at<float>(row, c0);
283 
            float pos1 = frames0Rect[2*level+2].at<unsigned char>(row, c1);
 280 
            float pos1 = frames0Rect[2*level+2].at<float>(row, c1);
284 
            float neg0 = frames0Rect[2*level+3].at<unsigned char>(row, c0);
 281 
            float neg0 = frames0Rect[2*level+3].at<float>(row, c0);
285 
            float neg1 = frames0Rect[2*level+3].at<unsigned char>(row, c1);
 282 
            float neg1 = frames0Rect[2*level+3].at<float>(row, c1);
286 
 
 283 
 
287 
            float col = c0 + (pos0 - neg0)/(neg1 - neg0 - pos1 + pos0);
 284 
            float col = c0 + (pos0 - neg0)/(neg1 - neg0 - pos1 + pos0);
288 
            q0.push_back(cv::Point2f(col, row));
 285 
            q0.push_back(cv::Point2f(col, row));
289 
 
 286 
 
290 
            // refine for camera 1
 287 
            // refine for camera 1
291 
            c0 = matchedEdges1[i][0];
 288 
            c0 = matchedEdges1[i][0];
292 
            c1 = c0+1;
 289 
            c1 = c0+1;
293 
 
 290 
 
294 
            pos0 = frames1Rect[2*level+2].at<unsigned char>(row, c0);
 291 
            pos0 = frames1Rect[2*level+2].at<float>(row, c0);
295 
            pos1 = frames1Rect[2*level+2].at<unsigned char>(row, c1);
 292 
            pos1 = frames1Rect[2*level+2].at<float>(row, c1);
296 
            neg0 = frames1Rect[2*level+3].at<unsigned char>(row, c0);
 293 
            neg0 = frames1Rect[2*level+3].at<float>(row, c0);
297 
            neg1 = frames1Rect[2*level+3].at<unsigned char>(row, c1);
 294 
            neg1 = frames1Rect[2*level+3].at<float>(row, c1);
298 
 
 295 
 
299 
            col = c0 + (pos0 - neg0)/(neg1 - neg0 - pos1 + pos0);
 296 
            col = c0 + (pos0 - neg0)/(neg1 - neg0 - pos1 + pos0);
300 
            q1.push_back(cv::Point2f(col, row));
 297 
            q1.push_back(cv::Point2f(col, row));
301 
 
 298 
 
302 
        }
 299 
        }
303 
 
 300 
 
304 
    }
 301 
    }
305 
 
 302 
 
306 
    int nMatches = q0.size();
 303 
    int nMatches = q0.size();
307 
 
 304 
 
308 
    if(nMatches < 1){
 305 
    if(nMatches < 1){
309 
        Q.resize(0);
 306 
        Q.resize(0);
310 
        color.resize(0);
 307 
        color.resize(0);
311 
 
 308 
 
312 
        return;
 309 
        return;
313 
    }
 310 
    }
314 
 
 311 
 
315 
    // retrieve color information (at integer coordinates)
 312 
    // retrieve color information (at integer coordinates)
316 
    color.resize(nMatches);
 313 
    color.resize(nMatches);
317 
    for(int i=0; i<nMatches; i++){
 314 
    for(int i=0; i<nMatches; i++){
318 
 
 315 
 
319 
        cv::Vec3b c0 = color0Rect.at<cv::Vec3b>(q0[i][1], q0[i][0]);
 316 
        cv::Vec3f c0 = color0Rect.at<cv::Vec3f>(q0[i][1], q0[i][0]);
320 
        cv::Vec3b c1 = color1Rect.at<cv::Vec3b>(q1[i][1], q1[i][0]);
 317 
        cv::Vec3f c1 = color1Rect.at<cv::Vec3f>(q1[i][1], q1[i][0]);
321 
 
 318 
 
322 
        color[i] = 0.5*c0 + 0.5*c1;
 319 
        color[i] = 0.5*c0 + 0.5*c1;
323 
    }
 320 
    }
324 
 
 321 
 
325 
    // Triangulate by means of disparity projection
 322 
    // Triangulate by means of disparity projection
326 
    Q.resize(q0.size());
 323 
    Q.resize(q0.size());
327 
    cv::Matx44f QRectx = cv::Matx44f(QRect);
 324 
    cv::Matx44f QRectx = cv::Matx44f(QRect);
328 
    cv::Matx33f R0invx = cv::Matx33f(cv::Mat(R0.t()));
 325 
    cv::Matx33f R0invx = cv::Matx33f(cv::Mat(R0.t()));
329 
 
 326 
 
330 
    #pragma omp parallel for
 327 
    #pragma omp parallel for
331 
    for(unsigned int i=0; i<q0.size(); i++){
 328 
    for(unsigned int i=0; i<q0.size(); i++){
332 
        float disparity = q0[i][0]-q1[i][0];
 329 
        float disparity = q0[i][0]-q1[i][0];
333 
        cv::Vec4f Qih = QRectx*cv::Vec4f(q0[i][0], q0[i][1], disparity, 1.0);
 330 
        cv::Vec4f Qih = QRectx*cv::Vec4f(q0[i][0], q0[i][1], disparity, 1.0);
334 
        float winv = float(1.0)/Qih[3];
 331 
        float winv = float(1.0)/Qih[3];
335 
        Q[i] = R0invx * cv::Point3f(Qih[0]*winv, Qih[1]*winv, Qih[2]*winv);
 332 
        Q[i] = R0invx * cv::Point3f(Qih[0]*winv, Qih[1]*winv, Qih[2]*winv);
336 
    }
 333 
    }
337 
 
 334 
 
338 
}
 335 
}
339 
 
 336