Subversion Repositories seema-scanner

Rev

Rev 167 | Rev 195 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 167 Rev 182
Line -... Line 1...
- 
 
 1 
//
- 
 
 2 
// Gray Code Structured Light with horizontal and vertical encoding
- 
 
 3 
//
- 
 
 4 
// This implementation of Gray encoding uses horizontal and vertial stripes, which adds some encoding redundancy, but avoid interpolation effects from rectifying homographies.
- 
 
 5 
//
- 
 
 6 
 
1 
#include "AlgorithmGrayCodeHorzVert.h"
 7 
#include "AlgorithmGrayCodeHorzVert.h"
2 
#include <cmath>
 8 
#include <cmath>
3 
#include <assert.h>
 9 
#include <assert.h>
4 
#include "cvtools.h"
 10 
#include "cvtools.h"
5 
 
 - 
 
6 
#ifndef log2f
 - 
 
7 
#define log2f(x) (log(x)/log(2.0))
 11 
#include "algorithmtools.h"
8 
#endif
 - 
 
9 
 
 - 
 
10 
//using namespace std;
 - 
 
11 
 
 - 
 
12 
/*
 - 
 
13 
 * The purpose of this function is to convert an unsigned
 - 
 
14 
 * binary number to reflected binary Gray code.
 - 
 
15 
 *
 - 
 
16 
 * The operator >> is shift right. The operator ^ is exclusive or.
 - 
 
17 
 * Source: http://en.wikipedia.org/wiki/Gray_code
 - 
 
18 
 */
 - 
 
19 
static unsigned int binaryToGray(unsigned int num) {
 - 
 
20 
    return (num >> 1) ^ num;
 - 
 
21 
}
 - 
 
22 
 
 - 
 
23 
/*
 - 
 
24 
 * From Wikipedia: http://en.wikipedia.org/wiki/Gray_code
 - 
 
25 
 * The purpose of this function is to convert a reflected binary
 - 
 
26 
 * Gray code number to a binary number.
 - 
 
27 
 */
 - 
 
28 
static unsigned int grayToBinary(unsigned int num){
 - 
 
29 
    unsigned int mask;
 - 
 
30 
    for(mask = num >> 1; mask != 0; mask = mask >> 1)
 - 
 
31 
        num = num ^ mask;
 - 
 
32 
    return num;
 - 
 
33 
}
 - 
 
34 
 
 - 
 
35 
/*
 - 
 
36 
 * Return the Nth bit of an unsigned integer number
 - 
 
37 
 */
 - 
 
38 
static bool getBit(int decimal, int N){
 - 
 
39 
 
 - 
 
40 
    return decimal & 1 << (N-1);
 - 
 
41 
}
 - 
 
42 
 
 - 
 
43 
///*
 - 
 
44 
// * Return the number of bits set in an integer
 - 
 
45 
// */
 - 
 
46 
//static int countBits(int n) {
 - 
 
47 
//  unsigned int c; // c accumulates the total bits set in v
 - 
 
48 
//  for (c = 0; n>0; c++)
 - 
 
49 
//    n &= n - 1; // clear the least significant bit set
 - 
 
50 
//  return c;
 - 
 
51 
//}
 - 
 
52 
 
 - 
 
53 
/*
 - 
 
54 
 * Return the position of the least significant bit that is set
 - 
 
55 
 */
 - 
 
56 
static int leastSignificantBitSet(int x){
 - 
 
57 
  if(x == 0)
 - 
 
58 
      return 0;
 - 
 
59 
 
 - 
 
60 
  int val = 1;
 - 
 
61 
  while(x>>=1)
 - 
 
62 
      val++;
 - 
 
63 
 
 - 
 
64 
  return val;
 - 
 
65 
}
 - 
 
66 
 
 - 
 
67 
//static int get_bit(int decimal, int N){
 - 
 
68 
 
 - 
 
69 
//    // Shifting the 1 for N-1 bits
 - 
 
70 
//    int constant = 1 << (N-1);
 - 
 
71 
 
 - 
 
72 
//    // If the bit is set, return 1
 - 
 
73 
//    if( decimal & constant )
 - 
 
74 
//        return 1;
 - 
 
75 
//    else
 - 
 
76 
//        return 0;
 - 
 
77 
//}
 - 
 
78 
 
 - 
 
79 
static inline unsigned int powi(int num, unsigned int exponent){
 - 
 
80 
 
 - 
 
81 
    if(exponent == 0)
 - 
 
82 
        return 1;
 - 
 
83 
 
 - 
 
84 
    float res = num;
 - 
 
85 
    for(unsigned int i=0; i<exponent-1; i++)
 - 
 
86 
        res *= num;
 - 
 
87 
 
 - 
 
88 
    return res;
 - 
 
89 
}
 - 
 
90 
 
 - 
 
91 
static inline unsigned int twopowi(unsigned int exponent){
 - 
 
92 
 
 - 
 
93 
    return 1 << exponent;
 - 
 
94 
}
 - 
 
95 
 
 12 
 
96 
// Algorithm
 13 
// Algorithm
97 
AlgorithmGrayCodeHorzVert::AlgorithmGrayCodeHorzVert(unsigned int _screenCols, unsigned int _screenRows) : Algorithm(_screenCols, _screenRows){
 14 
AlgorithmGrayCodeHorzVert::AlgorithmGrayCodeHorzVert(unsigned int _screenCols, unsigned int _screenRows) : Algorithm(_screenCols, _screenRows){
98 
 
 15 
 
99 
    NbitsHorz = ceilf(log2f((float)screenCols));
 16 
    NbitsHorz = ceilf(log2f((float)screenCols));
Line 260... Line 177...
260 
        q[i] = cv::Point2f(x, y);
 177 
        q[i] = cv::Point2f(x, y);
261 
 
 178 
 
262 
    }
 179 
    }
263 
}
 180 
}
264 
 
 181 
 
265 
//static cv::Vec3b getColorSubpix(const cv::Mat& img, cv::Point2f pt){
 - 
 
266 
//    assert(!img.empty());
 - 
 
267 
//    assert(img.channels() == 3);
 - 
 
268 
 
 - 
 
269 
//    int x = (int)pt.x;
 - 
 
270 
//    int y = (int)pt.y;
 - 
 
271 
 
 - 
 
272 
//    int x0 = cv::borderInterpolate(x,   img.cols, cv::BORDER_REFLECT_101);
 - 
 
273 
//    int x1 = cv::borderInterpolate(x+1, img.cols, cv::BORDER_REFLECT_101);
 - 
 
274 
//    int y0 = cv::borderInterpolate(y,   img.rows, cv::BORDER_REFLECT_101);
 - 
 
275 
//    int y1 = cv::borderInterpolate(y+1, img.rows, cv::BORDER_REFLECT_101);
 - 
 
276 
 
 - 
 
277 
//    float a = pt.x - (float)x;
 - 
 
278 
//    float c = pt.y - (float)y;
 - 
 
279 
 
 - 
 
280 
//    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)
 - 
 
281 
//                           + (img.at<cv::Vec3b>(y1, x0)[0] * (1.f - a) + img.at<cv::Vec3b>(y1, x1)[0] * a) * c);
 - 
 
282 
//    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)
 - 
 
283 
//                           + (img.at<cv::Vec3b>(y1, x0)[1] * (1.f - a) + img.at<cv::Vec3b>(y1, x1)[1] * a) * c);
 - 
 
284 
//    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)
 - 
 
285 
//                           + (img.at<cv::Vec3b>(y1, x0)[2] * (1.f - a) + img.at<cv::Vec3b>(y1, x1)[2] * a) * c);
 - 
 
286 
 
 - 
 
287 
//    return cv::Vec3b(b, g, r);
 - 
 
288 
//}
 - 
 
289 
 
 - 
 
290 
void AlgorithmGrayCodeHorzVert::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){
 182 
void AlgorithmGrayCodeHorzVert::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){
291 
 
 183 
 
292 
    assert(frames0.size() == N);
 184 
    assert(frames0.size() == N);
293 
    assert(frames1.size() == N);
 185 
    assert(frames1.size() == N);
294 
 
 186 
 
Line 422... Line 314...
422 
    // retrieve color information (at subpixel coordinates)
 314 
    // retrieve color information (at subpixel coordinates)
423 
    color.resize(nMatches);
 315 
    color.resize(nMatches);
424 
    for(int i=0; i<nMatches; i++){
 316 
    for(int i=0; i<nMatches; i++){
425 
        cv::Vec3b c0 = color0.at<cv::Vec3b>(std::floor(q0[i].y), std::floor(q0[i].x));
 317 
        cv::Vec3b c0 = color0.at<cv::Vec3b>(std::floor(q0[i].y), std::floor(q0[i].x));
426 
        cv::Vec3b c1 = color1.at<cv::Vec3b>(std::floor(q1[i].y), std::floor(q1[i].x));
 318 
        cv::Vec3b c1 = color1.at<cv::Vec3b>(std::floor(q1[i].y), std::floor(q1[i].x));
427 
//        cv::Vec3b c0 = getColorSubpix(color0, q0[i]);
 - 
 
428 
//        cv::Vec3b c1 = getColorSubpix(color1, q1[i]);
 - 
 
429 
 
 319 
 
430 
        color[i] = 0.5*c0 + 0.5*c1;
 320 
        color[i] = 0.5*c0 + 0.5*c1;
431 
    }
 321 
    }
432 
 
 322 
 
433 
    //cv::correctMatches(calibration.F, q0, q1, q0, q1);
 323 
    //cv::correctMatches(calibration.F, q0, q1, q0, q1);