Subversion Repositories seema-scanner

Rev

Rev 167 | Rev 207 | 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
- 
 
 3 
//
- 
 
 4 
// This implementation closely follows Henrik Aanaes, "Lecture Notes on Computer Vision" (2014).
- 
 
 5 
//
- 
 
 6 
 
1 
#include "AlgorithmGrayCode.h"
 7 
#include "AlgorithmGrayCode.h"
2 
#include <cmath>
 8 
#include <cmath>
3 
#include "cvtools.h"
 9 
#include "cvtools.h"
4 
 
 - 
 
5 
#ifndef log2f
 - 
 
6 
#define log2f(x) (log(x)/log(2.0))
 10 
#include "algorithmtools.h"
7 
#endif
 - 
 
8 
 
 - 
 
9 
//using namespace std;
 - 
 
10 
 
 - 
 
11 
/*
 - 
 
12 
 * The purpose of this function is to convert an unsigned
 - 
 
13 
 * binary number to reflected binary Gray code.
 - 
 
14 
 *
 - 
 
15 
 * The operator >> is shift right. The operator ^ is exclusive or.
 - 
 
16 
 * Source: http://en.wikipedia.org/wiki/Gray_code
 - 
 
17 
 */
 - 
 
18 
static unsigned int binaryToGray(unsigned int num) {
 - 
 
19 
    return (num >> 1) ^ num;
 - 
 
20 
}
 - 
 
21 
 
 - 
 
22 
/*
 - 
 
23 
 * From Wikipedia: http://en.wikipedia.org/wiki/Gray_code
 - 
 
24 
 * The purpose of this function is to convert a reflected binary
 - 
 
25 
 * Gray code number to a binary number.
 - 
 
26 
 */
 - 
 
27 
static unsigned int grayToBinary(unsigned int num){
 - 
 
28 
    unsigned int mask;
 - 
 
29 
    for(mask = num >> 1; mask != 0; mask = mask >> 1)
 - 
 
30 
        num = num ^ mask;
 - 
 
31 
    return num;
 - 
 
32 
}
 - 
 
33 
 
 - 
 
34 
/*
 - 
 
35 
 * Return the Nth bit of an unsigned integer number
 - 
 
36 
 */
 - 
 
37 
static bool getBit(int decimal, int N){
 - 
 
38 
 
 - 
 
39 
    return decimal & 1 << (N-1);
 - 
 
40 
}
 - 
 
41 
 
 - 
 
42 
///*
 - 
 
43 
// * Return the number of bits set in an integer
 - 
 
44 
// */
 - 
 
45 
//static int countBits(int n) {
 - 
 
46 
//  unsigned int c; // c accumulates the total bits set in v
 - 
 
47 
//  for (c = 0; n>0; c++)
 - 
 
48 
//    n &= n - 1; // clear the least significant bit set
 - 
 
49 
//  return c;
 - 
 
50 
//}
 - 
 
51 
 
 - 
 
52 
/*
 - 
 
53 
 * Return the position of the least significant bit that is set
 - 
 
54 
 */
 - 
 
55 
static int leastSignificantBitSet(int x){
 - 
 
56 
  if(x == 0)
 - 
 
57 
      return 0;
 - 
 
58 
 
 - 
 
59 
  int val = 1;
 - 
 
60 
  while(x>>=1)
 - 
 
61 
      val++;
 - 
 
62 
 
 - 
 
63 
  return val;
 - 
 
64 
}
 - 
 
65 
 
 - 
 
66 
//static int get_bit(int decimal, int N){
 - 
 
67 
 
 - 
 
68 
//    // Shifting the 1 for N-1 bits
 - 
 
69 
//    int constant = 1 << (N-1);
 - 
 
70 
 
 - 
 
71 
//    // If the bit is set, return 1
 - 
 
72 
//    if( decimal & constant )
 - 
 
73 
//        return 1;
 - 
 
74 
//    else
 - 
 
75 
//        return 0;
 - 
 
76 
//}
 - 
 
77 
 
 - 
 
78 
static inline unsigned int powi(int num, unsigned int exponent){
 - 
 
79 
 
 - 
 
80 
    if(exponent == 0)
 - 
 
81 
        return 1;
 - 
 
82 
 
 - 
 
83 
    float res = num;
 - 
 
84 
    for(unsigned int i=0; i<exponent-1; i++)
 - 
 
85 
        res *= num;
 - 
 
86 
 
 - 
 
87 
    return res;
 - 
 
88 
}
 - 
 
89 
 
 - 
 
90 
static inline unsigned int twopowi(unsigned int exponent){
 - 
 
91 
 
 - 
 
92 
    return 1 << exponent;
 - 
 
93 
}
 - 
 
94 
 
 11 
 
95 
// Algorithm
 12 
// Algorithm
96 
AlgorithmGrayCode::AlgorithmGrayCode(unsigned int _screenCols, unsigned int _screenRows) : Algorithm(_screenCols, _screenRows){
 13 
AlgorithmGrayCode::AlgorithmGrayCode(unsigned int _screenCols, unsigned int _screenRows) : Algorithm(_screenCols, _screenRows){
97 
 
 14 
 
98 
    Nbits = ceilf(log2f((float)screenCols)) - 1;
 15 
    Nbits = ceilf(log2f((float)screenCols)) - 1;
Line 164... Line 81...
164 
    std::vector<cv::Vec4i>::iterator it;
 81 
    std::vector<cv::Vec4i>::iterator it;
165 
    it = std::unique(edges.begin(), edges.end(), sortingEqual);
 82 
    it = std::unique(edges.begin(), edges.end(), sortingEqual);
166 
    edges.resize(std::distance(edges.begin(),it));
 83 
    edges.resize(std::distance(edges.begin(),it));
167 
}
 84 
}
168 
 
 85 
 
169 
//static cv::Vec3b getColorSubpix(const cv::Mat& img, cv::Point2f pt){
 - 
 
170 
//    assert(!img.empty());
 - 
 
171 
//    assert(img.channels() == 3);
 - 
 
172 
 
 - 
 
173 
//    int x = (int)pt.x;
 - 
 
174 
//    int y = (int)pt.y;
 - 
 
175 
 
 - 
 
176 
//    int x0 = cv::borderInterpolate(x,   img.cols, cv::BORDER_REFLECT_101);
 - 
 
177 
//    int x1 = cv::borderInterpolate(x+1, img.cols, cv::BORDER_REFLECT_101);
 - 
 
178 
//    int y0 = cv::borderInterpolate(y,   img.rows, cv::BORDER_REFLECT_101);
 - 
 
179 
//    int y1 = cv::borderInterpolate(y+1, img.rows, cv::BORDER_REFLECT_101);
 - 
 
180 
 
 - 
 
181 
//    float a = pt.x - (float)x;
 - 
 
182 
//    float c = pt.y - (float)y;
 - 
 
183 
 
 - 
 
184 
//    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)
 - 
 
185 
//                           + (img.at<cv::Vec3b>(y1, x0)[0] * (1.f - a) + img.at<cv::Vec3b>(y1, x1)[0] * a) * c);
 - 
 
186 
//    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)
 - 
 
187 
//                           + (img.at<cv::Vec3b>(y1, x0)[1] * (1.f - a) + img.at<cv::Vec3b>(y1, x1)[1] * a) * c);
 - 
 
188 
//    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)
 - 
 
189 
//                           + (img.at<cv::Vec3b>(y1, x0)[2] * (1.f - a) + img.at<cv::Vec3b>(y1, x1)[2] * a) * c);
 - 
 
190 
 
 - 
 
191 
//    return cv::Vec3b(b, g, r);
 - 
 
192 
//}
 - 
 
193 
 
 - 
 
194 
void AlgorithmGrayCode::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){
 86 
void AlgorithmGrayCode::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){
195 
 
 87 
 
196 
    assert(frames0.size() == N);
 88 
    assert(frames0.size() == N);
197 
    assert(frames1.size() == N);
 89 
    assert(frames1.size() == N);
198 
 
 90 
 
Line 453... Line 345...
453 
    color.resize(nMatches);
 345 
    color.resize(nMatches);
454 
    for(int i=0; i<nMatches; i++){
 346 
    for(int i=0; i<nMatches; i++){
455 
 
 347 
 
456 
        cv::Vec3b c0 = color0Rect.at<cv::Vec3b>(q0Rect[i][1], q0Rect[i][0]);
 348 
        cv::Vec3b c0 = color0Rect.at<cv::Vec3b>(q0Rect[i][1], q0Rect[i][0]);
457 
        cv::Vec3b c1 = color1Rect.at<cv::Vec3b>(q1Rect[i][1], q1Rect[i][0]);
 349 
        cv::Vec3b c1 = color1Rect.at<cv::Vec3b>(q1Rect[i][1], q1Rect[i][0]);
458 
//        cv::Vec3b c0 = getColorSubpix(color0Rect, q0Rect[i]);
 - 
 
459 
//        cv::Vec3b c1 = getColorSubpix(color1Rect, q0Rect[i]);
 - 
 
460 
 
 350 
 
461 
        color[i] = 0.5*c0 + 0.5*c1;
 351 
        color[i] = 0.5*c0 + 0.5*c1;
462 
    }
 352 
    }
463 
 
 353 
 
464 
    // triangulate points
 354 
    // triangulate points