Subversion Repositories seema-scanner

Rev

Rev 95 | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 95 Rev 96
Line 1... Line 1...
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
Line 115... Line 116...
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);
Line 133... Line 134...
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);
Line 149... Line 150...
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);
Line 254... Line 268...
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");
Line 297... Line 311...
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 
}