Subversion Repositories seema-scanner

Rev

Rev 95 | Only display areas with differences | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 95 Rev 96
1 
#include "AlgorithmGrayCodeHQ.h"
 1 
#include "AlgorithmGrayCodeHQ.h"
2 
#include <cmath>
 2 
#include <cmath>
- 
 
 3 
#include <assert.h>
3 
#include "cvtools.h"
 4 
#include "cvtools.h"
4 
 
 5 
 
5 
#ifndef log2f
 6 
#ifndef log2f
6 
#define log2f(x) (log(x)/log(2.0))
 7 
#define log2f(x) (log(x)/log(2.0))
7 
#endif
 8 
#endif
8 
 
 9 
 
9 
//using namespace std;
 10 
//using namespace std;
10 
 
 11 
 
11 
/*
 12 
/*
12 
 * The purpose of this function is to convert an unsigned
 13 
 * The purpose of this function is to convert an unsigned
13 
 * binary number to reflected binary Gray code.
 14 
 * binary number to reflected binary Gray code.
14 
 *
 15 
 *
15 
 * The operator >> is shift right. The operator ^ is exclusive or.
 16 
 * The operator >> is shift right. The operator ^ is exclusive or.
16 
 * Source: http://en.wikipedia.org/wiki/Gray_code
 17 
 * Source: http://en.wikipedia.org/wiki/Gray_code
17 
 */
 18 
 */
18 
static unsigned int binaryToGray(unsigned int num) {
 19 
static unsigned int binaryToGray(unsigned int num) {
19 
    return (num >> 1) ^ num;
 20 
    return (num >> 1) ^ num;
20 
}
 21 
}
21 
 
 22 
 
22 
/*
 23 
/*
23 
 * From Wikipedia: http://en.wikipedia.org/wiki/Gray_code
 24 
 * From Wikipedia: http://en.wikipedia.org/wiki/Gray_code
24 
 * The purpose of this function is to convert a reflected binary
 25 
 * The purpose of this function is to convert a reflected binary
25 
 * Gray code number to a binary number.
 26 
 * Gray code number to a binary number.
26 
 */
 27 
 */
27 
static unsigned int grayToBinary(unsigned int num){
 28 
static unsigned int grayToBinary(unsigned int num){
28 
    unsigned int mask;
 29 
    unsigned int mask;
29 
    for(mask = num >> 1; mask != 0; mask = mask >> 1)
 30 
    for(mask = num >> 1; mask != 0; mask = mask >> 1)
30 
        num = num ^ mask;
 31 
        num = num ^ mask;
31 
    return num;
 32 
    return num;
32 
}
 33 
}
33 
 
 34 
 
34 
/*
 35 
/*
35 
 * Return the Nth bit of an unsigned integer number
 36 
 * Return the Nth bit of an unsigned integer number
36 
 */
 37 
 */
37 
static bool getBit(int decimal, int N){
 38 
static bool getBit(int decimal, int N){
38 
 
 39 
 
39 
    return decimal & 1 << (N-1);
 40 
    return decimal & 1 << (N-1);
40 
}
 41 
}
41 
 
 42 
 
42 
/*
 43 
/*
43 
 * Return the number of bits set in an integer
 44 
 * Return the number of bits set in an integer
44 
 */
 45 
 */
45 
static int countBits(int n) {
 46 
static int countBits(int n) {
46 
  unsigned int c; // c accumulates the total bits set in v
 47 
  unsigned int c; // c accumulates the total bits set in v
47 
  for (c = 0; n>0; c++)
 48 
  for (c = 0; n>0; c++)
48 
    n &= n - 1; // clear the least significant bit set
 49 
    n &= n - 1; // clear the least significant bit set
49 
  return c;
 50 
  return c;
50 
}
 51 
}
51 
 
 52 
 
52 
/*
 53 
/*
53 
 * Return the position of the least significant bit that is set
 54 
 * Return the position of the least significant bit that is set
54 
 */
 55 
 */
55 
static int leastSignificantBitSet(int x){
 56 
static int leastSignificantBitSet(int x){
56 
  if(x == 0)
 57 
  if(x == 0)
57 
      return 0;
 58 
      return 0;
58 
 
 59 
 
59 
  int val = 1;
 60 
  int val = 1;
60 
  while(x>>=1)
 61 
  while(x>>=1)
61 
      val++;
 62 
      val++;
62 
 
 63 
 
63 
  return val;
 64 
  return val;
64 
}
 65 
}
65 
 
 66 
 
66 
//static int get_bit(int decimal, int N){
 67 
//static int get_bit(int decimal, int N){
67 
 
 68 
 
68 
//    // Shifting the 1 for N-1 bits
 69 
//    // Shifting the 1 for N-1 bits
69 
//    int constant = 1 << (N-1);
 70 
//    int constant = 1 << (N-1);
70 
 
 71 
 
71 
//    // If the bit is set, return 1
 72 
//    // If the bit is set, return 1
72 
//    if( decimal & constant )
 73 
//    if( decimal & constant )
73 
//        return 1;
 74 
//        return 1;
74 
//    else
 75 
//    else
75 
//        return 0;
 76 
//        return 0;
76 
//}
 77 
//}
77 
 
 78 
 
78 
static inline unsigned int powi(int num, unsigned int exponent){
 79 
static inline unsigned int powi(int num, unsigned int exponent){
79 
 
 80 
 
80 
    if(exponent == 0)
 81 
    if(exponent == 0)
81 
        return 1;
 82 
        return 1;
82 
 
 83 
 
83 
    float res = num;
 84 
    float res = num;
84 
    for(unsigned int i=0; i<exponent-1; i++)
 85 
    for(unsigned int i=0; i<exponent-1; i++)
85 
        res *= num;
 86 
        res *= num;
86 
 
 87 
 
87 
    return res;
 88 
    return res;
88 
}
 89 
}
89 
 
 90 
 
90 
static inline unsigned int twopowi(unsigned int exponent){
 91 
static inline unsigned int twopowi(unsigned int exponent){
91 
 
 92 
 
92 
    return 1 << exponent;
 93 
    return 1 << exponent;
93 
}
 94 
}
94 
 
 95 
 
95 
// Algorithm
 96 
// Algorithm
96 
AlgorithmGrayCodeHQ::AlgorithmGrayCodeHQ(unsigned int _screenCols, unsigned int _screenRows) : Algorithm(_screenCols, _screenRows){
 97 
AlgorithmGrayCodeHQ::AlgorithmGrayCodeHQ(unsigned int _screenCols, unsigned int _screenRows) : Algorithm(_screenCols, _screenRows){
97 
 
 98 
 
98 
    NbitsHorz = ceilf(log2f((float)screenCols));
 99 
    NbitsHorz = ceilf(log2f((float)screenCols));
99 
    NbitsVert =  ceilf(log2f((float)screenRows));
 100 
    NbitsVert =  ceilf(log2f((float)screenRows));
100 
    N = 2 + (NbitsHorz+NbitsVert)*2;
 101 
    N = 2 + (NbitsHorz+NbitsVert)*2;
101 
 
 102 
 
102 
    // all on pattern
 103 
    // all on pattern
103 
    cv::Mat allOn(1, screenCols, CV_8UC3, cv::Scalar::all(255));
 104 
    cv::Mat allOn(1, screenCols, CV_8UC3, cv::Scalar::all(255));
104 
    patterns.push_back(allOn);
 105 
    patterns.push_back(allOn);
105 
 
 106 
 
106 
    // all off pattern
 107 
    // all off pattern
107 
    cv::Mat allOff(1, screenCols, CV_8UC3, cv::Scalar::all(0));
 108 
    cv::Mat allOff(1, screenCols, CV_8UC3, cv::Scalar::all(0));
108 
    patterns.push_back(allOff);
 109 
    patterns.push_back(allOff);
109 
 
 110 
 
110 
 
 111 
 
111 
    // horizontally encoding patterns
 112 
    // horizontally encoding patterns
112 
    for(unsigned int p=0; p<NbitsHorz; p++){
 113 
    for(unsigned int p=0; p<NbitsHorz; p++){
113 
        cv::Mat pattern(1, screenCols, CV_8UC3);
 114 
        cv::Mat pattern(1, screenCols, CV_8UC3);
114 
        cv::Mat patternInv(1, screenCols, CV_8UC3);
 115 
        cv::Mat patternInv(1, screenCols, CV_8UC3);
115 
 
 116 
 
116 
        for(unsigned int j=0; j<screenCols; j++){
 117 
        for(unsigned int j=0; j<screenCols; j++){
117 
 
 118 
 
118 
            unsigned int jGray = binaryToGray(j);
 119 
            unsigned int jGray = binaryToGray(j);
119 
            // Amplitude of channels
 120 
            // Amplitude of channels
120 
            int bit = (int)getBit(jGray, Nbits-p);
 121 
            int bit = (int)getBit(jGray, NbitsHorz-p);
121 
            pattern.at<cv::Vec3b>(0,j) = cv::Vec3b(255.0*bit,255.0*bit,255.0*bit);
 122 
            pattern.at<cv::Vec3b>(0,j) = cv::Vec3b(255.0*bit,255.0*bit,255.0*bit);
122 
            int invBit = bit^1;
 123 
            int invBit = bit^1;
123 
            patternInv.at<cv::Vec3b>(0,j) = cv::Vec3b(255.0*invBit,255.0*invBit,255.0*invBit);
 124 
            patternInv.at<cv::Vec3b>(0,j) = cv::Vec3b(255.0*invBit,255.0*invBit,255.0*invBit);
124 
        }
 125 
        }
125 
        patterns.push_back(pattern);
 126 
        patterns.push_back(pattern);
126 
        patterns.push_back(patternInv);
 127 
        patterns.push_back(patternInv);
127 
    }
 128 
    }
128 
 
 129 
 
129 
    // vertical encoding patterns
 130 
    // vertical encoding patterns
130 
    for(unsigned int p=0; p<NbitsVert; p++){
 131 
    for(unsigned int p=0; p<NbitsVert; p++){
131 
        cv::Mat pattern(screenRows, 1, CV_8UC3);
 132 
        cv::Mat pattern(screenRows, 1, CV_8UC3);
132 
        cv::Mat patternInv(screenRows, 1, CV_8UC3);
 133 
        cv::Mat patternInv(screenRows, 1, CV_8UC3);
133 
 
 134 
 
134 
        for(unsigned int j=0; j<screenRows; j++){
 135 
        for(unsigned int j=0; j<screenRows; j++){
135 
 
 136 
 
136 
            unsigned int jGray = binaryToGray(j);
 137 
            unsigned int jGray = binaryToGray(j);
137 
            // Amplitude of channels
 138 
            // Amplitude of channels
138 
            int bit = (int)getBit(jGray, Nbits-p);
 139 
            int bit = (int)getBit(jGray, NbitsVert-p);
139 
            pattern.at<cv::Vec3b>(j,0) = cv::Vec3b(255.0*bit,255.0*bit,255.0*bit);
 140 
            pattern.at<cv::Vec3b>(j,0) = cv::Vec3b(255.0*bit,255.0*bit,255.0*bit);
140 
            int invBit = bit^1;
 141 
            int invBit = bit^1;
141 
            patternInv.at<cv::Vec3b>(j,0) = cv::Vec3b(255.0*invBit,255.0*invBit,255.0*invBit);
 142 
            patternInv.at<cv::Vec3b>(j,0) = cv::Vec3b(255.0*invBit,255.0*invBit,255.0*invBit);
142 
        }
 143 
        }
143 
        patterns.push_back(pattern);
 144 
        patterns.push_back(pattern);
144 
        patterns.push_back(patternInv);
 145 
        patterns.push_back(patternInv);
145 
    }
 146 
    }
146 
 
 147 
 
147 
}
 148 
}
148 
 
 149 
 
149 
cv::Mat AlgorithmGrayCodeHQ::getEncodingPattern(unsigned int depth){
 150 
cv::Mat AlgorithmGrayCodeHQ::getEncodingPattern(unsigned int depth){
150 
    return patterns[depth];
 151 
    return patterns[depth];
151 
}
 152 
}
152 
 
 153 
 
153 
 
 154 
 
154 
bool sortingLarger(cv::Vec4i i,cv::Vec4i j){ return (i[3]<j[3]);}
 155 
bool sortingLarger(cv::Vec3i i,cv::Vec3i j){ return (i[2]<j[2]);}
155 
bool sortingEqual(cv::Vec4i i,cv::Vec4i j){ return (i[3]==j[3]);}
 156 
bool sortingEqual(cv::Vec3i i,cv::Vec3i j){ return (i[2]==j[2]);}
- 
 
 157 
 
156 
void getEdgeLabels(const cv::Mat& scanLine, int Nbits, std::vector<cv::Vec4i>& edges){
 158 
void getIntersectionLabels(const cv::Mat& codeHorz, const cv::Mat& codeVert, int NbitsHorz, int NbitsVert, std::vector<cv::Vec3i>& intersections){
157 
 
 159 
 
158 
    int nCols = scanLine.cols;
 160 
    int nRows = codeHorz.rows;
159 
    const int *data = scanLine.ptr<const int>(0);
 161 
    int nCols = codeHorz.cols;
160 
 
 162 
 
- 
 
 163 
    int labelHorz;
161 
    int labelLeft;
 164 
    int labelVert;
- 
 
 165 
    int labelHorzRight;
162 
    int labelRight = data[0];
 166 
    int labelVertBelow;
163 
 
 167 
 
164 
    // collect edges
 168 
    // collect intersections
- 
 
 169 
    for(int row=0; row<nRows+1; row++){
165 
    for(int col=1; col<nCols; col++){
 170 
        for(int col=0; col<nCols+1; col++){
166 
 
 171 
 
167 
        labelLeft = labelRight;
 172 
            labelHorz = codeHorz.at<int>(row, col);
168 
        labelRight = data[col];
 173 
            labelHorzRight = codeHorz.at<int>(row, col+1);
- 
 
 174 
 
- 
 
 175 
            labelVert = codeVert.at<int>(row, col);
- 
 
 176 
            labelVertBelow = codeVert.at<int>(row+1, col);
169 
 
 177 
 
170 
        // labels need to be non-background, and differ in exactly one bit
 178 
            // labels need to be non-background, and differ in exactly one bit
- 
 
 179 
            if(labelHorz != -1 && labelHorzRight != -1 &&
171 
        if(labelLeft != -1 && labelRight != -1 && countBits(labelLeft^labelRight) == 1){
 180 
               countBits(labelHorz^labelHorzRight) == 1 &&
- 
 
 181 
               labelVert != -1 && labelVertBelow != -1 &&
- 
 
 182 
               countBits(labelVert^labelVertBelow) == 1){
- 
 
 183 
 
- 
 
 184 
                // OVERFLOW??
- 
 
 185 
                int orderingRelation = (labelHorz << NbitsHorz+2*NbitsVert) + (labelHorzRight << 2*NbitsVert) +
172 
            int orderingRelation = (labelLeft << Nbits) + labelRight;
 186 
                                       (labelVert << NbitsVert) + labelVertBelow;
173 
            // store left label column
 187 
                // store left label column
174 
            edges.push_back(cv::Vec4i(col-1, labelLeft, labelRight, orderingRelation));
 188 
                intersections.push_back(cv::Vec3i(row, col, orderingRelation));
- 
 
 189 
            }
175 
        }
 190 
        }
176 
    }
 191 
    }
177 
 
 - 
 
178 
    // sort
 192 
    // sort
179 
    std::sort(edges.begin(), edges.end(), sortingLarger);
 193 
    std::sort(intersections.begin(), intersections.end(), sortingLarger);
180 
 
 194 
 
181 
    // remove duplicates
 195 
    // remove duplicates
182 
    std::vector<cv::Vec4i>::iterator it;
 196 
    std::vector<cv::Vec3i>::iterator it;
183 
    it = std::unique(edges.begin(), edges.end(), sortingEqual);
 197 
    it = std::unique(intersections.begin(), intersections.end(), sortingEqual);
184 
    edges.resize(std::distance(edges.begin(),it));
 198 
    intersections.resize(std::distance(intersections.begin(),it));
185 
}
 199 
}
186 
 
 200 
 
187 
cv::Vec3b getColorSubpix(const cv::Mat& img, cv::Point2f pt){
 201 
cv::Vec3b getColorSubpix(const cv::Mat& img, cv::Point2f pt){
188 
    assert(!img.empty());
 202 
    assert(!img.empty());
189 
    assert(img.channels() == 3);
 203 
    assert(img.channels() == 3);
190 
 
 204 
 
191 
    int x = (int)pt.x;
 205 
    int x = (int)pt.x;
192 
    int y = (int)pt.y;
 206 
    int y = (int)pt.y;
193 
 
 207 
 
194 
    int x0 = cv::borderInterpolate(x,   img.cols, cv::BORDER_REFLECT_101);
 208 
    int x0 = cv::borderInterpolate(x,   img.cols, cv::BORDER_REFLECT_101);
195 
    int x1 = cv::borderInterpolate(x+1, img.cols, cv::BORDER_REFLECT_101);
 209 
    int x1 = cv::borderInterpolate(x+1, img.cols, cv::BORDER_REFLECT_101);
196 
    int y0 = cv::borderInterpolate(y,   img.rows, cv::BORDER_REFLECT_101);
 210 
    int y0 = cv::borderInterpolate(y,   img.rows, cv::BORDER_REFLECT_101);
197 
    int y1 = cv::borderInterpolate(y+1, img.rows, cv::BORDER_REFLECT_101);
 211 
    int y1 = cv::borderInterpolate(y+1, img.rows, cv::BORDER_REFLECT_101);
198 
 
 212 
 
199 
    float a = pt.x - (float)x;
 213 
    float a = pt.x - (float)x;
200 
    float c = pt.y - (float)y;
 214 
    float c = pt.y - (float)y;
201 
 
 215 
 
202 
    uchar b = (uchar)cvRound((img.at<cv::Vec3b>(y0, x0)[0] * (1.f - a) + img.at<cv::Vec3b>(y0, x1)[0] * a) * (1.f - c)
 216 
    uchar b = (uchar)cvRound((img.at<cv::Vec3b>(y0, x0)[0] * (1.f - a) + img.at<cv::Vec3b>(y0, x1)[0] * a) * (1.f - c)
203 
                           + (img.at<cv::Vec3b>(y1, x0)[0] * (1.f - a) + img.at<cv::Vec3b>(y1, x1)[0] * a) * c);
 217 
                           + (img.at<cv::Vec3b>(y1, x0)[0] * (1.f - a) + img.at<cv::Vec3b>(y1, x1)[0] * a) * c);
204 
    uchar g = (uchar)cvRound((img.at<cv::Vec3b>(y0, x0)[1] * (1.f - a) + img.at<cv::Vec3b>(y0, x1)[1] * a) * (1.f - c)
 218 
    uchar g = (uchar)cvRound((img.at<cv::Vec3b>(y0, x0)[1] * (1.f - a) + img.at<cv::Vec3b>(y0, x1)[1] * a) * (1.f - c)
205 
                           + (img.at<cv::Vec3b>(y1, x0)[1] * (1.f - a) + img.at<cv::Vec3b>(y1, x1)[1] * a) * c);
 219 
                           + (img.at<cv::Vec3b>(y1, x0)[1] * (1.f - a) + img.at<cv::Vec3b>(y1, x1)[1] * a) * c);
206 
    uchar r = (uchar)cvRound((img.at<cv::Vec3b>(y0, x0)[2] * (1.f - a) + img.at<cv::Vec3b>(y0, x1)[2] * a) * (1.f - c)
 220 
    uchar r = (uchar)cvRound((img.at<cv::Vec3b>(y0, x0)[2] * (1.f - a) + img.at<cv::Vec3b>(y0, x1)[2] * a) * (1.f - c)
207 
                           + (img.at<cv::Vec3b>(y1, x0)[2] * (1.f - a) + img.at<cv::Vec3b>(y1, x1)[2] * a) * c);
 221 
                           + (img.at<cv::Vec3b>(y1, x0)[2] * (1.f - a) + img.at<cv::Vec3b>(y1, x1)[2] * a) * c);
208 
 
 222 
 
209 
    return cv::Vec3b(b, g, r);
 223 
    return cv::Vec3b(b, g, r);
210 
}
 224 
}
211 
 
 225 
 
212 
void AlgorithmGrayCodeHQ::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){
 226 
void AlgorithmGrayCodeHQ::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){
213 
 
 227 
 
214 
    assert(frames0.size() == N);
 228 
    assert(frames0.size() == N);
215 
    assert(frames1.size() == N);
 229 
    assert(frames1.size() == N);
216 
 
 230 
 
217 
    int frameRows = frames0[0].rows;
 231 
    int frameRows = frames0[0].rows;
218 
    int frameCols = frames0[0].cols;
 232 
    int frameCols = frames0[0].cols;
219 
 
 233 
 
220 
    // gray-scale
 234 
    // gray-scale
221 
    std::vector<cv::Mat> frames0Gray(N);
 235 
    std::vector<cv::Mat> frames0Gray(N);
222 
    std::vector<cv::Mat> frames1Gray(N);
 236 
    std::vector<cv::Mat> frames1Gray(N);
223 
    for(int i=0; i<N; i++){
 237 
    for(int i=0; i<N; i++){
224 
        cv::cvtColor(frames0[i], frames0Gray, CV_RGB2GRAY);
 238 
        cv::cvtColor(frames0[i], frames0Gray, CV_RGB2GRAY);
225 
        cv::cvtColor(frames1[i], frames1Gray, CV_RGB2GRAY);
 239 
        cv::cvtColor(frames1[i], frames1Gray, CV_RGB2GRAY);
226 
    }
 240 
    }
227 
 
 241 
 
228 
    // colors
 242 
    // colors
229 
    cv::Mat color0 = frames0[0];
 243 
    cv::Mat color0 = frames0[0];
230 
    cv::Mat color1 = frames1[0];
 244 
    cv::Mat color1 = frames1[0];
231 
 
 245 
 
232 
    // occlusion masks
 246 
    // occlusion masks
233 
    cv::Mat occlusion0, occlusion1;
 247 
    cv::Mat occlusion0, occlusion1;
234 
    cv::subtract(frames0Gray[0], frames0Gray[1], occlusion0);
 248 
    cv::subtract(frames0Gray[0], frames0Gray[1], occlusion0);
235 
    occlusion0 = occlusion0 > 25;
 249 
    occlusion0 = occlusion0 > 25;
236 
    cv::subtract(frames1Gray[0], frames1Gray[1], occlusion1);
 250 
    cv::subtract(frames1Gray[0], frames1Gray[1], occlusion1);
237 
    occlusion1 = occlusion1 > 25;
 251 
    occlusion1 = occlusion1 > 25;
238 
 
 252 
 
239 
    // erode occlusion masks
 253 
    // erode occlusion masks
240 
    cv::Mat strel = cv::getStructuringElement(cv::MORPH_ELLIPSE, cv::Size(3,3));
 254 
    cv::Mat strel = cv::getStructuringElement(cv::MORPH_ELLIPSE, cv::Size(3,3));
241 
    cv::erode(occlusion0, occlusion0, strel);
 255 
    cv::erode(occlusion0, occlusion0, strel);
242 
    cv::erode(occlusion1, occlusion1, strel);
 256 
    cv::erode(occlusion1, occlusion1, strel);
243 
 
 257 
 
244 
//cvtools::writeMat(occlusion0, "occlusion0.mat", "occlusion0");
 258 
//cvtools::writeMat(occlusion0, "occlusion0.mat", "occlusion0");
245 
//cvtools::writeMat(occlusion1, "occlusion1.mat", "occlusion1");
 259 
//cvtools::writeMat(occlusion1, "occlusion1.mat", "occlusion1");
246 
 
 260 
 
247 
    // decode patterns
 261 
    // decode patterns
248 
    cv::Mat code0Horz(frameRows, frameCols, CV_32S, cv::Scalar(0));
 262 
    cv::Mat code0Horz(frameRows, frameCols, CV_32S, cv::Scalar(0));
249 
    cv::Mat code1Horz(frameRows, frameCols, CV_32S, cv::Scalar(0));
 263 
    cv::Mat code1Horz(frameRows, frameCols, CV_32S, cv::Scalar(0));
250 
    cv::Mat code0Vert(frameRows, frameCols, CV_32S, cv::Scalar(0));
 264 
    cv::Mat code0Vert(frameRows, frameCols, CV_32S, cv::Scalar(0));
251 
    cv::Mat code1Vert(frameRows, frameCols, CV_32S, cv::Scalar(0));
 265 
    cv::Mat code1Vert(frameRows, frameCols, CV_32S, cv::Scalar(0));
252 
 
 266 
 
253 
    // horizontal codes into gray code
 267 
    // horizontal codes into gray code
254 
    for(int i=0; i<NbitsHorz; i++){
 268 
    for(int i=0; i<NbitsHorz; i++){
255 
        cv::Mat bit0;
 269 
        cv::Mat bit0;
256 
        cv::subtract(frames0Gray[i*2+2], frames0Gray[i*2+3], bit0);
 270 
        cv::subtract(frames0Gray[i*2+2], frames0Gray[i*2+3], bit0);
257 
        bit0 = bit0 > 0;
 271 
        bit0 = bit0 > 0;
258 
        bit0.convertTo(bit0, CV_32S, 1.0/255.0);
 272 
        bit0.convertTo(bit0, CV_32S, 1.0/255.0);
259 
        cv::add(code0Horz, bit0*twopowi(Nbits-i-1), code0Horz, cv::Mat(), CV_32S);
 273 
        cv::add(code0Horz, bit0*twopowi(NbitsHorz-i-1), code0Horz, cv::Mat(), CV_32S);
260 
 
 274 
 
261 
        cv::Mat bit1;
 275 
        cv::Mat bit1;
262 
        cv::subtract(frames1Gray[i*2+2], frames1Gray[i*2+3], bit1);
 276 
        cv::subtract(frames1Gray[i*2+2], frames1Gray[i*2+3], bit1);
263 
        bit1 = bit1 > 0;
 277 
        bit1 = bit1 > 0;
264 
        bit1.convertTo(bit1, CV_32S, 1.0/255.0);
 278 
        bit1.convertTo(bit1, CV_32S, 1.0/255.0);
265 
        cv::add(code1Horz, bit1*twopowi(Nbits-i-1), code1Horz, cv::Mat(), CV_32S);
 279 
        cv::add(code1Horz, bit1*twopowi(NbitsHorz-i-1), code1Horz, cv::Mat(), CV_32S);
266 
    }
 280 
    }
267 
 
 281 
 
268 
    // vertical codes into gray code
 282 
    // vertical codes into gray code
269 
    for(int i=0; i<NbitsVert; i++){
 283 
    for(int i=0; i<NbitsVert; i++){
270 
        cv::Mat bit0;
 284 
        cv::Mat bit0;
271 
        cv::subtract(frames0Gray[i*2+NbitsHorz+2], frames0Gray[i*2+NbitsHorz+3], bit0);
 285 
        cv::subtract(frames0Gray[i*2+NbitsHorz+2], frames0Gray[i*2+NbitsHorz+3], bit0);
272 
        bit0 = bit0 > 0;
 286 
        bit0 = bit0 > 0;
273 
        bit0.convertTo(bit0, CV_32S, 1.0/255.0);
 287 
        bit0.convertTo(bit0, CV_32S, 1.0/255.0);
274 
        cv::add(code0Vert, bit0*twopowi(Nbits-i-1), code0Vert, cv::Mat(), CV_32S);
 288 
        cv::add(code0Vert, bit0*twopowi(NbitsVert-i-1), code0Vert, cv::Mat(), CV_32S);
275 
 
 289 
 
276 
        cv::Mat bit1;
 290 
        cv::Mat bit1;
277 
        cv::subtract(frames1Gray[i*2+NbitsHorz+2], frames1Gray[i*2+NbitsHorz+3], bit1);
 291 
        cv::subtract(frames1Gray[i*2+NbitsHorz+2], frames1Gray[i*2+NbitsHorz+3], bit1);
278 
        bit1 = bit1 > 0;
 292 
        bit1 = bit1 > 0;
279 
        bit1.convertTo(bit1, CV_32S, 1.0/255.0);
 293 
        bit1.convertTo(bit1, CV_32S, 1.0/255.0);
280 
        cv::add(code1Vert, bit1*twopowi(Nbits-i-1), code1Vert, cv::Mat(), CV_32S);
 294 
        cv::add(code1Vert, bit1*twopowi(NbitsVert-i-1), code1Vert, cv::Mat(), CV_32S);
281 
    }
 295 
    }
282 
 
 296 
 
283 
//cvtools::writeMat(code0Horz, "code0Horz.mat", "code0Horz");
 297 
//cvtools::writeMat(code0Horz, "code0Horz.mat", "code0Horz");
284 
//cvtools::writeMat(code1Horz, "code1Horz.mat", "code1Horz");
 298 
//cvtools::writeMat(code1Horz, "code1Horz.mat", "code1Horz");
285 
//cvtools::writeMat(code0Vert, "code0Vert.mat", "code0Vert");
 299 
//cvtools::writeMat(code0Vert, "code0Vert.mat", "code0Vert");
286 
//cvtools::writeMat(code1Vert, "code1Vert.mat", "code1Vert");
 300 
//cvtools::writeMat(code1Vert, "code1Vert.mat", "code1Vert");
287 
 
 301 
 
288 
    // set occluded pixels to -1
 302 
    // set occluded pixels to -1
289 
    for(int r=0; r<frameRows; r++){
 303 
    for(int r=0; r<frameRows; r++){
290 
        for(int c=0; c<frameCols; c++){
 304 
        for(int c=0; c<frameCols; c++){
291 
            if(occlusion0.at<char>(r,c) == 0){
 305 
            if(occlusion0.at<char>(r,c) == 0){
292 
                code0Horz.at<float>(r,c) = -1;
 306 
                code0Horz.at<float>(r,c) = -1;
293 
                code0Vert.at<float>(r,c) = -1;
 307 
                code0Vert.at<float>(r,c) = -1;
294 
            }
 308 
            }
295 
            if(occlusion1.at<char>(r,c) == 0){
 309 
            if(occlusion1.at<char>(r,c) == 0){
296 
                code1Horz.at<float>(r,c) = -1;
 310 
                code1Horz.at<float>(r,c) = -1;
297 
                code1Vert.at<float>(r,c) = -1;
 311 
                code1Vert.at<float>(r,c) = -1;
298 
            }
 312 
            }
299 
        }
 313 
        }
300 
    }
 314 
    }
301 
 
 315 
 
- 
 
 316 
    // matching
- 
 
 317 
    std::vector<cv::Vec3i> intersections0, intersections1;
302 
 
 318 
 
- 
 
 319 
    // intersection data structure containing [floor(row), floor(column), orderingRelation]
- 
 
 320 
    getIntersectionLabels(code0Horz, code0Vert, NbitsHorz, NbitsVert, intersections0);
- 
 
 321 
    getIntersectionLabels(code1Horz, code1Vert, NbitsHorz, NbitsVert, intersections1);
- 
 
 322 
 
- 
 
 323 
    // match intersections
- 
 
 324 
    std::vector<cv::Vec3i> matches0, matches1;
- 
 
 325 
    int i=0, j=0;
- 
 
 326 
 
- 
 
 327 
    while(i<intersections0.size() && j<intersections1.size()){
- 
 
 328 
        if(intersections0[i][2] == intersections1[j][2]){
- 
 
 329 
            matches0.push_back(intersections0[i]);
- 
 
 330 
            matches1.push_back(intersections1[j]);
- 
 
 331 
            i += 1;
- 
 
 332 
            j += 1;
- 
 
 333 
        } else if(intersections0[i][2] < intersections1[j][2]){
- 
 
 334 
            i += 1;
- 
 
 335 
        } else if(intersections0[i][2] > intersections1[j][2]){
- 
 
 336 
            j += 1;
- 
 
 337 
        }
- 
 
 338 
    }
303 
 
 339 
 
304 
    // TODO: REWRITE TO PERFORM HORIZONTAL + VERTICAL MATCHING
 340 
    std::vector<cv::Vec2f> q0, q1;
305 
 
 341 
 
306 
//    // matching
 - 
 
307 
//    std::vector<cv::Vec2f> q0Rect, q1Rect;
 - 
 
308 
//    for(int row=0; row<frameRectRows; row++){
 - 
 
309 
 
 - 
 
310 
//        // edge data structure containing [floor(column), labelLeft, labelRight, orderingRelation]
 - 
 
311 
//        std::vector<cv::Vec4i> edges0, edges1;
 - 
 
312 
 
 - 
 
313 
//        // sorted, unique edges
 - 
 
314 
//        getEdgeLabels(code0Rect.row(row), Nbits, edges0);
 - 
 
315 
//        getEdgeLabels(code1Rect.row(row), Nbits, edges1);
 - 
 
316 
 
 - 
 
317 
//        // match edges
 - 
 
318 
//        std::vector<cv::Vec4i> matchedEdges0, matchedEdges1;
 - 
 
319 
//        int i=0, j=0;
 - 
 
320 
//        while(i<edges0.size() && j<edges1.size()){
 - 
 
321 
 
 - 
 
322 
//            if(edges0[i][3] == edges1[j][3]){
 - 
 
323 
//                matchedEdges0.push_back(edges0[i]);
 - 
 
324 
//                matchedEdges1.push_back(edges1[j]);
 - 
 
325 
//                i += 1;
 - 
 
326 
//                j += 1;
 - 
 
327 
//            } else if(edges0[i][3] < edges1[j][3]){
 - 
 
328 
//                i += 1;
 - 
 
329 
//            } else if(edges0[i][3] > edges1[j][3]){
 - 
 
330 
//                j += 1;
 - 
 
331 
//            }
 - 
 
332 
//        }
 - 
 
333 
 
 - 
 
334 
//        // crude subpixel refinement
 342 
    // TODO: subpixel refinement in both horizontal and vertical
335 
//        // finds the intersection of linear interpolants in the positive/negative pattern
 343 
//    // subpixel refinement finds the intersection of linear interpolants in the positive/negative pattern
336 
//        for(int i=0; i<matchedEdges0.size(); i++){
 344 
//    for(int i=0; i<matchedEdges0.size(); i++){
337 
 
 - 
 
338 
//            int level = Nbits - leastSignificantBitSet(matchedEdges0[i][1]^matchedEdges0[i][2]);
 - 
 
339 
 
 - 
 
340 
//            // refine for camera 0
 - 
 
341 
//            float c0 = matchedEdges0[i][0];
 - 
 
342 
//            float c1 = c0+1;
 - 
 
343 
 
 - 
 
344 
//            float pos0 = frames0Rect[2*level+2].at<char>(row, c0);
 - 
 
345 
//            float pos1 = frames0Rect[2*level+2].at<char>(row, c1);
 - 
 
346 
//            float neg0 = frames0Rect[2*level+3].at<char>(row, c0);
 - 
 
347 
//            float neg1 = frames0Rect[2*level+3].at<char>(row, c1);
 - 
 
348 
 
 - 
 
349 
//            float col = c0 + (pos0 - neg0)/(neg1 - neg0 - pos1 + pos0);
 - 
 
350 
//            q0Rect.push_back(cv::Point2f(col, row));
 - 
 
351 
 
 - 
 
352 
//            // refine for camera 1
 - 
 
353 
//            c0 = matchedEdges1[i][0];
 - 
 
354 
//            c1 = c0+1;
 - 
 
355 
 
 - 
 
356 
//            pos0 = frames1Rect[2*level+2].at<char>(row, c0);
 - 
 
357 
//            pos1 = frames1Rect[2*level+2].at<char>(row, c1);
 - 
 
358 
//            neg0 = frames1Rect[2*level+3].at<char>(row, c0);
 - 
 
359 
//            neg1 = frames1Rect[2*level+3].at<char>(row, c1);
 - 
 
360 
 
 345 
 
361 
//            col = c0 + (pos0 - neg0)/(neg1 - neg0 - pos1 + pos0);
 346 
//        int level = Nbits - leastSignificantBitSet(matchedEdges0[i][1]^matchedEdges0[i][2]);
362 
//            q1Rect.push_back(cv::Point2f(col, row));
 - 
 
363 
 
 347 
 
- 
 
 348 
//        // refine for camera 0
- 
 
 349 
//        float c0 = matchedEdges0[i][0];
364 
//        }
 350 
//        float c1 = c0+1;
365 
 
 351 
 
- 
 
 352 
//        float pos0 = frames0Rect[2*level+2].at<char>(row, c0);
- 
 
 353 
//        float pos1 = frames0Rect[2*level+2].at<char>(row, c1);
- 
 
 354 
//        float neg0 = frames0Rect[2*level+3].at<char>(row, c0);
366 
//    }
 355 
//        float neg1 = frames0Rect[2*level+3].at<char>(row, c1);
367 
 
 356 
 
- 
 
 357 
//        float col = c0 + (pos0 - neg0)/(neg1 - neg0 - pos1 + pos0);
368 
//    int nMatches = q0Rect.size();
 358 
//        q0Rect.push_back(cv::Point2f(col, row));
369 
 
 359 
 
370 
//    if(nMatches < 1){
 360 
//        // refine for camera 1
371 
//        Q.resize(0);
 361 
//        c0 = matchedEdges1[i][0];
372 
//        color.resize(0);
 362 
//        c1 = c0+1;
373 
 
 363 
 
- 
 
 364 
//        pos0 = frames1Rect[2*level+2].at<char>(row, c0);
- 
 
 365 
//        pos1 = frames1Rect[2*level+2].at<char>(row, c1);
374 
//        return;
 366 
//        neg0 = frames1Rect[2*level+3].at<char>(row, c0);
375 
//    }
 367 
//        neg1 = frames1Rect[2*level+3].at<char>(row, c1);
376 
 
 368 
 
377 
//    // retrieve color information (at integer coordinates)
 - 
 
378 
//    color.resize(nMatches);
 - 
 
379 
//    for(int i=0; i<nMatches; i++){
 - 
 
380 
 
 - 
 
381 
//        cv::Vec3b c0 = color0Rect.at<cv::Vec3b>(q0Rect[i][1], q0Rect[i][0]);
 - 
 
382 
//        cv::Vec3b c1 = color1Rect.at<cv::Vec3b>(q1Rect[i][1], q1Rect[i][0]);
 - 
 
383 
////        cv::Vec3b c0 = getColorSubpix(color0Rect, q0Rect[i]);
 369 
//        col = c0 + (pos0 - neg0)/(neg1 - neg0 - pos1 + pos0);
384 
////        cv::Vec3b c1 = getColorSubpix(color1Rect, q0Rect[i]);
 370 
//        q1Rect.push_back(cv::Point2f(col, row));
385 
 
 371 
 
386 
//        color[i] = 0.5*c0 + 0.5*c1;
 - 
 
387 
//    }
 372 
//    }
388 
 
 373 
 
389 
//    // triangulate points
 - 
 
390 
//    cv::Mat QMatHomogenous, QMat;
 - 
 
391 
////    cv::Mat C0 = P0.clone();
 - 
 
392 
////    cv::Mat C1 = P1.clone();
 - 
 
393 
////    C0.colRange(0, 3) = C0.colRange(0, 3)*R0;
 - 
 
394 
////    C1.colRange(0, 3) = C1.colRange(0, 3)*R1.t();
 - 
 
395 
//    cv::triangulatePoints(P0, P1, q0Rect, q1Rect, QMatHomogenous);
 - 
 
396 
//    cvtools::convertMatFromHomogeneous(QMatHomogenous, QMat);
 - 
 
397 
 
 - 
 
398 
//    // undo rectifying rotation
 - 
 
399 
//    cv::Mat R0Inv;
 - 
 
400 
//    cv::Mat(R0.t()).convertTo(R0Inv, CV_32F);
 - 
 
401 
//    QMat = R0Inv*QMat;
 - 
 
402 
 
 374 
 
- 
 
 375 
    int nMatches = q0.size();
- 
 
 376 
 
- 
 
 377 
    if(nMatches < 1){
- 
 
 378 
        Q.resize(0);
- 
 
 379 
        color.resize(0);
- 
 
 380 
 
- 
 
 381 
        return;
- 
 
 382 
    }
- 
 
 383 
 
- 
 
 384 
    // retrieve color information (at integer coordinates)
- 
 
 385 
    color.resize(nMatches);
- 
 
 386 
    for(int i=0; i<nMatches; i++){
- 
 
 387 
 
- 
 
 388 
        cv::Vec3b c0 = color0.at<cv::Vec3b>(q0[i][1], q0[i][0]);
- 
 
 389 
        cv::Vec3b c1 = color1.at<cv::Vec3b>(q1[i][1], q1[i][0]);
- 
 
 390 
//        cv::Vec3b c0 = getColorSubpix(color0Rect, q0Rect[i]);
- 
 
 391 
//        cv::Vec3b c1 = getColorSubpix(color1Rect, q0Rect[i]);
- 
 
 392 
 
- 
 
 393 
        color[i] = 0.5*c0 + 0.5*c1;
- 
 
 394 
    }
- 
 
 395 
 
- 
 
 396 
    // triangulate points
- 
 
 397 
    cv::Mat P0(3, 4, CV_32F, cv::Scalar(0.0));
- 
 
 398 
    cv::Mat(calibration.K0).copyTo(P0.colRange(0, 3));
- 
 
 399 
 
- 
 
 400 
    cv::Mat P1(3, 4, CV_32F), temp(3,4,CV_32F);
- 
 
 401 
    cv::Mat(calibration.R1).copyTo(temp(cv::Range(0,3), cv::Range(0,3)));
- 
 
 402 
    cv::Mat(calibration.T1).copyTo(temp(cv::Range(0,3), cv::Range(3,4)));
- 
 
 403 
    P1 = cv::Mat(calibration.K1) * temp;
- 
 
 404 
 
- 
 
 405 
    cv::correctMatches(calibration.F, q0, q1, q0, q1);
- 
 
 406 
 
- 
 
 407 
    cv::Mat QMatHomogenous, QMat;
- 
 
 408 
    cv::triangulatePoints(P0, P1, q0, q1, QMatHomogenous);
- 
 
 409 
 
- 
 
 410 
    cvtools::convertMatFromHomogeneous(QMatHomogenous, QMat);
403 
//    cvtools::matToPoints3f(QMat, Q);
 411 
    cvtools::matToPoints3f(QMat, Q);
404 
 
 412 
 
405 
}
 413 
}
406 
 
 414